From 424ef0728f516964c6d90a034db12cd31755c22d Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 1 Aug 2013 16:54:44 +0000 Subject: [PATCH 01/26] Changed a filename to avoid having multiple object files of the same name in the library --- gtsam_unstable/gtsam_unstable.h | 2 +- .../{summarization.cpp => sequentialSummarization.cpp} | 2 +- .../nonlinear/{summarization.h => sequentialSummarization.h} | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename gtsam_unstable/nonlinear/{summarization.cpp => sequentialSummarization.cpp} (94%) rename gtsam_unstable/nonlinear/{summarization.h => sequentialSummarization.h} (100%) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index c4feeccef..2f4254cdc 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -451,7 +451,7 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor { //************************************************************************* // nonlinear //************************************************************************* -#include +#include gtsam::GaussianFactorGraph* summarizeGraphSequential( const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); gtsam::GaussianFactorGraph* summarizeGraphSequential( diff --git a/gtsam_unstable/nonlinear/summarization.cpp b/gtsam_unstable/nonlinear/sequentialSummarization.cpp similarity index 94% rename from gtsam_unstable/nonlinear/summarization.cpp rename to gtsam_unstable/nonlinear/sequentialSummarization.cpp index d691574b0..fa0b58836 100644 --- a/gtsam_unstable/nonlinear/summarization.cpp +++ b/gtsam_unstable/nonlinear/sequentialSummarization.cpp @@ -6,7 +6,7 @@ */ #include -#include +#include #include diff --git a/gtsam_unstable/nonlinear/summarization.h b/gtsam_unstable/nonlinear/sequentialSummarization.h similarity index 100% rename from gtsam_unstable/nonlinear/summarization.h rename to gtsam_unstable/nonlinear/sequentialSummarization.h From ff88c8fdc8b30c2fc9bac28428962bce1bb39e46 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 1 Aug 2013 21:57:03 +0000 Subject: [PATCH 02/26] Added wall time to timing functions --- gtsam/base/timing.cpp | 15 ++++++++------- gtsam/base/timing.h | 3 ++- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index ce5886621..bd9a719b5 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -41,8 +41,9 @@ GTSAM_EXPORT boost::weak_ptr timingCurrent(timingRoot); /* ************************************************************************* */ /* ************************************************************************* */ -void TimingOutline::add(size_t usecs) { +void TimingOutline::add(size_t usecs, size_t usecsWall) { t_ += usecs; + tWall_ += usecsWall; tIt_ += usecs; t2_ += (double(usecs)/1000000.0)*(double(usecs)/1000000.0); ++ n_; @@ -50,7 +51,7 @@ void TimingOutline::add(size_t usecs) { /* ************************************************************************* */ TimingOutline::TimingOutline(const std::string& label, size_t myId) : - myId_(myId), t_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_(0), lastChildOrder_(0), label_(label) + myId_(myId), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_(0), lastChildOrder_(0), label_(label) { #ifdef GTSAM_USING_NEW_BOOST_TIMERS timer_.stop(); @@ -75,9 +76,9 @@ size_t TimingOutline::time() const { void TimingOutline::print(const std::string& outline) const { std::string formattedLabel = label_; boost::replace_all(formattedLabel, "_", " "); - std::cout << outline << "-" << formattedLabel << ": " << double(t_)/1000000.0 << " (" << - n_ << " times, " << double(time())/1000000.0 << " children, min: " << double(tMin_)/1000000.0 << - " max: " << double(tMax_)/1000000.0 << ")\n"; + std::cout << outline << "-" << formattedLabel << ": " << double(t_)/1000000.0 << " CPU (" << + n_ << " times, " << double(tWall_)/1000000.0 << " wall, " << double(time())/1000000.0 << " children, min: " + << double(tMin_)/1000000.0 << " max: " << double(tMax_)/1000000.0 << ")\n"; // Order children typedef FastMap > ChildOrder; ChildOrder childOrder; @@ -163,11 +164,11 @@ void TimingOutline::tocInternal() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(!timer_.is_stopped()); timer_.stop(); - add((timer_.elapsed().user + timer_.elapsed().system) / 1000); + add((timer_.elapsed().user + timer_.elapsed().system) / 1000, timer_.elapsed().wall / 1000); #else assert(timerActive_); double elapsed = timer_.elapsed(); - add(size_t(elapsed * 1000000.0)); + add(size_t(elapsed * 1000000.0), 0); *timerActive_ = false; #endif } diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index b1cbe40ef..22392ce00 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -48,6 +48,7 @@ namespace gtsam { protected: size_t myId_; size_t t_; + size_t tWall_; double t2_ ; /* cache the \sum t_i^2 */ size_t tIt_; size_t tMax_; @@ -65,7 +66,7 @@ namespace gtsam { boost::timer timer_; gtsam::ValueWithDefault timerActive_; #endif - void add(size_t usecs); + void add(size_t usecs, size_t usecsWall); public: TimingOutline(const std::string& label, size_t myId); size_t time() const; From fcc6b804d1471d771073f2a55bcb83079ceae84a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 1 Aug 2013 21:57:05 +0000 Subject: [PATCH 03/26] Switched SolverComparer to QR --- examples/SolverComparer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index a20a3ef78..d66929d5a 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -394,7 +394,9 @@ void runBatch() measurements.push_back(boost::make_shared >(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); gttic_(Create_optimizer); - GaussNewtonOptimizer optimizer(measurements, initial); + GaussNewtonParams params; + params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR; + GaussNewtonOptimizer optimizer(measurements, initial, params); gttoc_(Create_optimizer); double lastError; do { From a48864452bd6e5d4226693eda16f9d9f45cb488a Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Fri, 2 Aug 2013 13:28:29 +0000 Subject: [PATCH 04/26] fix GTSAM_POSE3_EXPMAP warnings. Add GTSAM_ROT3_EXPMAP option. --- CMakeLists.txt | 7 ++----- gtsam/config.h.in | 7 ++++++- gtsam/geometry/Rot3.h | 19 ++++++++++++------- 3 files changed, 20 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4fc7fc6bc..b1678a5c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,11 +53,12 @@ if(GTSAM_UNSTABLE_AVAILABLE) endif() option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON) option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" OFF) -option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF) +option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) if(NOT MSVC) option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" OFF) endif() option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF) +option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) # Options relating to MATLAB wrapper @@ -181,10 +182,6 @@ if(GTSAM_ENABLE_CONSISTENCY_CHECKS) add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) endif() -if(GTSAM_POSE3_EXPMAP) - add_definitions(-DGTSAM_POSE3_EXPMAP) -endif() - ############################################################################### # Add components diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 9ecfcb3d7..120e06565 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -25,4 +25,9 @@ #cmakedefine GTSAM_USE_QUATERNIONS // Whether GTSAM is compiled to use Pose3::EXPMAP as the default coordinates mode for Pose3's retract and localCoordinates (otherwise, Pose3::FIRST_ORDER will be used) -#cmakedefine GTSAM_POSE3_EXPMAP \ No newline at end of file +#cmakedefine GTSAM_POSE3_EXPMAP + +// Whether GTSAM is compiled to use Rot3::EXPMAP as the default coordinates mode for Rot3's retract and localCoordinates (otherwise, Pose3::CAYLEY will be used) +#ifndef GTSAM_USE_QUATERNIONS + #cmakedefine GTSAM_ROT3_EXPMAP +#endif \ No newline at end of file diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 9316ab33d..f4a25ff94 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -25,13 +25,18 @@ // You can override the default coordinate mode using this flag #ifndef ROT3_DEFAULT_COORDINATES_MODE -#ifdef GTSAM_USE_QUATERNIONS -// Exponential map is very cheap for quaternions -#define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP -#else -// For rotation matrices, the Cayley transform is a fast retract alternative -#define ROT3_DEFAULT_COORDINATES_MODE Rot3::CAYLEY -#endif + #ifdef GTSAM_USE_QUATERNIONS + // Exponential map is very cheap for quaternions + #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP + #else + // If user doesn't require GTSAM_ROT3_EXPMAP in cmake when building + #ifndef GTSAM_ROT3_EXPMAP + // For rotation matrices, the Cayley transform is a fast retract alternative + #define ROT3_DEFAULT_COORDINATES_MODE Rot3::CAYLEY + #else + #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP + #endif + #endif #endif #include From ef99e1bf1a49e8bcb8ad4ee7cb14b8737119f1a1 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Fri, 2 Aug 2013 13:29:54 +0000 Subject: [PATCH 05/26] add some comments for Constrained model --- gtsam/linear/NoiseModel.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 5a953eda5..5c8d591b9 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -253,6 +253,9 @@ void Constrained::print(const std::string& name) const { /* ************************************************************************* */ Vector Constrained::whiten(const Vector& v) const { + // If sigmas[i] is not 0 then divide v[i] by sigmas[i], as usually done in + // other normal Gaussian noise model. Otherwise, sigmas[i] = 0 indicating + // a hard constraint, we don't do anything. const Vector& a = v; const Vector& b = sigmas_; size_t n = a.size(); @@ -287,6 +290,9 @@ Matrix Constrained::Whiten(const Matrix& H) const { /* ************************************************************************* */ void Constrained::WhitenInPlace(Matrix& H) const { // selective scaling + // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas) + // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0, + // indicating a hard constraint, we leave H's row i in place. vector_scale_inplace(invsigmas(), H, true); } From 95ffb0c4ea4199023ed776901f76c74735459bc8 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Fri, 2 Aug 2013 13:33:23 +0000 Subject: [PATCH 06/26] fix ofstream filename c_str compilation issues --- examples/SolverComparer.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index d66929d5a..b46ea60b8 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -156,7 +156,7 @@ int main(int argc, char *argv[]) { if(!inputFile.empty()) { cout << "Reading input file " << inputFile << endl; - std::ifstream readerStream(inputFile, ios::binary); + std::ifstream readerStream(inputFile.c_str(), ios::binary); boost::archive::binary_iarchive reader(readerStream); reader >> initial; } @@ -338,9 +338,10 @@ void runIncremental() { try { cout << "Writing output file " << outputFile << endl; - std::ofstream writerStream(outputFile, ios::binary); + std::ofstream writerStream(outputFile.c_str(), ios::binary); boost::archive::binary_oarchive writer(writerStream); - writer << isam2.calculateEstimate(); + Values estimates = isam2.calculateEstimate(); + writer << estimates; } catch(std::exception& e) { cout << e.what() << endl; exit(1); @@ -420,7 +421,7 @@ void runBatch() { try { cout << "Writing output file " << outputFile << endl; - std::ofstream writerStream(outputFile, ios::binary); + std::ofstream writerStream(outputFile.c_str(), ios::binary); boost::archive::binary_oarchive writer(writerStream); writer << optimizer.values(); } catch(std::exception& e) { @@ -437,14 +438,14 @@ void runCompare() cout << "Reading solution file " << compareFile1 << endl; { - std::ifstream readerStream(compareFile1, ios::binary); + std::ifstream readerStream(compareFile1.c_str(), ios::binary); boost::archive::binary_iarchive reader(readerStream); reader >> soln1; } cout << "Reading solution file " << compareFile2 << endl; { - std::ifstream readerStream(compareFile2, ios::binary); + std::ifstream readerStream(compareFile2.c_str(), ios::binary); boost::archive::binary_iarchive reader(readerStream); reader >> soln2; } @@ -492,7 +493,7 @@ void runPerturb() // Write results try { cout << "Writing output file " << outputFile << endl; - std::ofstream writerStream(outputFile, ios::binary); + std::ofstream writerStream(outputFile.c_str(), ios::binary); boost::archive::binary_oarchive writer(writerStream); writer << perturbed; } catch(std::exception& e) { From 3545b114ab6d0719c76e317c01f25597b89f82dd Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 2 Aug 2013 15:21:36 +0000 Subject: [PATCH 07/26] Altered [CHECK|EXPECT]_DOUBLES_EQUAL such that nan values automatically cause the test to fail. The previous approach would always return pass if one of the compared values is nan, resulting in tests passing when they shouldn't. If you have a test where you want to check that the value is nan, use EXPECT(isnan(x));, where x is the value you want to test. --- CppUnitLite/Test.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 9f97757b6..82040dceb 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -130,7 +130,7 @@ boost::lexical_cast(actualTemp))); return; } } #define DOUBLES_EQUAL(expected,actual,threshold)\ { double actualTemp = actual; \ double expectedTemp = expected; \ - if (fabs ((expectedTemp)-(actualTemp)) > threshold) \ + if (isnan(actualTemp) || isnan(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \ { result_.addFailure (Failure (name_, __FILE__, __LINE__, \ boost::lexical_cast((double)expectedTemp), boost::lexical_cast((double)actualTemp))); return; } } @@ -150,7 +150,7 @@ boost::lexical_cast(actualTemp))); } } #define EXPECT_DOUBLES_EQUAL(expected,actual,threshold)\ { double actualTemp = actual; \ double expectedTemp = expected; \ - if (fabs ((expectedTemp)-(actualTemp)) > threshold) \ + if (isnan(actualTemp) || isnan(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \ { result_.addFailure (Failure (name_, __FILE__, __LINE__, \ boost::lexical_cast((double)expectedTemp), boost::lexical_cast((double)actualTemp))); } } From 34dc67c1af9282744c5ad16baed7d7028e8914e5 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 2 Aug 2013 16:04:17 +0000 Subject: [PATCH 08/26] Brought some relevant changes from aspn-imu-factor branch (is correct this time) --- gtsam.h | 56 +- gtsam/navigation/ImuBias.h | 51 +- .../testInertialNavFactor_GlobalVelocity.cpp | 68 ++ gtsam/nonlinear/ISAM2.cpp | 15 +- gtsam/nonlinear/ISAM2.h | 9 + gtsam_unstable/dynamics/PoseRTV.cpp | 2 +- gtsam_unstable/gtsam_unstable.h | 69 ++ gtsam_unstable/slam/CombinedImuFactor.h | 646 ++++++++++++++++++ gtsam_unstable/slam/ImuFactor.h | 559 +++++++++++++++ .../slam/tests/testCombinedImuFactor.cpp | 298 ++++++++ gtsam_unstable/slam/tests/testImuFactor.cpp | 569 +++++++++++++++ 11 files changed, 2306 insertions(+), 36 deletions(-) create mode 100644 gtsam_unstable/slam/CombinedImuFactor.h create mode 100644 gtsam_unstable/slam/ImuFactor.h create mode 100644 gtsam_unstable/slam/tests/testCombinedImuFactor.cpp create mode 100644 gtsam_unstable/slam/tests/testImuFactor.cpp diff --git a/gtsam.h b/gtsam.h index a28bade6a..65c7ffcaa 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2079,8 +2079,9 @@ virtual class ISAM2 : gtsam::ISAM2BayesTree { gtsam::Values getLinearizationPoint() const; gtsam::Values calculateEstimate() const; - Matrix marginalCovariance(size_t key) const; + gtsam::Value calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; + Matrix marginalCovariance(size_t key) const; gtsam::VectorValues getDelta() const; gtsam::NonlinearFactorGraph getFactorsUnsafe() const; gtsam::Ordering getOrdering() const; @@ -2122,8 +2123,8 @@ class NonlinearISAM { #include #include -template -virtual class PriorFactor : gtsam::NoiseModelFactor { +template +virtual class PriorFactor : gtsam::NonlinearFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2133,8 +2134,8 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { #include -template -virtual class BetweenFactor : gtsam::NoiseModelFactor { +template +virtual class BetweenFactor : gtsam::NonlinearFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; @@ -2144,8 +2145,8 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include -template -virtual class NonlinearEquality : gtsam::NoiseModelFactor { +template +virtual class NonlinearEquality : gtsam::NonlinearFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); // Constructor - allows inexact evaluation @@ -2284,6 +2285,47 @@ pair load2D(string filename, pair load2D_robust(string filename, gtsam::noiseModel::Base* model); +//************************************************************************* +// Navigation +//************************************************************************* +namespace imuBias { +#include + +virtual class ConstantBias : gtsam::Value { + // Standard Constructor + ConstantBias(); + ConstantBias(Vector biasAcc, Vector biasGyro); + + // Testable + void print(string s) const; + bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; + + // Group + static gtsam::imuBias::ConstantBias identity(); + gtsam::imuBias::ConstantBias inverse() const; + gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::imuBias::ConstantBias retract(Vector v) const; + Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; + + // Lie Group + static gtsam::imuBias::ConstantBias Expmap(Vector v); + static Vector Logmap(const gtsam::imuBias::ConstantBias& b); + + // Standard Interface + Vector vector() const; + Vector accelerometer() const; + Vector gyroscope() const; + Vector correctAccelerometer(Vector measurement) const; + Vector correctGyroscope(Vector measurement) const; +}; + +}///\namespace imuBias + //************************************************************************* // Utilities //************************************************************************* diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 352bb1918..12e702b7c 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -70,21 +70,21 @@ namespace imuBias { const Vector3& gyroscope() const { return biasGyro_; } /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ - Vector correctAccelerometer(const Vector3& measurment, boost::optional H=boost::none) const { + Vector correctAccelerometer(const Vector3& measurement, boost::optional H=boost::none) const { if (H){ H->resize(3,6); (*H) << -Matrix3::Identity(), Matrix3::Zero(); } - return measurment - biasAcc_; + return measurement - biasAcc_; } /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ - Vector correctGyroscope(const Vector3& measurment, boost::optional H=boost::none) const { + Vector correctGyroscope(const Vector3& measurement, boost::optional H=boost::none) const { if (H){ H->resize(3,6); (*H) << Matrix3::Zero(), -Matrix3::Identity(); } - return measurment - biasGyro_; + return measurement - biasGyro_; } // // H1: Jacobian w.r.t. IMUBias @@ -148,30 +148,14 @@ namespace imuBias { inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } /** @return the local coordinates of another object */ - inline Vector localCoordinates(const ConstantBias& t2) const { return t2.vector() - vector(); } + inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); } /// @} /// @name Group /// @{ - ConstantBias compose(const ConstantBias& b2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = gtsam::Matrix::Identity(6,6); - if(H2) *H2 = gtsam::Matrix::Identity(6,6); - - return ConstantBias(biasAcc_ + b2.biasAcc_, biasGyro_ + b2.biasGyro_); - } - - /** between operation */ - ConstantBias between(const ConstantBias& b2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -gtsam::Matrix::Identity(6,6); - if(H2) *H2 = gtsam::Matrix::Identity(6,6); - - return ConstantBias(b2.biasAcc_ - biasAcc_, b2.biasGyro_ - biasGyro_); - } + /** identity for group operation */ + static ConstantBias identity() { return ConstantBias(); } /** invert the object and yield a new one */ inline ConstantBias inverse(boost::optional H=boost::none) const { @@ -180,6 +164,25 @@ namespace imuBias { return ConstantBias(-biasAcc_, -biasGyro_); } + ConstantBias compose(const ConstantBias& b, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = gtsam::Matrix::Identity(6,6); + if(H2) *H2 = gtsam::Matrix::Identity(6,6); + + return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); + } + + /** between operation */ + ConstantBias between(const ConstantBias& b, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -gtsam::Matrix::Identity(6,6); + if(H2) *H2 = gtsam::Matrix::Identity(6,6); + + return ConstantBias(b.biasAcc_ - biasAcc_, b.biasGyro_ - biasGyro_); + } + /// @} /// @name Lie Group /// @{ @@ -188,7 +191,7 @@ namespace imuBias { static inline ConstantBias Expmap(const Vector& v) { return ConstantBias(v.head(3), v.tail(3)); } /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const ConstantBias& p) { return p.vector(); } + static inline Vector Logmap(const ConstantBias& b) { return b.vector(); } /// @} diff --git a/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp index 6622e8bf9..4ba59d052 100644 --- a/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -677,6 +677,74 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); } + + + + +#include +/* ************************************************************************* */ +TEST( InertialNavFactor_GlobalVelocity, LinearizeTiming) +{ + gtsam::Key PoseKey1(11); + gtsam::Key PoseKey2(12); + gtsam::Key VelKey1(21); + gtsam::Key VelKey2(22); + gtsam::Key BiasKey1(31); + + double measurement_dt(0.1); + Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); + Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + + // Second test: zero angular motion, some acceleration - generated in matlab + Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); + + InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + + Rot3 R1(0.487316618, 0.125253866, 0.86419557, + 0.580273724, 0.693095498, -0.427669306, + -0.652537293, 0.709880342, 0.265075427); + Point3 t1(2.0,1.0,3.0); + Pose3 Pose1(R1, t1); + LieVector Vel1(3,0.5,-0.5,0.4); + Rot3 R2(0.473618898, 0.119523052, 0.872582019, + 0.609241153, 0.67099888, -0.422594037, + -0.636011287, 0.731761397, 0.244979388); + Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); + Pose3 Pose2(R2, t2); + Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); + LieVector Vel2 = Vel1.compose( dv ); + imuBias::ConstantBias Bias1; + + Values values; + values.insert(PoseKey1, Pose1); + values.insert(PoseKey2, Pose2); + values.insert(VelKey1, Vel1); + values.insert(VelKey2, Vel2); + values.insert(BiasKey1, Bias1); + + Ordering ordering; + ordering.push_back(PoseKey1); + ordering.push_back(VelKey1); + ordering.push_back(BiasKey1); + ordering.push_back(PoseKey2); + ordering.push_back(VelKey2); + + GaussianFactorGraph graph; + gttic_(LinearizeTiming); + for(size_t i = 0; i < 100000; ++i) { + GaussianFactor::shared_ptr g = f.linearize(values, ordering); + graph.push_back(g); + } + gttoc_(LinearizeTiming); + tictoc_finishedIteration_(); + tictoc_print_(); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index dc764c164..dd3361fb2 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -1019,10 +1019,10 @@ Values ISAM2::calculateEstimate() const { } /* ************************************************************************* */ -Matrix ISAM2::marginalCovariance(Index key) const { - return marginalFactor(ordering_[key], - params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky) - ->information().inverse(); +const Value& ISAM2::calculateEstimate(Key key) const { + const Index index = getOrdering()[key]; + const Vector& delta = getDelta()[index]; + return *theta_.at(key).retract_(delta); } /* ************************************************************************* */ @@ -1032,6 +1032,13 @@ Values ISAM2::calculateBestEstimate() const { return theta_.retract(delta, ordering_); } +/* ************************************************************************* */ +Matrix ISAM2::marginalCovariance(Index key) const { + return marginalFactor(ordering_[key], + params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky) + ->information().inverse(); +} + /* ************************************************************************* */ const VectorValues& ISAM2::getDelta() const { if(!deltaUptodate_) diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 687723392..fb7762ffa 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -585,6 +585,15 @@ public: template VALUE calculateEstimate(Key key) const; + /** Compute an estimate for a single variable using its incomplete linear delta computed + * during the last update. This is faster than calling the no-argument version of + * calculateEstimate, which operates on all variables. This is a non-templated version + * that returns a Value base class for use with the MATLAB wrapper. + * @param key + * @return + */ + GTSAM_EXPORT const Value& calculateEstimate(Key key) const; + /** Return marginal on any variable as a covariance matrix */ GTSAM_EXPORT Matrix marginalCovariance(Index key) const; diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index dd2671530..99964c9db 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -93,7 +93,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { /* ************************************************************************* */ PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } PoseRTV PoseRTV::inverse(boost::optional H1) const { - if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); + if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); return PoseRTV(Rt_.inverse(), v_.inverse()); } diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 2f4254cdc..061dd6ec3 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -13,6 +13,7 @@ virtual class gtsam::Point3; virtual class gtsam::Rot3; virtual class gtsam::Pose3; virtual class gtsam::noiseModel::Base; +virtual class gtsam::imuBias::ConstantBias; virtual class gtsam::NonlinearFactor; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; @@ -614,6 +615,74 @@ virtual class InvDepthFactorVariant3b : gtsam::NonlinearFactor { InvDepthFactorVariant3b(size_t poseKey1, size_t poseKey2, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); }; +#include +class ImuFactorPreintegratedMeasurements { + // Standard Constructor + ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); + ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); +}; + +virtual class ImuFactor : gtsam::NonlinearFactor { + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, + const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, + const gtsam::noiseModel::Base* model); + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, + const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, + const gtsam::noiseModel::Base* model, const gtsam::Pose3& body_P_sensor); + + // Standard Interface + gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; + void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, + const gtsam::imuBias::ConstantBias& bias, + const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, + Vector gravity, Vector omegaCoriolis) const; +}; + + +#include +class CombinedImuFactorPreintegratedMeasurements { + // Standard Constructor + CombinedImuFactorPreintegratedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit); + CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); +}; + +virtual class CombinedImuFactor : gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, + const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis, + const gtsam::noiseModel::Base* model); + + // Standard Interface + gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; + void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, + const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j, + const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, + Vector gravity, Vector omegaCoriolis) const; +}; + + #include class Mechanization_bRn2 { Mechanization_bRn2(); diff --git a/gtsam_unstable/slam/CombinedImuFactor.h b/gtsam_unstable/slam/CombinedImuFactor.h new file mode 100644 index 000000000..1879802da --- /dev/null +++ b/gtsam_unstable/slam/CombinedImuFactor.h @@ -0,0 +1,646 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CombinedImuFactor.h + * @author Luca Carlone, Stephen Williams, Richard Roberts + **/ + +#pragma once + +/* GTSAM includes */ +#include +#include +#include +#include +#include +#include + +/* External or standard includes */ +#include + + +namespace gtsam { + + /** + * + * @addtogroup SLAM + * + * REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built + * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + */ + + class CombinedImuFactor: public NoiseModelFactor6 { + + public: + + /** Struct to store results of preintegrating IMU measurements. Can be build + * incrementally so as to avoid costly integration at time of factor construction. */ + + /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jr; + if (normx < 10e-8){ + Jr = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + + ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian + } + return Jr; + } + + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jrinv; + + if (normx < 10e-8){ + Jrinv = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jrinv = Matrix3::Identity() + + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + } + return Jrinv; + } + + /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) + * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ + class CombinedPreintegratedMeasurements { + public: + imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration + Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector + ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + + Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) + Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) + double deltaTij; ///< Time interval from i to j + + Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases + ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] + /** Default constructor, initialize with no IMU measurements */ + CombinedPreintegratedMeasurements( + const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases + const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc + const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc + const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc + const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) + const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) + const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements + ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) + ) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) + { + // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) + measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3); + + } + + CombinedPreintegratedMeasurements() : + biasHat(imuBias::ConstantBias()), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) + { + } + + /** print */ + void print(const std::string& s = "Preintegrated Measurements:") const { + std::cout << s << std::endl; + biasHat.print(" biasHat"); + std::cout << " deltaTij " << deltaTij << std::endl; + std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl; + deltaRij.print(" deltaRij "); + std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl; + std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; + } + + /** equals */ + bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const { + return biasHat.equals(expected.biasHat, tol) + && equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol) + && equal_with_abs_tol(deltaPij, expected.deltaPij, tol) + && equal_with_abs_tol(deltaVij, expected.deltaVij, tol) + && deltaRij.equals(expected.deltaRij, tol) + && std::fabs(deltaTij - expected.deltaTij) < tol + && equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol) + && equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol) + && equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol) + && equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol) + && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); + } + + /** Add a single IMU measurement to the preintegration. */ + void integrateMeasurement( + const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) + const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) + double deltaT, ///< Time step + boost::optional body_P_sensor = boost::none ///< Sensor frame + ) { + // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. + // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty + Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc); + Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame + if(body_P_sensor){ + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } + + const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement + const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ + // delPdelBiasAcc += delVdelBiasAcc * deltaT; + // delPdelBiasOmega += delVdelBiasOmega * deltaT; + delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() + * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + + delVdelBiasAcc += -deltaRij.matrix() * deltaT; + delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; + delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; + + // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that + // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we + // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + Matrix3 Z_3x3 = Matrix3::Zero(); + Matrix3 I_3x3 = Matrix3::Identity(); + const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) + const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); + + Rot3 Rot_j = deltaRij * Rincr; + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j); + + // Single Jacobians to propagate covariance + Matrix3 H_pos_pos = I_3x3; + Matrix3 H_pos_vel = I_3x3 * deltaT; + Matrix3 H_pos_angles = Z_3x3; + + Matrix3 H_vel_pos = Z_3x3; + Matrix3 H_vel_vel = I_3x3; + Matrix3 H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); + Matrix3 H_vel_biasacc = - deltaRij.matrix() * deltaT; + + Matrix3 H_angles_pos = Z_3x3; + Matrix3 H_angles_vel = Z_3x3; + Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; + Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + + // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix F(15,15); + F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, + H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, + H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + + + // first order uncertainty propagation + // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() + + Matrix G_measCov_Gt = Matrix::Zero(15,15); + // BLOCK DIAGONAL TERMS + G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3); + + G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) * + (measurementCovariance.block(3,3,3,3) + measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * + (H_vel_biasacc.transpose()); + + G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) * + (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) + measurementCovariance.block(18,18,3,3) ) * + (H_angles_biasomega.transpose()); + + G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3); + + G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3); + + // OFF BLOCK DIAGONAL TERMS + Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3); + G_measCov_Gt.block(3,9,3,3) = block24; + G_measCov_Gt.block(9,3,3,3) = block24.transpose(); + + Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3); + G_measCov_Gt.block(6,12,3,3) = block35; + G_measCov_Gt.block(12,6,3,3) = block35.transpose(); + + PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt; + + // Update preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + deltaVij += deltaRij.matrix() * correctedAcc * deltaT; + deltaRij = deltaRij * Rincr; + deltaTij += deltaT; + } + + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0){ + + // Note: all delta terms refer to an IMU\sensor system at t0 + + Vector body_t_a_body = msr_acc_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } + + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles){ + + // Note: all delta terms refer to an IMU\sensor system at t0 + + // Calculate the corrected measurements using the Bias object + Vector body_t_omega_body= msr_gyro_t; + + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(biasHat); + ar & BOOST_SERIALIZATION_NVP(measurementCovariance); + ar & BOOST_SERIALIZATION_NVP(deltaPij); + ar & BOOST_SERIALIZATION_NVP(deltaVij); + ar & BOOST_SERIALIZATION_NVP(deltaRij); + ar & BOOST_SERIALIZATION_NVP(deltaTij); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega); + } + }; + + private: + + typedef CombinedImuFactor This; + typedef NoiseModelFactor6 Base; + + CombinedPreintegratedMeasurements preintegratedMeasurements_; + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + public: + + /** Shorthand for a smart pointer to a factor */ + typedef typename boost::shared_ptr shared_ptr; + + /** Default constructor - only use for serialization */ + CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} + + /** Constructor */ + CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, + const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : + Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), + preintegratedMeasurements_(preintegratedMeasurements), + gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor) { + } + + virtual ~CombinedImuFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "CombinedImuFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "," + << keyFormatter(this->key6()) << ")\n"; + preintegratedMeasurements_.print(" preintegrated measurements:"); + std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; + this->noiseModel_->print(" noise model: "); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && equal_with_abs_tol(gravity_, e->gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /** Access the preintegrated measurements. */ + const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { + return preintegratedMeasurements_; } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none, + boost::optional H6 = boost::none) const + { + + const double& deltaTij = preintegratedMeasurements_.deltaTij; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3 Rot_i = pose_i.rotation(); + const Rot3 Rot_j = pose_j.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + const Vector3 pos_j = pose_j.translation().vector(); + + // We compute factor's Jacobians, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + + const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(15,6); + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + - Rot_i.matrix(), + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + Matrix3::Zero(), + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(), + //dBiasAcc/dPi + Matrix3::Zero(), Matrix3::Zero(), + //dBiasOmega/dPi + Matrix3::Zero(), Matrix3::Zero(); + } + + if(H2) { + H2->resize(15,3); + (*H2) << + // dfP/dVi + - Matrix3::Identity() * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - Matrix3::Identity() + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(), + //dBiasAcc/dVi + Matrix3::Zero(), + //dBiasOmega/dVi + Matrix3::Zero(); + + } + + if(H3) { + + H3->resize(15,6); + (*H3) << + // dfP/dPosej + Matrix3::Zero(), Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(), + //dBiasAcc/dPosej + Matrix3::Zero(), Matrix3::Zero(), + //dBiasOmega/dPosej + Matrix3::Zero(), Matrix3::Zero(); + } + + if(H4) { + H4->resize(15,3); + (*H4) << + // dfP/dVj + Matrix3::Zero(), + // dfV/dVj + Matrix3::Identity(), + // dfR/dVj + Matrix3::Zero(), + //dBiasAcc/dVj + Matrix3::Zero(), + //dBiasOmega/dVj + Matrix3::Zero(); + } + + if(H5) { + const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); + const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; + + H5->resize(15,6); + (*H5) << + // dfP/dBias_i + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc, + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega, + // dfV/dBias_i + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc, + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega, + // dfR/dBias_i + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), + //dBiasAcc/dBias_i + -Matrix3::Identity(), Matrix3::Zero(), + //dBiasOmega/dBias_i + Matrix3::Zero(), -Matrix3::Identity(); + } + + if(H6) { + + H6->resize(15,6); + (*H6) << + // dfP/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + // dfV/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + // dfR/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + //dBiasAcc/dBias_j + Matrix3::Identity(), Matrix3::Zero(), + //dBiasOmega/dBias_j + Matrix3::Zero(), Matrix3::Identity(); + } + + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 fp = + pos_j - pos_i + - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) + - vel_i * deltaTij + + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * gravity_ * deltaTij*deltaTij; + + const Vector3 fv = + vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) + + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + - gravity_ * deltaTij; + + const Vector3 fR = Rot3::Logmap(fRhat); + + const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); + + const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); + + Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 + + return r; + } + + + /** predicted states from IMU */ + static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, + const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) + { + + const double& deltaTij = preintegratedMeasurements.deltaTij; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat.gyroscope(); + + const Rot3 Rot_i = pose_i.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; + + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + + pose_j = Pose3( Rot_j, Point3(pos_j) ); + + bias_j = bias_i; + } + + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor6", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(gravity_); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } + }; // \class CombinedImuFactor + + typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/ImuFactor.h b/gtsam_unstable/slam/ImuFactor.h new file mode 100644 index 000000000..4a8a84b6d --- /dev/null +++ b/gtsam_unstable/slam/ImuFactor.h @@ -0,0 +1,559 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ImuFactor.h + * @author Luca Carlone, Stephen Williams, Richard Roberts + **/ + +#pragma once + +/* GTSAM includes */ +#include +#include +#include +#include +#include +#include + +/* External or standard includes */ +#include + + +namespace gtsam { + + /** + * + * @addtogroup SLAM + * * REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built + * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + */ + + class ImuFactor: public NoiseModelFactor5 { + + public: + + /** Struct to store results of preintegrating IMU measurements. Can be build + * incrementally so as to avoid costly integration at time of factor construction. */ + + /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jr; + if (normx < 10e-8){ + Jr = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + + ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian + } + return Jr; + } + + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jrinv; + + if (normx < 10e-8){ + Jrinv = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jrinv = Matrix3::Identity() + + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + } + return Jrinv; + } + + /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) + * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ + class PreintegratedMeasurements { + public: + imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration + Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) + + Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i) + Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) + Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) + double deltaTij; ///< Time interval from i to j + + Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + + /** Default constructor, initialize with no IMU measurements */ + PreintegratedMeasurements( + const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases + const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc + const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc + const Matrix3& integrationErrorCovariance ///< Covariance matrix of measuredAcc + ) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9) + { + measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; + PreintMeasCov = Matrix::Zero(9,9); + } + + PreintegratedMeasurements() : + biasHat(imuBias::ConstantBias()), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9) + { + measurementCovariance = Matrix::Zero(9,9); + PreintMeasCov = Matrix::Zero(9,9); + } + + /** print */ + void print(const std::string& s = "Preintegrated Measurements:") const { + std::cout << s << std::endl; + biasHat.print(" biasHat"); + std::cout << " deltaTij " << deltaTij << std::endl; + std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl; + deltaRij.print(" deltaRij "); + std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl; + std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; + } + + /** equals */ + bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const { + return biasHat.equals(expected.biasHat, tol) + && equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol) + && equal_with_abs_tol(deltaPij, expected.deltaPij, tol) + && equal_with_abs_tol(deltaVij, expected.deltaVij, tol) + && deltaRij.equals(expected.deltaRij, tol) + && std::fabs(deltaTij - expected.deltaTij) < tol + && equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol) + && equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol) + && equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol) + && equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol) + && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); + } + + /** Add a single IMU measurement to the preintegration. */ + void integrateMeasurement( + const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) + const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) + double deltaT, ///< Time step + boost::optional body_P_sensor = boost::none ///< Sensor frame + ) { + + // NOTE: order is important here because each update uses old values. + // First we compensate the measurements for the bias + Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc); + Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame + if(body_P_sensor){ + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + + correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } + + const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement + + const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ + // delPdelBiasAcc += delVdelBiasAcc * deltaT; + // delPdelBiasOmega += delVdelBiasOmega * deltaT; + delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() + * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + delVdelBiasAcc += -deltaRij.matrix() * deltaT; + delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; + delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; + + // Update preintegrated mesurements covariance + /* ----------------------------------------------------------------------------------------------------------------------- */ + Matrix3 Z_3x3 = Matrix3::Zero(); + Matrix3 I_3x3 = Matrix3::Identity(); + const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) + const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); + + Rot3 Rot_j = deltaRij * Rincr; + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j); + + // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that + // can be seen as a prediction phase in an EKF framework + Matrix H_pos_pos = I_3x3; + Matrix H_pos_vel = I_3x3 * deltaT; + Matrix H_pos_angles = Z_3x3; + + Matrix H_vel_pos = Z_3x3; + Matrix H_vel_vel = I_3x3; + Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); + + Matrix H_angles_pos = Z_3x3; + Matrix H_angles_vel = Z_3x3; + Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; + // analytic expression corresponding to the following numerical derivative + // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + + // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix F(9,9); + F << H_pos_pos, H_pos_vel, H_pos_angles, + H_vel_pos, H_vel_vel, H_vel_angles, + H_angles_pos, H_angles_vel, H_angles_angles; + + + // first order uncertainty propagation + // the deltaT allows to pass from continuous time noise to discrete time noise + PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ; + + // Update preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + deltaVij += deltaRij.matrix() * correctedAcc * deltaT; + deltaRij = deltaRij * Rincr; + deltaTij += deltaT; + } + + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0){ + + // Note: all delta terms refer to an IMU\sensor system at t0 + + Vector body_t_a_body = msr_acc_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } + + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles){ + + // Note: all delta terms refer to an IMU\sensor system at t0 + + // Calculate the corrected measurements using the Bias object + Vector body_t_omega_body= msr_gyro_t; + + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(biasHat); + ar & BOOST_SERIALIZATION_NVP(measurementCovariance); + ar & BOOST_SERIALIZATION_NVP(deltaPij); + ar & BOOST_SERIALIZATION_NVP(deltaVij); + ar & BOOST_SERIALIZATION_NVP(deltaRij); + ar & BOOST_SERIALIZATION_NVP(deltaTij); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega); + } + }; + + private: + + typedef ImuFactor This; + typedef NoiseModelFactor5 Base; + + PreintegratedMeasurements preintegratedMeasurements_; + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + public: + + /** Shorthand for a smart pointer to a factor */ + typedef typename boost::shared_ptr shared_ptr; + + /** Default constructor - only use for serialization */ + ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} + + /** Constructor */ + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, + const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : + Base(model, pose_i, vel_i, pose_j, vel_j, bias), + preintegratedMeasurements_(preintegratedMeasurements), + gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor) { + } + + virtual ~ImuFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "ImuFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << ")\n"; + preintegratedMeasurements_.print(" preintegrated measurements:"); + std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; + this->noiseModel_->print(" noise model: "); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && equal_with_abs_tol(gravity_, e->gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /** Access the preintegrated measurements. */ + const PreintegratedMeasurements& preintegratedMeasurements() const { + return preintegratedMeasurements_; } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none) const + { + + const double& deltaTij = preintegratedMeasurements_.deltaTij; + const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer(); + const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3 Rot_i = pose_i.rotation(); + const Rot3 Rot_j = pose_j.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + const Vector3 pos_j = pose_j.translation().vector(); + + // We compute factor's Jacobians + /* ---------------------------------------------------------------------------------------------------- */ + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + + const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(9,6); + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + - Rot_i.matrix(), + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + Matrix3::Zero(), + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(); + } + if(H2) { + H2->resize(9,3); + (*H2) << + // dfP/dVi + - Matrix3::Identity() * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - Matrix3::Identity() + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(); + + } + if(H3) { + + H3->resize(9,6); + (*H3) << + // dfP/dPosej + Matrix3::Zero(), Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); + } + if(H4) { + H4->resize(9,3); + (*H4) << + // dfP/dVj + Matrix3::Zero(), + // dfV/dVj + Matrix3::Identity(), + // dfR/dVj + Matrix3::Zero(); + } + if(H5) { + + const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); + const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; + + H5->resize(9,6); + (*H5) << + // dfP/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc, + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega, + // dfV/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc, + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega, + // dfR/dBias + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); + } + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 fp = + pos_j - pos_i + - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) + - vel_i * deltaTij + + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * gravity_ * deltaTij*deltaTij; + + const Vector3 fv = + vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) + + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + - gravity_ * deltaTij; + + const Vector3 fR = Rot3::Logmap(fRhat); + + Vector r(9); r << fp, fv, fR; + return r; + } + + + /** predicted states from IMU */ + static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, + const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) + { + + const double& deltaTij = preintegratedMeasurements.deltaTij; + const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat.accelerometer(); + const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat.gyroscope(); + + const Rot3 Rot_i = pose_i.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; + + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + + pose_j = Pose3( Rot_j, Point3(pos_j) ); + } + + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor5", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(gravity_); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } + }; // \class ImuFactor + + typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp b/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp new file mode 100644 index 000000000..370ccc3b9 --- /dev/null +++ b/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp @@ -0,0 +1,298 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testImuFactor.cpp + * @brief Unit test for ImuFactor + * @author Luca Carlone, Stephen Williams, Richard Roberts + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::V; +using symbol_shorthand::B; + +/* ************************************************************************* */ +namespace { + +Vector callEvaluateError(const ImuFactor& factor, + const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias) +{ + return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); +} + +Rot3 evaluateRotationError(const ImuFactor& factor, + const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias) +{ + return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; +} + +ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( + const imuBias::ConstantBias& bias, + const list& measuredAccs, + const list& measuredOmegas, + const list& deltaTs, + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) + ) +{ + ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), + Matrix3::Identity(), Matrix3::Identity()); + + list::const_iterator itAcc = measuredAccs.begin(); + list::const_iterator itOmega = measuredOmegas.begin(); + list::const_iterator itDeltaT = deltaTs.begin(); + for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); + } + + return result; +} + +Vector3 evaluatePreintegratedMeasurementsPosition( + const imuBias::ConstantBias& bias, + const list& measuredAccs, + const list& measuredOmegas, + const list& deltaTs, + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) +{ + return evaluatePreintegratedMeasurements(bias, + measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij; +} + +Vector3 evaluatePreintegratedMeasurementsVelocity( + const imuBias::ConstantBias& bias, + const list& measuredAccs, + const list& measuredOmegas, + const list& deltaTs, + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) +{ + return evaluatePreintegratedMeasurements(bias, + measuredAccs, measuredOmegas, deltaTs).deltaVij; +} + +Rot3 evaluatePreintegratedMeasurementsRotation( + const imuBias::ConstantBias& bias, + const list& measuredAccs, + const list& measuredOmegas, + const list& deltaTs, + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) +{ + return evaluatePreintegratedMeasurements(bias, + measuredAccs, measuredOmegas, deltaTs).deltaRij; +} + +Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) +{ + return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); +} + + +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) +{ + return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +} + +} + +/* ************************************************************************* */ +TEST( CombinedImuFactor, PreintegratedMeasurements ) +{ + cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; + // Linearization point + imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases + + // Measurements + Vector3 measuredAcc(0.1, 0.0, 0.0); + Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + double deltaT = 0.5; + double tol = 1e-6; + + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero()); + expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); + +// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases +// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc +// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc +// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc +// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) +// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) +// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements + + actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + EXPECT(assert_equal(Vector(expected1.deltaPij), Vector(actual1.deltaPij), tol)); + EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol)); + EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol)); + DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol); +} + + +/* ************************************************************************* */ +TEST( CombinedImuFactor, ErrorWithBiases ) +{ + cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl; + + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); + LieVector v1(3, 0.5, 0.0, 0.0); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + LieVector v2(3, 0.5, 0.0, 0.0); + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + double deltaT = 1.0; + double tol = 1e-6; + + // const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases + // const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc + // const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc + // const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc + // const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) + // const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) + // const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements + + Matrix I6x6(6,6); + I6x6 = Matrix::Identity(6,6); + + + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); + + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); + + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + + // Create factor + noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); + + noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis, Combinedmodel); + + + Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + + Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); + + + EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); + + // Expected Jacobians + Matrix H1e, H2e, H3e, H4e, H5e; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); + + + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a, H6a; + (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); + + EXPECT(assert_equal(H1e, H1a.topRows(9))); + EXPECT(assert_equal(H2e, H2a.topRows(9))); + EXPECT(assert_equal(H3e, H3a.topRows(9))); + EXPECT(assert_equal(H4e, H4a.topRows(9))); + EXPECT(assert_equal(H5e, H5a.topRows(9))); +} + +/* ************************************************************************* */ +TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) +{ + cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; + // Linearization point + imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases + + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for(int i=1;i<100;i++) + { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + deltaTs.push_back(0.01); + } + + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + + // Compute numerical derivatives + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); + + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); + + Matrix expectedDelRdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); + + // Compare Jacobians + EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc)); + EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega)); + EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); + EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); + EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega)); +} + +#include + + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testImuFactor.cpp b/gtsam_unstable/slam/tests/testImuFactor.cpp new file mode 100644 index 000000000..9601e778e --- /dev/null +++ b/gtsam_unstable/slam/tests/testImuFactor.cpp @@ -0,0 +1,569 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testImuFactor.cpp + * @brief Unit test for ImuFactor + * @author Luca Carlone, Stephen Williams, Richard Roberts + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::V; +using symbol_shorthand::B; + +/* ************************************************************************* */ +namespace { +Vector callEvaluateError(const ImuFactor& factor, + const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias) +{ + return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); +} + +Rot3 evaluateRotationError(const ImuFactor& factor, + const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias) +{ + return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; +} + +ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( + const imuBias::ConstantBias& bias, + const list& measuredAccs, + const list& measuredOmegas, + const list& deltaTs, + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) + ) +{ + ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), + Matrix3::Identity(), Matrix3::Identity()); + + list::const_iterator itAcc = measuredAccs.begin(); + list::const_iterator itOmega = measuredOmegas.begin(); + list::const_iterator itDeltaT = deltaTs.begin(); + for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); + } + + return result; +} + +Vector3 evaluatePreintegratedMeasurementsPosition( + const imuBias::ConstantBias& bias, + const list& measuredAccs, + const list& measuredOmegas, + const list& deltaTs, + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) +{ + return evaluatePreintegratedMeasurements(bias, + measuredAccs, measuredOmegas, deltaTs).deltaPij; +} + +Vector3 evaluatePreintegratedMeasurementsVelocity( + const imuBias::ConstantBias& bias, + const list& measuredAccs, + const list& measuredOmegas, + const list& deltaTs, + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) +{ + return evaluatePreintegratedMeasurements(bias, + measuredAccs, measuredOmegas, deltaTs).deltaVij; +} + +Rot3 evaluatePreintegratedMeasurementsRotation( + const imuBias::ConstantBias& bias, + const list& measuredAccs, + const list& measuredOmegas, + const list& deltaTs, + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) +{ + return evaluatePreintegratedMeasurements(bias, + measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij; +} + +Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) +{ + return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); +} + + +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) +{ + return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +} + +} + +/* ************************************************************************* */ +TEST( ImuFactor, PreintegratedMeasurements ) +{ + // Linearization point + imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases + + // Measurements + Vector3 measuredAcc(0.1, 0.0, 0.0); + Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + double deltaT = 0.5; + + // Expected preintegrated values + Vector3 expectedDeltaP1; expectedDeltaP1 << 0.5*0.1*0.5*0.5, 0, 0; + Vector3 expectedDeltaV1(0.05, 0.0, 0.0); + Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0); + double expectedDeltaT1(0.5); + + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij), 1e-6)); + EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij), 1e-6)); + EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij, 1e-6)); + DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij, 1e-6); + + // Integrate again + Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0; + Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5; + Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0); + double expectedDeltaT2(1); + + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements actual2 = actual1; + actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij), 1e-6)); + EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij), 1e-6)); + EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij, 1e-6)); + DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij, 1e-6); +} + +/* ************************************************************************* */ +TEST( ImuFactor, Error ) +{ + // Linearization point + imuBias::ConstantBias bias; // Bias + Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); + LieVector v1(3, 0.5, 0.0, 0.0); + Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); + LieVector v2(3, 0.5, 0.0, 0.0); + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector(); + double deltaT = 1.0; + ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5)); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); + + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + + // Expected error + Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + + + // positions and velocities + Matrix H1etop6 = H1e.topRows(6); + Matrix H1atop6 = H1a.topRows(6); + EXPECT(assert_equal(H1etop6, H1atop6)); + // rotations + EXPECT(assert_equal(RH1e, H1a.bottomRows(3))); // evaluate only the rotation residue + + EXPECT(assert_equal(H2e, H2a)); + + // positions and velocities + Matrix H3etop6 = H3e.topRows(6); + Matrix H3atop6 = H3a.topRows(6); + EXPECT(assert_equal(H3etop6, H3atop6)); + // rotations + EXPECT(assert_equal(RH3e, H3a.bottomRows(3))); // evaluate only the rotation residue + + EXPECT(assert_equal(H4e, H4a)); +// EXPECT(assert_equal(H5e, H5a)); +} + +/* ************************************************************************* */ +TEST( ImuFactor, ErrorWithBiases ) +{ + // Linearization point +// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) +// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); +// LieVector v1(3, 0.5, 0.0, 0.0); +// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); +// LieVector v2(3, 0.5, 0.0, 0.0); + + + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); + LieVector v1(3, 0.5, 0.0, 0.0); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + LieVector v2(3, 0.5, 0.0, 0.0); + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + double deltaT = 1.0; + + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + +// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3)); +// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5)); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); + + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + SETDEBUG("ImuFactor evaluateError", false); + + // Expected error + Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; +// EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); + EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H4e, H4a)); + EXPECT(assert_equal(H5e, H5a)); +} + +/* ************************************************************************* */ +TEST( ImuFactor, PartialDerivativeExpmap ) +{ + // Linearization point + Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + + // Measurements + Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + double deltaT = 0.5; + + + // Compute numerical derivatives + Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( + &evaluateRotation, measuredOmega, _1, deltaT), biasOmega); + + const Matrix3 Jr = ImuFactor::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + + Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + + // Compare Jacobians + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega)); + +} + +/* ************************************************************************* */ +TEST( ImuFactor, PartialDerivativeLogmap ) +{ + // Linearization point + Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias + + // Measurements + Vector3 deltatheta; deltatheta << 0, 0, 0; + + + // Compute numerical derivatives + Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( + &evaluateLogRotation, thetahat, _1), deltatheta); + + const Vector3 x = thetahat; // parametrization of so(3) + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + double normx = norm_2(x); + const Matrix3 actualDelFdeltheta = Matrix3::Identity() + + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + +// std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl; +// std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl; + + // Compare Jacobians + EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); + +} + +/* ************************************************************************* */ +TEST( ImuFactor, fistOrderExponential ) +{ + // Linearization point + Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + + // Measurements + Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + double deltaT = 1.0; + + // change w.r.t. linearization point + double alpha = 0.0; + Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; + + + const Matrix3 Jr = ImuFactor::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + + Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + + const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + + const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); + const Matrix3 actualRot = + hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); + //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); + + // Compare Jacobians + EXPECT(assert_equal(expectedRot, actualRot)); +} + +/* ************************************************************************* */ +TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) +{ + // Linearization point + imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases + + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for(int i=1;i<100;i++) + { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + deltaTs.push_back(0.01); + } + + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + + // Compute numerical derivatives + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); + + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); + + Matrix expectedDelRdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); + + // Compare Jacobians + EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc)); + EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega)); + EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); + EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); + EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega)); +} + +//#include +///* ************************************************************************* */ +//TEST( ImuFactor, LinearizeTiming) +//{ +// // Linearization point +// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); +// LieVector v1(3, 0.5, 0.0, 0.0); +// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); +// LieVector v2(3, 0.5, 0.0, 0.0); +// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); +// +// // Pre-integrator +// imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10)); +// Vector3 gravity; gravity << 0, 0, 9.81; +// Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01; +// ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); +// +// // Pre-integrate Measurements +// Vector3 measuredAcc(0.1, 0.0, 0.0); +// Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); +// double deltaT = 0.5; +// for(size_t i = 0; i < 50; ++i) { +// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); +// } +// +// // Create factor +// noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.preintegratedMeasurementsCovariance()); +// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); +// +// Values values; +// values.insert(X(1), x1); +// values.insert(X(2), x2); +// values.insert(V(1), v1); +// values.insert(V(2), v2); +// values.insert(B(1), bias); +// +// Ordering ordering; +// ordering.push_back(X(1)); +// ordering.push_back(V(1)); +// ordering.push_back(X(2)); +// ordering.push_back(V(2)); +// ordering.push_back(B(1)); +// +// GaussianFactorGraph graph; +// gttic_(LinearizeTiming); +// for(size_t i = 0; i < 100000; ++i) { +// GaussianFactor::shared_ptr g = factor.linearize(values, ordering); +// graph.push_back(g); +// } +// gttoc_(LinearizeTiming); +// tictoc_finishedIteration_(); +// std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl; +// tictoc_print_(); +//} + + +/* ************************************************************************* */ +TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) +{ + + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); + LieVector v1(3, 0.5, 0.0, 0.0); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + LieVector v2(3, 0.5, 0.0, 0.0); + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + double deltaT = 1.0; + + const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0)); + +// ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), +// Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega); + + + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), + Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + + + + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5)); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); + + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); + EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H4e, H4a)); + EXPECT(assert_equal(H5e, H5a)); +} + + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From f273b8f9157133da4f99cf7cce1e9ebfb0df8c24 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 2 Aug 2013 16:04:29 +0000 Subject: [PATCH 09/26] Removed extra 'typename' --- gtsam_unstable/slam/CombinedImuFactor.h | 2 +- gtsam_unstable/slam/ImuFactor.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/CombinedImuFactor.h b/gtsam_unstable/slam/CombinedImuFactor.h index 1879802da..0a7fb270c 100644 --- a/gtsam_unstable/slam/CombinedImuFactor.h +++ b/gtsam_unstable/slam/CombinedImuFactor.h @@ -341,7 +341,7 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ - typedef typename boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /** Default constructor - only use for serialization */ CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} diff --git a/gtsam_unstable/slam/ImuFactor.h b/gtsam_unstable/slam/ImuFactor.h index 4a8a84b6d..8ba169770 100644 --- a/gtsam_unstable/slam/ImuFactor.h +++ b/gtsam_unstable/slam/ImuFactor.h @@ -304,7 +304,7 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ - typedef typename boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /** Default constructor - only use for serialization */ ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} From 74f7e3e16a2794042f1f2a42b79571baef7eafad Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Fri, 2 Aug 2013 16:59:46 +0000 Subject: [PATCH 10/26] added a funtion for computing log-Determinant --- gtsam/linear/GaussianBayesTree.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 7011524e4..17d54635c 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -82,10 +82,15 @@ void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g) { /* ************************************************************************* */ double determinant(const GaussianBayesTree& bayesTree) { + return exp(logDeterminant(bayesTree)); +} + +/* ************************************************************************* */ +double logDeterminant(const GaussianBayesTree& bayesTree) { if (!bayesTree.root()) return 0.0; - return exp(internal::logDeterminant(bayesTree.root())); + return internal::logDeterminant(bayesTree.root()); } /* ************************************************************************* */ From 3200c999b24e3cd73cda87c9cdd26a4722a48baf Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Fri, 2 Aug 2013 17:00:22 +0000 Subject: [PATCH 11/26] function for computing log-determinant --- gtsam/linear/GaussianBayesTree.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index 9df3d67b4..63411b610 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -102,6 +102,16 @@ GTSAM_EXPORT void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValue */ GTSAM_EXPORT double determinant(const GaussianBayesTree& bayesTree); +/** + * Computes the log determinant of a GassianBayesTree + * A GassianBayesTree is an upper triangular matrix and for an upper triangular matrix + * determinant is the product of the diagonal elements. Instead of actually multiplying + * we add the logarithms of the diagonal elements because this is more numerically stable. + * @param bayesTree The input GassianBayesTree + * @return The log determinant + */ +GTSAM_EXPORT double logDeterminant(const GaussianBayesTree& bayesTree); + namespace internal { template From bf7dc7d48065f04417e23ebc07b1a6eb6544c659 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Fri, 2 Aug 2013 18:15:58 +0000 Subject: [PATCH 12/26] failed unit test of BetweenFactor when the measurement is not the ground truth --- gtsam/slam/tests/testBetweenFactor.cpp | 49 ++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 gtsam/slam/tests/testBetweenFactor.cpp diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp new file mode 100644 index 000000000..415524071 --- /dev/null +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -0,0 +1,49 @@ +/** + * @file testBetweenFactor.cpp + * @brief + * @author Duy-Nguyen Ta + * @date Aug 2, 2013 + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::symbol_shorthand; +using namespace gtsam::noiseModel; + +TEST(BetweenFactor, Rot3) { + Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::rodriguez(0.4, 0.5, 0.6); + Rot3 noise = Rot3::rodriguez(0.01, 0.01, 0.01); + Rot3 measured = R1.between(R2)*noise ; + + BetweenFactor factor(R(1), R(2), measured, Isotropic::Sigma(3, 0.05)); + Matrix actualH1, actualH2; + Vector actual = factor.evaluateError(R1, R2, actualH1, actualH2); + + Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2)); + CHECK(assert_equal(expected,actual, 1e-100)); + + Matrix numericalH1 = numericalDerivative21( + boost::function(boost::bind( + &BetweenFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), R1, R2, 1e-5); + CHECK(assert_equal(numericalH1,actualH1)); + + Matrix numericalH2 = numericalDerivative22( + boost::function(boost::bind( + &BetweenFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), R1, R2, 1e-5); + CHECK(assert_equal(numericalH2,actualH2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From c9c2025d521974d623c4fee84ec1de3e4b3eb8d2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 2 Aug 2013 19:04:52 +0000 Subject: [PATCH 13/26] Swiched from isnan() check to isfinite() check for DOUBLES_EQUAL - also handles infinite case --- CppUnitLite/Test.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 82040dceb..3ea6a6318 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -130,7 +130,7 @@ boost::lexical_cast(actualTemp))); return; } } #define DOUBLES_EQUAL(expected,actual,threshold)\ { double actualTemp = actual; \ double expectedTemp = expected; \ - if (isnan(actualTemp) || isnan(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \ + if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \ { result_.addFailure (Failure (name_, __FILE__, __LINE__, \ boost::lexical_cast((double)expectedTemp), boost::lexical_cast((double)actualTemp))); return; } } @@ -150,7 +150,7 @@ boost::lexical_cast(actualTemp))); } } #define EXPECT_DOUBLES_EQUAL(expected,actual,threshold)\ { double actualTemp = actual; \ double expectedTemp = expected; \ - if (isnan(actualTemp) || isnan(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \ + if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \ { result_.addFailure (Failure (name_, __FILE__, __LINE__, \ boost::lexical_cast((double)expectedTemp), boost::lexical_cast((double)actualTemp))); } } From 334d71ce51d4648cf5fc62c31ac396f5be9342c7 Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Fri, 2 Aug 2013 20:07:52 +0000 Subject: [PATCH 14/26] IMUKittiExample: added infrastructure for reading and processing data - work in progress --- matlab/gtsam_examples/IMUKittiExample.m | 150 ++++++++++++++---------- 1 file changed, 85 insertions(+), 65 deletions(-) diff --git a/matlab/gtsam_examples/IMUKittiExample.m b/matlab/gtsam_examples/IMUKittiExample.m index c9ec7bc78..92239353b 100644 --- a/matlab/gtsam_examples/IMUKittiExample.m +++ b/matlab/gtsam_examples/IMUKittiExample.m @@ -9,6 +9,9 @@ IMU_metadata = cell2struct(num2cell(IMU_metadata.data), IMU_metadata.colheaders, IMUinBody = Pose3.Expmap([ IMU_metadata.BodyPtx; IMU_metadata.BodyPty; IMU_metadata.BodyPtz; IMU_metadata.BodyPrx; IMU_metadata.BodyPry; IMU_metadata.BodyPrz; ]); +if ~IMUinBody.equals(Pose3, 1e-5) + error 'Currently only support IMUinBody is identity, i.e. IMU and body frame are the same'; +end VO_metadata = importdata('KittiRelativePose_metadata.txt'); VO_metadata = cell2struct(num2cell(VO_metadata.data), VO_metadata.colheaders, 2); @@ -38,19 +41,14 @@ VO_data = cell2struct(num2cell(VO_data.data), VO_data.colheaders, 2); % Merge relative pose fields and convert to Pose3 logposes = [ [VO_data.dtx]' [VO_data.dty]' [VO_data.dtz]' [VO_data.drx]' [VO_data.dry]' [VO_data.drz]' ]; logposes = num2cell(logposes, 2); -relposes = arrayfun(@(x) {gtsam.Pose3.Expmap(x{1}')}, logposes); -% TODO: convert to IMU frame %relposes = arrayfun( +relposes = arrayfun(@(x) {gtsam.Pose3.Expmap(x{:}')}, logposes); +relposes = arrayfun(@(x) {VOinIMU.compose(x{:}).compose(VOinIMU.inverse())}, relposes); [VO_data.RelativePose] = deal(relposes{:}); VO_data = rmfield(VO_data, { 'dtx' 'dty' 'dtz' 'drx' 'dry' 'drz' }); clear logposes relposes GPS_data = importdata('KittiGps.txt'); GPS_data = cell2struct(num2cell(GPS_data.data), GPS_data.colheaders, 2); - -%% -SummaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ... - gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ... - 1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3)); %% Set initial conditions for the estimated trajectory disp('TODO: we have GPS so this initialization is not right') @@ -69,14 +67,15 @@ factors = NonlinearFactorGraph; values = Values; %% Create prior on initial pose, velocity, and biases -sigma_init_x = 1.0 -sigma_init_v = 1.0 -sigma_init_b = 1.0 +sigma_init_x = 1.0; +sigma_init_v = 1.0; +sigma_init_b = 1.0; values.insert(symbol('x',0), currentPoseGlobal); values.insert(symbol('v',0), LieVector(currentVelocityGlobal) ); values.insert(symbol('b',0), imuBias.ConstantBias(bias_acc,bias_omega) ); +disp('TODO: we have GPS so this initialization is not right') % Prior on initial pose factors.add(PriorFactorPose3(symbol('x',0), ... currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x))); @@ -92,83 +91,104 @@ factors.add(PriorFactorConstantBias(symbol('b',0), ... % (2) we create the corresponding factors in the graph % (3) we solve the graph to obtain and optimal estimate of robot trajectory -i = 2; -lastTime = 0; -lastIndex = 0; -currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); +% lastTime = 0; TODO: delete? +% lastIndex = 0; TODO: delete? +currentSummarizedMeasurement = []; -times = sort([VO_data(:,1); GPS_data(:,1)]); % this are the time-stamps at which we want to initialize a new node in the graph -IMU_times = IMU_data(:,1); +% Measurement types: +% 1: VO +% 2: GPS +% 3: IMU +times = sortrows( [ ... + [VO_data.Time]' 1*ones(length([VO_data.Time]), 1); ... + %[GPS_data.Time]' 2*ones(length([GPS_data.Time]), 1); ... + [IMU_data.Time]' 3*ones(length([IMU_data.Time]), 1); ... + ], 1); % this are the time-stamps at which we want to initialize a new node in the graph -disp('TODO: still needed to take care of the initial time') - -for t = times +t_previous = 0; +poseIndex = 0; +for measurementIndex = 1:size(times,1) % At each non=IMU measurement we initialize a new node in the graph - currentIndex = find( times == t ); - currentPoseKey = symbol('x',currentIndex); - currentVelKey = symbol('v',currentIndex); - currentBiasKey = symbol('b',currentIndex); + currentPoseKey = symbol('x',poseIndex); + currentVelKey = symbol('v',poseIndex); + currentBiasKey = symbol('b',poseIndex); - %% add preintegrated IMU factor between previous state and current state - % ======================================================================= - IMUbetweenTimesIndices = find( IMU_times>+t_previous & IMU_times<= t); - % all imu measurements occurred between t_previous and t + t = times(measurementIndex, 1); + type = times(measurementIndex, 2); - % we assume that each row of the IMU.txt file has the following structure: - % timestamp delta_t acc_x acc_y acc_z omega_x omega_y omega_z - disp('TODO: We want don t want to preintegrate with zero bias, but to use the most recent estimate') - currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); - for i=1:length(IMUbetweenTimesIndices) - index = IMUbetweenTimesIndices(i); % the row of the IMU_data matrix that we have to integrate - deltaT = IMU_data(index,2); - accMeas = IMU_data(index,3:5); - omegaMeas = IMU_data(index,6:8); + if type == 3 + % Integrate IMU + + if isempty(currentSummarizedMeasurement) + % Create initial empty summarized measurement + % we assume that each row of the IMU.txt file has the following structure: + % timestamp delta_t acc_x acc_y acc_z omega_x omega_y omega_z + currentBias = isam.calculateEstimate(currentBiasKey - 1); + currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... + currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ... + IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3)); + end + % Accumulate preintegrated measurement + deltaT = IMU_data(index).dt; + accMeas = IMU_data(index).acc_omega(1:3); + omegaMeas = IMU_data(index).acc_omega(4:6); currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT); + + else + % Create IMU factor + factors.add(ImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentSummarizedMeasurement, g, cor_v, ... + currentSummarizedMeasurement.PreintMeasCov)); + + % Reset summarized measurement + currentSummarizedMeasurement = []; + + if type == 1 + % Create VO factor + elseif type == 2 + % Create GPS factor + end + + poseIndex = poseIndex + 1; end - disp('TODO: is the imu noise right?') - % Create IMU factor - factors.add(ImuFactor( ... - previousPoseKey, previousVelKey, ... - currentPoseKey, currentVelKey, ... - currentBiasKey, currentSummarizedMeasurement, g, cor_v, ... - noiseModel.Isotropic.Sigma(9, 1e-6))); + % ======================================================================= %% add factor corresponding to GPS measurements (if available at the current time) - % ======================================================================= - if isempty( find(GPS_data(:,1) == t ) ) == 0 % it is a GPS measurement - if length( find(GPS_data(:,1)) ) > 1 - error('more GPS measurements at the same time stamp: it should be an error') - end - - index = find(GPS_data(:,1) == t ); % the row of the IMU_data matrix that we have to integrate - GPSmeas = GPS_data(index,2:4); - - noiseModelGPS = ???; % noiseModelGPS.Isotropic.Sigma(6, sigma_init_x)) - - % add factor - disp('TODO: is the GPS noise right?') - factors.add(PriorFactor???(currentPoseKey, GPSmeas, noiseModelGPS) ); - end +% % ======================================================================= +% if isempty( find(GPS_data(:,1) == t ) ) == 0 % it is a GPS measurement +% if length( find(GPS_data(:,1)) ) > 1 +% error('more GPS measurements at the same time stamp: it should be an error') +% end +% +% index = find(GPS_data(:,1) == t ); % the row of the IMU_data matrix that we have to integrate +% GPSmeas = GPS_data(index,2:4); +% +% noiseModelGPS = ???; % noiseModelGPS.Isotropic.Sigma(6, sigma_init_x)) +% +% % add factor +% disp('TODO: is the GPS noise right?') +% factors.add(PriorFactor???(currentPoseKey, GPSmeas, noiseModelGPS) ); +% end % ======================================================================= %% add factor corresponding to VO measurements (if available at the current time) % ======================================================================= - if isempty( find(VO_data(:,1) == t ) )== 0 % it is a GPS measurement - if length( find(VO_data(:,1)) ) > 1 + if isempty( find([VO_data.Time] == t, 1) )== 0 % it is a GPS measurement + if length( find([VO_data.Time] == t) ) > 1 error('more VO measurements at the same time stamp: it should be an error') end - index = find( VO_data(:,1) == t ); % the row of the IMU_data matrix that we have to integrate - VOmeas_pos = VO_data(index,2:4)'; - VOmeas_ang = VO_data(index,5:7)'; + index = find([VO_data.Time] == t, 1); % the row of the IMU_data matrix that we have to integrate - VOpose = Pose3( Rot3(VOmeas_ang) , Point3(VOmeas_pos) ); - noiseModelVO = ???; % noiseModelGPS.Isotropic.Sigma(6, sigma_init_x)) + VOpose = VO_data(index).RelativePose; + noiseModelVO = noiseModel.Diagonal.Sigmas([ IMU_metadata.RotationSigma * [1;1;1]; IMU_metadata.TranslationSigma * [1;1;1] ]); % add factor disp('TODO: is the VO noise right?') From d1419a3d11987d32b02eca7cc78e391daec54fed Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Fri, 2 Aug 2013 23:35:39 +0000 Subject: [PATCH 15/26] Added SmartProjectionFactor (+unit tests) --- gtsam_unstable/slam/SmartProjectionFactor.h | 292 +++++++++++++++++ .../slam/tests/testSmartProjectionFactor.cpp | 308 ++++++++++++++++++ 2 files changed, 600 insertions(+) create mode 100644 gtsam_unstable/slam/SmartProjectionFactor.h create mode 100644 gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h new file mode 100644 index 000000000..dc0fd0e64 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -0,0 +1,292 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ProjectionFactor.h + * @brief Basic bearing factor from 2D measurement + * @author Chris Beall + * @author Richard Roberts + * @author Frank Dellaert + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + + /** + * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. + * i.e. the main building block for visual SLAM. + * @addtogroup SLAM + */ + template + class SmartProjectionFactor: public NonlinearFactor { + protected: + + // Keep a copy of measurement and calibration for I/O + std::vector measured_; ///< 2D measurement for each of the n views + ///< (important that the order is the same as the keys that we use to create the factor) + boost::shared_ptr K_; ///< shared pointer to calibration object + const SharedNoiseModel noise_; ///< noise model used + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + + + // verbosity handling for Cheirality Exceptions + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + + public: + + /// shorthand for base class type + typedef NonlinearFactor Base; + + /// shorthand for this class + typedef SmartProjectionFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + SmartProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {} + + /** + * Constructor + * TODO: Mark argument order standard (keys, measurement, parameters) + * @param measured is the 2n dimensional location of the n points in the n views (the measurements) + * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views) + * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark + * @param K shared pointer to the constant calibration + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + SmartProjectionFactor(const std::vector measured, const SharedNoiseModel& model, + std::vector poseKeys, const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) : + measured_(measured), K_(K), noise_(model), body_P_sensor_(body_P_sensor), + throwCheirality_(false), verboseCheirality_(false) { + keys_.assign(poseKeys.begin(), poseKeys.end()); + } + + /** + * Constructor with exception-handling flags + * TODO: Mark argument order standard (keys, measurement, parameters) + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param K shared pointer to the constant calibration + * @param throwCheirality determines whether Cheirality exceptions are rethrown + * @param verboseCheirality determines whether exceptions are printed for Cheirality + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + SmartProjectionFactor(const std::vector measured, const SharedNoiseModel& model, + std::vector poseKeys, const boost::shared_ptr& K, + bool throwCheirality, bool verboseCheirality, + boost::optional body_P_sensor = boost::none) : + measured_(measured), K_(K), noise_(model), body_P_sensor_(body_P_sensor), + throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} + + /** Virtual destructor */ + virtual ~SmartProjectionFactor() {} + + /// @return a deep copy of this factor +// virtual gtsam::NonlinearFactor::shared_ptr clone() const { +// return boost::static_pointer_cast( +// gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "SmartProjectionFactor, z = "; + BOOST_FOREACH(const Point2& p, measured_) { + std::cout << "measurement, p = "<< p << std::endl; + } + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + + bool areMeasurementsEqual = true; + for(size_t i = 0; i < measured_.size(); i++) { + if(this->measured_.at(i).equals(e->measured_.at(i), tol) == false) + areMeasurementsEqual = false; + break; + } + + return e + && Base::equals(p, tol) + && areMeasurementsEqual + && this->K_->equals(*e->K_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const{ + + Vector a; + return a; + +// Point3 point = x.at(*keys_.end()); +// +// std::vector::iterator vit; +// for (vit = keys_.begin(); vit != keys_.end()-1; vit++) { +// Key key = (*vit); +// Pose3 pose = x.at(key); +// +// if(body_P_sensor_) { +// if(H1) { +// gtsam::Matrix H0; +// PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); +// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +// *H1 = *H1 * H0; +// return reprojectionError.vector(); +// } else { +// PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); +// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +// return reprojectionError.vector(); +// } +// } else { +// PinholeCamera camera(pose, *K_); +// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +// return reprojectionError.vector(); +// } +// } + + } + + /// get the dimension of the factor (number of rows on linearization) + virtual size_t dim() const { + return 6*keys_.size(); + } + + /// linearize returns a Hessianfactor that is an approximation of error(p) + virtual boost::shared_ptr linearize(const Values& x, const Ordering& ordering) const { + + // fill in the keys + std::vector js; + BOOST_FOREACH(const Key& k, keys_) { + js += ordering[k]; + } + + + std::vector Gs; + + std::vector gs; + // Shur complement trick + +// double e = u + b - z , e2 = e * e; +// double c = 2 * logSqrt2PI - log(p) + e2 * p; +// Vector g1 = Vector_(1, -e * p); +// Vector g2 = Vector_(1, 0.5 / p - 0.5 * e2); +// Vector g3 = Vector_(1, -e * p); +// Matrix G11 = Matrix_(1, 1, p); +// Matrix G12 = Matrix_(1, 1, e); +// Matrix G13 = Matrix_(1, 1, p); +// Matrix G22 = Matrix_(1, 1, 0.5 / (p * p)); +// Matrix G23 = Matrix_(1, 1, e); +// Matrix G33 = Matrix_(1, 1, p); + + double f = 0; + + return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); + } + + /** + * Calculate the error of the factor. + * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. + * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model + * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. + */ + virtual double error(const Values& values) const { + if (this->active(values)) { + double overallError=0; + + // Collect all poses (Cameras) + std::vector cameraPoses; + + BOOST_FOREACH(const Key& k, keys_) { + if(body_P_sensor_) + cameraPoses.push_back(values.at(k).compose(*body_P_sensor_)); + else + cameraPoses.push_back(values.at(k)); + } + + // We triangulate the 3D position of the landmark + boost::optional point = triangulatePoint3(cameraPoses, measured_, *K_); + + if(point) + { // triangulation produced a good estimate of landmark position + + std::cout << "point " << *point << std::endl; + + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_); + std::cout << "pose.compose(*body_P_sensor_) " << pose << std::endl; + + Point2 reprojectionError(camera.project(*point) - measured_.at(i)); + std::cout << "reprojectionError " << reprojectionError << std::endl; + overallError += noise_->distance( reprojectionError.vector() ); + std::cout << "noise_->distance( reprojectionError.vector() ) " << noise_->distance( reprojectionError.vector() ) << std::endl; + } + return sqrt(overallError); + }else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error + return 0.0; + } + } else { + return 0.0; + } + } + + /** return the measurements */ + const Vector& measured() const { + return measured_; + } + + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { + return K_; + } + + /** return verbosity */ + inline bool verboseCheirality() const { return verboseCheirality_; } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { return throwCheirality_; } + + private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(K_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } + }; +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp new file mode 100644 index 000000000..c7bb9f483 --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -0,0 +1,308 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testProjectionFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Frank Dellaert + * @date Nov 2009 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace std; +using namespace gtsam; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w=640,h=480; +static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); + +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(2)); + +// Convenience for named keys +//using symbol_shorthand::X; +//using symbol_shorthand::L; + +//typedef GenericProjectionFactor TestProjectionFactor; + + +///* ************************************************************************* */ +TEST( MultiProjectionFactor, noiseless ){ + cout << " ************************ MultiProjectionFactor: noiseless ****************************" << endl; + Values theta; + NonlinearFactorGraph graph; + + Symbol x1('X', 1); + Symbol x2('X', 2); +// Symbol x3('X', 3); + + const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); + + std::vector views; + views += x1, x2; //, x3; + + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera level_camera(level_pose, *K); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera level_camera_right(level_pose_right, *K); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 level_uv = level_camera.project(landmark); + Point2 level_uv_right = level_camera_right.project(landmark); + + Values value; + value.insert(x1, level_pose); + value.insert(x2, level_pose_right); + +// poses += level_pose, level_pose_right; + vector measurements; + measurements += level_uv, level_uv_right; + + SmartProjectionFactor smartFactor(measurements, noiseProjection, views, K); + + double actualError = smartFactor.error(value); + double expectedError = 0.0; + + DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +///* ************************************************************************* */ +TEST( MultiProjectionFactor, noisy ){ + cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl; + + Values theta; + NonlinearFactorGraph graph; + + Symbol x1('X', 1); + Symbol x2('X', 2); +// Symbol x3('X', 3); + + const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); + + std::vector views; + views += x1, x2; //, x3; + + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera level_camera(level_pose, *K); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera level_camera_right(level_pose_right, *K); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 pixelError(0.2,0.2); + Point2 level_uv = level_camera.project(landmark) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark); + + Values value; + value.insert(x1, level_pose); + value.insert(x2, level_pose_right); + +// poses += level_pose, level_pose_right; + vector measurements; + measurements += level_uv, level_uv_right; + + SmartProjectionFactor smartFactor(measurements, noiseProjection, views, K); + + double actualError = smartFactor.error(value); + double expectedError = sqrt(0.08); + + // we do not expect to be able to predict the error, since the error on the pixel will change + // the triangulation of the landmark which is internal to the factor. + // DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +///* ************************************************************************* */ +//TEST( ProjectionFactor, nonStandard ) { +// GenericProjectionFactor f; +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, Constructor) { +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// +// Point2 measurement(323.0, 240.0); +// +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, ConstructorWithTransform) { +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// +// 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)); +// +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, Equals ) { +// // Create two identical factors and make sure they're equal +// Point2 measurement(323.0, 240.0); +// +// TestProjectionFactor factor1(measurement, model, X(1), L(1), K); +// TestProjectionFactor factor2(measurement, model, X(1), L(1), K); +// +// CHECK(assert_equal(factor1, factor2)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, 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)); +// +// TestProjectionFactor factor1(measurement, model, X(1), L(1), K, body_P_sensor); +// TestProjectionFactor factor2(measurement, model, X(1), L(1), K, body_P_sensor); +// +// CHECK(assert_equal(factor1, factor2)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, Error ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K); +// +// // Set the linearization point +// Pose3 pose(Rot3(), Point3(0,0,-6)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the error +// Vector actualError(factor.evaluateError(pose, point)); +// +// // The expected error is (-3.0, 0.0) pixels / UnitCovariance +// Vector expectedError = Vector_(2, -3.0, 0.0); +// +// // Verify we get the expected error +// CHECK(assert_equal(expectedError, actualError, 1e-9)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, ErrorWithTransform ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// 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)); +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor); +// +// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) +// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the error +// Vector actualError(factor.evaluateError(pose, point)); +// +// // The expected error is (-3.0, 0.0) pixels / UnitCovariance +// Vector expectedError = Vector_(2, -3.0, 0.0); +// +// // Verify we get the expected error +// CHECK(assert_equal(expectedError, actualError, 1e-9)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, Jacobian ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K); +// +// // Set the linearization point +// Pose3 pose(Rot3(), Point3(0,0,-6)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the Jacobians +// Matrix H1Actual, H2Actual; +// factor.evaluateError(pose, point, H1Actual, H2Actual); +// +// // The expected Jacobians +// Matrix H1Expected = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); +// Matrix H2Expected = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.); +// +// // Verify the Jacobians are correct +// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); +// CHECK(assert_equal(H2Expected, H2Actual, 1e-3)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, JacobianWithTransform ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// 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)); +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor); +// +// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) +// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the Jacobians +// Matrix H1Actual, H2Actual; +// factor.evaluateError(pose, point, H1Actual, H2Actual); +// +// // The expected Jacobians +// Matrix H1Expected = Matrix_(2, 6, -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); +// Matrix H2Expected = Matrix_(2, 3, 0., -92.376, 0., 0., 0., -92.376); +// +// // Verify the Jacobians are correct +// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); +// CHECK(assert_equal(H2Expected, H2Actual, 1e-3)); +//} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + From 39ec641c4a73090f46fea18c0022c5a51975193a Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Mon, 5 Aug 2013 15:09:03 +0000 Subject: [PATCH 17/26] Modifications to SmartProjectionFactor and unit test: work in progress --- gtsam_unstable/slam/SmartProjectionFactor.h | 78 +++++++++++++---- .../slam/tests/testSmartProjectionFactor.cpp | 87 +++++++++++++++++-- 2 files changed, 141 insertions(+), 24 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index dc0fd0e64..7b63b7814 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -181,7 +181,46 @@ namespace gtsam { } /// linearize returns a Hessianfactor that is an approximation of error(p) - virtual boost::shared_ptr linearize(const Values& x, const Ordering& ordering) const { + virtual boost::shared_ptr linearize(const Values& values, const Ordering& ordering) const { + + std::vector Hx(keys_.size()); + std::vector Hl(keys_.size()); + std::vector b(keys_.size()); + + // Collect all poses (Cameras) + std::vector cameraPoses; + + BOOST_FOREACH(const Key& k, keys_) { + if(body_P_sensor_) + cameraPoses.push_back(values.at(k).compose(*body_P_sensor_)); + else + cameraPoses.push_back(values.at(k)); + } + + // We triangulate the 3D position of the landmark + boost::optional point = triangulatePoint3(cameraPoses, measured_, *K_); + + if(point){ + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_); + b.at(i) = ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector(); + } + } + else{ + return HessianFactor::shared_ptr(new HessianFactor()); + } + + // Allocate m^2 matrix blocks + std::vector< std::vector > Hxl(keys_.size(), std::vector( keys_.size())); + + // Allocate inv(Hl'Hl) + Matrix3 C; + for(size_t i1 = 0; i1 < keys_.size(); i1++) { + C += Hl.at(i1).transpose() * Hl.at(i1); + } + C = C.inverse(); + // fill in the keys std::vector js; @@ -189,25 +228,32 @@ namespace gtsam { js += ordering[k]; } + // Calculate sub blocks + for(size_t i1 = 0; i1 < keys_.size(); i1++) { + for(size_t i2 = 0; i2 < keys_.size(); i2++) { + Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose(); + } + } - std::vector Gs; - - std::vector gs; // Shur complement trick -// double e = u + b - z , e2 = e * e; -// double c = 2 * logSqrt2PI - log(p) + e2 * p; -// Vector g1 = Vector_(1, -e * p); -// Vector g2 = Vector_(1, 0.5 / p - 0.5 * e2); -// Vector g3 = Vector_(1, -e * p); -// Matrix G11 = Matrix_(1, 1, p); -// Matrix G12 = Matrix_(1, 1, e); -// Matrix G13 = Matrix_(1, 1, p); -// Matrix G22 = Matrix_(1, 1, 0.5 / (p * p)); -// Matrix G23 = Matrix_(1, 1, e); -// Matrix G33 = Matrix_(1, 1, p); - + // Populate Gs and gs + std::vector Gs(keys_.size()*(keys_.size()+1)/2); + std::vector gs(keys_.size()); double f = 0; + int GsCount = 0; + for(size_t i1 = 0; i1 < keys_.size(); i1++) { + gs.at(i1) = Hx.at(i1).transpose() * b.at(i1); + + for(size_t i2 = 0; i2 < keys_.size(); i2++) { + gs.at(i1) += Hxl[i1][i2] * b.at(i2); + + if (i2 >= i1) { + Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2); + GsCount++; + } + } + } return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index c7bb9f483..f152b7def 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -110,9 +110,6 @@ TEST( MultiProjectionFactor, noiseless ){ TEST( MultiProjectionFactor, noisy ){ cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl; - Values theta; - NonlinearFactorGraph graph; - Symbol x1('X', 1); Symbol x2('X', 2); // Symbol x3('X', 3); @@ -139,24 +136,98 @@ TEST( MultiProjectionFactor, noisy ){ Point2 level_uv = level_camera.project(landmark) + pixelError; Point2 level_uv_right = level_camera_right.project(landmark); - Values value; - value.insert(x1, level_pose); - value.insert(x2, level_pose_right); + Values values; + values.insert(x1, level_pose); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + values.insert(x2, level_pose_right.compose(noise_pose)); // poses += level_pose, level_pose_right; vector measurements; measurements += level_uv, level_uv_right; - SmartProjectionFactor smartFactor(measurements, noiseProjection, views, K); + SmartProjectionFactor::shared_ptr + smartFactor(new SmartProjectionFactor(measurements, noiseProjection, views, K)); - double actualError = smartFactor.error(value); + double actualError = smartFactor->error(values); double expectedError = sqrt(0.08); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor); + graph.add(PriorFactor(x1, level_pose, noisePrior)); + + LevenbergMarquardtOptimizer optimizer(graph, values); + Values result = optimizer.optimize(); + + result.print("results of the optimization \n"); // we do not expect to be able to predict the error, since the error on the pixel will change // the triangulation of the landmark which is internal to the factor. // DOUBLES_EQUAL(expectedError, actualError, 1e-7); } + +///* ************************************************************************* */ +TEST( MultiProjectionFactor, 3poses ){ + cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl; + + Symbol x1('X', 1); + Symbol x2('X', 2); + Symbol x3('X', 3); + + const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); + + std::vector views; + views += x1, x2; //, x3; + + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera level_camera(level_pose, *K); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera level_camera_right(level_pose_right, *K); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 pixelError(0.2,0.2); + Point2 level_uv = level_camera.project(landmark) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark); + + Values values; + values.insert(x1, level_pose); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + values.insert(x2, level_pose_right.compose(noise_pose)); + +// poses += level_pose, level_pose_right; + vector measurements; + measurements += level_uv, level_uv_right; + + SmartProjectionFactor::shared_ptr + smartFactor(new SmartProjectionFactor(measurements, noiseProjection, views, K)); + + double actualError = smartFactor->error(values); + double expectedError = sqrt(0.08); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor); + graph.add(PriorFactor(x1, level_pose, noisePrior)); + + LevenbergMarquardtOptimizer optimizer(graph, values); + Values result = optimizer.optimize(); + + result.print("results of the optimization \n"); + // we do not expect to be able to predict the error, since the error on the pixel will change + // the triangulation of the landmark which is internal to the factor. + // DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + + ///* ************************************************************************* */ //TEST( ProjectionFactor, nonStandard ) { // GenericProjectionFactor f; From d1de6b29a93f422f86a92f1037b2a0e51cc3fe01 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Mon, 5 Aug 2013 15:50:19 +0000 Subject: [PATCH 18/26] adding 3 camera, 3 landmark test --- .../slam/tests/testSmartProjectionFactor.cpp | 95 +++++++++++-------- 1 file changed, 55 insertions(+), 40 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index f152b7def..5e65f3022 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -151,16 +151,6 @@ TEST( MultiProjectionFactor, noisy ){ double actualError = smartFactor->error(values); double expectedError = sqrt(0.08); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor); - graph.add(PriorFactor(x1, level_pose, noisePrior)); - - LevenbergMarquardtOptimizer optimizer(graph, values); - Values result = optimizer.optimize(); - - result.print("results of the optimization \n"); // we do not expect to be able to predict the error, since the error on the pixel will change // the triangulation of the landmark which is internal to the factor. // DOUBLES_EQUAL(expectedError, actualError, 1e-7); @@ -169,7 +159,7 @@ TEST( MultiProjectionFactor, noisy ){ ///* ************************************************************************* */ TEST( MultiProjectionFactor, 3poses ){ - cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl; + cout << " ************************ MultiProjectionFactor: 3 cams + 3 landmarks **********************" << endl; Symbol x1('X', 1); Symbol x2('X', 2); @@ -178,53 +168,78 @@ TEST( MultiProjectionFactor, 3poses ){ const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); std::vector views; - views += x1, x2; //, x3; + views += x1, x2, x3; Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera level_camera(level_pose, *K); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera level_camera_right(level_pose_right, *K); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose2, *K); - // 1. Project two landmarks into two cameras and triangulate - Point2 pixelError(0.2,0.2); - Point2 level_uv = level_camera.project(landmark) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark); + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(5, 0, 3.0); - Values values; - values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - values.insert(x2, level_pose_right.compose(noise_pose)); + vector measurements_cam1, measurements_cam2, measurements_cam3; -// poses += level_pose, level_pose_right; - vector measurements; - measurements += level_uv, level_uv_right; + // 1. Project three landmarks into three cameras and triangulate + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2 cam3_uv1 = cam3.project(landmark1); + measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; - SmartProjectionFactor::shared_ptr - smartFactor(new SmartProjectionFactor(measurements, noiseProjection, views, K)); + // + Point2 cam1_uv2 = cam1.project(landmark2); + Point2 cam2_uv2 = cam2.project(landmark2); + Point2 cam3_uv2 = cam3.project(landmark2); + measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; - double actualError = smartFactor->error(values); - double expectedError = sqrt(0.08); + + Point2 cam1_uv3 = cam1.project(landmark3); + Point2 cam2_uv3 = cam2.project(landmark3); + Point2 cam3_uv3 = cam3.project(landmark3); + measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; + + typedef SmartProjectionFactor SmartFactor; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K)); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K)); + +// double actualError = smartFactor->error(values); +// double expectedError = sqrt(0.08); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); NonlinearFactorGraph graph; - graph.push_back(smartFactor); - graph.add(PriorFactor(x1, level_pose, noisePrior)); + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.add(PriorFactor(x1, pose1, noisePrior)); - LevenbergMarquardtOptimizer optimizer(graph, values); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Values values; + values.insert(x1, pose1); + values.insert(x2, pose1); + values.insert(x3, pose3* noise_pose); + + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + LevenbergMarquardtOptimizer optimizer(graph, values, params); Values result = optimizer.optimize(); - result.print("results of the optimization \n"); - // we do not expect to be able to predict the error, since the error on the pixel will change - // the triangulation of the landmark which is internal to the factor. - // DOUBLES_EQUAL(expectedError, actualError, 1e-7); + result.print("results of 3 camera, 3 landmark optimization \n"); + } From 587bfd3772eebbaf328537f6797de3248b2113ca Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Mon, 5 Aug 2013 17:01:56 +0000 Subject: [PATCH 19/26] VectorValues constructor and append with initial values --- gtsam/linear/VectorValues.h | 32 +++++++++++++++++++++++++ gtsam/linear/tests/testVectorValues.cpp | 30 +++++++++++++++++++++++ 2 files changed, 62 insertions(+) diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index bcd106a0e..af13231fb 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -179,6 +179,12 @@ namespace gtsam { template explicit VectorValues(const CONTAINER& dimensions) { this->append(dimensions); } + /** Construct from a container of variable dimensions (in variable order), with initial values. */ + template + explicit VectorValues(const Vector& d, const CONTAINER& dimensions) { + this->append(d, dimensions); + } + /** Construct to hold nVars vectors of varDim dimension each. */ VectorValues(Index nVars, size_t varDim) { this->resize(nVars, varDim); } @@ -237,6 +243,19 @@ namespace gtsam { template void append(const CONTAINER& dimensions); + /** Append to the VectorValues to additionally contain variables of the + * dimensions stored in \c dimensions. Initial values for the new variables + * are extracted from the input vector, holding values for all components + * in the same order specified in the dimensions container. + * This function preserves the original data, so all previously-existing + * variables are left unchanged. + * @param d A vector holding values for all variables, which order + * specified in the below container + * @param dimensions A container of the dimension of each variable to create. + */ + template + void append(const Vector& d, const CONTAINER& dimensions); + /** Removes the last subvector from the VectorValues */ void pop_back() { values_.pop_back(); }; @@ -409,6 +428,19 @@ namespace gtsam { } } + /* ************************************************************************* */ + template + void VectorValues::append(const Vector& d, const CONTAINER& dimensions) { + size_t i = size(); + size_t idx = 0; + values_.resize(size() + dimensions.size()); + BOOST_FOREACH(size_t dim, dimensions) { + values_[i] = sub(d, idx, idx+dim); + ++ i; + idx += dim; + } + } + /* ************************************************************************* */ template VectorValues VectorValues::Zero(const CONTAINER& dimensions) { diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index 8bfd42781..c416937ed 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -395,6 +395,36 @@ TEST(VectorValues, append) { CHECK_EXCEPTION(actual.insert(3, Vector()), invalid_argument); } +/* ************************************************************************* */ +TEST(VectorValues, append_withValuesFromVector) { + // Constructor with initial values + vector dims(3); + dims[0] = 1; + dims[1] = 2; + dims[2] = 2; + Vector d = Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0); + VectorValues actual(d, dims); + + VectorValues expected; + expected.insert(0, Vector_(1, 1.0)); + expected.insert(1, Vector_(2, 2.0, 3.0)); + expected.insert(2, Vector_(2, 4.0, 5.0)); + + EXPECT(assert_equal(expected, actual)); + + // Append with initial values + expected.insert(3, Vector_(1, 6.0)); + expected.insert(4, Vector_(3, 7.0, 8.0, 9.0)); + + vector dims2(2); + dims2[0] = 1; + dims2[1] = 3; + Vector d2 = Vector_(4, 6.0, 7.0, 8.0, 9.0); + actual.append(d2, dims2); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST(VectorValues, hasSameStructure) { VectorValues v1(2, 3); From 5518007317a7dfdb7105fe321dc141a1084e4c25 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Mon, 5 Aug 2013 17:58:32 +0000 Subject: [PATCH 20/26] adding test with regular projection factors for comparison, and bug fixes in SmartFactor test. --- .../slam/tests/testSmartProjectionFactor.cpp | 231 ++++++------------ 1 file changed, 80 insertions(+), 151 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index 5e65f3022..3557ed808 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -53,13 +53,13 @@ static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); static SharedNoiseModel model(noiseModel::Unit::Create(2)); // Convenience for named keys -//using symbol_shorthand::X; -//using symbol_shorthand::L; +using symbol_shorthand::X; +using symbol_shorthand::L; //typedef GenericProjectionFactor TestProjectionFactor; -///* ************************************************************************* */ +/* ************************************************************************* * TEST( MultiProjectionFactor, noiseless ){ cout << " ************************ MultiProjectionFactor: noiseless ****************************" << endl; Values theta; @@ -106,7 +106,7 @@ TEST( MultiProjectionFactor, noiseless ){ DOUBLES_EQUAL(expectedError, actualError, 1e-7); } -///* ************************************************************************* */ +/* ************************************************************************* * TEST( MultiProjectionFactor, noisy ){ cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl; @@ -157,7 +157,7 @@ TEST( MultiProjectionFactor, noisy ){ } -///* ************************************************************************* */ +/* ************************************************************************* */ TEST( MultiProjectionFactor, 3poses ){ cout << " ************************ MultiProjectionFactor: 3 cams + 3 landmarks **********************" << endl; @@ -181,12 +181,12 @@ TEST( MultiProjectionFactor, 3poses ){ // create third camera 1 meter above the first camera Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose2, *K); + SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera Point3 landmark1(5, 0.5, 1.2); Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(5, 0, 3.0); + Point3 landmark3(3, 0, 3.0); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -224,12 +224,13 @@ TEST( MultiProjectionFactor, 3poses ){ graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.add(PriorFactor(x1, pose1, noisePrior)); + graph.add(PriorFactor(x2, pose2, noisePrior)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); Values values; values.insert(x1, pose1); - values.insert(x2, pose1); + values.insert(x2, pose2); values.insert(x3, pose3* noise_pose); LevenbergMarquardtParams params; @@ -244,149 +245,77 @@ TEST( MultiProjectionFactor, 3poses ){ ///* ************************************************************************* */ -//TEST( ProjectionFactor, nonStandard ) { -// GenericProjectionFactor f; -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactor, Constructor) { -// Key poseKey(X(1)); -// Key pointKey(L(1)); -// -// Point2 measurement(323.0, 240.0); -// -// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactor, ConstructorWithTransform) { -// Key poseKey(X(1)); -// Key pointKey(L(1)); -// -// 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)); -// -// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactor, Equals ) { -// // Create two identical factors and make sure they're equal -// Point2 measurement(323.0, 240.0); -// -// TestProjectionFactor factor1(measurement, model, X(1), L(1), K); -// TestProjectionFactor factor2(measurement, model, X(1), L(1), K); -// -// CHECK(assert_equal(factor1, factor2)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactor, 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)); -// -// TestProjectionFactor factor1(measurement, model, X(1), L(1), K, body_P_sensor); -// TestProjectionFactor factor2(measurement, model, X(1), L(1), K, body_P_sensor); -// -// CHECK(assert_equal(factor1, factor2)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactor, Error ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key pointKey(L(1)); -// Point2 measurement(323.0, 240.0); -// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K); -// -// // Set the linearization point -// Pose3 pose(Rot3(), Point3(0,0,-6)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the error -// Vector actualError(factor.evaluateError(pose, point)); -// -// // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = Vector_(2, -3.0, 0.0); -// -// // Verify we get the expected error -// CHECK(assert_equal(expectedError, actualError, 1e-9)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactor, ErrorWithTransform ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key pointKey(L(1)); -// 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)); -// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor); -// -// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) -// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the error -// Vector actualError(factor.evaluateError(pose, point)); -// -// // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = Vector_(2, -3.0, 0.0); -// -// // Verify we get the expected error -// CHECK(assert_equal(expectedError, actualError, 1e-9)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactor, Jacobian ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key pointKey(L(1)); -// Point2 measurement(323.0, 240.0); -// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K); -// -// // Set the linearization point -// Pose3 pose(Rot3(), Point3(0,0,-6)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the Jacobians -// Matrix H1Actual, H2Actual; -// factor.evaluateError(pose, point, H1Actual, H2Actual); -// -// // The expected Jacobians -// Matrix H1Expected = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); -// Matrix H2Expected = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.); -// -// // Verify the Jacobians are correct -// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); -// CHECK(assert_equal(H2Expected, H2Actual, 1e-3)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactor, JacobianWithTransform ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key pointKey(L(1)); -// 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)); -// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor); -// -// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) -// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the Jacobians -// Matrix H1Actual, H2Actual; -// factor.evaluateError(pose, point, H1Actual, H2Actual); -// -// // The expected Jacobians -// Matrix H1Expected = Matrix_(2, 6, -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); -// Matrix H2Expected = Matrix_(2, 3, 0., -92.376, 0., 0., 0., -92.376); -// -// // Verify the Jacobians are correct -// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); -// CHECK(assert_equal(H2Expected, H2Actual, 1e-3)); -//} +TEST( MultiProjectionFactor, 3poses_projection_factor ){ + cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; + + Symbol x1('X', 1); + Symbol x2('X', 2); + Symbol x3('X', 3); + + const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); + + std::vector views; + views += x1, x2, x3; + + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + pose3.print("Pose3: "); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + typedef GenericProjectionFactor ProjectionFactor; + NonlinearFactorGraph graph; + + // 1. Project three landmarks into three cameras and triangulate + graph.add(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K)); + graph.add(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K)); + graph.add(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K)); + + // + graph.add(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K)); + graph.add(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K)); + graph.add(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K)); + + graph.add(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K)); + graph.add(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K)); + graph.add(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K)); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + graph.add(PriorFactor(x1, pose1, noisePrior)); + graph.add(PriorFactor(x2, pose2, noisePrior)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3* noise_pose); + values.insert(L(1), landmark1); + values.insert(L(2), landmark2); + values.insert(L(3), landmark3); + + LevenbergMarquardtParams params; +// params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; +// params.verbosity = NonlinearOptimizerParams::ERROR; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + Values result = optimizer.optimize(); + + result.print("Regular Projection Factor: results of 3 camera, 3 landmark optimization \n"); + +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } From 2e085ace91b3d00b9989818bcc4851ca19b77b8a Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Mon, 5 Aug 2013 19:25:08 +0000 Subject: [PATCH 21/26] Changes in SmartProjectionFactor --- .cproject | 390 ++++++++++---------- gtsam_unstable/slam/SmartProjectionFactor.h | 225 +++++++---- 2 files changed, 352 insertions(+), 263 deletions(-) diff --git a/.cproject b/.cproject index 24b5f8921..7257e427d 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -149,8 +147,16 @@ - - + + + + + + + + + + @@ -309,6 +315,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -335,7 +349,6 @@ make - tests/testBayesTree.run true false @@ -343,7 +356,6 @@ make - testBinaryBayesNet.run true false @@ -391,7 +403,6 @@ make - testSymbolicBayesNet.run true false @@ -399,7 +410,6 @@ make - tests/testSymbolicFactor.run true false @@ -407,7 +417,6 @@ make - testSymbolicFactorGraph.run true false @@ -423,20 +432,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -525,22 +525,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -557,6 +541,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -581,26 +581,34 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run true true true @@ -685,34 +693,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run + -j2 + clean true true true @@ -845,6 +845,22 @@ true true + + make + -j5 + testMultiProjectionFactor.run + true + true + true + + + make + -j5 + testSmartProjectionFactor.run + true + true + true + make -j5 @@ -933,6 +949,14 @@ true true + + make + -j5 + testTriangulation.run + true + true + true + make -j2 @@ -1079,7 +1103,6 @@ make - testGraph.run true false @@ -1087,7 +1110,6 @@ make - testJunctionTree.run true false @@ -1095,7 +1117,6 @@ make - testSymbolicBayesNetB.run true false @@ -1263,7 +1284,6 @@ make - testErrors.run true false @@ -1309,6 +1329,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1389,14 +1417,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1703,6 +1723,7 @@ make + testSimulated2DOriented.run true false @@ -1742,6 +1763,7 @@ make + testSimulated2D.run true false @@ -1749,6 +1771,7 @@ make + testSimulated3D.run true false @@ -1964,6 +1987,7 @@ make + tests/testGaussianISAM2 true false @@ -1985,6 +2009,102 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make -j3 @@ -2186,7 +2306,6 @@ cpack - -G DEB true false @@ -2194,7 +2313,6 @@ cpack - -G RPM true false @@ -2202,7 +2320,6 @@ cpack - -G TGZ true false @@ -2210,7 +2327,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2352,98 +2468,34 @@ true true - + make - -j2 - testRot3.run + -j5 + testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap true true true - + make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2487,38 +2539,6 @@ false true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 7b63b7814..de298e3bb 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -141,39 +141,39 @@ namespace gtsam { && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); } - /// Evaluate error h(x)-z and optionally derivatives - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const{ - - Vector a; - return a; - -// Point3 point = x.at(*keys_.end()); +// /// Evaluate error h(x)-z and optionally derivatives +// Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const{ // -// std::vector::iterator vit; -// for (vit = keys_.begin(); vit != keys_.end()-1; vit++) { -// Key key = (*vit); -// Pose3 pose = x.at(key); +// Vector a; +// return a; // -// if(body_P_sensor_) { -// if(H1) { -// gtsam::Matrix H0; -// PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); -// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -// *H1 = *H1 * H0; -// return reprojectionError.vector(); -// } else { -// PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); -// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -// return reprojectionError.vector(); -// } -// } else { -// PinholeCamera camera(pose, *K_); -// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -// return reprojectionError.vector(); -// } -// } - - } +//// Point3 point = x.at(*keys_.end()); +//// +//// std::vector::iterator vit; +//// for (vit = keys_.begin(); vit != keys_.end()-1; vit++) { +//// Key key = (*vit); +//// Pose3 pose = x.at(key); +//// +//// if(body_P_sensor_) { +//// if(H1) { +//// gtsam::Matrix H0; +//// PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); +//// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +//// *H1 = *H1 * H0; +//// return reprojectionError.vector(); +//// } else { +//// PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); +//// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +//// return reprojectionError.vector(); +//// } +//// } else { +//// PinholeCamera camera(pose, *K_); +//// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +//// return reprojectionError.vector(); +//// } +//// } +// +// } /// get the dimension of the factor (number of rows on linearization) virtual size_t dim() const { @@ -183,10 +183,6 @@ namespace gtsam { /// linearize returns a Hessianfactor that is an approximation of error(p) virtual boost::shared_ptr linearize(const Values& values, const Ordering& ordering) const { - std::vector Hx(keys_.size()); - std::vector Hl(keys_.size()); - std::vector b(keys_.size()); - // Collect all poses (Cameras) std::vector cameraPoses; @@ -196,66 +192,139 @@ namespace gtsam { else cameraPoses.push_back(values.at(k)); } - - // We triangulate the 3D position of the landmark + // We triangulate the 3D position of the landmark boost::optional point = triangulatePoint3(cameraPoses, measured_, *K_); - if(point){ - for(size_t i = 0; i < measured_.size(); i++) { - Pose3 pose = cameraPoses.at(i); - PinholeCamera camera(pose, *K_); - b.at(i) = ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector(); - } - } - else{ + if (!point) return HessianFactor::shared_ptr(new HessianFactor()); - } - // Allocate m^2 matrix blocks - std::vector< std::vector > Hxl(keys_.size(), std::vector( keys_.size())); - - // Allocate inv(Hl'Hl) - Matrix3 C; - for(size_t i1 = 0; i1 < keys_.size(); i1++) { - C += Hl.at(i1).transpose() * Hl.at(i1); - } - C = C.inverse(); + std::cout << "point " << *point << std::endl; + std::vector Gs(keys_.size()*(keys_.size()+1)/2); + std::vector gs(keys_.size()); + double f = 0; // fill in the keys std::vector js; BOOST_FOREACH(const Key& k, keys_) { js += ordering[k]; } - // Calculate sub blocks - for(size_t i1 = 0; i1 < keys_.size(); i1++) { - for(size_t i2 = 0; i2 < keys_.size(); i2++) { - Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose(); + bool blockwise = false; + +// { + // ========================================================================================================== + std::vector Hx(keys_.size()); + std::vector Hl(keys_.size()); + std::vector b(keys_.size()); + + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + + std::cout << "pose " << pose << std::endl; + + PinholeCamera camera(pose, *K_); + b.at(i) = ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector(); } - } - // Shur complement trick + // Shur complement trick - // Populate Gs and gs - std::vector Gs(keys_.size()*(keys_.size()+1)/2); - std::vector gs(keys_.size()); - double f = 0; - int GsCount = 0; - for(size_t i1 = 0; i1 < keys_.size(); i1++) { - gs.at(i1) = Hx.at(i1).transpose() * b.at(i1); + // Allocate m^2 matrix blocks + std::vector< std::vector > Hxl(keys_.size(), std::vector( keys_.size())); - for(size_t i2 = 0; i2 < keys_.size(); i2++) { - gs.at(i1) += Hxl[i1][i2] * b.at(i2); + // Allocate inv(Hl'Hl) + Matrix3 C; + for(size_t i1 = 0; i1 < keys_.size(); i1++) { + C += Hl.at(i1).transpose() * Hl.at(i1); + } + C = C.inverse(); - if (i2 >= i1) { - Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2); - GsCount++; + // Calculate sub blocks + for(size_t i1 = 0; i1 < keys_.size(); i1++) { + for(size_t i2 = 0; i2 < keys_.size(); i2++) { + Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose(); } } + // Populate Gs and gs + int GsCount = 0; + for(size_t i1 = 0; i1 < keys_.size(); i1++) { + gs.at(i1) = Hx.at(i1).transpose() * b.at(i1); + + for(size_t i2 = 0; i2 < keys_.size(); i2++) { + gs.at(i1) += Hxl[i1][i2] * b.at(i2); + + if (i2 >= i1) { + Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2); + GsCount++; + } + } + } +// } + + // debug only + std::vector Gs2(keys_.size()*(keys_.size()+1)/2); + std::vector gs2(keys_.size()); + +// { // version with full matrix multiplication + // ========================================================================================================== + Matrix Hx2 = zeros(2*keys_.size(), 6*keys_.size()); + Matrix Hl2 = zeros(2*keys_.size(), 3); + Vector b2 = zero(2*keys_.size()); + + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_); + Matrix Hxi, Hli; + Vector bi = ( camera.project(*point,Hxi,Hli) - measured_.at(i) ).vector(); + Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; + Hl2.block( 2*i, 0, 2, 3 ) = Hli; + subInsert(b2,bi,2*i); + + std::cout << "Hx " << Hx2 << std::endl; + std::cout << "Hl " << Hl2 << std::endl; + std::cout << "b " << b2.transpose() << std::endl; + std::cout << "Hxi - Hx.at(i) " << Hxi - Hx.at(i) << std::endl; + std::cout << "Hli - Hl.at(i) " << Hli - Hl.at(i) << std::endl; + } + + // Shur complement trick + Matrix H(6*keys_.size(), 6*keys_.size()); + Matrix3 C2 = (Hl2.transpose() * Hl2).inverse(); + H = Hx2.transpose() * Hx2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * Hx2; + Vector gs2_vector = Hx2.transpose() * b2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * b2; + + std::cout << "C - C2 " << C - C2 << std::endl; + + // Populate Gs and gs + int GsCount2 = 0; + for(size_t i1 = 0; i1 < keys_.size(); i1++) { + gs2.at(i1) = sub(gs2_vector, 6*i1, 6*i1 + 6); + + for(size_t i2 = 0; i2 < keys_.size(); i2++) { + if (i2 >= i1) { + Gs2.at(GsCount2) = H.block(6*i1, 6*i2, 6, 6); + GsCount2++; + } + } + } +// } + + // Compare blockwise and full version + bool gs2_equal_gs = true; + for(size_t i = 0; i < measured_.size(); i++) { + std::cout << "gs.at(i) " << gs.at(i).transpose() << std::endl; + std::cout << "gs2.at(i) " << gs2.at(i).transpose() << std::endl; + std::cout << "gs.error " << (gs.at(i)- gs2.at(i)).transpose() << std::endl; + if( !equal(gs.at(i), gs2.at(i)), 1e-7) { + gs2_equal_gs = false; + } } - return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); + std::cout << "gs2_equal_gs " << gs2_equal_gs << std::endl; + + + // ========================================================================================================== + return HessianFactor::shared_ptr(new HessianFactor(js, Gs2, gs2, f)); } /** @@ -284,17 +353,17 @@ namespace gtsam { if(point) { // triangulation produced a good estimate of landmark position - std::cout << "point " << *point << std::endl; +// std::cout << "point " << *point << std::endl; for(size_t i = 0; i < measured_.size(); i++) { Pose3 pose = cameraPoses.at(i); PinholeCamera camera(pose, *K_); - std::cout << "pose.compose(*body_P_sensor_) " << pose << std::endl; +// std::cout << "pose.compose(*body_P_sensor_) " << pose << std::endl; Point2 reprojectionError(camera.project(*point) - measured_.at(i)); - std::cout << "reprojectionError " << reprojectionError << std::endl; +// std::cout << "reprojectionError " << reprojectionError << std::endl; overallError += noise_->distance( reprojectionError.vector() ); - std::cout << "noise_->distance( reprojectionError.vector() ) " << noise_->distance( reprojectionError.vector() ) << std::endl; +// std::cout << "noise_->distance( reprojectionError.vector() ) " << noise_->distance( reprojectionError.vector() ) << std::endl; } return sqrt(overallError); }else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error From 482c5df6478a3c665870566ff22c8935fdc51a7a Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Mon, 5 Aug 2013 22:34:31 +0000 Subject: [PATCH 22/26] updates in SmartProjectionFactor: first working version with full and block version producing the same results (non-optimized) --- gtsam_unstable/slam/SmartProjectionFactor.h | 136 ++++++++++-------- .../slam/tests/testSmartProjectionFactor.cpp | 10 +- 2 files changed, 80 insertions(+), 66 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index de298e3bb..47e855b74 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -13,9 +13,8 @@ * @file ProjectionFactor.h * @brief Basic bearing factor from 2D measurement * @author Chris Beall - * @author Richard Roberts - * @author Frank Dellaert - * @author Alex Cunningham + * @author Luca Carlone + * @author Zsolt Kira */ #pragma once @@ -23,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -30,8 +30,7 @@ namespace gtsam { /** - * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. - * i.e. the main building block for visual SLAM. + * The calibration is known here. * @addtogroup SLAM */ template @@ -39,7 +38,7 @@ namespace gtsam { protected: // Keep a copy of measurement and calibration for I/O - std::vector measured_; ///< 2D measurement for each of the n views + std::vector measured_; ///< 2D measurement for each of the m views ///< (important that the order is the same as the keys that we use to create the factor) boost::shared_ptr K_; ///< shared pointer to calibration object const SharedNoiseModel noise_; ///< noise model used @@ -68,7 +67,7 @@ namespace gtsam { /** * Constructor * TODO: Mark argument order standard (keys, measurement, parameters) - * @param measured is the 2n dimensional location of the n points in the n views (the measurements) + * @param measured is the 2m dimensional location of the projection of a single landmark in the m views (the measurements) * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views) * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark * @param K shared pointer to the constant calibration @@ -85,9 +84,9 @@ namespace gtsam { /** * Constructor with exception-handling flags * TODO: Mark argument order standard (keys, measurement, parameters) - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param model is the standard deviation - * @param poseKey is the index of the camera + * @param measured is the 2m dimensional location of the projection of a single landmark in the m views (the measurements) + * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views) + * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark * @param K shared pointer to the constant calibration * @param throwCheirality determines whether Cheirality exceptions are rethrown * @param verboseCheirality determines whether exceptions are printed for Cheirality @@ -141,39 +140,6 @@ namespace gtsam { && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); } -// /// Evaluate error h(x)-z and optionally derivatives -// Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const{ -// -// Vector a; -// return a; -// -//// Point3 point = x.at(*keys_.end()); -//// -//// std::vector::iterator vit; -//// for (vit = keys_.begin(); vit != keys_.end()-1; vit++) { -//// Key key = (*vit); -//// Pose3 pose = x.at(key); -//// -//// if(body_P_sensor_) { -//// if(H1) { -//// gtsam::Matrix H0; -//// PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); -//// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -//// *H1 = *H1 * H0; -//// return reprojectionError.vector(); -//// } else { -//// PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); -//// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -//// return reprojectionError.vector(); -//// } -//// } else { -//// PinholeCamera camera(pose, *K_); -//// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -//// return reprojectionError.vector(); -//// } -//// } -// -// } /// get the dimension of the factor (number of rows on linearization) virtual size_t dim() const { @@ -183,24 +149,25 @@ namespace gtsam { /// linearize returns a Hessianfactor that is an approximation of error(p) virtual boost::shared_ptr linearize(const Values& values, const Ordering& ordering) const { +// std::cout.precision(20); + + // Collect all poses (Cameras) std::vector cameraPoses; - BOOST_FOREACH(const Key& k, keys_) { if(body_P_sensor_) cameraPoses.push_back(values.at(k).compose(*body_P_sensor_)); else cameraPoses.push_back(values.at(k)); } - // We triangulate the 3D position of the landmark + + // We triangulate the 3D position of the landmark boost::optional point = triangulatePoint3(cameraPoses, measured_, *K_); if (!point) return HessianFactor::shared_ptr(new HessianFactor()); std::cout << "point " << *point << std::endl; - - std::vector Gs(keys_.size()*(keys_.size()+1)/2); std::vector gs(keys_.size()); double f = 0; @@ -220,11 +187,10 @@ namespace gtsam { for(size_t i = 0; i < measured_.size(); i++) { Pose3 pose = cameraPoses.at(i); - std::cout << "pose " << pose << std::endl; - PinholeCamera camera(pose, *K_); b.at(i) = ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector(); +// std::cout << "b.at(i) " << b.at(i) << std::endl; } // Shur complement trick @@ -233,16 +199,27 @@ namespace gtsam { std::vector< std::vector > Hxl(keys_.size(), std::vector( keys_.size())); // Allocate inv(Hl'Hl) - Matrix3 C; + Matrix3 C = zeros(3,3); for(size_t i1 = 0; i1 < keys_.size(); i1++) { C += Hl.at(i1).transpose() * Hl.at(i1); } - C = C.inverse(); +// std::cout << "Cnoinv"<< "=[" << Ctemp << "];" << std::endl; + + C = C.inverse().eval(); // this is very important: without eval, because of eigen aliasing the results will be incorrect + // Calculate sub blocks for(size_t i1 = 0; i1 < keys_.size(); i1++) { for(size_t i2 = 0; i2 < keys_.size(); i2++) { + // we only need the upper triangular entries Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose(); + if (i1==0 & i2==0){ + std::cout << "Hoff"<< i1 << i2 << "=[" << Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose() << "];" << std::endl; + std::cout << "Hxoff"<< "=[" << Hx.at(i1) << "];" << std::endl; + std::cout << "Hloff"<< "=[" << Hl.at(i1) << "];" << std::endl; + std::cout << "Hloff2"<< "=[" << Hl.at(i2) << "];" << std::endl; + std::cout << "C"<< "=[" << C << "];" << std::endl; + } } } // Populate Gs and gs @@ -251,14 +228,26 @@ namespace gtsam { gs.at(i1) = Hx.at(i1).transpose() * b.at(i1); for(size_t i2 = 0; i2 < keys_.size(); i2++) { - gs.at(i1) += Hxl[i1][i2] * b.at(i2); + gs.at(i1) -= Hxl[i1][i2] * b.at(i2); - if (i2 >= i1) { + if (i2 == i1){ Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2); + std::cout << "HxlH"<< GsCount << "=[" << Hxl[i1][i2] * Hx.at(i2) << "];" << std::endl; + std::cout << "Hx2_"<< GsCount << "=[" << Hx.at(i2) << "];" << std::endl; + std::cout << "H"<< GsCount << "=[" << Gs.at(GsCount) << "];" << std::endl; + GsCount++; + } + if (i2 > i1) { + Gs.at(GsCount) = - Hxl[i1][i2] * Hx.at(i2); + std::cout << "HxlH"<< GsCount << "=[" << Hxl[i1][i2] * Hx.at(i2) << "];" << std::endl; + std::cout << "Hx2_"<< GsCount << "=[" << Hx.at(i2) << "];" << std::endl; + std::cout << "H"<< GsCount << "=[" << Gs.at(GsCount) << "];" << std::endl; GsCount++; } } } + +// std::cout << "GsCount " << GsCount << std::endl; // } // debug only @@ -278,22 +267,39 @@ namespace gtsam { Vector bi = ( camera.project(*point,Hxi,Hli) - measured_.at(i) ).vector(); Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; Hl2.block( 2*i, 0, 2, 3 ) = Hli; +// std::cout << "Hxi= \n" << Hxi << std::endl; +// std::cout << "Hxi.transpose() * Hxi= \n" << Hxi.transpose() * Hxi << std::endl; +// std::cout << "Hxl.transpose() * Hxl= \n" << Hli.transpose() * Hli << std::endl; subInsert(b2,bi,2*i); - std::cout << "Hx " << Hx2 << std::endl; - std::cout << "Hl " << Hl2 << std::endl; - std::cout << "b " << b2.transpose() << std::endl; - std::cout << "Hxi - Hx.at(i) " << Hxi - Hx.at(i) << std::endl; - std::cout << "Hli - Hl.at(i) " << Hli - Hl.at(i) << std::endl; +// std::cout << "================= measurement " << i << std::endl; +// std::cout << "Hx " << Hx2 << std::endl; +// std::cout << "Hl " << Hl2 << std::endl; +// std::cout << "b " << b2.transpose() << std::endl; +// std::cout << "b.at(i) " << b.at(i) << std::endl; +// std::cout << "Hxi - Hx.at(i) " << Hxi - Hx.at(i) << std::endl; +// std::cout << "Hli - Hl.at(i) " << Hli - Hl.at(i) << std::endl; } // Shur complement trick Matrix H(6*keys_.size(), 6*keys_.size()); Matrix3 C2 = (Hl2.transpose() * Hl2).inverse(); H = Hx2.transpose() * Hx2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * Hx2; + + std::cout << "Hx2" << "=[" << Hx2 << "];" << std::endl; + std::cout << "Hl2" << "=[" << Hl2 << "];" << std::endl; + std::cout << "H" << "=[" << H << "];" << std::endl; + + + std::cout << "Cnoinv2"<< "=[" << Hl2.transpose() * Hl2 << "];" << std::endl; + std::cout << "C2"<< "=[" << C2 << "];" << std::endl; + +// std::cout << "Hx2= \n" << Hx2 << std::endl; +// std::cout << "Hx2.transpose() * Hx2= \n" << Hx2.transpose() * Hx2 << std::endl; + Vector gs2_vector = Hx2.transpose() * b2 - Hx2.transpose() * Hl2 * C2 * Hl2.transpose() * b2; - std::cout << "C - C2 " << C - C2 << std::endl; + std::cout << "================================================================================" << std::endl; // Populate Gs and gs int GsCount2 = 0; @@ -308,7 +314,7 @@ namespace gtsam { } } // } - +// // Compare blockwise and full version bool gs2_equal_gs = true; for(size_t i = 0; i < measured_.size(); i++) { @@ -319,12 +325,18 @@ namespace gtsam { gs2_equal_gs = false; } } - std::cout << "gs2_equal_gs " << gs2_equal_gs << std::endl; + for(size_t i = 0; i < keys_.size()*(keys_.size()+1)/2; i++) { + std::cout << "Gs.at(i) " << Gs.at(i).transpose() << std::endl; + std::cout << "Gs2.at(i) " << Gs2.at(i).transpose() << std::endl; + std::cout << "Gs.error " << (Gs.at(i)- Gs2.at(i)).transpose() << std::endl; + } + std::cout << "Gs2_equal_Gs " << gs2_equal_gs << std::endl; + // ========================================================================================================== - return HessianFactor::shared_ptr(new HessianFactor(js, Gs2, gs2, f)); + return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); } /** diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index 3557ed808..86cb29e8e 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -226,12 +226,14 @@ TEST( MultiProjectionFactor, 3poses ){ graph.add(PriorFactor(x1, pose1, noisePrior)); graph.add(PriorFactor(x2, pose2, noisePrior)); +// smartFactor1->print("smartFactor1"); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); Values values; values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3* noise_pose); + values.insert(x2, pose2*noise_pose); + values.insert(x3, pose3); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; @@ -244,7 +246,7 @@ TEST( MultiProjectionFactor, 3poses ){ } -///* ************************************************************************* */ +/* ************************************************************************* TEST( MultiProjectionFactor, 3poses_projection_factor ){ cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; From bcae0afd317a7c977090a72ec1cfd1f7c47481ea Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Tue, 6 Aug 2013 14:24:10 +0000 Subject: [PATCH 23/26] Putting sub matrices along the diagonal of a big matrix. dexpL for Rot3 and Point3 --- gtsam/base/Matrix.cpp | 22 ++++++++++++++++++++++ gtsam/base/Matrix.h | 10 ++++++++++ gtsam/base/tests/testMatrix.cpp | 24 ++++++++++++++++++++++++ gtsam/geometry/Point3.h | 5 +++++ gtsam/geometry/Rot3.h | 3 +++ gtsam/geometry/Rot3M.cpp | 13 +++++++++++++ gtsam/geometry/tests/testRot3M.cpp | 19 +++++++++++++++++++ 7 files changed, 96 insertions(+) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 2b72a55e4..ad0c7c9f4 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -72,6 +72,11 @@ Matrix zeros( size_t m, size_t n ) { return Matrix::Zero(m,n); } +/* ************************************************************************* */ +Matrix ones( size_t m, size_t n ) { + return Matrix::Ones(m,n); +} + /* ************************************************************************* */ Matrix eye( size_t m, size_t n) { return Matrix::Identity(m, n); @@ -280,6 +285,23 @@ void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; } +/* ************************************************************************* */ +Matrix diag(const std::vector& Hs) { + size_t rows = 0, cols = 0; + for (size_t i = 0; i sub(const MATRIX& A, size_t i1, size_t i2, size_t j1, */ GTSAM_EXPORT void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j); +/** + * Create a matrix with submatrices along its diagonal + */ +GTSAM_EXPORT Matrix diag(const std::vector& Hs); + /** * Extracts a column view from a matrix that avoids a copy * @param A matrix to extract column from diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 4f57a6b6b..196f139a9 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -262,6 +262,30 @@ TEST( matrix, insert_sub ) EXPECT(assert_equal(expected, big)); } +/* ************************************************************************* */ +TEST( matrix, diagMatrices ) +{ + std::vector Hs; + Hs.push_back(ones(3,3)); + Hs.push_back(ones(4,4)*2); + Hs.push_back(ones(2,2)*3); + + Matrix actual = diag(Hs); + + Matrix expected = Matrix_(9, 9, + 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST( matrix, stream_read ) { Matrix expected = Matrix_(3,4, diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 5930d0e94..752caec73 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -143,6 +143,11 @@ namespace gtsam { /** Log map at identity - return the x,y,z of this point */ static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); } + /// Left-trivialized derivative of the exponential map + static Matrix dexpL(const Vector& v) { + return eye(3); + } + /// @} /// @name Vector Space /// @{ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index f4a25ff94..fc632f8d6 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -268,6 +268,9 @@ namespace gtsam { */ static Vector3 Logmap(const Rot3& R); + /// Left-trivialized derivative of the exponential map + static Matrix3 dexpL(const Vector3& v); + /// @} /// @name Group Action on Point3 /// @{ diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index cb6660dae..9f8ef1960 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -303,6 +303,19 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const } } +/* ************************************************************************* */ +/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) +Matrix3 Rot3::dexpL(const Vector3& v) { + if(zero(v)) return eye(3); + Matrix x = skewSymmetric(v); + Matrix x2 = x*x; + double theta = v.norm(), vi = theta/2.0; + double s1 = sin(vi)/vi; + double s2 = (theta - sin(theta))/(theta*theta*theta); + Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; + return res; +} + /* ************************************************************************* */ Matrix3 Rot3::matrix() const { return rot_; diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index b50ad1253..56e1a7cce 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -402,6 +402,25 @@ TEST( Rot3, between ) CHECK(assert_equal(numericalH2,actualH2)); } +/* ************************************************************************* */ +Vector w = Vector_(3, 0.1, 0.27, -0.2); + +// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes? +// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 +// => y = log (exp(-w) * exp(w+dw)) +Vector testDexpL(const LieVector& dw) { + Vector y = Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); + return y; +} + +TEST( Rot3, dexpL) { + Matrix actualDexpL = Rot3::dexpL(w); + Matrix expectedDexpL = numericalDerivative11( + boost::function( + boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); +} + /* ************************************************************************* */ TEST( Rot3, xyz ) { From 81b9fc33b7c01ba66897d4d2324faf2d1bbe7701 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Tue, 6 Aug 2013 15:35:37 +0000 Subject: [PATCH 24/26] in diag with matrices: initialize the result with zeros first. --- gtsam/base/Matrix.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index ad0c7c9f4..c4ad41879 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -292,7 +292,7 @@ Matrix diag(const std::vector& Hs) { rows+= Hs[i].rows(); cols+= Hs[i].cols(); } - Matrix results(rows,cols); + Matrix results = zeros(rows,cols); size_t r = 0, c = 0; for (size_t i = 0; i Date: Tue, 6 Aug 2013 16:26:35 +0000 Subject: [PATCH 25/26] Point3 and Rot3 dexpInvL --- gtsam/geometry/Point3.h | 5 +++++ gtsam/geometry/Rot3.h | 3 +++ gtsam/geometry/Rot3M.cpp | 13 +++++++++++++ gtsam/geometry/tests/testRot3M.cpp | 3 +++ 4 files changed, 24 insertions(+) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 752caec73..5e2e0ff42 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -148,6 +148,11 @@ namespace gtsam { return eye(3); } + /// Left-trivialized derivative inverse of the exponential map + static Matrix dexpInvL(const Vector& v) { + return eye(3); + } + /// @} /// @name Vector Space /// @{ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index fc632f8d6..9c99cd7aa 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -271,6 +271,9 @@ namespace gtsam { /// Left-trivialized derivative of the exponential map static Matrix3 dexpL(const Vector3& v); + /// Left-trivialized derivative inverse of the exponential map + static Matrix3 dexpInvL(const Vector3& v); + /// @} /// @name Group Action on Point3 /// @{ diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 9f8ef1960..2663f1bf8 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -316,6 +316,19 @@ Matrix3 Rot3::dexpL(const Vector3& v) { return res; } +/* ************************************************************************* */ +/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) +Matrix3 Rot3::dexpInvL(const Vector3& v) { + if(zero(v)) return eye(3); + Matrix x = skewSymmetric(v); + Matrix x2 = x*x; + double theta = v.norm(), vi = theta/2.0; + double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); + Matrix res = eye(3) + 0.5*x - s2*x2; + return res; +} + + /* ************************************************************************* */ Matrix3 Rot3::matrix() const { return rot_; diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 56e1a7cce..5874af9a0 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -419,6 +419,9 @@ TEST( Rot3, dexpL) { boost::function( boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); + + Matrix actualDexpInvL = Rot3::dexpInvL(w); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); } /* ************************************************************************* */ From 43ebc2c55eafba1719939f1add033269d3d86d36 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 6 Aug 2013 18:24:06 +0000 Subject: [PATCH 26/26] Upgraded Eigen to the new release: 3.2.0 --- README | 4 +- gtsam/3rdparty/Eigen/.hg_archival.txt | 4 + gtsam/3rdparty/Eigen/.hgeol | 8 + gtsam/3rdparty/Eigen/.hgignore | 32 + gtsam/3rdparty/Eigen/.hgtags | 25 + gtsam/3rdparty/Eigen/CMakeLists.txt | 80 +- gtsam/3rdparty/Eigen/COPYING.MINPACK | 104 +- gtsam/3rdparty/Eigen/CTestConfig.cmake | 4 + gtsam/3rdparty/Eigen/CTestCustom.cmake.in | 5 +- gtsam/3rdparty/Eigen/Eigen/Core | 48 +- gtsam/3rdparty/Eigen/Eigen/Eigenvalues | 2 + .../Eigen/Eigen/IterativeLinearSolvers | 2 +- gtsam/3rdparty/Eigen/Eigen/MetisSupport | 28 + gtsam/3rdparty/Eigen/Eigen/OrderingMethods | 51 +- gtsam/3rdparty/Eigen/Eigen/SPQRSupport | 29 + gtsam/3rdparty/Eigen/Eigen/Sparse | 14 +- gtsam/3rdparty/Eigen/Eigen/SparseCholesky | 21 +- gtsam/3rdparty/Eigen/Eigen/SparseCore | 4 +- gtsam/3rdparty/Eigen/Eigen/SparseLU | 49 + gtsam/3rdparty/Eigen/Eigen/SparseQR | 33 + .../3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h | 44 +- gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h | 20 +- .../Eigen/src/CholmodSupport/CholmodSupport.h | 43 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h | 32 +- .../3rdparty/Eigen/Eigen/src/Core/ArrayBase.h | 2 +- .../Eigen/Eigen/src/Core/ArrayWrapper.h | 54 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h | 32 +- .../Eigen/Eigen/src/Core/Assign_MKL.h | 2 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h | 184 +- .../Eigen/Eigen/src/Core/BooleanRedux.h | 28 +- .../Eigen/Eigen/src/Core/CommaInitializer.h | 2 + .../src/{SparseCore => Core}/CoreIterators.h | 0 .../Eigen/Eigen/src/Core/CwiseBinaryOp.h | 22 +- .../Eigen/Eigen/src/Core/CwiseNullaryOp.h | 134 +- .../Eigen/Eigen/src/Core/CwiseUnaryOp.h | 8 +- .../Eigen/Eigen/src/Core/CwiseUnaryView.h | 16 +- .../3rdparty/Eigen/Eigen/src/Core/DenseBase.h | 72 +- .../Eigen/Eigen/src/Core/DenseCoeffsBase.h | 12 +- .../Eigen/Eigen/src/Core/DenseStorage.h | 114 +- .../3rdparty/Eigen/Eigen/src/Core/Diagonal.h | 41 +- .../Eigen/Eigen/src/Core/DiagonalMatrix.h | 14 +- .../Eigen/Eigen/src/Core/DiagonalProduct.h | 51 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h | 12 +- .../3rdparty/Eigen/Eigen/src/Core/EigenBase.h | 3 +- .../3rdparty/Eigen/Eigen/src/Core/Functors.h | 86 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Fuzzy.h | 22 +- .../Eigen/Eigen/src/Core/GeneralProduct.h | 100 +- .../Eigen/Eigen/src/Core/GenericPacketMath.h | 50 +- .../Eigen/Eigen/src/Core/GlobalFunctions.h | 49 +- gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h | 4 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h | 30 +- gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h | 50 +- .../Eigen/Eigen/src/Core/MathFunctions.h | 354 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h | 8 +- .../Eigen/Eigen/src/Core/MatrixBase.h | 32 +- gtsam/3rdparty/Eigen/Eigen/src/Core/NoAlias.h | 9 + .../3rdparty/Eigen/Eigen/src/Core/NumTraits.h | 3 + .../Eigen/Eigen/src/Core/PermutationMatrix.h | 43 +- .../Eigen/Eigen/src/Core/PlainObjectBase.h | 159 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h | 98 - .../Eigen/Eigen/src/Core/ProductBase.h | 20 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Random.h | 14 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h | 6 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h | 255 + .../3rdparty/Eigen/Eigen/src/Core/Replicate.h | 28 +- .../Eigen/Eigen/src/Core/ReturnByValue.h | 2 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Select.h | 14 +- .../Eigen/Eigen/src/Core/SelfAdjointView.h | 16 +- .../Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h | 5 +- .../Eigen/Eigen/src/Core/StableNorm.h | 188 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Swap.h | 34 +- .../3rdparty/Eigen/Eigen/src/Core/Transpose.h | 41 +- .../Eigen/Eigen/src/Core/Transpositions.h | 18 +- .../Eigen/Eigen/src/Core/TriangularMatrix.h | 14 +- .../Eigen/Eigen/src/Core/VectorBlock.h | 189 - .../Eigen/Eigen/src/Core/VectorwiseOp.h | 47 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h | 28 +- .../Eigen/src/Core/arch/AltiVec/PacketMath.h | 3 + .../Eigen/Eigen/src/Core/arch/NEON/Complex.h | 10 +- .../Eigen/src/Core/arch/NEON/PacketMath.h | 50 +- .../Eigen/Eigen/src/Core/arch/SSE/Complex.h | 22 +- .../Eigen/src/Core/arch/SSE/MathFunctions.h | 90 +- .../Eigen/src/Core/arch/SSE/PacketMath.h | 20 +- .../src/Core/products/CoeffBasedProduct.h | 2 +- .../Core/products/GeneralBlockPanelKernel.h | 336 +- .../src/Core/products/GeneralMatrixMatrix.h | 7 +- .../products/GeneralMatrixMatrixTriangular.h | 112 +- .../src/Core/products/GeneralMatrixVector.h | 27 +- .../Core/products/GeneralMatrixVector_MKL.h | 6 +- .../Core/products/SelfadjointMatrixMatrix.h | 66 +- .../products/SelfadjointMatrixMatrix_MKL.h | 10 +- .../Core/products/SelfadjointMatrixVector.h | 27 +- .../products/SelfadjointMatrixVector_MKL.h | 4 +- .../src/Core/products/SelfadjointProduct.h | 20 +- .../Core/products/SelfadjointRank2Update.h | 18 +- .../Core/products/TriangularMatrixMatrix.h | 40 +- .../products/TriangularMatrixMatrix_MKL.h | 4 +- .../Core/products/TriangularMatrixVector.h | 32 +- .../products/TriangularMatrixVector_MKL.h | 12 +- .../Core/products/TriangularSolverMatrix.h | 18 +- .../products/TriangularSolverMatrix_MKL.h | 4 +- .../Eigen/Eigen/src/Core/util/BlasUtil.h | 10 +- .../Eigen/Eigen/src/Core/util/Constants.h | 15 +- .../Eigen/src/Core/util/ForwardDeclarations.h | 7 +- .../Eigen/Eigen/src/Core/util/Macros.h | 32 +- .../Eigen/Eigen/src/Core/util/Memory.h | 59 +- .../3rdparty/Eigen/Eigen/src/Core/util/Meta.h | 20 +- .../Eigen/Eigen/src/Core/util/StaticAssert.h | 3 +- .../Eigen/Eigen/src/Core/util/XprHelper.h | 24 +- .../src/Eigen2Support/Geometry/AlignedBox.h | 4 +- .../src/Eigen2Support/Geometry/AngleAxis.h | 2 +- .../src/Eigen2Support/Geometry/Hyperplane.h | 4 +- .../Eigen2Support/Geometry/ParametrizedLine.h | 4 +- .../src/Eigen2Support/Geometry/Quaternion.h | 2 +- .../src/Eigen2Support/Geometry/Rotation2D.h | 2 +- .../src/Eigen2Support/Geometry/RotationBase.h | 2 +- .../src/Eigen2Support/Geometry/Scaling.h | 2 +- .../src/Eigen2Support/Geometry/Transform.h | 2 +- .../src/Eigen2Support/Geometry/Translation.h | 2 +- .../Eigen/src/Eigen2Support/LeastSquares.h | 2 +- .../Eigen/src/Eigen2Support/MathFunctions.h | 24 +- .../Eigen/Eigen/src/Eigen2Support/SVD.h | 10 +- .../src/Eigenvalues/ComplexEigenSolver.h | 26 +- .../Eigen/src/Eigenvalues/ComplexSchur.h | 98 +- .../Eigen/src/Eigenvalues/ComplexSchur_MKL.h | 2 +- .../Eigen/Eigen/src/Eigenvalues/EigenSolver.h | 71 +- .../src/Eigenvalues/GeneralizedEigenSolver.h | 341 + .../src/Eigenvalues/HessenbergDecomposition.h | 6 +- .../src/Eigenvalues/MatrixBaseEigenvalues.h | 3 +- .../Eigen/Eigen/src/Eigenvalues/RealQZ.h | 624 ++ .../Eigen/Eigen/src/Eigenvalues/RealSchur.h | 117 +- .../Eigen/src/Eigenvalues/RealSchur_MKL.h | 2 +- .../src/Eigenvalues/SelfAdjointEigenSolver.h | 26 +- .../Eigenvalues/SelfAdjointEigenSolver_MKL.h | 2 +- .../src/Eigenvalues/Tridiagonalization.h | 12 +- .../Eigen/Eigen/src/Geometry/AlignedBox.h | 12 +- .../Eigen/Eigen/src/Geometry/AngleAxis.h | 13 +- .../Eigen/Eigen/src/Geometry/EulerAngles.h | 60 +- .../Eigen/Eigen/src/Geometry/Homogeneous.h | 2 +- .../Eigen/Eigen/src/Geometry/Hyperplane.h | 11 +- .../Eigen/Eigen/src/Geometry/OrthoMethods.h | 30 +- .../Eigen/src/Geometry/ParametrizedLine.h | 8 +- .../Eigen/Eigen/src/Geometry/Quaternion.h | 71 +- .../Eigen/Eigen/src/Geometry/Rotation2D.h | 15 +- .../Eigen/Eigen/src/Geometry/Scaling.h | 6 +- .../Eigen/Eigen/src/Geometry/Transform.h | 18 +- .../Eigen/Eigen/src/Geometry/Umeyama.h | 23 +- .../Eigen/Eigen/src/Householder/Householder.h | 13 +- .../src/Householder/HouseholderSequence.h | 14 +- .../src/IterativeLinearSolvers/BiCGSTAB.h | 31 +- .../ConjugateGradient.h | 26 +- .../IterativeLinearSolvers/IncompleteLUT.h | 215 +- .../IterativeSolverBase.h | 2 +- .../3rdparty/Eigen/Eigen/src/Jacobi/Jacobi.h | 99 +- .../3rdparty/Eigen/Eigen/src/LU/Determinant.h | 2 +- gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h | 10 +- gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h | 6 +- .../Eigen/Eigen/src/LU/PartialPivLU.h | 9 +- .../Eigen/src/MetisSupport/CMakeLists.txt | 6 + .../Eigen/src/MetisSupport/MetisSupport.h | 137 + .../Eigen/Eigen/src/OrderingMethods/Amd.h | 6 +- .../Eigen/src/OrderingMethods/Eigen_Colamd.h | 1850 ++++++ .../Eigen/src/OrderingMethods/Ordering.h | 150 + .../Eigen/src/PaStiXSupport/PaStiXSupport.h | 23 +- .../Eigen/src/PardisoSupport/PardisoSupport.h | 25 +- .../Eigen/Eigen/src/QR/ColPivHouseholderQR.h | 105 +- .../Eigen/src/QR/ColPivHouseholderQR_MKL.h | 5 +- .../Eigen/Eigen/src/QR/FullPivHouseholderQR.h | 39 +- .../Eigen/Eigen/src/QR/HouseholderQR.h | 47 +- .../Eigen/src/SPQRSupport/CMakeLists.txt | 6 + .../src/SPQRSupport/SuiteSparseQRSupport.h | 306 + .../3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h | 84 +- .../Eigen/src/SVD/UpperBidiagonalization.h | 4 +- .../src/SparseCholesky/SimplicialCholesky.h | 214 +- .../SparseCholesky/SimplicialCholesky_impl.h | 199 + .../Eigen/Eigen/src/SparseCore/AmbiVector.h | 10 +- .../Eigen/src/SparseCore/CompressedStorage.h | 8 +- .../ConservativeSparseSparseProduct.h | 6 +- .../Eigen/Eigen/src/SparseCore/SparseAssign.h | 0 .../Eigen/Eigen/src/SparseCore/SparseBlock.h | 436 +- .../Eigen/src/SparseCore/SparseColEtree.h | 204 + .../src/SparseCore/SparseCwiseBinaryOp.h | 6 +- .../Eigen/src/SparseCore/SparseDenseProduct.h | 15 +- .../src/SparseCore/SparseDiagonalProduct.h | 22 +- .../Eigen/Eigen/src/SparseCore/SparseDot.h | 19 +- .../Eigen/Eigen/src/SparseCore/SparseMatrix.h | 594 +- .../Eigen/src/SparseCore/SparseMatrixBase.h | 69 +- .../Eigen/src/SparseCore/SparseProduct.h | 7 +- .../src/SparseCore/SparseSelfAdjointView.h | 45 +- .../SparseSparseProductWithPruning.h | 10 +- .../Eigen/src/SparseCore/SparseTranspose.h | 12 +- .../src/SparseCore/SparseTriangularView.h | 31 +- .../Eigen/Eigen/src/SparseCore/SparseUtil.h | 17 +- .../Eigen/Eigen/src/SparseCore/SparseVector.h | 159 +- .../Eigen/Eigen/src/SparseCore/SparseView.h | 5 +- .../Eigen/Eigen/src/SparseLU/CMakeLists.txt | 6 + .../Eigen/Eigen/src/SparseLU/SparseLU.h | 756 +++ .../Eigen/Eigen/src/SparseLU/SparseLUImpl.h | 64 + .../Eigen/src/SparseLU/SparseLU_Memory.h | 222 + .../Eigen/src/SparseLU/SparseLU_Structs.h | 111 + .../src/SparseLU/SparseLU_SupernodalMatrix.h | 298 + .../Eigen/Eigen/src/SparseLU/SparseLU_Utils.h | 80 + .../Eigen/src/SparseLU/SparseLU_column_bmod.h | 180 + .../Eigen/src/SparseLU/SparseLU_column_dfs.h | 177 + .../src/SparseLU/SparseLU_copy_to_ucol.h | 106 + .../Eigen/src/SparseLU/SparseLU_gemm_kernel.h | 279 + .../src/SparseLU/SparseLU_heap_relax_snode.h | 127 + .../Eigen/src/SparseLU/SparseLU_kernel_bmod.h | 130 + .../Eigen/src/SparseLU/SparseLU_panel_bmod.h | 223 + .../Eigen/src/SparseLU/SparseLU_panel_dfs.h | 258 + .../Eigen/src/SparseLU/SparseLU_pivotL.h | 134 + .../Eigen/src/SparseLU/SparseLU_pruneL.h | 135 + .../Eigen/src/SparseLU/SparseLU_relax_snode.h | 83 + .../Eigen/Eigen/src/SparseQR/CMakeLists.txt | 6 + .../Eigen/Eigen/src/SparseQR/SparseQR.h | 654 ++ .../Eigen/src/SuperLUSupport/SuperLUSupport.h | 23 +- .../Eigen/src/UmfPackSupport/UmfPackSupport.h | 37 +- .../Eigen/Eigen/src/misc/SparseSolve.h | 17 + .../Eigen/src/plugins/ArrayCwiseBinaryOps.h | 14 +- .../Eigen/Eigen/src/plugins/BlockMethods.h | 331 +- gtsam/3rdparty/Eigen/bench/benchGeometry.cpp | 134 + gtsam/3rdparty/Eigen/bench/bench_gemm.cpp | 2 +- .../Eigen/bench/spbench/CMakeLists.txt | 13 + .../Eigen/bench/spbench/sp_solver.cpp | 125 + .../3rdparty/Eigen/bench/spbench/spbench.dtd | 31 + .../Eigen/bench/spbench/spbenchsolver.cpp | 15 +- .../Eigen/bench/spbench/spbenchsolver.h | 477 +- .../Eigen/bench/spbench/spbenchstyle.h | 94 + .../Eigen/bench/spbench/test_sparseLU.cpp | 93 + gtsam/3rdparty/Eigen/bench/spmv.cpp | 2 +- gtsam/3rdparty/Eigen/blas/CMakeLists.txt | 8 +- .../3rdparty/Eigen/blas/GeneralRank1Update.h | 44 + .../Eigen/blas/PackedSelfadjointProduct.h | 53 + .../Eigen/blas/PackedTriangularMatrixVector.h | 79 + .../Eigen/blas/PackedTriangularSolverVector.h | 88 + gtsam/3rdparty/Eigen/blas/Rank2Update.h | 57 + gtsam/3rdparty/Eigen/blas/chpr.f | 220 - gtsam/3rdparty/Eigen/blas/chpr2.f | 255 - gtsam/3rdparty/Eigen/blas/common.h | 13 +- gtsam/3rdparty/Eigen/blas/ctpmv.f | 329 - gtsam/3rdparty/Eigen/blas/ctpsv.f | 332 - gtsam/3rdparty/Eigen/blas/double.cpp | 14 + gtsam/3rdparty/Eigen/blas/dspr.f | 202 - gtsam/3rdparty/Eigen/blas/dspr2.f | 233 - gtsam/3rdparty/Eigen/blas/dtpmv.f | 293 - gtsam/3rdparty/Eigen/blas/dtpsv.f | 296 - gtsam/3rdparty/Eigen/blas/level1_cplx_impl.h | 2 +- gtsam/3rdparty/Eigen/blas/level1_impl.h | 21 +- gtsam/3rdparty/Eigen/blas/level2_cplx_impl.h | 178 +- gtsam/3rdparty/Eigen/blas/level2_impl.h | 147 +- gtsam/3rdparty/Eigen/blas/level2_real_impl.h | 210 +- gtsam/3rdparty/Eigen/blas/level3_impl.h | 22 +- gtsam/3rdparty/Eigen/blas/single.cpp | 3 + gtsam/3rdparty/Eigen/blas/sspr.f | 202 - gtsam/3rdparty/Eigen/blas/sspr2.f | 233 - gtsam/3rdparty/Eigen/blas/stpmv.f | 293 - gtsam/3rdparty/Eigen/blas/stpsv.f | 296 - gtsam/3rdparty/Eigen/blas/testing/dblat1.f | 476 +- gtsam/3rdparty/Eigen/blas/testing/sblat1.f | 430 +- gtsam/3rdparty/Eigen/blas/zhpr.f | 220 - gtsam/3rdparty/Eigen/blas/zhpr2.f | 255 - gtsam/3rdparty/Eigen/blas/ztpmv.f | 329 - gtsam/3rdparty/Eigen/blas/ztpsv.f | 332 - .../cmake/CMakeDetermineVSServicePack.cmake | 103 - .../Eigen/cmake/EigenConfigureTesting.cmake | 12 +- .../cmake/EigenDetermineVSServicePack.cmake | 27 + gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake | 35 +- gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake | 11 +- gtsam/3rdparty/Eigen/cmake/FindMetis.cmake | 3 +- gtsam/3rdparty/Eigen/cmake/FindSPQR.cmake | 36 + gtsam/3rdparty/Eigen/cmake/FindSuperLU.cmake | 3 +- gtsam/3rdparty/Eigen/cmake/FindUmfpack.cmake | 27 +- .../Eigen/cmake/language_support.cmake | 2 + gtsam/3rdparty/Eigen/debug/msvc/eigen.natvis | 235 + .../Eigen/doc/A05_PortingFrom2To3.dox | 23 +- .../Eigen/doc/A10_Eigen2SupportModes.dox | 10 +- .../Eigen/doc/AsciiQuickReference.txt | 43 +- gtsam/3rdparty/Eigen/doc/B01_Experimental.dox | 5 +- gtsam/3rdparty/Eigen/doc/CMakeLists.txt | 52 +- ..._ClassHierarchy.dox => ClassHierarchy.dox} | 6 +- ...tomizingEigen.dox => CustomizingEigen.dox} | 9 +- gtsam/3rdparty/Eigen/doc/Doxyfile.in | 735 ++- ...orizable.dox => FixedSizeVectorizable.dox} | 2 +- ...ypes.dox => FunctionsTakingEigenTypes.dox} | 57 +- ...02_HiPerformance.dox => HiPerformance.dox} | 2 +- gtsam/3rdparty/Eigen/doc/I10_Assertions.dox | 13 - ...igenExample.dox => InsideEigenExample.dox} | 7 +- .../3rdparty/Eigen/doc/LinearLeastSquares.dox | 27 + gtsam/3rdparty/Eigen/doc/Manual.dox | 159 + gtsam/3rdparty/Eigen/doc/Overview.dox | 62 +- ..._PassingByValue.dox => PassingByValue.dox} | 2 +- ...ectives.dox => PreprocessorDirectives.dox} | 20 +- gtsam/3rdparty/Eigen/doc/QuickReference.dox | 24 +- ...uickStartGuide.dox => QuickStartGuide.dox} | 3 +- .../Eigen/doc/SparseLinearSystems.dox | 183 + .../Eigen/doc/SparseQuickReference.dox | 243 +- ...01_StlContainers.dox => StlContainers.dox} | 11 +- ...15_StorageOrders.dox => StorageOrders.dox} | 7 +- ...mbers.dox => StructHavingEigenMembers.dox} | 12 +- ...emplateKeyword.dox => TemplateKeyword.dox} | 6 +- .../{I11_Aliasing.dox => TopicAliasing.dox} | 59 +- gtsam/3rdparty/Eigen/doc/TopicAssertions.dox | 108 + ....dox => TopicEigenExpressionTemplates.dox} | 0 ...Evaluation.dox => TopicLazyEvaluation.dox} | 0 .../doc/TopicLinearAlgebraDecompositions.dox | 5 +- .../{I08_Resizing.dox => TopicResizing.dox} | 0 ...icScalarTypes.dox => TopicScalarTypes.dox} | 0 ...ctorization.dox => TopicVectorization.dox} | 0 ...dox => TutorialAdvancedInitialization.dox} | 14 +- ...lArrayClass.dox => TutorialArrayClass.dox} | 27 +- ...ations.dox => TutorialBlockOperations.dox} | 17 +- ...orialGeometry.dox => TutorialGeometry.dox} | 18 +- ...rAlgebra.dox => TutorialLinearAlgebra.dox} | 24 +- ...orialMapClass.dox => TutorialMapClass.dox} | 26 +- ...metic.dox => TutorialMatrixArithmetic.dox} | 21 +- ...atrixClass.dox => TutorialMatrixClass.dox} | 37 +- ...utorialReductionsVisitorsBroadcasting.dox} | 22 +- ..._TutorialSparse.dox => TutorialSparse.dox} | 147 +- ...rayAssert.dox => UnalignedArrayAssert.dox} | 11 +- gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox | 6 +- ...kAlignment.dox => WrongStackAlignment.dox} | 2 +- .../3rdparty/Eigen/doc/eigen_navtree_hacks.js | 240 + gtsam/3rdparty/Eigen/doc/eigendoxy.css | 804 +-- .../Eigen/doc/eigendoxy_footer.html.in | 23 +- .../Eigen/doc/eigendoxy_header.html.in | 63 +- .../Eigen/doc/eigendoxy_layout.xml.in | 178 + .../doc/examples/function_taking_ref.cpp | 19 + .../Eigen/doc/snippets/Cwise_asin.cpp | 2 + .../doc/snippets/DenseBase_setLinSpaced.cpp | 2 +- .../doc/snippets/GeneralizedEigenSolver.cpp | 7 + .../snippets/HouseholderQR_householderQ.cpp | 7 + ...template_int_int_block_int_int_int_int.cpp | 5 + ...plate_int_int_bottomLeftCorner_int_int.cpp | 6 + ...late_int_int_bottomRightCorner_int_int.cpp | 6 + ...template_int_int_topLeftCorner_int_int.cpp | 6 + ...emplate_int_int_topRightCorner_int_int.cpp | 6 + .../Eigen/doc/snippets/RealQZ_compute.cpp | 17 + gtsam/3rdparty/Eigen/lapack/CMakeLists.txt | 742 ++- gtsam/3rdparty/Eigen/lapack/cholesky.cpp | 4 +- gtsam/3rdparty/Eigen/lapack/clacgv.f | 116 + gtsam/3rdparty/Eigen/lapack/cladiv.f | 97 + gtsam/3rdparty/Eigen/lapack/clarf.f | 232 + gtsam/3rdparty/Eigen/lapack/clarfb.f | 771 +++ gtsam/3rdparty/Eigen/lapack/clarfg.f | 203 + gtsam/3rdparty/Eigen/lapack/clarft.f | 328 + gtsam/3rdparty/Eigen/lapack/dladiv.f | 128 + gtsam/3rdparty/Eigen/lapack/dlamch.f | 189 + gtsam/3rdparty/Eigen/lapack/dlapy2.f | 104 + gtsam/3rdparty/Eigen/lapack/dlapy3.f | 111 + gtsam/3rdparty/Eigen/lapack/dlarf.f | 227 + gtsam/3rdparty/Eigen/lapack/dlarfb.f | 762 +++ gtsam/3rdparty/Eigen/lapack/dlarfg.f | 196 + gtsam/3rdparty/Eigen/lapack/dlarft.f | 326 + gtsam/3rdparty/Eigen/lapack/dsecnd_NONE.f | 52 + gtsam/3rdparty/Eigen/lapack/ilaclc.f | 118 + gtsam/3rdparty/Eigen/lapack/ilaclr.f | 121 + gtsam/3rdparty/Eigen/lapack/iladlc.f | 118 + gtsam/3rdparty/Eigen/lapack/iladlr.f | 121 + gtsam/3rdparty/Eigen/lapack/ilaslc.f | 118 + gtsam/3rdparty/Eigen/lapack/ilaslr.f | 121 + gtsam/3rdparty/Eigen/lapack/ilazlc.f | 118 + gtsam/3rdparty/Eigen/lapack/ilazlr.f | 121 + gtsam/3rdparty/Eigen/lapack/lu.cpp | 4 +- gtsam/3rdparty/Eigen/lapack/second_NONE.f | 52 + gtsam/3rdparty/Eigen/lapack/sladiv.f | 128 + gtsam/3rdparty/Eigen/lapack/slamch.f | 192 + gtsam/3rdparty/Eigen/lapack/slapy2.f | 104 + gtsam/3rdparty/Eigen/lapack/slapy3.f | 111 + gtsam/3rdparty/Eigen/lapack/slarf.f | 227 + gtsam/3rdparty/Eigen/lapack/slarfb.f | 763 +++ gtsam/3rdparty/Eigen/lapack/slarfg.f | 196 + gtsam/3rdparty/Eigen/lapack/slarft.f | 326 + gtsam/3rdparty/Eigen/lapack/zlacgv.f | 116 + gtsam/3rdparty/Eigen/lapack/zladiv.f | 97 + gtsam/3rdparty/Eigen/lapack/zlarf.f | 232 + gtsam/3rdparty/Eigen/lapack/zlarfb.f | 774 +++ gtsam/3rdparty/Eigen/lapack/zlarfg.f | 203 + gtsam/3rdparty/Eigen/lapack/zlarft.f | 327 + .../Eigen/scripts/cdashtesting.cmake.in | 49 + gtsam/3rdparty/Eigen/scripts/eigen_gen_docs | 3 +- gtsam/3rdparty/Eigen/test/CMakeLists.txt | 51 +- gtsam/3rdparty/Eigen/test/adjoint.cpp | 83 +- gtsam/3rdparty/Eigen/test/array.cpp | 88 +- .../3rdparty/Eigen/test/array_for_matrix.cpp | 22 +- gtsam/3rdparty/Eigen/test/array_replicate.cpp | 1 - gtsam/3rdparty/Eigen/test/basicstuff.cpp | 11 +- gtsam/3rdparty/Eigen/test/block.cpp | 14 +- gtsam/3rdparty/Eigen/test/cholesky.cpp | 64 +- .../Eigen/test/conservative_resize.cpp | 33 +- gtsam/3rdparty/Eigen/test/corners.cpp | 20 + gtsam/3rdparty/Eigen/test/cwiseop.cpp | 63 +- gtsam/3rdparty/Eigen/test/denseLM.cpp | 190 + gtsam/3rdparty/Eigen/test/determinant.cpp | 6 +- gtsam/3rdparty/Eigen/test/diagonal.cpp | 16 +- .../3rdparty/Eigen/test/diagonalmatrices.cpp | 1 - gtsam/3rdparty/Eigen/test/dynalloc.cpp | 1 + gtsam/3rdparty/Eigen/test/eigen2support.cpp | 4 +- .../Eigen/test/eigensolver_complex.cpp | 21 +- .../test/eigensolver_generalized_real.cpp | 59 + .../Eigen/test/eigensolver_generic.cpp | 20 +- .../Eigen/test/eigensolver_selfadjoint.cpp | 11 +- gtsam/3rdparty/Eigen/test/exceptions.cpp | 4 + gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp | 11 +- gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp | 61 +- gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp | 4 +- .../Eigen/test/geo_parametrizedline.cpp | 5 +- gtsam/3rdparty/Eigen/test/geo_quaternion.cpp | 63 +- .../Eigen/test/geo_transformations.cpp | 27 +- gtsam/3rdparty/Eigen/test/householder.cpp | 29 +- gtsam/3rdparty/Eigen/test/inverse.cpp | 14 +- gtsam/3rdparty/Eigen/test/jacobi.cpp | 12 +- gtsam/3rdparty/Eigen/test/jacobisvd.cpp | 13 +- gtsam/3rdparty/Eigen/test/linearstructure.cpp | 3 +- gtsam/3rdparty/Eigen/test/lu.cpp | 4 - gtsam/3rdparty/Eigen/test/main.h | 73 +- .../Eigen/test/{map.cpp => mapped_matrix.cpp} | 5 +- gtsam/3rdparty/Eigen/test/mapstride.cpp | 4 +- gtsam/3rdparty/Eigen/test/meta.cpp | 5 +- gtsam/3rdparty/Eigen/test/metis_support.cpp | 39 + gtsam/3rdparty/Eigen/test/miscmatrices.cpp | 1 - gtsam/3rdparty/Eigen/test/nesting_ops.cpp | 12 +- gtsam/3rdparty/Eigen/test/nomalloc.cpp | 7 +- gtsam/3rdparty/Eigen/test/nullary.cpp | 6 + gtsam/3rdparty/Eigen/test/packetmath.cpp | 54 +- gtsam/3rdparty/Eigen/test/pastix_support.cpp | 2 +- .../Eigen/test/permutationmatrices.cpp | 1 - .../3rdparty/Eigen/test/prec_inverse_4x4.cpp | 4 +- gtsam/3rdparty/Eigen/test/product.h | 1 - gtsam/3rdparty/Eigen/test/product_extra.cpp | 3 +- gtsam/3rdparty/Eigen/test/product_mmtr.cpp | 2 - .../Eigen/test/product_notemporary.cpp | 5 +- .../Eigen/test/product_selfadjoint.cpp | 9 +- gtsam/3rdparty/Eigen/test/product_symm.cpp | 2 - gtsam/3rdparty/Eigen/test/product_syrk.cpp | 47 +- gtsam/3rdparty/Eigen/test/product_trmm.cpp | 4 +- gtsam/3rdparty/Eigen/test/product_trmv.cpp | 4 +- gtsam/3rdparty/Eigen/test/qr.cpp | 7 +- gtsam/3rdparty/Eigen/test/qr_colpivoting.cpp | 8 +- gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp | 7 +- gtsam/3rdparty/Eigen/test/real_qz.cpp | 65 + gtsam/3rdparty/Eigen/test/redux.cpp | 39 +- gtsam/3rdparty/Eigen/test/ref.cpp | 232 + gtsam/3rdparty/Eigen/test/schur_complex.cpp | 19 +- gtsam/3rdparty/Eigen/test/schur_real.cpp | 21 +- gtsam/3rdparty/Eigen/test/selfadjoint.cpp | 5 +- gtsam/3rdparty/Eigen/test/sizeof.cpp | 2 +- gtsam/3rdparty/Eigen/test/sparse.h | 49 +- gtsam/3rdparty/Eigen/test/sparseLM.cpp | 176 + gtsam/3rdparty/Eigen/test/sparse_basic.cpp | 346 +- gtsam/3rdparty/Eigen/test/sparse_product.cpp | 65 +- gtsam/3rdparty/Eigen/test/sparse_solver.h | 145 +- gtsam/3rdparty/Eigen/test/sparse_vector.cpp | 13 + gtsam/3rdparty/Eigen/test/sparselu.cpp | 55 + gtsam/3rdparty/Eigen/test/sparseqr.cpp | 90 + gtsam/3rdparty/Eigen/test/special_numbers.cpp | 58 + gtsam/3rdparty/Eigen/test/spqr_support.cpp | 62 + gtsam/3rdparty/Eigen/test/stable_norm.cpp | 24 +- gtsam/3rdparty/Eigen/test/triangular.cpp | 9 +- gtsam/3rdparty/Eigen/test/umeyama.cpp | 11 +- gtsam/3rdparty/Eigen/test/unalignedcount.cpp | 2 + .../Eigen/test/upperbidiagonalization.cpp | 4 +- gtsam/3rdparty/Eigen/test/vectorwiseop.cpp | 25 +- gtsam/3rdparty/Eigen/test/visitor.cpp | 16 +- .../Eigen/unsupported/Eigen/AdolcForward | 2 +- .../Eigen/unsupported/Eigen/AlignedVector3 | 5 +- .../Eigen/unsupported/Eigen/ArpackSupport | 31 + .../3rdparty/Eigen/unsupported/Eigen/AutoDiff | 2 +- gtsam/3rdparty/Eigen/unsupported/Eigen/BVH | 2 +- .../Eigen/unsupported/Eigen/CMakeLists.txt | 2 +- gtsam/3rdparty/Eigen/unsupported/Eigen/FFT | 2 +- .../Eigen/unsupported/Eigen/IterativeSolvers | 7 +- .../Eigen/unsupported/Eigen/KroneckerProduct | 10 +- .../unsupported/Eigen/LevenbergMarquardt | 45 + .../Eigen/unsupported/Eigen/MPRealSupport | 175 +- .../Eigen/unsupported/Eigen/MatrixFunctions | 93 +- .../Eigen/unsupported/Eigen/MoreVectorization | 10 +- .../unsupported/Eigen/NonLinearOptimization | 4 +- .../Eigen/unsupported/Eigen/NumericalDiff | 2 +- .../Eigen/unsupported/Eigen/OpenGLSupport | 2 +- .../Eigen/unsupported/Eigen/Polynomials | 13 +- gtsam/3rdparty/Eigen/unsupported/Eigen/SVD | 39 + .../3rdparty/Eigen/unsupported/Eigen/Skyline | 10 +- .../Eigen/unsupported/Eigen/SparseExtra | 11 +- .../3rdparty/Eigen/unsupported/Eigen/Splines | 2 +- .../Eigen/src/AutoDiff/AutoDiffScalar.h | 38 +- .../Eigen/src/AutoDiff/AutoDiffVector.h | 4 +- .../unsupported/Eigen/src/BVH/BVAlgorithms.h | 2 +- .../ArpackSelfAdjointEigenSolver.h | 805 +++ .../Eigen/src/FFT/ei_kissfft_impl.h | 2 + .../IterativeSolvers/ConstrainedConjGrad.h | 4 - .../Eigen/src/IterativeSolvers/DGMRES.h | 542 ++ .../Eigen/src/IterativeSolvers/GMRES.h | 4 +- .../src/IterativeSolvers/IncompleteCholesky.h | 278 + .../IterativeSolvers/IterationController.h | 7 +- .../Eigen/src/IterativeSolvers/MINRES.h | 302 + .../Eigen/src/IterativeSolvers/Scaling.h | 22 +- .../KroneckerProduct/KroneckerTensorProduct.h | 289 +- .../src/LevenbergMarquardt/CMakeLists.txt | 6 + .../LevenbergMarquardt/CopyrightMINPACK.txt | 52 + .../Eigen/src/LevenbergMarquardt/LMcovar.h | 85 + .../Eigen/src/LevenbergMarquardt/LMonestep.h | 202 + .../Eigen/src/LevenbergMarquardt/LMpar.h | 160 + .../Eigen/src/LevenbergMarquardt/LMqrsolv.h | 189 + .../LevenbergMarquardt/LevenbergMarquardt.h | 377 ++ .../src/MatrixFunctions/MatrixExponential.h | 47 +- .../src/MatrixFunctions/MatrixFunction.h | 19 +- .../src/MatrixFunctions/MatrixLogarithm.h | 79 +- .../Eigen/src/MatrixFunctions/MatrixPower.h | 509 ++ .../src/MatrixFunctions/MatrixSquareRoot.h | 58 +- .../HybridNonLinearSolver.h | 29 +- .../LevenbergMarquardt.h | 52 +- .../Eigen/src/NonLinearOptimization/chkder.h | 4 + .../Eigen/src/NonLinearOptimization/covar.h | 5 +- .../Eigen/src/NonLinearOptimization/dogleg.h | 13 +- .../Eigen/src/NonLinearOptimization/fdjac1.h | 5 +- .../Eigen/src/NonLinearOptimization/lmpar.h | 18 +- .../Eigen/src/NonLinearOptimization/r1updt.h | 8 +- .../Eigen/src/NonLinearOptimization/rwupdt.h | 2 +- .../Eigen/src/NumericalDiff/NumericalDiff.h | 10 +- .../Eigen/src/Polynomials/Companion.h | 1 + .../Eigen/src/Polynomials/PolynomialSolver.h | 25 +- .../Eigen/src/Polynomials/PolynomialUtils.h | 10 +- .../Eigen/unsupported/Eigen/src/SVD/BDCSVD.h | 748 +++ .../unsupported/Eigen/src/SVD/CMakeLists.txt | 6 + .../unsupported/Eigen/src/SVD/JacobiSVD.h | 782 +++ .../Eigen/unsupported/Eigen/src/SVD/SVDBase.h | 236 + .../unsupported/Eigen/src/SVD/TODOBdcsvd.txt | 29 + .../Eigen/src/SVD/doneInBDCSVD.txt | 21 + .../SparseExtra/BlockOfDynamicSparseMatrix.h | 8 + .../Eigen/src/SparseExtra/MarketIO.h | 6 +- .../src/SparseExtra/MatrixMarketIterator.h | 13 +- .../unsupported/Eigen/src/Splines/Spline.h | 22 +- .../Eigen/src/Splines/SplineFitting.h | 3 - .../Eigen/unsupported/bench/bench_svd.cpp | 123 + .../Eigen/unsupported/doc/Doxyfile.in | 1460 ----- .../Eigen/unsupported/doc/Overview.dox | 3 + .../unsupported/doc/eigendoxy_layout.xml.in | 177 + .../unsupported/doc/examples/MatrixPower.cpp | 16 + .../doc/examples/MatrixPower_optimal.cpp | 17 + .../doc/examples/PolynomialSolver1.cpp | 2 +- .../Eigen/unsupported/test/CMakeLists.txt | 10 +- .../test/NonLinearOptimization.cpp | 27 +- .../Eigen/unsupported/test/bdcsvd.cpp | 213 + .../Eigen/unsupported/test/dgmres.cpp | 31 + .../3rdparty/Eigen/unsupported/test/gmres.cpp | 6 +- .../Eigen/unsupported/test/jacobisvd.cpp | 198 + .../unsupported/test/kronecker_product.cpp | 76 +- .../unsupported/test/levenberg_marquardt.cpp | 1448 +++++ .../unsupported/test/matrix_exponential.cpp | 12 +- .../unsupported/test/matrix_function.cpp | 1 - .../Eigen/unsupported/test/matrix_functions.h | 47 + .../Eigen/unsupported/test/matrix_power.cpp | 133 + .../unsupported/test/matrix_square_root.cpp | 33 +- .../Eigen/unsupported/test/minres.cpp | 32 + .../Eigen/unsupported/test/mpreal/dlmalloc.c | 5703 ----------------- .../Eigen/unsupported/test/mpreal/dlmalloc.h | 562 -- .../Eigen/unsupported/test/mpreal/mpreal.cpp | 597 -- .../Eigen/unsupported/test/mpreal/mpreal.h | 3367 +++++----- .../Eigen/unsupported/test/mpreal_support.cpp | 9 +- .../unsupported/test/polynomialsolver.cpp | 14 +- .../Eigen/unsupported/test/sparse_extra.cpp | 1 - .../Eigen/unsupported/test/splines.cpp | 4 + .../Eigen/unsupported/test/svd_common.h | 261 + 563 files changed, 39535 insertions(+), 21315 deletions(-) create mode 100644 gtsam/3rdparty/Eigen/.hg_archival.txt create mode 100644 gtsam/3rdparty/Eigen/.hgeol create mode 100644 gtsam/3rdparty/Eigen/.hgignore create mode 100644 gtsam/3rdparty/Eigen/.hgtags create mode 100644 gtsam/3rdparty/Eigen/Eigen/MetisSupport create mode 100644 gtsam/3rdparty/Eigen/Eigen/SPQRSupport create mode 100644 gtsam/3rdparty/Eigen/Eigen/SparseLU create mode 100644 gtsam/3rdparty/Eigen/Eigen/SparseQR rename gtsam/3rdparty/Eigen/Eigen/src/{SparseCore => Core}/CoreIterators.h (100%) delete mode 100644 gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/MetisSupport/CMakeLists.txt create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/MetisSupport/MetisSupport.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Eigen_Colamd.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Ordering.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/CMakeLists.txt create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h delete mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseAssign.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/CMakeLists.txt create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLUImpl.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Structs.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Utils.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_dfs.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_gemm_kernel.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_dfs.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pivotL.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pruneL.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_relax_snode.h create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseQR/CMakeLists.txt create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h create mode 100644 gtsam/3rdparty/Eigen/bench/benchGeometry.cpp create mode 100644 gtsam/3rdparty/Eigen/bench/spbench/sp_solver.cpp create mode 100644 gtsam/3rdparty/Eigen/bench/spbench/spbench.dtd create mode 100644 gtsam/3rdparty/Eigen/bench/spbench/spbenchstyle.h create mode 100644 gtsam/3rdparty/Eigen/bench/spbench/test_sparseLU.cpp create mode 100644 gtsam/3rdparty/Eigen/blas/GeneralRank1Update.h create mode 100644 gtsam/3rdparty/Eigen/blas/PackedSelfadjointProduct.h create mode 100644 gtsam/3rdparty/Eigen/blas/PackedTriangularMatrixVector.h create mode 100644 gtsam/3rdparty/Eigen/blas/PackedTriangularSolverVector.h create mode 100644 gtsam/3rdparty/Eigen/blas/Rank2Update.h delete mode 100644 gtsam/3rdparty/Eigen/blas/chpr.f delete mode 100644 gtsam/3rdparty/Eigen/blas/chpr2.f delete mode 100644 gtsam/3rdparty/Eigen/blas/ctpmv.f delete mode 100644 gtsam/3rdparty/Eigen/blas/ctpsv.f delete mode 100644 gtsam/3rdparty/Eigen/blas/dspr.f delete mode 100644 gtsam/3rdparty/Eigen/blas/dspr2.f delete mode 100644 gtsam/3rdparty/Eigen/blas/dtpmv.f delete mode 100644 gtsam/3rdparty/Eigen/blas/dtpsv.f delete mode 100644 gtsam/3rdparty/Eigen/blas/sspr.f delete mode 100644 gtsam/3rdparty/Eigen/blas/sspr2.f delete mode 100644 gtsam/3rdparty/Eigen/blas/stpmv.f delete mode 100644 gtsam/3rdparty/Eigen/blas/stpsv.f delete mode 100644 gtsam/3rdparty/Eigen/blas/zhpr.f delete mode 100644 gtsam/3rdparty/Eigen/blas/zhpr2.f delete mode 100644 gtsam/3rdparty/Eigen/blas/ztpmv.f delete mode 100644 gtsam/3rdparty/Eigen/blas/ztpsv.f delete mode 100644 gtsam/3rdparty/Eigen/cmake/CMakeDetermineVSServicePack.cmake create mode 100644 gtsam/3rdparty/Eigen/cmake/EigenDetermineVSServicePack.cmake create mode 100644 gtsam/3rdparty/Eigen/cmake/FindSPQR.cmake create mode 100644 gtsam/3rdparty/Eigen/debug/msvc/eigen.natvis rename gtsam/3rdparty/Eigen/doc/{I12_ClassHierarchy.dox => ClassHierarchy.dox} (97%) rename gtsam/3rdparty/Eigen/doc/{I00_CustomizingEigen.dox => CustomizingEigen.dox} (96%) rename gtsam/3rdparty/Eigen/doc/{I05_FixedSizeVectorizable.dox => FixedSizeVectorizable.dox} (94%) rename gtsam/3rdparty/Eigen/doc/{I13_FunctionsTakingEigenTypes.dox => FunctionsTakingEigenTypes.dox} (74%) rename gtsam/3rdparty/Eigen/doc/{I02_HiPerformance.dox => HiPerformance.dox} (99%) delete mode 100644 gtsam/3rdparty/Eigen/doc/I10_Assertions.dox rename gtsam/3rdparty/Eigen/doc/{I03_InsideEigenExample.dox => InsideEigenExample.dox} (99%) create mode 100644 gtsam/3rdparty/Eigen/doc/LinearLeastSquares.dox create mode 100644 gtsam/3rdparty/Eigen/doc/Manual.dox rename gtsam/3rdparty/Eigen/doc/{D07_PassingByValue.dox => PassingByValue.dox} (92%) rename gtsam/3rdparty/Eigen/doc/{I14_PreprocessorDirectives.dox => PreprocessorDirectives.dox} (89%) rename gtsam/3rdparty/Eigen/doc/{C00_QuickStartGuide.dox => QuickStartGuide.dox} (99%) create mode 100644 gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox rename gtsam/3rdparty/Eigen/doc/{D01_StlContainers.dox => StlContainers.dox} (95%) rename gtsam/3rdparty/Eigen/doc/{I15_StorageOrders.dox => StorageOrders.dox} (95%) rename gtsam/3rdparty/Eigen/doc/{D09_StructHavingEigenMembers.dox => StructHavingEigenMembers.dox} (96%) rename gtsam/3rdparty/Eigen/doc/{I16_TemplateKeyword.dox => TemplateKeyword.dox} (96%) rename gtsam/3rdparty/Eigen/doc/{I11_Aliasing.dox => TopicAliasing.dox} (77%) create mode 100644 gtsam/3rdparty/Eigen/doc/TopicAssertions.dox rename gtsam/3rdparty/Eigen/doc/{I06_TopicEigenExpressionTemplates.dox => TopicEigenExpressionTemplates.dox} (100%) rename gtsam/3rdparty/Eigen/doc/{I01_TopicLazyEvaluation.dox => TopicLazyEvaluation.dox} (100%) rename gtsam/3rdparty/Eigen/doc/{I08_Resizing.dox => TopicResizing.dox} (100%) rename gtsam/3rdparty/Eigen/doc/{I07_TopicScalarTypes.dox => TopicScalarTypes.dox} (100%) rename gtsam/3rdparty/Eigen/doc/{I09_Vectorization.dox => TopicVectorization.dox} (100%) rename gtsam/3rdparty/Eigen/doc/{C05_TutorialAdvancedInitialization.dox => TutorialAdvancedInitialization.dox} (94%) rename gtsam/3rdparty/Eigen/doc/{C03_TutorialArrayClass.dox => TutorialArrayClass.dox} (91%) rename gtsam/3rdparty/Eigen/doc/{C04_TutorialBlockOperations.dox => TutorialBlockOperations.dox} (93%) rename gtsam/3rdparty/Eigen/doc/{C08_TutorialGeometry.dox => TutorialGeometry.dox} (93%) rename gtsam/3rdparty/Eigen/doc/{C06_TutorialLinearAlgebra.dox => TutorialLinearAlgebra.dox} (92%) rename gtsam/3rdparty/Eigen/doc/{C10_TutorialMapClass.dox => TutorialMapClass.dox} (75%) rename gtsam/3rdparty/Eigen/doc/{C02_TutorialMatrixArithmetic.dox => TutorialMatrixArithmetic.dox} (93%) rename gtsam/3rdparty/Eigen/doc/{C01_TutorialMatrixClass.dox => TutorialMatrixClass.dox} (89%) rename gtsam/3rdparty/Eigen/doc/{C07_TutorialReductionsVisitorsBroadcasting.dox => TutorialReductionsVisitorsBroadcasting.dox} (92%) rename gtsam/3rdparty/Eigen/doc/{C09_TutorialSparse.dox => TutorialSparse.dox} (71%) rename gtsam/3rdparty/Eigen/doc/{D11_UnalignedArrayAssert.dox => UnalignedArrayAssert.dox} (96%) rename gtsam/3rdparty/Eigen/doc/{D03_WrongStackAlignment.dox => WrongStackAlignment.dox} (96%) create mode 100644 gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js create mode 100644 gtsam/3rdparty/Eigen/doc/eigendoxy_layout.xml.in create mode 100644 gtsam/3rdparty/Eigen/doc/examples/function_taking_ref.cpp create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/Cwise_asin.cpp create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/GeneralizedEigenSolver.cpp create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/HouseholderQR_householderQ.cpp create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_block_int_int_int_int.cpp create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_bottomLeftCorner_int_int.cpp create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_bottomRightCorner_int_int.cpp create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_topLeftCorner_int_int.cpp create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_template_int_int_topRightCorner_int_int.cpp create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/RealQZ_compute.cpp create mode 100644 gtsam/3rdparty/Eigen/lapack/clacgv.f create mode 100644 gtsam/3rdparty/Eigen/lapack/cladiv.f create mode 100644 gtsam/3rdparty/Eigen/lapack/clarf.f create mode 100644 gtsam/3rdparty/Eigen/lapack/clarfb.f create mode 100644 gtsam/3rdparty/Eigen/lapack/clarfg.f create mode 100644 gtsam/3rdparty/Eigen/lapack/clarft.f create mode 100644 gtsam/3rdparty/Eigen/lapack/dladiv.f create mode 100644 gtsam/3rdparty/Eigen/lapack/dlamch.f create mode 100644 gtsam/3rdparty/Eigen/lapack/dlapy2.f create mode 100644 gtsam/3rdparty/Eigen/lapack/dlapy3.f create mode 100644 gtsam/3rdparty/Eigen/lapack/dlarf.f create mode 100644 gtsam/3rdparty/Eigen/lapack/dlarfb.f create mode 100644 gtsam/3rdparty/Eigen/lapack/dlarfg.f create mode 100644 gtsam/3rdparty/Eigen/lapack/dlarft.f create mode 100644 gtsam/3rdparty/Eigen/lapack/dsecnd_NONE.f create mode 100644 gtsam/3rdparty/Eigen/lapack/ilaclc.f create mode 100644 gtsam/3rdparty/Eigen/lapack/ilaclr.f create mode 100644 gtsam/3rdparty/Eigen/lapack/iladlc.f create mode 100644 gtsam/3rdparty/Eigen/lapack/iladlr.f create mode 100644 gtsam/3rdparty/Eigen/lapack/ilaslc.f create mode 100644 gtsam/3rdparty/Eigen/lapack/ilaslr.f create mode 100644 gtsam/3rdparty/Eigen/lapack/ilazlc.f create mode 100644 gtsam/3rdparty/Eigen/lapack/ilazlr.f create mode 100644 gtsam/3rdparty/Eigen/lapack/second_NONE.f create mode 100644 gtsam/3rdparty/Eigen/lapack/sladiv.f create mode 100644 gtsam/3rdparty/Eigen/lapack/slamch.f create mode 100644 gtsam/3rdparty/Eigen/lapack/slapy2.f create mode 100644 gtsam/3rdparty/Eigen/lapack/slapy3.f create mode 100644 gtsam/3rdparty/Eigen/lapack/slarf.f create mode 100644 gtsam/3rdparty/Eigen/lapack/slarfb.f create mode 100644 gtsam/3rdparty/Eigen/lapack/slarfg.f create mode 100644 gtsam/3rdparty/Eigen/lapack/slarft.f create mode 100644 gtsam/3rdparty/Eigen/lapack/zlacgv.f create mode 100644 gtsam/3rdparty/Eigen/lapack/zladiv.f create mode 100644 gtsam/3rdparty/Eigen/lapack/zlarf.f create mode 100644 gtsam/3rdparty/Eigen/lapack/zlarfb.f create mode 100644 gtsam/3rdparty/Eigen/lapack/zlarfg.f create mode 100644 gtsam/3rdparty/Eigen/lapack/zlarft.f create mode 100644 gtsam/3rdparty/Eigen/scripts/cdashtesting.cmake.in create mode 100644 gtsam/3rdparty/Eigen/test/denseLM.cpp create mode 100644 gtsam/3rdparty/Eigen/test/eigensolver_generalized_real.cpp rename gtsam/3rdparty/Eigen/test/{map.cpp => mapped_matrix.cpp} (98%) create mode 100644 gtsam/3rdparty/Eigen/test/metis_support.cpp create mode 100644 gtsam/3rdparty/Eigen/test/real_qz.cpp create mode 100644 gtsam/3rdparty/Eigen/test/ref.cpp create mode 100644 gtsam/3rdparty/Eigen/test/sparseLM.cpp create mode 100644 gtsam/3rdparty/Eigen/test/sparselu.cpp create mode 100644 gtsam/3rdparty/Eigen/test/sparseqr.cpp create mode 100644 gtsam/3rdparty/Eigen/test/special_numbers.cpp create mode 100644 gtsam/3rdparty/Eigen/test/spqr_support.cpp create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/ArpackSupport create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/LevenbergMarquardt create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/SVD create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/IncompleteCholesky.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CMakeLists.txt create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMcovar.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMpar.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/BDCSVD.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/CMakeLists.txt create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/JacobiSVD.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/SVDBase.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/TODOBdcsvd.txt create mode 100644 gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/doneInBDCSVD.txt create mode 100644 gtsam/3rdparty/Eigen/unsupported/bench/bench_svd.cpp delete mode 100644 gtsam/3rdparty/Eigen/unsupported/doc/Doxyfile.in create mode 100644 gtsam/3rdparty/Eigen/unsupported/doc/eigendoxy_layout.xml.in create mode 100644 gtsam/3rdparty/Eigen/unsupported/doc/examples/MatrixPower.cpp create mode 100644 gtsam/3rdparty/Eigen/unsupported/doc/examples/MatrixPower_optimal.cpp create mode 100644 gtsam/3rdparty/Eigen/unsupported/test/bdcsvd.cpp create mode 100644 gtsam/3rdparty/Eigen/unsupported/test/dgmres.cpp create mode 100644 gtsam/3rdparty/Eigen/unsupported/test/jacobisvd.cpp create mode 100644 gtsam/3rdparty/Eigen/unsupported/test/levenberg_marquardt.cpp create mode 100644 gtsam/3rdparty/Eigen/unsupported/test/matrix_functions.h create mode 100644 gtsam/3rdparty/Eigen/unsupported/test/matrix_power.cpp create mode 100644 gtsam/3rdparty/Eigen/unsupported/test/minres.cpp delete mode 100755 gtsam/3rdparty/Eigen/unsupported/test/mpreal/dlmalloc.c delete mode 100755 gtsam/3rdparty/Eigen/unsupported/test/mpreal/dlmalloc.h delete mode 100644 gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.cpp create mode 100644 gtsam/3rdparty/Eigen/unsupported/test/svd_common.h diff --git a/README b/README index d60faea45..a20a683a0 100644 --- a/README +++ b/README @@ -37,7 +37,7 @@ of licensing as follows: - CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree ordering library - http://www.cise.ufl.edu/research/sparse - Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt - - Eigen 3.1: General C++ matrix and linear algebra library + - Eigen 3.2: General C++ matrix and linear algebra library - Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README (some code that is 3rd-party to Eigen is BSD and LGPL) @@ -63,7 +63,7 @@ Tested compilers - MSVC 2010, 2012 Tested systems: - - Ubuntu 11.04, 11.10, 12.04 + - Ubuntu 11.04, 11.10, 12.04, 12.10, 13.04 - MacOS 10.6, 10.7 - Windows 7 diff --git a/gtsam/3rdparty/Eigen/.hg_archival.txt b/gtsam/3rdparty/Eigen/.hg_archival.txt new file mode 100644 index 000000000..9fceca658 --- /dev/null +++ b/gtsam/3rdparty/Eigen/.hg_archival.txt @@ -0,0 +1,4 @@ +repo: 8a21fd850624c931e448cbcfb38168cb2717c790 +node: ffa86ffb557094721ca71dcea6aed2651b9fd610 +branch: 3.2 +tag: 3.2.0 diff --git a/gtsam/3rdparty/Eigen/.hgeol b/gtsam/3rdparty/Eigen/.hgeol new file mode 100644 index 000000000..423676d31 --- /dev/null +++ b/gtsam/3rdparty/Eigen/.hgeol @@ -0,0 +1,8 @@ +[patterns] +scripts/*.in = LF +debug/msvc/*.dat = CRLF +unsupported/test/mpreal/*.* = CRLF +** = native + +[repository] +native = LF diff --git a/gtsam/3rdparty/Eigen/.hgignore b/gtsam/3rdparty/Eigen/.hgignore new file mode 100644 index 000000000..e33ba2e9d --- /dev/null +++ b/gtsam/3rdparty/Eigen/.hgignore @@ -0,0 +1,32 @@ +syntax: glob +qrc_*cxx +*.orig +*.pyc +*.diff +diff +*.save +save +*.old +*.gmo +*.qm +core +core.* +*.bak +*~ +build* +*.moc.* +*.moc +ui_* +CMakeCache.txt +tags +.*.swp +activity.png +*.out +*.php* +*.log +*.orig +*.rej +log +patch +a +a.* diff --git a/gtsam/3rdparty/Eigen/.hgtags b/gtsam/3rdparty/Eigen/.hgtags new file mode 100644 index 000000000..1b9b1142e --- /dev/null +++ b/gtsam/3rdparty/Eigen/.hgtags @@ -0,0 +1,25 @@ +2db9468678c6480c9633b6272ff0e3599d1e11a3 2.0-beta3 +375224817dce669b6fa31d920d4c895a63fabf32 2.0-beta1 +3b8120f077865e2a072e10f5be33e1d942b83a06 2.0-rc1 +19dfc0e7666bcee26f7a49eb42f39a0280a3485e 2.0-beta5 +7a7d8a9526f003ffa2430dfb0c2c535b5add3023 2.0-beta4 +7d14ad088ac23769c349518762704f0257f6a39b 2.0.1 +b9d48561579fd7d4c05b2aa42235dc9de6484bf2 2.0-beta6 +e17630a40408243cb1a51ad0fe3a99beb75b7450 before-hg-migration +eda654d4cda2210ce80719addcf854773e6dec5a 2.0.0 +ee9a7c468a9e73fab12f38f02bac24b07f29ed71 2.0-beta2 +d49097c25d8049e730c254a2fed725a240ce4858 after-hg-migration +655348878731bcb5d9bbe0854077b052e75e5237 actual-start-from-scratch +12a658962d4e6dfdc9a1c350fe7b69e36e70675c 3.0-beta1 +5c4180ad827b3f869b13b1d82f5a6ce617d6fcee 3.0-beta2 +7ae24ca6f3891d5ac58ddc7db60ad413c8d6ec35 3.0-beta3 +c40708b9088d622567fecc9208ad4a426621d364 3.0-beta4 +b6456624eae74f49ae8683d8e7b2882a2ca0342a 3.0-rc1 +a810d5dbab47acfe65b3350236efdd98f67d4d8a 3.1.0-alpha1 +304c88ca3affc16dd0b008b1104873986edd77af 3.1.0-alpha2 +920fc730b5930daae0a6dbe296d60ce2e3808215 3.1.0-beta1 +8383e883ebcc6f14695ff0b5e20bb631abab43fb 3.1.0-rc1 +bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2 +da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1 +4b687cad1d23066f66863f4f87298447298443df 3.2-rc1 +1eeda7b1258bcd306018c0738e2b6a8543661141 3.2-rc2 diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt index 3ba310a27..ad0269ea6 100644 --- a/gtsam/3rdparty/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/CMakeLists.txt @@ -1,6 +1,6 @@ project(Eigen) -cmake_minimum_required(VERSION 2.6.2) +cmake_minimum_required(VERSION 2.8.2) # guard against in-source builds @@ -105,26 +105,66 @@ if(EIGEN_DEFAULT_TO_ROW_MAJOR) add_definitions("-DEIGEN_DEFAULT_TO_ROW_MAJOR") endif() -add_definitions("-DEIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS") - set(EIGEN_TEST_MAX_SIZE "320" CACHE STRING "Maximal matrix/vector size, default is 320") -if(CMAKE_COMPILER_IS_GNUCXX) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wnon-virtual-dtor -Wno-long-long -ansi -Wundef -Wcast-align -Wchar-subscripts -Wall -W -Wpointer-arith -Wwrite-strings -Wformat-security -fexceptions -fno-check-new -fno-common -fstrict-aliasing") +macro(ei_add_cxx_compiler_flag FLAG) + string(REGEX REPLACE "-" "" SFLAG ${FLAG}) + check_cxx_compiler_flag(${FLAG} COMPILER_SUPPORT_${SFLAG}) + if(COMPILER_SUPPORT_${SFLAG}) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}") + endif() +endmacro(ei_add_cxx_compiler_flag) + +if(NOT MSVC) + # We assume that other compilers are partly compatible with GNUCC + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fexceptions") set(CMAKE_CXX_FLAGS_DEBUG "-g3") set(CMAKE_CXX_FLAGS_RELEASE "-g0 -O2") - - check_cxx_compiler_flag("-Wno-variadic-macros" COMPILER_SUPPORT_WNOVARIADICMACRO) - if(COMPILER_SUPPORT_WNOVARIADICMACRO) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-variadic-macros") + + # clang outputs some warnings for unknwon flags that are not caught by check_cxx_compiler_flag + # adding -Werror turns such warnings into errors + check_cxx_compiler_flag("-Werror" COMPILER_SUPPORT_WERROR) + if(COMPILER_SUPPORT_WERROR) + set(CMAKE_REQUIRED_FLAGS "-Werror") endif() - - check_cxx_compiler_flag("-Wextra" COMPILER_SUPPORT_WEXTRA) - if(COMPILER_SUPPORT_WEXTRA) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wextra") + + ei_add_cxx_compiler_flag("-pedantic") + ei_add_cxx_compiler_flag("-Wall") + ei_add_cxx_compiler_flag("-Wextra") + #ei_add_cxx_compiler_flag("-Weverything") # clang + + ei_add_cxx_compiler_flag("-Wundef") + ei_add_cxx_compiler_flag("-Wcast-align") + ei_add_cxx_compiler_flag("-Wchar-subscripts") + ei_add_cxx_compiler_flag("-Wnon-virtual-dtor") + ei_add_cxx_compiler_flag("-Wunused-local-typedefs") + ei_add_cxx_compiler_flag("-Wpointer-arith") + ei_add_cxx_compiler_flag("-Wwrite-strings") + ei_add_cxx_compiler_flag("-Wformat-security") + + ei_add_cxx_compiler_flag("-Wno-psabi") + ei_add_cxx_compiler_flag("-Wno-variadic-macros") + ei_add_cxx_compiler_flag("-Wno-long-long") + + ei_add_cxx_compiler_flag("-fno-check-new") + ei_add_cxx_compiler_flag("-fno-common") + ei_add_cxx_compiler_flag("-fstrict-aliasing") + ei_add_cxx_compiler_flag("-wd981") # disable ICC's "operands are evaluated in unspecified order" remark + ei_add_cxx_compiler_flag("-wd2304") # disbale ICC's "warning #2304: non-explicit constructor with single argument may cause implicit type conversion" produced by -Wnon-virtual-dtor + + # The -ansi flag must be added last, otherwise it is also used as a linker flag by check_cxx_compiler_flag making it fails + # Moreover we should not set both -strict-ansi and -ansi + check_cxx_compiler_flag("-strict-ansi" COMPILER_SUPPORT_STRICTANSI) + ei_add_cxx_compiler_flag("-Qunused-arguments") # disable clang warning: argument unused during compilation: '-ansi' + + if(COMPILER_SUPPORT_STRICTANSI) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -strict-ansi") + else() + ei_add_cxx_compiler_flag("-ansi") endif() - - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pedantic") + + set(CMAKE_REQUIRED_FLAGS "") option(EIGEN_TEST_SSE2 "Enable/Disable SSE2 in tests/examples" OFF) if(EIGEN_TEST_SSE2) @@ -164,7 +204,7 @@ if(CMAKE_COMPILER_IS_GNUCXX) option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF) if(EIGEN_TEST_NEON) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a8") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a"8) message(STATUS "Enabling NEON in tests/examples") endif() @@ -177,9 +217,8 @@ if(CMAKE_COMPILER_IS_GNUCXX) endif() endif() -endif(CMAKE_COMPILER_IS_GNUCXX) +else(NOT MSVC) -if(MSVC) # C4127 - conditional expression is constant # C4714 - marked as __forceinline not inlined (I failed to deactivate it selectively) # We can disable this warning in the unit tests since it is clear that it occurs @@ -209,7 +248,7 @@ if(MSVC) endif(NOT CMAKE_CL_64) message(STATUS "Enabling SSE2 in tests/examples") endif(EIGEN_TEST_SSE2) -endif(MSVC) +endif(NOT MSVC) option(EIGEN_TEST_NO_EXPLICIT_VECTORIZATION "Disable explicit vectorization in tests/examples" OFF) option(EIGEN_TEST_X87 "Force using X87 instructions. Implies no vectorization." OFF) @@ -308,6 +347,7 @@ add_subdirectory(Eigen) add_subdirectory(doc EXCLUDE_FROM_ALL) include(EigenConfigureTesting) + # fixme, not sure this line is still needed: enable_testing() # must be called from the root CMakeLists, see man page @@ -342,6 +382,8 @@ if(NOT WIN32) add_subdirectory(bench/spbench EXCLUDE_FROM_ALL) endif(NOT WIN32) +configure_file(scripts/cdashtesting.cmake.in cdashtesting.cmake @ONLY) + ei_testing_print_summary() message(STATUS "") diff --git a/gtsam/3rdparty/Eigen/COPYING.MINPACK b/gtsam/3rdparty/Eigen/COPYING.MINPACK index ae7984dae..11d8a9a6c 100644 --- a/gtsam/3rdparty/Eigen/COPYING.MINPACK +++ b/gtsam/3rdparty/Eigen/COPYING.MINPACK @@ -1,52 +1,52 @@ -Minpack Copyright Notice (1999) University of Chicago. All rights reserved - -Redistribution and use in source and binary forms, with or -without modification, are permitted provided that the -following conditions are met: - -1. Redistributions of source code must retain the above -copyright notice, this list of conditions and the following -disclaimer. - -2. Redistributions in binary form must reproduce the above -copyright notice, this list of conditions and the following -disclaimer in the documentation and/or other materials -provided with the distribution. - -3. The end-user documentation included with the -redistribution, if any, must include the following -acknowledgment: - - "This product includes software developed by the - University of Chicago, as Operator of Argonne National - Laboratory. - -Alternately, this acknowledgment may appear in the software -itself, if and wherever such third-party acknowledgments -normally appear. - -4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS" -WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE -UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND -THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES -OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE -OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY -OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR -USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF -THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4) -DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION -UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL -BE CORRECTED. - -5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT -HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF -ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT, -INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF -ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF -PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER -SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT -(INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE, -EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE -POSSIBILITY OF SUCH LOSS OR DAMAGES. - +Minpack Copyright Notice (1999) University of Chicago. All rights reserved + +Redistribution and use in source and binary forms, with or +without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above +copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above +copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials +provided with the distribution. + +3. The end-user documentation included with the +redistribution, if any, must include the following +acknowledgment: + + "This product includes software developed by the + University of Chicago, as Operator of Argonne National + Laboratory. + +Alternately, this acknowledgment may appear in the software +itself, if and wherever such third-party acknowledgments +normally appear. + +4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS" +WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE +UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND +THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES +OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE +OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY +OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR +USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF +THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4) +DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION +UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL +BE CORRECTED. + +5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT +HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF +ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT, +INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF +ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF +PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER +SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT +(INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE, +EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE +POSSIBILITY OF SUCH LOSS OR DAMAGES. + diff --git a/gtsam/3rdparty/Eigen/CTestConfig.cmake b/gtsam/3rdparty/Eigen/CTestConfig.cmake index a5a4eb012..4c0027824 100644 --- a/gtsam/3rdparty/Eigen/CTestConfig.cmake +++ b/gtsam/3rdparty/Eigen/CTestConfig.cmake @@ -11,3 +11,7 @@ set(CTEST_DROP_METHOD "http") set(CTEST_DROP_SITE "manao.inria.fr") set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen") set(CTEST_DROP_SITE_CDASH TRUE) +set(CTEST_PROJECT_SUBPROJECTS +Official +Unsupported +) diff --git a/gtsam/3rdparty/Eigen/CTestCustom.cmake.in b/gtsam/3rdparty/Eigen/CTestCustom.cmake.in index 3b2bacaa0..9fed9d327 100644 --- a/gtsam/3rdparty/Eigen/CTestCustom.cmake.in +++ b/gtsam/3rdparty/Eigen/CTestCustom.cmake.in @@ -1,4 +1,3 @@ -## A tribute to Dynamic! -set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_WARNINGS "33331") -set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_ERRORS "33331") +set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_WARNINGS "2000") +set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_ERRORS "2000") diff --git a/gtsam/3rdparty/Eigen/Eigen/Core b/gtsam/3rdparty/Eigen/Eigen/Core index d48017022..9131cc3fc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Core +++ b/gtsam/3rdparty/Eigen/Eigen/Core @@ -19,6 +19,12 @@ // defined e.g. EIGEN_DONT_ALIGN) so it needs to be done before we do anything with vectorization. #include "src/Core/util/Macros.h" +// Disable the ipa-cp-clone optimization flag with MinGW 6.x or newer (enabled by default with -O3) +// See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=556 for details. +#if defined(__MINGW32__) && EIGEN_GNUC_AT_LEAST(4,6) + #pragma GCC optimize ("-fno-ipa-cp-clone") +#endif + #include // this include file manages BLAS and MKL related macros @@ -44,7 +50,7 @@ #endif #else // Remember that usage of defined() in a #define is undefined by the standard - #if (defined __SSE2__) && ( (!defined __GNUC__) || EIGEN_GNUC_AT_LEAST(4,2) ) + #if (defined __SSE2__) && ( (!defined __GNUC__) || (defined __INTEL_COMPILER) || EIGEN_GNUC_AT_LEAST(4,2) ) #define EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC #endif #endif @@ -87,19 +93,25 @@ // so, to avoid compile errors when windows.h is included after Eigen/Core, ensure intrinsics are extern "C" here too. // notice that since these are C headers, the extern "C" is theoretically needed anyways. extern "C" { - #include - #include - #ifdef EIGEN_VECTORIZE_SSE3 - #include - #endif - #ifdef EIGEN_VECTORIZE_SSSE3 - #include - #endif - #ifdef EIGEN_VECTORIZE_SSE4_1 - #include - #endif - #ifdef EIGEN_VECTORIZE_SSE4_2 - #include + // 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 + #include + #else + #include + #include + #ifdef EIGEN_VECTORIZE_SSE3 + #include + #endif + #ifdef EIGEN_VECTORIZE_SSSE3 + #include + #endif + #ifdef EIGEN_VECTORIZE_SSE4_1 + #include + #endif + #ifdef EIGEN_VECTORIZE_SSE4_2 + #include + #endif #endif } // end extern "C" #elif defined __ALTIVEC__ @@ -236,15 +248,11 @@ using std::ptrdiff_t; * \endcode */ -/** \defgroup Support_modules Support modules [category] - * Category of modules which add support for external libraries. - */ - #include "src/Core/util/Constants.h" #include "src/Core/util/ForwardDeclarations.h" #include "src/Core/util/Meta.h" -#include "src/Core/util/XprHelper.h" #include "src/Core/util/StaticAssert.h" +#include "src/Core/util/XprHelper.h" #include "src/Core/util/Memory.h" #include "src/Core/NumTraits.h" @@ -297,6 +305,7 @@ using std::ptrdiff_t; #include "src/Core/Map.h" #include "src/Core/Block.h" #include "src/Core/VectorBlock.h" +#include "src/Core/Ref.h" #include "src/Core/Transpose.h" #include "src/Core/DiagonalMatrix.h" #include "src/Core/Diagonal.h" @@ -330,6 +339,7 @@ using std::ptrdiff_t; #include "src/Core/products/TriangularSolverMatrix.h" #include "src/Core/products/TriangularSolverVector.h" #include "src/Core/BandMatrix.h" +#include "src/Core/CoreIterators.h" #include "src/Core/BooleanRedux.h" #include "src/Core/Select.h" diff --git a/gtsam/3rdparty/Eigen/Eigen/Eigenvalues b/gtsam/3rdparty/Eigen/Eigen/Eigenvalues index af99ccd1f..53c5a73a2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Eigenvalues +++ b/gtsam/3rdparty/Eigen/Eigen/Eigenvalues @@ -33,6 +33,8 @@ #include "src/Eigenvalues/HessenbergDecomposition.h" #include "src/Eigenvalues/ComplexSchur.h" #include "src/Eigenvalues/ComplexEigenSolver.h" +#include "src/Eigenvalues/RealQZ.h" +#include "src/Eigenvalues/GeneralizedEigenSolver.h" #include "src/Eigenvalues/MatrixBaseEigenvalues.h" #ifdef EIGEN_USE_LAPACKE #include "src/Eigenvalues/RealSchur_MKL.h" diff --git a/gtsam/3rdparty/Eigen/Eigen/IterativeLinearSolvers b/gtsam/3rdparty/Eigen/Eigen/IterativeLinearSolvers index 315c2dd1e..0f4159dc1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/IterativeLinearSolvers +++ b/gtsam/3rdparty/Eigen/Eigen/IterativeLinearSolvers @@ -6,7 +6,7 @@ #include "src/Core/util/DisableStupidWarnings.h" -/** \ingroup Sparse_modules +/** * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module * * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse. diff --git a/gtsam/3rdparty/Eigen/Eigen/MetisSupport b/gtsam/3rdparty/Eigen/Eigen/MetisSupport new file mode 100644 index 000000000..6a113f7a8 --- /dev/null +++ b/gtsam/3rdparty/Eigen/Eigen/MetisSupport @@ -0,0 +1,28 @@ +#ifndef EIGEN_METISSUPPORT_MODULE_H +#define EIGEN_METISSUPPORT_MODULE_H + +#include "SparseCore" + +#include "src/Core/util/DisableStupidWarnings.h" + +extern "C" { +#include +} + + +/** \ingroup Support_modules + * \defgroup MetisSupport_Module MetisSupport module + * + * \code + * #include + * \endcode + * This module defines an interface to the METIS reordering package (http://glaros.dtc.umn.edu/gkhome/views/metis). + * It can be used just as any other built-in method as explained in \link OrderingMethods_Module here. \endlink + */ + + +#include "src/MetisSupport/MetisSupport.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif // EIGEN_METISSUPPORT_MODULE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/OrderingMethods b/gtsam/3rdparty/Eigen/Eigen/OrderingMethods index 1e2d87452..7c0f1ffff 100644 --- a/gtsam/3rdparty/Eigen/Eigen/OrderingMethods +++ b/gtsam/3rdparty/Eigen/Eigen/OrderingMethods @@ -5,19 +5,62 @@ #include "src/Core/util/DisableStupidWarnings.h" -/** \ingroup Sparse_modules +/** * \defgroup OrderingMethods_Module OrderingMethods module * - * This module is currently for internal use only. - * - * + * This module is currently for internal use only + * + * It defines various built-in and external ordering methods for sparse matrices. + * They are typically used to reduce the number of elements during + * the sparse matrix decomposition (LLT, LU, QR). + * Precisely, in a preprocessing step, a permutation matrix P is computed using + * those ordering methods and applied to the columns of the matrix. + * Using for instance the sparse Cholesky decomposition, it is expected that + * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A). + * + * + * Usage : * \code * #include * \endcode + * + * A simple usage is as a template parameter in the sparse decomposition classes : + * + * \code + * SparseLU > solver; + * \endcode + * + * \code + * SparseQR > solver; + * \endcode + * + * It is possible as well to call directly a particular ordering method for your own purpose, + * \code + * AMDOrdering ordering; + * PermutationMatrix perm; + * SparseMatrix A; + * //Fill the matrix ... + * + * ordering(A, perm); // Call AMD + * \endcode + * + * \note Some of these methods (like AMD or METIS), need the sparsity pattern + * of the input matrix to be symmetric. When the matrix is structurally unsymmetric, + * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method. + * If your matrix is already symmetric (at leat in structure), you can avoid that + * by calling the method with a SelfAdjointView type. + * + * \code + * // Call the ordering on the pattern of the lower triangular matrix A + * ordering(A.selfadjointView(), perm); + * \endcode */ +#ifndef EIGEN_MPL2_ONLY #include "src/OrderingMethods/Amd.h" +#endif +#include "src/OrderingMethods/Ordering.h" #include "src/Core/util/ReenableStupidWarnings.h" #endif // EIGEN_ORDERINGMETHODS_MODULE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/SPQRSupport b/gtsam/3rdparty/Eigen/Eigen/SPQRSupport new file mode 100644 index 000000000..77016442e --- /dev/null +++ b/gtsam/3rdparty/Eigen/Eigen/SPQRSupport @@ -0,0 +1,29 @@ +#ifndef EIGEN_SPQRSUPPORT_MODULE_H +#define EIGEN_SPQRSUPPORT_MODULE_H + +#include "SparseCore" + +#include "src/Core/util/DisableStupidWarnings.h" + +#include "SuiteSparseQR.hpp" + +/** \ingroup Support_modules + * \defgroup SPQRSupport_Module SuiteSparseQR module + * + * This module provides an interface to the SPQR library, which is part of the suitesparse package. + * + * \code + * #include + * \endcode + * + * In order to use this module, the SPQR headers must be accessible from the include paths, and your binary must be linked to the SPQR library and its dependencies (Cholmod, AMD, COLAMD,...). + * For a cmake based project, you can use our FindSPQR.cmake and FindCholmod.Cmake modules + * + */ + +#include "src/misc/Solve.h" +#include "src/misc/SparseSolve.h" +#include "src/CholmodSupport/CholmodSupport.h" +#include "src/SPQRSupport/SuiteSparseQRSupport.h" + +#endif diff --git a/gtsam/3rdparty/Eigen/Eigen/Sparse b/gtsam/3rdparty/Eigen/Eigen/Sparse index 2d1757172..7cc9c0913 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Sparse +++ b/gtsam/3rdparty/Eigen/Eigen/Sparse @@ -1,13 +1,15 @@ #ifndef EIGEN_SPARSE_MODULE_H #define EIGEN_SPARSE_MODULE_H -/** \defgroup Sparse_modules Sparse modules +/** \defgroup Sparse_Module Sparse meta-module * * Meta-module including all related modules: - * - SparseCore - * - OrderingMethods - * - SparseCholesky - * - IterativeLinearSolvers + * - \ref SparseCore_Module + * - \ref OrderingMethods_Module + * - \ref SparseCholesky_Module + * - \ref SparseLU_Module + * - \ref SparseQR_Module + * - \ref IterativeLinearSolvers_Module * * \code * #include @@ -17,6 +19,8 @@ #include "SparseCore" #include "OrderingMethods" #include "SparseCholesky" +#include "SparseLU" +#include "SparseQR" #include "IterativeLinearSolvers" #endif // EIGEN_SPARSE_MODULE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/SparseCholesky b/gtsam/3rdparty/Eigen/Eigen/SparseCholesky index 5f82742f7..9f5056aa1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/SparseCholesky +++ b/gtsam/3rdparty/Eigen/Eigen/SparseCholesky @@ -1,11 +1,21 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2013 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 +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + #ifndef EIGEN_SPARSECHOLESKY_MODULE_H #define EIGEN_SPARSECHOLESKY_MODULE_H #include "SparseCore" +#include "OrderingMethods" #include "src/Core/util/DisableStupidWarnings.h" -/** \ingroup Sparse_modules +/** * \defgroup SparseCholesky_Module SparseCholesky module * * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices. @@ -20,11 +30,18 @@ * \endcode */ +#ifdef EIGEN_MPL2_ONLY +#error The SparseCholesky module has nothing to offer in MPL2 only mode +#endif + #include "src/misc/Solve.h" #include "src/misc/SparseSolve.h" - #include "src/SparseCholesky/SimplicialCholesky.h" +#ifndef EIGEN_MPL2_ONLY +#include "src/SparseCholesky/SimplicialCholesky_impl.h" +#endif + #include "src/Core/util/ReenableStupidWarnings.h" #endif // EIGEN_SPARSECHOLESKY_MODULE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/SparseCore b/gtsam/3rdparty/Eigen/Eigen/SparseCore index 41d28c928..9b5be5e15 100644 --- a/gtsam/3rdparty/Eigen/Eigen/SparseCore +++ b/gtsam/3rdparty/Eigen/Eigen/SparseCore @@ -11,7 +11,7 @@ #include #include -/** \ingroup Sparse_modules +/** * \defgroup SparseCore_Module SparseCore module * * This module provides a sparse matrix representation, and basic associatd matrix manipulations @@ -40,14 +40,12 @@ struct Sparse {}; #include "src/SparseCore/SparseMatrix.h" #include "src/SparseCore/MappedSparseMatrix.h" #include "src/SparseCore/SparseVector.h" -#include "src/SparseCore/CoreIterators.h" #include "src/SparseCore/SparseBlock.h" #include "src/SparseCore/SparseTranspose.h" #include "src/SparseCore/SparseCwiseUnaryOp.h" #include "src/SparseCore/SparseCwiseBinaryOp.h" #include "src/SparseCore/SparseDot.h" #include "src/SparseCore/SparsePermutation.h" -#include "src/SparseCore/SparseAssign.h" #include "src/SparseCore/SparseRedux.h" #include "src/SparseCore/SparseFuzzy.h" #include "src/SparseCore/ConservativeSparseSparseProduct.h" diff --git a/gtsam/3rdparty/Eigen/Eigen/SparseLU b/gtsam/3rdparty/Eigen/Eigen/SparseLU new file mode 100644 index 000000000..8527a49bd --- /dev/null +++ b/gtsam/3rdparty/Eigen/Eigen/SparseLU @@ -0,0 +1,49 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 Désiré Nuentsa-Wakam +// Copyright (C) 2012 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 +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_SPARSELU_MODULE_H +#define EIGEN_SPARSELU_MODULE_H + +#include "SparseCore" + +/** + * \defgroup SparseLU_Module SparseLU module + * This module defines a supernodal factorization of general sparse matrices. + * The code is fully optimized for supernode-panel updates with specialized kernels. + * Please, see the documentation of the SparseLU class for more details. + */ + +#include "src/misc/Solve.h" +#include "src/misc/SparseSolve.h" + +// Ordering interface +#include "OrderingMethods" + +#include "src/SparseLU/SparseLU_gemm_kernel.h" + +#include "src/SparseLU/SparseLU_Structs.h" +#include "src/SparseLU/SparseLU_SupernodalMatrix.h" +#include "src/SparseLU/SparseLUImpl.h" +#include "src/SparseCore/SparseColEtree.h" +#include "src/SparseLU/SparseLU_Memory.h" +#include "src/SparseLU/SparseLU_heap_relax_snode.h" +#include "src/SparseLU/SparseLU_relax_snode.h" +#include "src/SparseLU/SparseLU_pivotL.h" +#include "src/SparseLU/SparseLU_panel_dfs.h" +#include "src/SparseLU/SparseLU_kernel_bmod.h" +#include "src/SparseLU/SparseLU_panel_bmod.h" +#include "src/SparseLU/SparseLU_column_dfs.h" +#include "src/SparseLU/SparseLU_column_bmod.h" +#include "src/SparseLU/SparseLU_copy_to_ucol.h" +#include "src/SparseLU/SparseLU_pruneL.h" +#include "src/SparseLU/SparseLU_Utils.h" +#include "src/SparseLU/SparseLU.h" + +#endif // EIGEN_SPARSELU_MODULE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/SparseQR b/gtsam/3rdparty/Eigen/Eigen/SparseQR new file mode 100644 index 000000000..4ee42065e --- /dev/null +++ b/gtsam/3rdparty/Eigen/Eigen/SparseQR @@ -0,0 +1,33 @@ +#ifndef EIGEN_SPARSEQR_MODULE_H +#define EIGEN_SPARSEQR_MODULE_H + +#include "SparseCore" +#include "OrderingMethods" +#include "src/Core/util/DisableStupidWarnings.h" + +/** \defgroup SparseQR_Module SparseQR module + * \brief Provides QR decomposition for sparse matrices + * + * This module provides a simplicial version of the left-looking Sparse QR decomposition. + * The columns of the input matrix should be reordered to limit the fill-in during the + * decomposition. Built-in methods (COLAMD, AMD) or external methods (METIS) can be used to this end. + * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list + * of built-in and external ordering methods. + * + * \code + * #include + * \endcode + * + * + */ + +#include "src/misc/Solve.h" +#include "src/misc/SparseSolve.h" + +#include "OrderingMethods" +#include "src/SparseCore/SparseColEtree.h" +#include "src/SparseQR/SparseQR.h" + +#include "src/Core/util/ReenableStupidWarnings.h" + +#endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index 68e54b1d4..d19cb3968 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -196,7 +196,7 @@ template class LDLT LDLT& compute(const MatrixType& matrix); template - LDLT& rankUpdate(const MatrixBase& w,RealScalar alpha=1); + LDLT& rankUpdate(const MatrixBase& w, const RealScalar& alpha=1); /** \returns the internal LDLT decomposition matrix * @@ -248,6 +248,7 @@ template<> struct ldlt_inplace template static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0) { + using std::abs; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; typedef typename MatrixType::Index Index; @@ -258,7 +259,7 @@ template<> struct ldlt_inplace { transpositions.setIdentity(); if(sign) - *sign = real(mat.coeff(0,0))>0 ? 1:-1; + *sign = numext::real(mat.coeff(0,0))>0 ? 1:-1; return true; } @@ -277,15 +278,13 @@ template<> struct ldlt_inplace // are compared; if any diagonal is negligible compared // to the largest overall, the algorithm bails. cutoff = abs(NumTraits::epsilon() * biggest_in_corner); - - if(sign) - *sign = real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0 ? 1 : -1; } // 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; + if(sign) *sign = 0; break; } @@ -301,11 +300,11 @@ template<> struct ldlt_inplace for(int i=k+1;i::IsComplex) - mat.coeffRef(index_of_biggest_in_corner,k) = conj(mat.coeff(index_of_biggest_in_corner,k)); + mat.coeffRef(index_of_biggest_in_corner,k) = numext::conj(mat.coeff(index_of_biggest_in_corner,k)); } // partition the matrix: @@ -326,6 +325,16 @@ template<> struct ldlt_inplace } if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff)) A21 /= mat.coeffRef(k,k); + + if(sign) + { + // LDLT is not guaranteed to work for indefinite matrices, but let's try to get the sign right + int newSign = numext::real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0; + if(k == 0) + *sign = newSign; + else if(*sign != newSign) + *sign = 0; + } } return true; @@ -339,9 +348,9 @@ template<> struct ldlt_inplace // Here only rank-1 updates are implemented, to reduce the // requirement for intermediate storage and improve accuracy template - static bool updateInPlace(MatrixType& mat, MatrixBase& w, typename MatrixType::RealScalar sigma=1) + static bool updateInPlace(MatrixType& mat, MatrixBase& w, const typename MatrixType::RealScalar& sigma=1) { - using internal::isfinite; + using numext::isfinite; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; typedef typename MatrixType::Index Index; @@ -359,9 +368,9 @@ template<> struct ldlt_inplace break; // Update the diagonal terms - RealScalar dj = real(mat.coeff(j,j)); + RealScalar dj = numext::real(mat.coeff(j,j)); Scalar wj = w.coeff(j); - RealScalar swj2 = sigma*abs2(wj); + RealScalar swj2 = sigma*numext::abs2(wj); RealScalar gamma = dj*alpha + swj2; mat.coeffRef(j,j) += swj2/alpha; @@ -372,13 +381,13 @@ template<> struct ldlt_inplace Index rs = size-j-1; w.tail(rs) -= wj * mat.col(j).tail(rs); if(gamma != 0) - mat.col(j).tail(rs) += (sigma*conj(wj)/gamma)*w.tail(rs); + mat.col(j).tail(rs) += (sigma*numext::conj(wj)/gamma)*w.tail(rs); } return true; } template - static bool update(MatrixType& mat, const TranspositionType& transpositions, Workspace& tmp, const WType& w, typename MatrixType::RealScalar sigma=1) + static bool update(MatrixType& mat, const TranspositionType& transpositions, Workspace& tmp, const WType& w, const typename MatrixType::RealScalar& sigma=1) { // Apply the permutation to the input w tmp = transpositions * w; @@ -397,7 +406,7 @@ template<> struct ldlt_inplace } template - static EIGEN_STRONG_INLINE bool update(MatrixType& mat, TranspositionType& transpositions, Workspace& tmp, WType& w, typename MatrixType::RealScalar sigma=1) + static EIGEN_STRONG_INLINE bool update(MatrixType& mat, TranspositionType& transpositions, Workspace& tmp, WType& w, const typename MatrixType::RealScalar& sigma=1) { Transpose matt(mat); return ldlt_inplace::update(matt, transpositions, tmp, w.conjugate(), sigma); @@ -449,7 +458,7 @@ LDLT& LDLT::compute(const MatrixType& a) */ template template -LDLT& LDLT::rankUpdate(const MatrixBase& w,typename NumTraits::Real sigma) +LDLT& LDLT::rankUpdate(const MatrixBase& w, const typename NumTraits::Real& sigma) { const Index size = w.rows(); if (m_isInitialized) @@ -534,8 +543,7 @@ template bool LDLT::solveInPlace(MatrixBase &bAndX) const { eigen_assert(m_isInitialized && "LDLT is not initialized."); - const Index size = m_matrix.rows(); - eigen_assert(size == bAndX.rows()); + eigen_assert(m_matrix.rows() == bAndX.rows()); bAndX = this->solve(bAndX); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h index 41d14e532..2e6189f7d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h @@ -190,6 +190,7 @@ template struct llt_inplace; template static typename MatrixType::Index llt_rank_update_lower(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma) { + using std::sqrt; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; typedef typename MatrixType::Index Index; @@ -199,7 +200,7 @@ static typename MatrixType::Index llt_rank_update_lower(MatrixType& mat, const V typedef Matrix TempVectorType; typedef typename TempVectorType::SegmentReturnType TempVecSegment; - int n = mat.cols(); + Index n = mat.cols(); eigen_assert(mat.rows()==n && vec.size()==n); TempVectorType temp; @@ -211,12 +212,12 @@ static typename MatrixType::Index llt_rank_update_lower(MatrixType& mat, const V // i.e., for sigma > 0 temp = sqrt(sigma) * vec; - for(int i=0; i g; g.makeGivens(mat(i,i), -temp(i), &mat(i,i)); - int rs = n-i-1; + Index rs = n-i-1; if(rs>0) { ColXprSegment x(mat.col(i).tail(rs)); @@ -229,12 +230,12 @@ static typename MatrixType::Index llt_rank_update_lower(MatrixType& mat, const V { temp = vec; RealScalar beta = 1; - for(int j=0; j struct llt_inplace template static typename MatrixType::Index unblocked(MatrixType& mat) { + using std::sqrt; typedef typename MatrixType::Index Index; eigen_assert(mat.rows()==mat.cols()); @@ -275,7 +277,7 @@ template struct llt_inplace Block A10(mat,k,0,1,k); Block A20(mat,k+1,0,rs,k); - RealScalar x = real(mat.coeff(k,k)); + RealScalar x = numext::real(mat.coeff(k,k)); if (k>0) x -= A10.squaredNorm(); if (x<=RealScalar(0)) return k; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h index 37f142150..783324b0b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h @@ -51,7 +51,6 @@ void cholmod_configure_matrix(CholmodType& mat) template cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat) { - typedef SparseMatrix<_Scalar,_Options,_Index> MatrixType; cholmod_sparse res; res.nzmax = mat.nonZeros(); res.nrow = mat.rows();; @@ -77,9 +76,13 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat) { res.itype = CHOLMOD_INT; } + else if (internal::is_same<_Index,UF_long>::value) + { + res.itype = CHOLMOD_LONG; + } else { - eigen_assert(false && "Index type different than int is not supported yet"); + eigen_assert(false && "Index type not supported yet"); } // setup res.xtype @@ -123,7 +126,7 @@ cholmod_dense viewAsCholmod(MatrixBase& mat) res.ncol = mat.cols(); res.nzmax = res.nrow * res.ncol; res.d = Derived::IsVectorAtCompileTime ? mat.derived().size() : mat.derived().outerStride(); - res.x = mat.derived().data(); + res.x = (void*)(mat.derived().data()); res.z = 0; internal::cholmod_configure_matrix(res); @@ -137,8 +140,8 @@ template MappedSparseMatrix viewAsEigen(cholmod_sparse& cm) { return MappedSparseMatrix - (cm.nrow, cm.ncol, reinterpret_cast(cm.p)[cm.ncol], - reinterpret_cast(cm.p), reinterpret_cast(cm.i),reinterpret_cast(cm.x) ); + (cm.nrow, cm.ncol, static_cast(cm.p)[cm.ncol], + static_cast(cm.p), static_cast(cm.i),static_cast(cm.x) ); } enum CholmodMode { @@ -173,6 +176,7 @@ class CholmodBase : internal::noncopyable CholmodBase(const MatrixType& matrix) : m_cholmodFactor(0), m_info(Success), m_isInitialized(false) { + m_shiftOffset[0] = m_shiftOffset[1] = RealScalar(0.0); cholmod_start(&m_cholmod); compute(matrix); } @@ -269,9 +273,10 @@ class CholmodBase : internal::noncopyable { eigen_assert(m_analysisIsOk && "You must first call analyzePattern()"); cholmod_sparse A = viewAsCholmod(matrix.template selfadjointView()); - cholmod_factorize(&A, m_cholmodFactor, &m_cholmod); + cholmod_factorize_p(&A, m_shiftOffset, 0, 0, m_cholmodFactor, &m_cholmod); - this->m_info = Success; + // If the factorization failed, minor is the column at which it did. On success minor == n. + this->m_info = (m_cholmodFactor->minor == m_cholmodFactor->n ? Success : NumericalIssue); m_factorizationIsOk = true; } @@ -286,10 +291,12 @@ class CholmodBase : internal::noncopyable { eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()"); const Index size = m_cholmodFactor->n; + EIGEN_UNUSED_VARIABLE(size); eigen_assert(size==b.rows()); // note: cd stands for Cholmod Dense - cholmod_dense b_cd = viewAsCholmod(b.const_cast_derived()); + Rhs& b_ref(b.const_cast_derived()); + cholmod_dense b_cd = viewAsCholmod(b_ref); cholmod_dense* x_cd = cholmod_solve(CHOLMOD_A, m_cholmodFactor, &b_cd, &m_cholmod); if(!x_cd) { @@ -306,6 +313,7 @@ class CholmodBase : internal::noncopyable { eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()"); const Index size = m_cholmodFactor->n; + EIGEN_UNUSED_VARIABLE(size); eigen_assert(size==b.rows()); // note: cs stands for Cholmod Sparse @@ -321,13 +329,30 @@ class CholmodBase : internal::noncopyable } #endif // EIGEN_PARSED_BY_DOXYGEN + + /** Sets the shift parameter that will be used to adjust the diagonal coefficients during the numerical factorization. + * + * During the numerical factorization, an offset term is added to the diagonal coefficients:\n + * \c d_ii = \a offset + \c d_ii + * + * The default is \a offset=0. + * + * \returns a reference to \c *this. + */ + Derived& setShift(const RealScalar& offset) + { + m_shiftOffset[0] = offset; + return derived(); + } + template - void dumpMemory(Stream& s) + void dumpMemory(Stream& /*s*/) {} protected: mutable cholmod_common m_cholmod; cholmod_factor* m_cholmodFactor; + RealScalar m_shiftOffset[2]; mutable ComputationInfo m_info; bool m_isInitialized; int m_factorizationIsOk; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h index aaa389978..497efff66 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h @@ -107,10 +107,10 @@ class Array * * \sa resize(Index,Index) */ - EIGEN_STRONG_INLINE explicit Array() : Base() + EIGEN_STRONG_INLINE Array() : Base() { Base::_check_template_params(); - EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED + EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } #ifndef EIGEN_PARSED_BY_DOXYGEN @@ -120,7 +120,7 @@ class Array : Base(internal::constructor_without_unaligned_array_assert()) { Base::_check_template_params(); - EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED + EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } #endif @@ -137,15 +137,15 @@ class Array EIGEN_STATIC_ASSERT_VECTOR_ONLY(Array) eigen_assert(dim >= 0); eigen_assert(SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim); - EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED + EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } #ifndef EIGEN_PARSED_BY_DOXYGEN template - EIGEN_STRONG_INLINE Array(const T0& x, const T1& y) + EIGEN_STRONG_INLINE Array(const T0& val0, const T1& val1) { Base::_check_template_params(); - this->template _init2(x, y); + this->template _init2(val0, val1); } #else /** constructs an uninitialized matrix with \a rows rows and \a cols columns. @@ -155,27 +155,27 @@ class Array * Matrix() instead. */ Array(Index rows, Index cols); /** constructs an initialized 2D vector with given coefficients */ - Array(const Scalar& x, const Scalar& y); + Array(const Scalar& val0, const Scalar& val1); #endif /** constructs an initialized 3D vector with given coefficients */ - EIGEN_STRONG_INLINE Array(const Scalar& x, const Scalar& y, const Scalar& z) + EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2) { Base::_check_template_params(); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Array, 3) - m_storage.data()[0] = x; - m_storage.data()[1] = y; - m_storage.data()[2] = z; + m_storage.data()[0] = val0; + m_storage.data()[1] = val1; + m_storage.data()[2] = val2; } /** constructs an initialized 4D vector with given coefficients */ - EIGEN_STRONG_INLINE Array(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w) + EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2, const Scalar& val3) { Base::_check_template_params(); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Array, 4) - m_storage.data()[0] = x; - m_storage.data()[1] = y; - m_storage.data()[2] = z; - m_storage.data()[3] = w; + m_storage.data()[0] = val0; + m_storage.data()[1] = val1; + m_storage.data()[2] = val2; + m_storage.data()[3] = val3; } explicit Array(const Scalar *data); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h index 004b117c9..38852600d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h @@ -143,7 +143,7 @@ template class ArrayBase ArrayBase& array() { return *this; } const ArrayBase& array() const { return *this; } - /** \returns an \link MatrixBase Matrix \endlink expression of this array + /** \returns an \link Eigen::MatrixBase Matrix \endlink expression of this array * \sa MatrixBase::array() */ MatrixWrapper matrix() { return derived(); } const MatrixWrapper matrix() const { return derived(); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h index 65ffd64ca..a791bc358 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h @@ -55,22 +55,22 @@ class ArrayWrapper : public ArrayBase > inline Index outerStride() const { return m_expression.outerStride(); } inline Index innerStride() const { return m_expression.innerStride(); } - inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); } + inline ScalarWithConstIfNotLvalue* data() { return m_expression.const_cast_derived().data(); } inline const Scalar* data() const { return m_expression.data(); } - inline CoeffReturnType coeff(Index row, Index col) const + inline CoeffReturnType coeff(Index rowId, Index colId) const { - return m_expression.coeff(row, col); + return m_expression.coeff(rowId, colId); } - inline Scalar& coeffRef(Index row, Index col) + inline Scalar& coeffRef(Index rowId, Index colId) { - return m_expression.const_cast_derived().coeffRef(row, col); + return m_expression.const_cast_derived().coeffRef(rowId, colId); } - inline const Scalar& coeffRef(Index row, Index col) const + inline const Scalar& coeffRef(Index rowId, Index colId) const { - return m_expression.const_cast_derived().coeffRef(row, col); + return m_expression.const_cast_derived().coeffRef(rowId, colId); } inline CoeffReturnType coeff(Index index) const @@ -89,15 +89,15 @@ class ArrayWrapper : public ArrayBase > } template - inline const PacketScalar packet(Index row, Index col) const + inline const PacketScalar packet(Index rowId, Index colId) const { - return m_expression.template packet(row, col); + return m_expression.template packet(rowId, colId); } template - inline void writePacket(Index row, Index col, const PacketScalar& x) + inline void writePacket(Index rowId, Index colId, const PacketScalar& val) { - m_expression.const_cast_derived().template writePacket(row, col, x); + m_expression.const_cast_derived().template writePacket(rowId, colId, val); } template @@ -107,9 +107,9 @@ class ArrayWrapper : public ArrayBase > } template - inline void writePacket(Index index, const PacketScalar& x) + inline void writePacket(Index index, const PacketScalar& val) { - m_expression.const_cast_derived().template writePacket(index, x); + m_expression.const_cast_derived().template writePacket(index, val); } template @@ -168,29 +168,29 @@ class MatrixWrapper : public MatrixBase > typedef typename internal::nested::type NestedExpressionType; - inline MatrixWrapper(ExpressionType& matrix) : m_expression(matrix) {} + inline MatrixWrapper(ExpressionType& a_matrix) : m_expression(a_matrix) {} inline Index rows() const { return m_expression.rows(); } inline Index cols() const { return m_expression.cols(); } inline Index outerStride() const { return m_expression.outerStride(); } inline Index innerStride() const { return m_expression.innerStride(); } - inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); } + inline ScalarWithConstIfNotLvalue* data() { return m_expression.const_cast_derived().data(); } inline const Scalar* data() const { return m_expression.data(); } - inline CoeffReturnType coeff(Index row, Index col) const + inline CoeffReturnType coeff(Index rowId, Index colId) const { - return m_expression.coeff(row, col); + return m_expression.coeff(rowId, colId); } - inline Scalar& coeffRef(Index row, Index col) + inline Scalar& coeffRef(Index rowId, Index colId) { - return m_expression.const_cast_derived().coeffRef(row, col); + return m_expression.const_cast_derived().coeffRef(rowId, colId); } - inline const Scalar& coeffRef(Index row, Index col) const + inline const Scalar& coeffRef(Index rowId, Index colId) const { - return m_expression.derived().coeffRef(row, col); + return m_expression.derived().coeffRef(rowId, colId); } inline CoeffReturnType coeff(Index index) const @@ -209,15 +209,15 @@ class MatrixWrapper : public MatrixBase > } template - inline const PacketScalar packet(Index row, Index col) const + inline const PacketScalar packet(Index rowId, Index colId) const { - return m_expression.template packet(row, col); + return m_expression.template packet(rowId, colId); } template - inline void writePacket(Index row, Index col, const PacketScalar& x) + inline void writePacket(Index rowId, Index colId, const PacketScalar& val) { - m_expression.const_cast_derived().template writePacket(row, col, x); + m_expression.const_cast_derived().template writePacket(rowId, colId, val); } template @@ -227,9 +227,9 @@ class MatrixWrapper : public MatrixBase > } template - inline void writePacket(Index index, const PacketScalar& x) + inline void writePacket(Index index, const PacketScalar& val) { - m_expression.const_cast_derived().template writePacket(index, x); + m_expression.const_cast_derived().template writePacket(index, val); } const typename internal::remove_all::type& diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h index cd29a88f0..1dccc2f42 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h @@ -155,7 +155,7 @@ struct assign_DefaultTraversal_CompleteUnrolling template struct assign_DefaultTraversal_InnerUnrolling { - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src, int outer) + static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src, typename Derived1::Index outer) { dst.copyCoeffByOuterInner(outer, Index, src); assign_DefaultTraversal_InnerUnrolling::run(dst, src, outer); @@ -165,7 +165,7 @@ struct assign_DefaultTraversal_InnerUnrolling template struct assign_DefaultTraversal_InnerUnrolling { - static EIGEN_STRONG_INLINE void run(Derived1 &, const Derived2 &, int) {} + static EIGEN_STRONG_INLINE void run(Derived1 &, const Derived2 &, typename Derived1::Index) {} }; /*********************** @@ -218,7 +218,7 @@ struct assign_innervec_CompleteUnrolling template struct assign_innervec_InnerUnrolling { - static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src, int outer) + static EIGEN_STRONG_INLINE void run(Derived1 &dst, const Derived2 &src, typename Derived1::Index outer) { dst.template copyPacketByOuterInner(outer, Index, src); assign_innervec_InnerUnrolling struct assign_innervec_InnerUnrolling { - static EIGEN_STRONG_INLINE void run(Derived1 &, const Derived2 &, int) {} + static EIGEN_STRONG_INLINE void run(Derived1 &, const Derived2 &, typename Derived1::Index) {} }; /*************************************************************************** @@ -507,19 +507,19 @@ EIGEN_STRONG_INLINE Derived& DenseBase namespace internal { template + bool EvalBeforeAssigning = (int(internal::traits::Flags) & EvalBeforeAssigningBit) != 0, + bool NeedToTranspose = ((int(Derived::RowsAtCompileTime) == 1 && int(OtherDerived::ColsAtCompileTime) == 1) + | // FIXME | instead of || to please GCC 4.4.0 stupid warning "suggest parentheses around &&". + // revert to || as soon as not needed anymore. + (int(Derived::ColsAtCompileTime) == 1 && int(OtherDerived::RowsAtCompileTime) == 1)) + && int(Derived::SizeAtCompileTime) != 1> struct assign_selector; template struct assign_selector { static EIGEN_STRONG_INLINE Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.derived()); } + template + static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst, const ActualOtherDerived& other) { other.evalTo(dst); return dst; } }; template struct assign_selector { @@ -528,6 +528,8 @@ struct assign_selector { template struct assign_selector { static EIGEN_STRONG_INLINE Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.transpose()); } + template + static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst, const ActualOtherDerived& other) { Transpose dstTrans(dst); other.evalTo(dstTrans); return dst; } }; template struct assign_selector { @@ -566,16 +568,14 @@ template template EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const EigenBase& other) { - other.derived().evalTo(derived()); - return derived(); + return internal::assign_selector::evalTo(derived(), other.derived()); } template template EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const ReturnByValue& other) { - other.evalTo(derived()); - return derived(); + return internal::assign_selector::evalTo(derived(), other.derived()); } } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign_MKL.h index 428c6367b..7772951b9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign_MKL.h @@ -210,7 +210,7 @@ EIGEN_MKL_VML_DECLARE_UNARY_CALLS_LA(sqrt, Sqrt) EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(square, Sqr) // The vm*powx functions are not avaibale in the windows version of MKL. -#ifdef _WIN32 +#ifndef _WIN32 EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmspowx_, float, float) EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmdpowx_, double, double) EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmcpowx_, scomplex, MKL_Complex8) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h index 5f29cb3d1..358b3188b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h @@ -21,7 +21,6 @@ namespace Eigen { * \param XprType the type of the expression in which we are taking a block * \param BlockRows the number of rows of the block we are taking at compile time (optional) * \param BlockCols the number of columns of the block we are taking at compile time (optional) - * \param _DirectAccessStatus \internal used for partial specialization * * This class represents an expression of either a fixed-size or dynamic-size block. It is the return * type of DenseBase::block(Index,Index,Index,Index) and DenseBase::block(Index,Index) and @@ -47,8 +46,8 @@ namespace Eigen { */ namespace internal { -template -struct traits > : traits +template +struct traits > : traits { typedef typename traits::Scalar Scalar; typedef typename traits::StorageKind StorageKind; @@ -92,21 +91,92 @@ struct traits Flags = Flags0 | FlagsLinearAccessBit | FlagsLvalueBit | FlagsRowMajorBit }; }; -} -template class Block - : public internal::dense_xpr_base >::type +template::ret> class BlockImpl_dense; + +} // end namespace internal + +template class BlockImpl; + +template class Block + : public BlockImpl::StorageKind> { + typedef BlockImpl::StorageKind> Impl; + public: + //typedef typename Impl::Base Base; + typedef Impl Base; + EIGEN_GENERIC_PUBLIC_INTERFACE(Block) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block) + + /** Column or Row constructor + */ + inline Block(XprType& xpr, Index i) : Impl(xpr,i) + { + eigen_assert( (i>=0) && ( + ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && i= 0 && BlockRows >= 1 && a_startRow + BlockRows <= xpr.rows() + && a_startCol >= 0 && BlockCols >= 1 && a_startCol + BlockCols <= xpr.cols()); + } + + /** Dynamic-size constructor + */ + inline Block(XprType& xpr, + Index a_startRow, Index a_startCol, + Index blockRows, Index blockCols) + : Impl(xpr, a_startRow, a_startCol, blockRows, blockCols) + { + eigen_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==blockRows) + && (ColsAtCompileTime==Dynamic || ColsAtCompileTime==blockCols)); + eigen_assert(a_startRow >= 0 && blockRows >= 0 && a_startRow <= xpr.rows() - blockRows + && a_startCol >= 0 && blockCols >= 0 && a_startCol <= xpr.cols() - blockCols); + } +}; + +// The generic default implementation for dense block simplu forward to the internal::BlockImpl_dense +// that must be specialized for direct and non-direct access... +template +class BlockImpl + : public internal::BlockImpl_dense +{ + typedef internal::BlockImpl_dense Impl; + typedef typename XprType::Index Index; + public: + typedef Impl Base; + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + inline BlockImpl(XprType& xpr, Index i) : Impl(xpr,i) {} + inline BlockImpl(XprType& xpr, Index a_startRow, Index a_startCol) : Impl(xpr, a_startRow, a_startCol) {} + inline BlockImpl(XprType& xpr, Index a_startRow, Index a_startCol, Index blockRows, Index blockCols) + : Impl(xpr, a_startRow, a_startCol, blockRows, blockCols) {} +}; + +namespace internal { + +/** \internal Internal implementation of dense Blocks in the general case. */ +template class BlockImpl_dense + : public internal::dense_xpr_base >::type +{ + typedef Block BlockType; public: - typedef typename internal::dense_xpr_base::type Base; - EIGEN_DENSE_PUBLIC_INTERFACE(Block) + typedef typename internal::dense_xpr_base::type Base; + EIGEN_DENSE_PUBLIC_INTERFACE(BlockType) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense) class InnerIterator; /** Column or Row constructor */ - inline Block(XprType& xpr, Index i) + inline BlockImpl_dense(XprType& xpr, Index i) : m_xpr(xpr), // It is a row if and only if BlockRows==1 and BlockCols==XprType::ColsAtCompileTime, // and it is a column if and only if BlockRows==XprType::RowsAtCompileTime and BlockCols==1, @@ -116,58 +186,43 @@ template=0) && ( - ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && i= 0 && BlockRows >= 1 && startRow + BlockRows <= xpr.rows() - && startCol >= 0 && BlockCols >= 1 && startCol + BlockCols <= xpr.cols()); - } + inline BlockImpl_dense(XprType& xpr, Index a_startRow, Index a_startCol) + : m_xpr(xpr), m_startRow(a_startRow), m_startCol(a_startCol), + m_blockRows(BlockRows), m_blockCols(BlockCols) + {} /** Dynamic-size constructor */ - inline Block(XprType& xpr, - Index startRow, Index startCol, + inline BlockImpl_dense(XprType& xpr, + Index a_startRow, Index a_startCol, Index blockRows, Index blockCols) - : m_xpr(xpr), m_startRow(startRow), m_startCol(startCol), - m_blockRows(blockRows), m_blockCols(blockCols) - { - eigen_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==blockRows) - && (ColsAtCompileTime==Dynamic || ColsAtCompileTime==blockCols)); - eigen_assert(startRow >= 0 && blockRows >= 0 && startRow + blockRows <= xpr.rows() - && startCol >= 0 && blockCols >= 0 && startCol + blockCols <= xpr.cols()); - } - - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block) + : m_xpr(xpr), m_startRow(a_startRow), m_startCol(a_startCol), + m_blockRows(blockRows), m_blockCols(blockCols) + {} inline Index rows() const { return m_blockRows.value(); } inline Index cols() const { return m_blockCols.value(); } - inline Scalar& coeffRef(Index row, Index col) + inline Scalar& coeffRef(Index rowId, Index colId) { EIGEN_STATIC_ASSERT_LVALUE(XprType) return m_xpr.const_cast_derived() - .coeffRef(row + m_startRow.value(), col + m_startCol.value()); + .coeffRef(rowId + m_startRow.value(), colId + m_startCol.value()); } - inline const Scalar& coeffRef(Index row, Index col) const + inline const Scalar& coeffRef(Index rowId, Index colId) const { return m_xpr.derived() - .coeffRef(row + m_startRow.value(), col + m_startCol.value()); + .coeffRef(rowId + m_startRow.value(), colId + m_startCol.value()); } - EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index row, Index col) const + EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index rowId, Index colId) const { - return m_xpr.coeff(row + m_startRow.value(), col + m_startCol.value()); + return m_xpr.coeff(rowId + m_startRow.value(), colId + m_startCol.value()); } inline Scalar& coeffRef(Index index) @@ -193,17 +248,17 @@ template - inline PacketScalar packet(Index row, Index col) const + inline PacketScalar packet(Index rowId, Index colId) const { return m_xpr.template packet - (row + m_startRow.value(), col + m_startCol.value()); + (rowId + m_startRow.value(), colId + m_startCol.value()); } template - inline void writePacket(Index row, Index col, const PacketScalar& x) + inline void writePacket(Index rowId, Index colId, const PacketScalar& val) { m_xpr.const_cast_derived().template writePacket - (row + m_startRow.value(), col + m_startCol.value(), x); + (rowId + m_startRow.value(), colId + m_startCol.value(), val); } template @@ -215,11 +270,11 @@ template - inline void writePacket(Index index, const PacketScalar& x) + inline void writePacket(Index index, const PacketScalar& val) { m_xpr.const_cast_derived().template writePacket (m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), - m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0), x); + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0), val); } #ifdef EIGEN_PARSED_BY_DOXYGEN @@ -253,21 +308,21 @@ template m_blockCols; }; -/** \internal */ +/** \internal Internal implementation of dense Blocks in the direct access case.*/ template -class Block - : public MapBase > +class BlockImpl_dense + : public MapBase > { + typedef Block BlockType; public: - typedef MapBase Base; - EIGEN_DENSE_PUBLIC_INTERFACE(Block) - - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block) + typedef MapBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(BlockType) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense) /** Column or Row constructor */ - inline Block(XprType& xpr, Index i) + inline BlockImpl_dense(XprType& xpr, Index i) : Base(internal::const_cast_ptr(&xpr.coeffRef( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0, (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0)), @@ -275,34 +330,25 @@ class Block BlockCols==1 ? 1 : xpr.cols()), m_xpr(xpr) { - eigen_assert( (i>=0) && ( - ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && i= 0 && BlockRows >= 1 && startRow + BlockRows <= xpr.rows() - && startCol >= 0 && BlockCols >= 1 && startCol + BlockCols <= xpr.cols()); init(); } /** Dynamic-size constructor */ - inline Block(XprType& xpr, + inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols) : Base(internal::const_cast_ptr(&xpr.coeffRef(startRow,startCol)), blockRows, blockCols), m_xpr(xpr) { - eigen_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==blockRows) - && (ColsAtCompileTime==Dynamic || ColsAtCompileTime==blockCols)); - eigen_assert(startRow >= 0 && blockRows >= 0 && startRow + blockRows <= xpr.rows() - && startCol >= 0 && blockCols >= 0 && startCol + blockCols <= xpr.cols()); init(); } @@ -314,7 +360,7 @@ class Block /** \sa MapBase::innerStride() */ inline Index innerStride() const { - return internal::traits::HasSameStorageOrderAsXprType + return internal::traits::HasSameStorageOrderAsXprType ? m_xpr.innerStride() : m_xpr.outerStride(); } @@ -333,7 +379,7 @@ class Block #ifndef EIGEN_PARSED_BY_DOXYGEN /** \internal used by allowAligned() */ - inline Block(XprType& xpr, const Scalar* data, Index blockRows, Index blockCols) + inline BlockImpl_dense(XprType& xpr, const Scalar* data, Index blockRows, Index blockCols) : Base(data, blockRows, blockCols), m_xpr(xpr) { init(); @@ -343,7 +389,7 @@ class Block protected: void init() { - m_outerStride = internal::traits::HasSameStorageOrderAsXprType + m_outerStride = internal::traits::HasSameStorageOrderAsXprType ? m_xpr.outerStride() : m_xpr.innerStride(); } @@ -352,6 +398,8 @@ class Block Index m_outerStride; }; +} // end namespace internal + } // end namespace Eigen #endif // EIGEN_BLOCK_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h index 57efd8e69..6e37e031a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h @@ -85,9 +85,7 @@ inline bool DenseBase::all() const && SizeAtCompileTime * (CoeffReadCost + NumTraits::AddCost) <= EIGEN_UNROLLING_LIMIT }; if(unroll) - return internal::all_unroller::run(derived()); + return internal::all_unroller::run(derived()); else { for(Index j = 0; j < cols(); ++j) @@ -111,9 +109,7 @@ inline bool DenseBase::any() const && SizeAtCompileTime * (CoeffReadCost + NumTraits::AddCost) <= EIGEN_UNROLLING_LIMIT }; if(unroll) - return internal::any_unroller::run(derived()); + return internal::any_unroller::run(derived()); else { for(Index j = 0; j < cols(); ++j) @@ -133,6 +129,26 @@ inline typename DenseBase::Index DenseBase::count() const return derived().template cast().template cast().sum(); } +/** \returns true is \c *this contains at least one Not A Number (NaN). + * + * \sa allFinite() + */ +template +inline bool DenseBase::hasNaN() const +{ + return !((derived().array()==derived().array()).all()); +} + +/** \returns true if \c *this contains only finite numbers, i.e., no NaN and no +/-INF values. + * + * \sa hasNaN() + */ +template +inline bool DenseBase::allFinite() const +{ + return !((derived()-derived()).hasNaN()); +} + } // end namespace Eigen #endif // EIGEN_ALLANDANY_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h index f20c1774c..a96867af4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h @@ -118,6 +118,8 @@ struct CommaInitializer * * 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 */ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CoreIterators.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CoreIterators.h similarity index 100% rename from gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CoreIterators.h rename to gtsam/3rdparty/Eigen/Eigen/src/Core/CoreIterators.h diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h index 1b93af31b..586f77aaf 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h @@ -94,8 +94,8 @@ struct traits > // So allowing mixing different types gives very unexpected errors when enabling vectorization, when the user tries to // add together a float matrix and a double matrix. #define EIGEN_CHECK_BINARY_COMPATIBILIY(BINOP,LHS,RHS) \ - EIGEN_STATIC_ASSERT((internal::functor_allows_mixing_real_and_complex::ret \ - ? int(internal::is_same::Real, typename NumTraits::Real>::value) \ + EIGEN_STATIC_ASSERT((internal::functor_is_product_like::ret \ + ? int(internal::scalar_product_traits::Defined) \ : int(internal::is_same::value)), \ YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) @@ -122,13 +122,13 @@ class CwiseBinaryOp : internal::no_assignment_operator, typedef typename internal::remove_reference::type _LhsNested; typedef typename internal::remove_reference::type _RhsNested; - EIGEN_STRONG_INLINE CwiseBinaryOp(const Lhs& lhs, const Rhs& rhs, const BinaryOp& func = BinaryOp()) - : m_lhs(lhs), m_rhs(rhs), m_functor(func) + EIGEN_STRONG_INLINE CwiseBinaryOp(const Lhs& aLhs, const Rhs& aRhs, const BinaryOp& func = BinaryOp()) + : m_lhs(aLhs), m_rhs(aRhs), m_functor(func) { EIGEN_CHECK_BINARY_COMPATIBILIY(BinaryOp,typename Lhs::Scalar,typename Rhs::Scalar); // require the sizes to match EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Lhs, Rhs) - eigen_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); + eigen_assert(aLhs.rows() == aRhs.rows() && aLhs.cols() == aRhs.cols()); } EIGEN_STRONG_INLINE Index rows() const { @@ -169,17 +169,17 @@ class CwiseBinaryOpImpl typedef typename internal::dense_xpr_base >::type Base; EIGEN_DENSE_PUBLIC_INTERFACE( Derived ) - EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const + EIGEN_STRONG_INLINE const Scalar coeff(Index rowId, Index colId) const { - return derived().functor()(derived().lhs().coeff(row, col), - derived().rhs().coeff(row, col)); + return derived().functor()(derived().lhs().coeff(rowId, colId), + derived().rhs().coeff(rowId, colId)); } template - EIGEN_STRONG_INLINE PacketScalar packet(Index row, Index col) const + EIGEN_STRONG_INLINE PacketScalar packet(Index rowId, Index colId) const { - return derived().functor().packetOp(derived().lhs().template packet(row, col), - derived().rhs().template packet(row, col)); + return derived().functor().packetOp(derived().lhs().template packet(rowId, colId), + derived().rhs().template packet(rowId, colId)); } EIGEN_STRONG_INLINE const Scalar coeff(Index index) const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h index 2635a62b0..a93bab2d0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h @@ -54,27 +54,27 @@ class CwiseNullaryOp : internal::no_assignment_operator, typedef typename internal::dense_xpr_base::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(CwiseNullaryOp) - CwiseNullaryOp(Index rows, Index cols, const NullaryOp& func = NullaryOp()) - : m_rows(rows), m_cols(cols), m_functor(func) + CwiseNullaryOp(Index nbRows, Index nbCols, const NullaryOp& func = NullaryOp()) + : m_rows(nbRows), m_cols(nbCols), m_functor(func) { - eigen_assert(rows >= 0 - && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) - && cols >= 0 - && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)); + eigen_assert(nbRows >= 0 + && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == nbRows) + && nbCols >= 0 + && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == nbCols)); } EIGEN_STRONG_INLINE Index rows() const { return m_rows.value(); } EIGEN_STRONG_INLINE Index cols() const { return m_cols.value(); } - EIGEN_STRONG_INLINE const Scalar coeff(Index rows, Index cols) const + EIGEN_STRONG_INLINE const Scalar coeff(Index rowId, Index colId) const { - return m_functor(rows, cols); + return m_functor(rowId, colId); } template - EIGEN_STRONG_INLINE PacketScalar packet(Index row, Index col) const + EIGEN_STRONG_INLINE PacketScalar packet(Index rowId, Index colId) const { - return m_functor.packetOp(row, col); + return m_functor.packetOp(rowId, colId); } EIGEN_STRONG_INLINE const Scalar coeff(Index index) const @@ -163,11 +163,11 @@ DenseBase::NullaryExpr(const CustomNullaryOp& func) /** \returns an expression of a constant matrix of value \a value * - * The parameters \a rows and \a cols are the number of rows and of columns of + * The parameters \a nbRows and \a nbCols are the number of rows and of columns of * the returned matrix. Must be compatible with this DenseBase type. * * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, - * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used + * it is redundant to pass \a nbRows and \a nbCols as arguments, so Zero() should be used * instead. * * The template parameter \a CustomNullaryOp is the type of the functor. @@ -176,9 +176,9 @@ DenseBase::NullaryExpr(const CustomNullaryOp& func) */ template EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType -DenseBase::Constant(Index rows, Index cols, const Scalar& value) +DenseBase::Constant(Index nbRows, Index nbCols, const Scalar& value) { - return DenseBase::NullaryExpr(rows, cols, internal::scalar_constant_op(value)); + return DenseBase::NullaryExpr(nbRows, nbCols, internal::scalar_constant_op(value)); } /** \returns an expression of a constant matrix of value \a value @@ -292,14 +292,14 @@ DenseBase::LinSpaced(const Scalar& low, const Scalar& high) return DenseBase::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op(low,high,Derived::SizeAtCompileTime)); } -/** \returns true if all coefficients in this matrix are approximately equal to \a value, to within precision \a prec */ +/** \returns true if all coefficients in this matrix are approximately equal to \a val, to within precision \a prec */ template bool DenseBase::isApproxToConstant -(const Scalar& value, RealScalar prec) const +(const Scalar& val, const RealScalar& prec) const { for(Index j = 0; j < cols(); ++j) for(Index i = 0; i < rows(); ++i) - if(!internal::isApprox(this->coeff(i, j), value, prec)) + if(!internal::isApprox(this->coeff(i, j), val, prec)) return false; return true; } @@ -309,19 +309,19 @@ bool DenseBase::isApproxToConstant * \returns true if all coefficients in this matrix are approximately equal to \a value, to within precision \a prec */ template bool DenseBase::isConstant -(const Scalar& value, RealScalar prec) const +(const Scalar& val, const RealScalar& prec) const { - return isApproxToConstant(value, prec); + return isApproxToConstant(val, prec); } -/** Alias for setConstant(): sets all coefficients in this expression to \a value. +/** Alias for setConstant(): sets all coefficients in this expression to \a val. * * \sa setConstant(), Constant(), class CwiseNullaryOp */ template -EIGEN_STRONG_INLINE void DenseBase::fill(const Scalar& value) +EIGEN_STRONG_INLINE void DenseBase::fill(const Scalar& val) { - setConstant(value); + setConstant(val); } /** Sets all coefficients in this expression to \a value. @@ -329,9 +329,9 @@ EIGEN_STRONG_INLINE void DenseBase::fill(const Scalar& value) * \sa fill(), setConstant(Index,const Scalar&), setConstant(Index,Index,const Scalar&), setZero(), setOnes(), Constant(), class CwiseNullaryOp, setZero(), setOnes() */ template -EIGEN_STRONG_INLINE Derived& DenseBase::setConstant(const Scalar& value) +EIGEN_STRONG_INLINE Derived& DenseBase::setConstant(const Scalar& val) { - return derived() = Constant(rows(), cols(), value); + return derived() = Constant(rows(), cols(), val); } /** Resizes to the given \a size, and sets all coefficients in this expression to the given \a value. @@ -345,17 +345,17 @@ EIGEN_STRONG_INLINE Derived& DenseBase::setConstant(const Scalar& value */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setConstant(Index size, const Scalar& value) +PlainObjectBase::setConstant(Index size, const Scalar& val) { resize(size); - return setConstant(value); + return setConstant(val); } /** Resizes to the given size, and sets all coefficients in this expression to the given \a value. * - * \param rows the new number of rows - * \param cols the new number of columns - * \param value the value to which all coefficients are set + * \param nbRows the new number of rows + * \param nbCols the new number of columns + * \param val the value to which all coefficients are set * * Example: \include Matrix_setConstant_int_int.cpp * Output: \verbinclude Matrix_setConstant_int_int.out @@ -364,10 +364,10 @@ PlainObjectBase::setConstant(Index size, const Scalar& value) */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setConstant(Index rows, Index cols, const Scalar& value) +PlainObjectBase::setConstant(Index nbRows, Index nbCols, const Scalar& val) { - resize(rows, cols); - return setConstant(value); + resize(nbRows, nbCols); + return setConstant(val); } /** @@ -384,10 +384,10 @@ PlainObjectBase::setConstant(Index rows, Index cols, const Scalar& valu * \sa CwiseNullaryOp */ template -EIGEN_STRONG_INLINE Derived& DenseBase::setLinSpaced(Index size, const Scalar& low, const Scalar& high) +EIGEN_STRONG_INLINE Derived& DenseBase::setLinSpaced(Index newSize, const Scalar& low, const Scalar& high) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return derived() = Derived::NullaryExpr(size, internal::linspaced_op(low,high,size)); + return derived() = Derived::NullaryExpr(newSize, internal::linspaced_op(low,high,newSize)); } /** @@ -425,9 +425,9 @@ EIGEN_STRONG_INLINE Derived& DenseBase::setLinSpaced(const Scalar& low, */ template EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType -DenseBase::Zero(Index rows, Index cols) +DenseBase::Zero(Index nbRows, Index nbCols) { - return Constant(rows, cols, Scalar(0)); + return Constant(nbRows, nbCols, Scalar(0)); } /** \returns an expression of a zero vector. @@ -479,7 +479,7 @@ DenseBase::Zero() * \sa class CwiseNullaryOp, Zero() */ template -bool DenseBase::isZero(RealScalar prec) const +bool DenseBase::isZero(const RealScalar& prec) const { for(Index j = 0; j < cols(); ++j) for(Index i = 0; i < rows(); ++i) @@ -512,16 +512,16 @@ EIGEN_STRONG_INLINE Derived& DenseBase::setZero() */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setZero(Index size) +PlainObjectBase::setZero(Index newSize) { - resize(size); + resize(newSize); return setConstant(Scalar(0)); } /** Resizes to the given size, and sets all coefficients in this expression to zero. * - * \param rows the new number of rows - * \param cols the new number of columns + * \param nbRows the new number of rows + * \param nbCols the new number of columns * * Example: \include Matrix_setZero_int_int.cpp * Output: \verbinclude Matrix_setZero_int_int.out @@ -530,9 +530,9 @@ PlainObjectBase::setZero(Index size) */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setZero(Index rows, Index cols) +PlainObjectBase::setZero(Index nbRows, Index nbCols) { - resize(rows, cols); + resize(nbRows, nbCols); return setConstant(Scalar(0)); } @@ -540,7 +540,7 @@ PlainObjectBase::setZero(Index rows, Index cols) /** \returns an expression of a matrix where all coefficients equal one. * - * The parameters \a rows and \a cols are the number of rows and of columns of + * The parameters \a nbRows and \a nbCols are the number of rows and of columns of * the returned matrix. Must be compatible with this MatrixBase type. * * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, @@ -554,14 +554,14 @@ PlainObjectBase::setZero(Index rows, Index cols) */ template EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType -DenseBase::Ones(Index rows, Index cols) +DenseBase::Ones(Index nbRows, Index nbCols) { - return Constant(rows, cols, Scalar(1)); + return Constant(nbRows, nbCols, Scalar(1)); } /** \returns an expression of a vector where all coefficients equal one. * - * The parameter \a size is the size of the returned vector. + * The parameter \a newSize is the size of the returned vector. * Must be compatible with this MatrixBase type. * * \only_for_vectors @@ -577,9 +577,9 @@ DenseBase::Ones(Index rows, Index cols) */ template EIGEN_STRONG_INLINE const typename DenseBase::ConstantReturnType -DenseBase::Ones(Index size) +DenseBase::Ones(Index newSize) { - return Constant(size, Scalar(1)); + return Constant(newSize, Scalar(1)); } /** \returns an expression of a fixed-size matrix or vector where all coefficients equal one. @@ -609,7 +609,7 @@ DenseBase::Ones() */ template bool DenseBase::isOnes -(RealScalar prec) const +(const RealScalar& prec) const { return isApproxToConstant(Scalar(1), prec); } @@ -627,7 +627,7 @@ EIGEN_STRONG_INLINE Derived& DenseBase::setOnes() return setConstant(Scalar(1)); } -/** Resizes to the given \a size, and sets all coefficients in this expression to one. +/** Resizes to the given \a newSize, and sets all coefficients in this expression to one. * * \only_for_vectors * @@ -638,16 +638,16 @@ EIGEN_STRONG_INLINE Derived& DenseBase::setOnes() */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setOnes(Index size) +PlainObjectBase::setOnes(Index newSize) { - resize(size); + resize(newSize); return setConstant(Scalar(1)); } /** Resizes to the given size, and sets all coefficients in this expression to one. * - * \param rows the new number of rows - * \param cols the new number of columns + * \param nbRows the new number of rows + * \param nbCols the new number of columns * * Example: \include Matrix_setOnes_int_int.cpp * Output: \verbinclude Matrix_setOnes_int_int.out @@ -656,9 +656,9 @@ PlainObjectBase::setOnes(Index size) */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setOnes(Index rows, Index cols) +PlainObjectBase::setOnes(Index nbRows, Index nbCols) { - resize(rows, cols); + resize(nbRows, nbCols); return setConstant(Scalar(1)); } @@ -666,7 +666,7 @@ PlainObjectBase::setOnes(Index rows, Index cols) /** \returns an expression of the identity matrix (not necessarily square). * - * The parameters \a rows and \a cols are the number of rows and of columns of + * The parameters \a nbRows and \a nbCols are the number of rows and of columns of * the returned matrix. Must be compatible with this MatrixBase type. * * This variant is meant to be used for dynamic-size matrix types. For fixed-size types, @@ -680,9 +680,9 @@ PlainObjectBase::setOnes(Index rows, Index cols) */ template EIGEN_STRONG_INLINE const typename MatrixBase::IdentityReturnType -MatrixBase::Identity(Index rows, Index cols) +MatrixBase::Identity(Index nbRows, Index nbCols) { - return DenseBase::NullaryExpr(rows, cols, internal::scalar_identity_op()); + return DenseBase::NullaryExpr(nbRows, nbCols, internal::scalar_identity_op()); } /** \returns an expression of the identity matrix (not necessarily square). @@ -714,7 +714,7 @@ MatrixBase::Identity() */ template bool MatrixBase::isIdentity -(RealScalar prec) const +(const RealScalar& prec) const { for(Index j = 0; j < cols(); ++j) { @@ -776,8 +776,8 @@ EIGEN_STRONG_INLINE Derived& MatrixBase::setIdentity() /** \brief Resizes to the given size, and writes the identity expression (not necessarily square) into *this. * - * \param rows the new number of rows - * \param cols the new number of columns + * \param nbRows the new number of rows + * \param nbCols the new number of columns * * Example: \include Matrix_setIdentity_int_int.cpp * Output: \verbinclude Matrix_setIdentity_int_int.out @@ -785,9 +785,9 @@ EIGEN_STRONG_INLINE Derived& MatrixBase::setIdentity() * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Identity() */ template -EIGEN_STRONG_INLINE Derived& MatrixBase::setIdentity(Index rows, Index cols) +EIGEN_STRONG_INLINE Derived& MatrixBase::setIdentity(Index nbRows, Index nbCols) { - derived().resize(rows, cols); + derived().resize(nbRows, nbCols); return setIdentity(); } @@ -798,10 +798,10 @@ EIGEN_STRONG_INLINE Derived& MatrixBase::setIdentity(Index rows, Index * \sa MatrixBase::Unit(Index), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW() */ template -EIGEN_STRONG_INLINE const typename MatrixBase::BasisReturnType MatrixBase::Unit(Index size, Index i) +EIGEN_STRONG_INLINE const typename MatrixBase::BasisReturnType MatrixBase::Unit(Index newSize, Index i) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return BasisReturnType(SquareMatrixType::Identity(size,size), i); + return BasisReturnType(SquareMatrixType::Identity(newSize,newSize), i); } /** \returns an expression of the i-th unit (basis) vector. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h index 063355ae5..f2de749f9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h @@ -98,15 +98,15 @@ class CwiseUnaryOpImpl typedef typename internal::dense_xpr_base >::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(Derived) - EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const + EIGEN_STRONG_INLINE const Scalar coeff(Index rowId, Index colId) const { - return derived().functor()(derived().nestedExpression().coeff(row, col)); + return derived().functor()(derived().nestedExpression().coeff(rowId, colId)); } template - EIGEN_STRONG_INLINE PacketScalar packet(Index row, Index col) const + EIGEN_STRONG_INLINE PacketScalar packet(Index rowId, Index colId) const { - return derived().functor().packetOp(derived().nestedExpression().template packet(row, col)); + return derived().functor().packetOp(derived().nestedExpression().template packet(rowId, colId)); } EIGEN_STRONG_INLINE const Scalar coeff(Index index) const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h index 66f73a950..b2638d326 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h @@ -44,9 +44,10 @@ struct traits > // "error: no integral type can represent all of the enumerator values InnerStrideAtCompileTime = MatrixTypeInnerStride == Dynamic ? int(Dynamic) - : int(MatrixTypeInnerStride) - * int(sizeof(typename traits::Scalar) / sizeof(Scalar)), - OuterStrideAtCompileTime = outer_stride_at_compile_time::ret + : int(MatrixTypeInnerStride) * int(sizeof(typename traits::Scalar) / sizeof(Scalar)), + OuterStrideAtCompileTime = outer_stride_at_compile_time::ret == Dynamic + ? int(Dynamic) + : outer_stride_at_compile_time::ret * int(sizeof(typename traits::Scalar) / sizeof(Scalar)) }; }; } @@ -55,8 +56,7 @@ template class CwiseUnaryViewImpl; template -class CwiseUnaryView : internal::no_assignment_operator, - public CwiseUnaryViewImpl::StorageKind> +class CwiseUnaryView : public CwiseUnaryViewImpl::StorageKind> { public: @@ -98,6 +98,10 @@ class CwiseUnaryViewImpl typedef typename internal::dense_xpr_base< CwiseUnaryView >::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(Derived) + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryViewImpl) + + inline Scalar* data() { return &coeffRef(0); } + inline const Scalar* data() const { return &coeff(0); } inline Index innerStride() const { @@ -106,7 +110,7 @@ class CwiseUnaryViewImpl inline Index outerStride() const { - return derived().nestedExpression().outerStride(); + return derived().nestedExpression().outerStride() * sizeof(typename internal::traits::Scalar) / sizeof(Scalar); } EIGEN_STRONG_INLINE CoeffReturnType coeff(Index row, Index col) const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index 1cc0314ef..c5800f6c8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -13,6 +13,16 @@ namespace Eigen { +namespace internal { + +// The index type defined by EIGEN_DEFAULT_DENSE_INDEX_TYPE must be a signed type. +// This dummy function simply aims at checking that at compile time. +static inline void check_DenseIndex_is_signed() { + EIGEN_STATIC_ASSERT(NumTraits::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE); +} + +} // end namespace internal + /** \class DenseBase * \ingroup Core_Module * @@ -204,21 +214,21 @@ template class DenseBase * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and does * nothing else. */ - void resize(Index size) + void resize(Index newSize) { - EIGEN_ONLY_USED_FOR_DEBUG(size); - eigen_assert(size == this->size() + EIGEN_ONLY_USED_FOR_DEBUG(newSize); + eigen_assert(newSize == this->size() && "DenseBase::resize() does not actually allow to resize."); } /** Only plain matrices/arrays, not expressions, may be resized; therefore the only useful resize methods are * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and does * nothing else. */ - void resize(Index rows, Index cols) + void resize(Index nbRows, Index nbCols) { - EIGEN_ONLY_USED_FOR_DEBUG(rows); - EIGEN_ONLY_USED_FOR_DEBUG(cols); - eigen_assert(rows == this->rows() && cols == this->cols() + EIGEN_ONLY_USED_FOR_DEBUG(nbRows); + EIGEN_ONLY_USED_FOR_DEBUG(nbCols); + eigen_assert(nbRows == this->rows() && nbCols == this->cols() && "DenseBase::resize() does not actually allow to resize."); } @@ -271,7 +281,7 @@ template class DenseBase CommaInitializer operator<< (const DenseBase& other); Eigen::Transpose transpose(); - typedef const Transpose ConstTransposeReturnType; + typedef typename internal::add_const >::type ConstTransposeReturnType; ConstTransposeReturnType transpose() const; void transposeInPlace(); #ifndef EIGEN_NO_DEBUG @@ -281,29 +291,6 @@ template class DenseBase public: #endif - typedef VectorBlock SegmentReturnType; - typedef const VectorBlock ConstSegmentReturnType; - template struct FixedSegmentReturnType { typedef VectorBlock Type; }; - template struct ConstFixedSegmentReturnType { typedef const VectorBlock Type; }; - - // Note: The "DenseBase::" prefixes are added to help MSVC9 to match these declarations with the later implementations. - SegmentReturnType segment(Index start, Index size); - typename DenseBase::ConstSegmentReturnType segment(Index start, Index size) const; - - SegmentReturnType head(Index size); - typename DenseBase::ConstSegmentReturnType head(Index size) const; - - SegmentReturnType tail(Index size); - typename DenseBase::ConstSegmentReturnType tail(Index size) const; - - template typename FixedSegmentReturnType::Type head(); - template typename ConstFixedSegmentReturnType::Type head() const; - - template typename FixedSegmentReturnType::Type tail(); - template typename ConstFixedSegmentReturnType::Type tail() const; - - template typename FixedSegmentReturnType::Type segment(Index start); - template typename ConstFixedSegmentReturnType::Type segment(Index start) const; static const ConstantReturnType Constant(Index rows, Index cols, const Scalar& value); @@ -348,17 +335,20 @@ template class DenseBase template bool isApprox(const DenseBase& other, - RealScalar prec = NumTraits::dummy_precision()) const; + const RealScalar& prec = NumTraits::dummy_precision()) const; bool isMuchSmallerThan(const RealScalar& other, - RealScalar prec = NumTraits::dummy_precision()) const; + const RealScalar& prec = NumTraits::dummy_precision()) const; template bool isMuchSmallerThan(const DenseBase& other, - RealScalar prec = NumTraits::dummy_precision()) const; + const RealScalar& prec = NumTraits::dummy_precision()) const; - bool isApproxToConstant(const Scalar& value, RealScalar prec = NumTraits::dummy_precision()) const; - bool isConstant(const Scalar& value, RealScalar prec = NumTraits::dummy_precision()) const; - bool isZero(RealScalar prec = NumTraits::dummy_precision()) const; - bool isOnes(RealScalar prec = NumTraits::dummy_precision()) const; + bool isApproxToConstant(const Scalar& value, const RealScalar& prec = NumTraits::dummy_precision()) const; + bool isConstant(const Scalar& value, const RealScalar& prec = NumTraits::dummy_precision()) const; + bool isZero(const RealScalar& prec = NumTraits::dummy_precision()) const; + bool isOnes(const RealScalar& prec = NumTraits::dummy_precision()) const; + + inline bool hasNaN() const; + inline bool allFinite() const; inline Derived& operator*=(const Scalar& other); inline Derived& operator/=(const Scalar& other); @@ -438,8 +428,6 @@ template class DenseBase return derived().coeff(0,0); } -/////////// Array module /////////// - bool all(void) const; bool any(void) const; Index count() const; @@ -465,11 +453,11 @@ template class DenseBase template inline const Select - select(const DenseBase& thenMatrix, typename ThenDerived::Scalar elseScalar) const; + select(const DenseBase& thenMatrix, const typename ThenDerived::Scalar& elseScalar) const; template inline const Select - select(typename ElseDerived::Scalar thenScalar, const DenseBase& elseMatrix) const; + select(const typename ElseDerived::Scalar& thenScalar, const DenseBase& elseMatrix) const; template RealScalar lpNorm() const; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseCoeffsBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseCoeffsBase.h index 72704c2d7..3c890f215 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseCoeffsBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseCoeffsBase.h @@ -427,22 +427,22 @@ class DenseCoeffsBase : public DenseCoeffsBase EIGEN_STRONG_INLINE void writePacket - (Index row, Index col, const typename internal::packet_traits::type& x) + (Index row, Index col, const typename internal::packet_traits::type& val) { eigen_internal_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); - derived().template writePacket(row,col,x); + derived().template writePacket(row,col,val); } /** \internal */ template EIGEN_STRONG_INLINE void writePacketByOuterInner - (Index outer, Index inner, const typename internal::packet_traits::type& x) + (Index outer, Index inner, const typename internal::packet_traits::type& val) { writePacket(rowIndexByOuterInner(outer, inner), colIndexByOuterInner(outer, inner), - x); + val); } /** \internal @@ -456,10 +456,10 @@ class DenseCoeffsBase : public DenseCoeffsBase EIGEN_STRONG_INLINE void writePacket - (Index index, const typename internal::packet_traits::type& x) + (Index index, const typename internal::packet_traits::type& val) { eigen_internal_assert(index >= 0 && index < size()); - derived().template writePacket(index,x); + derived().template writePacket(index,val); } #ifndef EIGEN_PARSED_BY_DOXYGEN diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h index 1fc2daf2c..3e7f9c1b7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h @@ -35,17 +35,36 @@ template + EIGEN_ALWAYS_INLINE PtrType eigen_unaligned_array_assert_workaround_gcc47(PtrType array) { return array; } + #define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask) \ + eigen_assert((reinterpret_cast(eigen_unaligned_array_assert_workaround_gcc47(array)) & sizemask) == 0 \ + && "this assertion is explained here: " \ + "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" \ + " **** READ THIS WEB PAGE !!! ****"); #else #define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask) \ eigen_assert((reinterpret_cast(array) & sizemask) == 0 \ && "this assertion is explained here: " \ - "http://eigen.tuxfamily.org/dox-devel/TopicUnalignedArrayAssert.html" \ + "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" \ " **** READ THIS WEB PAGE !!! ****"); #endif @@ -53,8 +72,17 @@ template struct plain_array { EIGEN_USER_ALIGN16 T array[Size]; - plain_array() { EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(0xf) } - plain_array(constructor_without_unaligned_array_assert) {} + + plain_array() + { + EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(0xf); + EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG); + } + + plain_array(constructor_without_unaligned_array_assert) + { + EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG); + } }; template @@ -86,7 +114,7 @@ template class DenseSt { internal::plain_array m_data; public: - inline explicit DenseStorage() {} + inline DenseStorage() {} inline DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()) {} inline DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} @@ -103,7 +131,7 @@ template class DenseSt template class DenseStorage { public: - inline explicit DenseStorage() {} + inline DenseStorage() {} inline DenseStorage(internal::constructor_without_unaligned_array_assert) {} inline DenseStorage(DenseIndex,DenseIndex,DenseIndex) {} inline void swap(DenseStorage& ) {} @@ -132,16 +160,16 @@ template class DenseStorage class DenseStorage m_data; DenseIndex m_rows; public: - inline explicit DenseStorage() : m_rows(0) {} + inline DenseStorage() : m_rows(0) {} inline DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0) {} - inline DenseStorage(DenseIndex, DenseIndex rows, DenseIndex) : m_rows(rows) {} + inline DenseStorage(DenseIndex, DenseIndex nbRows, DenseIndex) : m_rows(nbRows) {} inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } inline DenseIndex rows(void) const {return m_rows;} inline DenseIndex cols(void) const {return _Cols;} - inline void conservativeResize(DenseIndex, DenseIndex rows, DenseIndex) { m_rows = rows; } - inline void resize(DenseIndex, DenseIndex rows, DenseIndex) { m_rows = rows; } + inline void conservativeResize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } + inline void resize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; } inline const T *data() const { return m_data.array; } inline T *data() { return m_data.array; } }; @@ -171,15 +199,15 @@ template class DenseStorage m_data; DenseIndex m_cols; public: - inline explicit DenseStorage() : m_cols(0) {} + inline DenseStorage() : m_cols(0) {} inline DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(0) {} - inline DenseStorage(DenseIndex, DenseIndex, DenseIndex cols) : m_cols(cols) {} + inline DenseStorage(DenseIndex, DenseIndex, DenseIndex nbCols) : m_cols(nbCols) {} inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } inline DenseIndex rows(void) const {return _Rows;} inline DenseIndex cols(void) const {return m_cols;} - inline void conservativeResize(DenseIndex, DenseIndex, DenseIndex cols) { m_cols = cols; } - inline void resize(DenseIndex, DenseIndex, DenseIndex cols) { m_cols = cols; } + inline void conservativeResize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; } + inline void resize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; } inline const T *data() const { return m_data.array; } inline T *data() { return m_data.array; } }; @@ -191,24 +219,24 @@ template class DenseStorage(size)), m_rows(rows), m_cols(cols) + inline DenseStorage(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols) + : m_data(internal::conditional_aligned_new_auto(size)), m_rows(nbRows), m_cols(nbCols) { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } inline ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, m_rows*m_cols); } inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); } inline DenseIndex rows(void) const {return m_rows;} inline DenseIndex cols(void) const {return m_cols;} - inline void conservativeResize(DenseIndex size, DenseIndex rows, DenseIndex cols) + inline void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, m_rows*m_cols); - m_rows = rows; - m_cols = cols; + m_rows = nbRows; + m_cols = nbCols; } - void resize(DenseIndex size, DenseIndex rows, DenseIndex cols) + void resize(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols) { if(size != m_rows*m_cols) { @@ -219,8 +247,8 @@ template class DenseStorage class DenseStorage(size)), m_cols(cols) + inline DenseStorage(DenseIndex size, DenseIndex, DenseIndex nbCols) : m_data(internal::conditional_aligned_new_auto(size)), m_cols(nbCols) { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } inline ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Rows*m_cols); } inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); } static inline DenseIndex rows(void) {return _Rows;} inline DenseIndex cols(void) const {return m_cols;} - inline void conservativeResize(DenseIndex size, DenseIndex, DenseIndex cols) + inline void conservativeResize(DenseIndex size, DenseIndex, DenseIndex nbCols) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, _Rows*m_cols); - m_cols = cols; + m_cols = nbCols; } - EIGEN_STRONG_INLINE void resize(DenseIndex size, DenseIndex, DenseIndex cols) + EIGEN_STRONG_INLINE void resize(DenseIndex size, DenseIndex, DenseIndex nbCols) { if(size != _Rows*m_cols) { @@ -256,7 +284,7 @@ template class DenseStorage class DenseStorage(size)), m_rows(rows) + inline DenseStorage(DenseIndex size, DenseIndex nbRows, DenseIndex) : m_data(internal::conditional_aligned_new_auto(size)), m_rows(nbRows) { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN } inline ~DenseStorage() { internal::conditional_aligned_delete_auto(m_data, _Cols*m_rows); } inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); } inline DenseIndex rows(void) const {return m_rows;} static inline DenseIndex cols(void) {return _Cols;} - inline void conservativeResize(DenseIndex size, DenseIndex rows, DenseIndex) + inline void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex) { m_data = internal::conditional_aligned_realloc_new_auto(m_data, size, m_rows*_Cols); - m_rows = rows; + m_rows = nbRows; } - EIGEN_STRONG_INLINE void resize(DenseIndex size, DenseIndex rows, DenseIndex) + EIGEN_STRONG_INLINE void resize(DenseIndex size, DenseIndex nbRows, DenseIndex) { if(size != m_rows*_Cols) { @@ -292,7 +320,7 @@ template class DenseStorage > typedef typename remove_reference::type _MatrixTypeNested; typedef typename MatrixType::StorageKind StorageKind; enum { - RowsAtCompileTime = (int(DiagIndex) == Dynamic || int(MatrixType::SizeAtCompileTime) == Dynamic) ? Dynamic - : (EIGEN_PLAIN_ENUM_MIN(MatrixType::RowsAtCompileTime - EIGEN_PLAIN_ENUM_MAX(-DiagIndex, 0), - MatrixType::ColsAtCompileTime - EIGEN_PLAIN_ENUM_MAX( DiagIndex, 0))), + RowsAtCompileTime = (int(DiagIndex) == DynamicIndex || int(MatrixType::SizeAtCompileTime) == Dynamic) ? Dynamic + : (EIGEN_PLAIN_ENUM_MIN(MatrixType::RowsAtCompileTime - EIGEN_PLAIN_ENUM_MAX(-DiagIndex, 0), + MatrixType::ColsAtCompileTime - EIGEN_PLAIN_ENUM_MAX( DiagIndex, 0))), ColsAtCompileTime = 1, MaxRowsAtCompileTime = int(MatrixType::MaxSizeAtCompileTime) == Dynamic ? Dynamic - : DiagIndex == Dynamic ? EIGEN_SIZE_MIN_PREFER_FIXED(MatrixType::MaxRowsAtCompileTime, + : DiagIndex == DynamicIndex ? EIGEN_SIZE_MIN_PREFER_FIXED(MatrixType::MaxRowsAtCompileTime, MatrixType::MaxColsAtCompileTime) : (EIGEN_PLAIN_ENUM_MIN(MatrixType::MaxRowsAtCompileTime - EIGEN_PLAIN_ENUM_MAX(-DiagIndex, 0), MatrixType::MaxColsAtCompileTime - EIGEN_PLAIN_ENUM_MAX( DiagIndex, 0))), @@ -61,20 +61,21 @@ struct traits > }; } -template class Diagonal - : public internal::dense_xpr_base< Diagonal >::type +template class Diagonal + : public internal::dense_xpr_base< Diagonal >::type { public: + enum { DiagIndex = _DiagIndex }; typedef typename internal::dense_xpr_base::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(Diagonal) - inline Diagonal(MatrixType& matrix, Index index = DiagIndex) : m_matrix(matrix), m_index(index) {} + inline Diagonal(MatrixType& matrix, Index a_index = DiagIndex) : m_matrix(matrix), m_index(a_index) {} EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Diagonal) inline Index rows() const - { return m_index.value()<0 ? (std::min)(m_matrix.cols(),m_matrix.rows()+m_index.value()) : (std::min)(m_matrix.rows(),m_matrix.cols()-m_index.value()); } + { return m_index.value()<0 ? (std::min)(m_matrix.cols(),m_matrix.rows()+m_index.value()) : (std::min)(m_matrix.rows(),m_matrix.cols()-m_index.value()); } inline Index cols() const { return 1; } @@ -113,20 +114,20 @@ template class Diagonal return m_matrix.coeff(row+rowOffset(), row+colOffset()); } - inline Scalar& coeffRef(Index index) + inline Scalar& coeffRef(Index idx) { EIGEN_STATIC_ASSERT_LVALUE(MatrixType) - return m_matrix.const_cast_derived().coeffRef(index+rowOffset(), index+colOffset()); + return m_matrix.const_cast_derived().coeffRef(idx+rowOffset(), idx+colOffset()); } - inline const Scalar& coeffRef(Index index) const + inline const Scalar& coeffRef(Index idx) const { - return m_matrix.const_cast_derived().coeffRef(index+rowOffset(), index+colOffset()); + return m_matrix.const_cast_derived().coeffRef(idx+rowOffset(), idx+colOffset()); } - inline CoeffReturnType coeff(Index index) const + inline CoeffReturnType coeff(Index idx) const { - return m_matrix.coeff(index+rowOffset(), index+colOffset()); + return m_matrix.coeff(idx+rowOffset(), idx+colOffset()); } const typename internal::remove_all::type& @@ -142,7 +143,7 @@ template class Diagonal protected: typename MatrixType::Nested m_matrix; - const internal::variable_if_dynamic m_index; + const internal::variable_if_dynamicindex m_index; private: // some compilers may fail to optimize std::max etc in case of compile-time constants... @@ -171,7 +172,7 @@ MatrixBase::diagonal() /** This is the const version of diagonal(). */ template -inline const typename MatrixBase::ConstDiagonalReturnType +inline typename MatrixBase::ConstDiagonalReturnType MatrixBase::diagonal() const { return ConstDiagonalReturnType(derived()); @@ -189,18 +190,18 @@ MatrixBase::diagonal() const * * \sa MatrixBase::diagonal(), class Diagonal */ template -inline typename MatrixBase::template DiagonalIndexReturnType::Type +inline typename MatrixBase::template DiagonalIndexReturnType::Type MatrixBase::diagonal(Index index) { - return typename DiagonalIndexReturnType::Type(derived(), index); + return typename DiagonalIndexReturnType::Type(derived(), index); } /** This is the const version of diagonal(Index). */ template -inline typename MatrixBase::template ConstDiagonalIndexReturnType::Type +inline typename MatrixBase::template ConstDiagonalIndexReturnType::Type MatrixBase::diagonal(Index index) const { - return typename ConstDiagonalIndexReturnType::Type(derived(), index); + return typename ConstDiagonalIndexReturnType::Type(derived(), index); } /** \returns an expression of the \a DiagIndex-th sub or super diagonal of the matrix \c *this diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h index 6e8b50fab..e6c220f41 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h @@ -56,9 +56,14 @@ class DiagonalBase : public EigenBase inline Index rows() const { return diagonal().size(); } inline Index cols() const { return diagonal().size(); } + /** \returns the diagonal matrix product of \c *this by the matrix \a matrix. + */ template const DiagonalProduct - operator*(const MatrixBase &matrix) const; + operator*(const MatrixBase &matrix) const + { + return DiagonalProduct(matrix.derived(), derived()); + } inline const DiagonalWrapper, const DiagonalVectorType> > inverse() const @@ -250,7 +255,7 @@ class DiagonalWrapper #endif /** Constructor from expression of diagonal coefficients to wrap. */ - inline DiagonalWrapper(DiagonalVectorType& diagonal) : m_diagonal(diagonal) {} + inline DiagonalWrapper(DiagonalVectorType& a_diagonal) : m_diagonal(a_diagonal) {} /** \returns a const reference to the wrapped expression of diagonal coefficients. */ const DiagonalVectorType& diagonal() const { return m_diagonal; } @@ -284,13 +289,14 @@ MatrixBase::asDiagonal() const * \sa asDiagonal() */ template -bool MatrixBase::isDiagonal(RealScalar prec) const +bool MatrixBase::isDiagonal(const RealScalar& prec) const { + using std::abs; if(cols() != rows()) return false; RealScalar maxAbsOnDiagonal = static_cast(-1); for(Index j = 0; j < cols(); ++j) { - RealScalar absOnDiagonal = internal::abs(coeff(j,j)); + RealScalar absOnDiagonal = abs(coeff(j,j)); if(absOnDiagonal > maxAbsOnDiagonal) maxAbsOnDiagonal = absOnDiagonal; } for(Index j = 0; j < cols(); ++j) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h index 598c6b3e1..c03a0c2e1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h @@ -26,14 +26,15 @@ struct traits > MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime, _StorageOrder = MatrixType::Flags & RowMajorBit ? RowMajor : ColMajor, - _PacketOnDiag = !((int(_StorageOrder) == RowMajor && int(ProductOrder) == OnTheLeft) - ||(int(_StorageOrder) == ColMajor && int(ProductOrder) == OnTheRight)), + _ScalarAccessOnDiag = !((int(_StorageOrder) == ColMajor && int(ProductOrder) == OnTheLeft) + ||(int(_StorageOrder) == RowMajor && int(ProductOrder) == OnTheRight)), _SameTypes = is_same::value, // FIXME currently we need same types, but in the future the next rule should be the one - //_Vectorizable = bool(int(MatrixType::Flags)&PacketAccessBit) && ((!_PacketOnDiag) || (_SameTypes && bool(int(DiagonalType::Flags)&PacketAccessBit))), - _Vectorizable = bool(int(MatrixType::Flags)&PacketAccessBit) && _SameTypes && ((!_PacketOnDiag) || (bool(int(DiagonalType::Flags)&PacketAccessBit))), + //_Vectorizable = bool(int(MatrixType::Flags)&PacketAccessBit) && ((!_PacketOnDiag) || (_SameTypes && bool(int(DiagonalType::DiagonalVectorType::Flags)&PacketAccessBit))), + _Vectorizable = bool(int(MatrixType::Flags)&PacketAccessBit) && _SameTypes && (_ScalarAccessOnDiag || (bool(int(DiagonalType::DiagonalVectorType::Flags)&PacketAccessBit))), + _LinearAccessMask = (RowsAtCompileTime==1 || ColsAtCompileTime==1) ? LinearAccessBit : 0, - Flags = (HereditaryBits & (unsigned int)(MatrixType::Flags)) | (_Vectorizable ? PacketAccessBit : 0), + Flags = ((HereditaryBits|_LinearAccessMask) & (unsigned int)(MatrixType::Flags)) | (_Vectorizable ? PacketAccessBit : 0) | AlignedBit,//(int(MatrixType::Flags)&int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit), CoeffReadCost = NumTraits::MulCost + MatrixType::CoeffReadCost + DiagonalType::DiagonalVectorType::CoeffReadCost }; }; @@ -54,13 +55,21 @@ class DiagonalProduct : internal::no_assignment_operator, eigen_assert(diagonal.diagonal().size() == (ProductOrder == OnTheLeft ? matrix.rows() : matrix.cols())); } - inline Index rows() const { return m_matrix.rows(); } - inline Index cols() const { return m_matrix.cols(); } + EIGEN_STRONG_INLINE Index rows() const { return m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return m_matrix.cols(); } - const Scalar coeff(Index row, Index col) const + EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const { return m_diagonal.diagonal().coeff(ProductOrder == OnTheLeft ? row : col) * m_matrix.coeff(row, col); } + + EIGEN_STRONG_INLINE const Scalar coeff(Index idx) const + { + enum { + StorageOrder = int(MatrixType::Flags) & RowMajorBit ? RowMajor : ColMajor + }; + return coeff(int(StorageOrder)==ColMajor?idx:0,int(StorageOrder)==ColMajor?0:idx); + } template EIGEN_STRONG_INLINE PacketScalar packet(Index row, Index col) const @@ -69,11 +78,19 @@ class DiagonalProduct : internal::no_assignment_operator, StorageOrder = Flags & RowMajorBit ? RowMajor : ColMajor }; const Index indexInDiagonalVector = ProductOrder == OnTheLeft ? row : col; - return packet_impl(row,col,indexInDiagonalVector,typename internal::conditional< ((int(StorageOrder) == RowMajor && int(ProductOrder) == OnTheLeft) ||(int(StorageOrder) == ColMajor && int(ProductOrder) == OnTheRight)), internal::true_type, internal::false_type>::type()); } + + template + EIGEN_STRONG_INLINE PacketScalar packet(Index idx) const + { + enum { + StorageOrder = int(MatrixType::Flags) & RowMajorBit ? RowMajor : ColMajor + }; + return packet(int(StorageOrder)==ColMajor?idx:0,int(StorageOrder)==ColMajor?0:idx); + } protected: template @@ -88,7 +105,7 @@ class DiagonalProduct : internal::no_assignment_operator, { enum { InnerSize = (MatrixType::Flags & RowMajorBit) ? MatrixType::ColsAtCompileTime : MatrixType::RowsAtCompileTime, - DiagonalVectorPacketLoadMode = (LoadMode == Aligned && ((InnerSize%16) == 0)) ? Aligned : Unaligned + DiagonalVectorPacketLoadMode = (LoadMode == Aligned && (((InnerSize%16) == 0) || (int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit)==AlignedBit) ? Aligned : Unaligned) }; return internal::pmul(m_matrix.template packet(row, col), m_diagonal.diagonal().template packet(id)); @@ -103,19 +120,9 @@ class DiagonalProduct : internal::no_assignment_operator, template template inline const DiagonalProduct -MatrixBase::operator*(const DiagonalBase &diagonal) const +MatrixBase::operator*(const DiagonalBase &a_diagonal) const { - return DiagonalProduct(derived(), diagonal.derived()); -} - -/** \returns the diagonal matrix product of \c *this by the matrix \a matrix. - */ -template -template -inline const DiagonalProduct -DiagonalBase::operator*(const MatrixBase &matrix) const -{ - return DiagonalProduct(matrix.derived(), derived()); + return DiagonalProduct(derived(), a_diagonal.derived()); } } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h index ae9274e36..9d7651f1f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h @@ -112,7 +112,7 @@ MatrixBase::eigen2_dot(const MatrixBase& other) const template EIGEN_STRONG_INLINE typename NumTraits::Scalar>::Real MatrixBase::squaredNorm() const { - return internal::real((*this).cwiseAbs2().sum()); + return numext::real((*this).cwiseAbs2().sum()); } /** \returns, for vectors, the \em l2 norm of \c *this, and for matrices the Frobenius norm. @@ -124,7 +124,8 @@ EIGEN_STRONG_INLINE typename NumTraits::Scala template inline typename NumTraits::Scalar>::Real MatrixBase::norm() const { - return internal::sqrt(squaredNorm()); + using std::sqrt; + return sqrt(squaredNorm()); } /** \returns an expression of the quotient of *this by its own norm. @@ -165,6 +166,7 @@ struct lpNorm_selector typedef typename NumTraits::Scalar>::Real RealScalar; static inline RealScalar run(const MatrixBase& m) { + using std::pow; return pow(m.cwiseAbs().array().pow(p).sum(), RealScalar(1)/p); } }; @@ -223,11 +225,11 @@ MatrixBase::lpNorm() const template template bool MatrixBase::isOrthogonal -(const MatrixBase& other, RealScalar prec) const +(const MatrixBase& other, const RealScalar& prec) const { typename internal::nested::type nested(derived()); typename internal::nested::type otherNested(other.derived()); - return internal::abs2(nested.dot(otherNested)) <= prec * prec * nested.squaredNorm() * otherNested.squaredNorm(); + return numext::abs2(nested.dot(otherNested)) <= prec * prec * nested.squaredNorm() * otherNested.squaredNorm(); } /** \returns true if *this is approximately an unitary matrix, @@ -242,7 +244,7 @@ bool MatrixBase::isOrthogonal * Output: \verbinclude MatrixBase_isUnitary.out */ template -bool MatrixBase::isUnitary(RealScalar prec) const +bool MatrixBase::isUnitary(const RealScalar& prec) const { typename Derived::Nested nested(derived()); for(Index i = 0; i < cols(); ++i) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h index 0bbd28bec..2b8dd1b70 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h @@ -139,7 +139,8 @@ MatrixBase::operator*=(const EigenBase &other) return derived(); } -/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=() */ +/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=(). + */ template template inline void MatrixBase::applyOnTheRight(const EigenBase &other) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h index 2f46abfdd..04fb21732 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h @@ -154,6 +154,7 @@ template struct scalar_hypot_op { { using std::max; using std::min; + using std::sqrt; Scalar p = (max)(_x, _y); Scalar q = (min)(_x, _y); Scalar qp = q/p; @@ -170,7 +171,7 @@ struct functor_traits > { */ template struct scalar_binary_pow_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_binary_pow_op) - inline Scalar operator() (const Scalar& a, const OtherScalar& b) const { return internal::pow(a, b); } + inline Scalar operator() (const Scalar& a, const OtherScalar& b) const { return numext::pow(a, b); } }; template struct functor_traits > { @@ -204,21 +205,28 @@ struct functor_traits > { * * \sa class CwiseBinaryOp, Cwise::operator/() */ -template struct scalar_quotient_op { +template struct scalar_quotient_op { + enum { + // TODO vectorize mixed product + Vectorizable = is_same::value && packet_traits::HasDiv && packet_traits::HasDiv + }; + typedef typename scalar_product_traits::ReturnType result_type; EIGEN_EMPTY_STRUCT_CTOR(scalar_quotient_op) - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a / b; } + EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a / b; } template EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const { return internal::pdiv(a,b); } }; -template -struct functor_traits > { +template +struct functor_traits > { enum { - Cost = 2 * NumTraits::MulCost, - PacketAccess = packet_traits::HasDiv + Cost = (NumTraits::MulCost + NumTraits::MulCost), // rough estimate! + PacketAccess = scalar_quotient_op::Vectorizable }; }; + + /** \internal * \brief Template functor to compute the and of two booleans * @@ -280,7 +288,7 @@ struct functor_traits > template struct scalar_abs_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_abs_op) typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return internal::abs(a); } + EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { using std::abs; return abs(a); } template EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const { return internal::pabs(a); } @@ -302,7 +310,7 @@ struct functor_traits > template struct scalar_abs2_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_abs2_op) typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return internal::abs2(a); } + EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return numext::abs2(a); } template EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const { return internal::pmul(a,a); } @@ -318,7 +326,7 @@ struct functor_traits > */ template struct scalar_conjugate_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_conjugate_op) - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return internal::conj(a); } + EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { using numext::conj; return conj(a); } template EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const { return internal::pconj(a); } }; @@ -355,7 +363,7 @@ template struct scalar_real_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_real_op) typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return internal::real(a); } + EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return numext::real(a); } }; template struct functor_traits > @@ -370,7 +378,7 @@ template struct scalar_imag_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_imag_op) typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return internal::imag(a); } + EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return numext::imag(a); } }; template struct functor_traits > @@ -385,7 +393,7 @@ template struct scalar_real_ref_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_real_ref_op) typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE result_type& operator() (const Scalar& a) const { return internal::real_ref(*const_cast(&a)); } + EIGEN_STRONG_INLINE result_type& operator() (const Scalar& a) const { return numext::real_ref(*const_cast(&a)); } }; template struct functor_traits > @@ -400,7 +408,7 @@ template struct scalar_imag_ref_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_imag_ref_op) typedef typename NumTraits::Real result_type; - EIGEN_STRONG_INLINE result_type& operator() (const Scalar& a) const { return internal::imag_ref(*const_cast(&a)); } + EIGEN_STRONG_INLINE result_type& operator() (const Scalar& a) const { return numext::imag_ref(*const_cast(&a)); } }; template struct functor_traits > @@ -414,7 +422,7 @@ struct functor_traits > */ template struct scalar_exp_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_exp_op) - inline const Scalar operator() (const Scalar& a) const { return internal::exp(a); } + inline const Scalar operator() (const Scalar& a) const { using std::exp; return exp(a); } typedef typename packet_traits::type Packet; inline Packet packetOp(const Packet& a) const { return internal::pexp(a); } }; @@ -430,7 +438,7 @@ struct functor_traits > */ template struct scalar_log_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_log_op) - inline const Scalar operator() (const Scalar& a) const { return internal::log(a); } + inline const Scalar operator() (const Scalar& a) const { using std::log; return log(a); } typedef typename packet_traits::type Packet; inline Packet packetOp(const Packet& a) const { return internal::plog(a); } }; @@ -533,20 +541,28 @@ template struct linspaced_op_impl; // linear access for packet ops: // 1) initialization // base = [low, ..., low] + ([step, ..., step] * [-size, ..., 0]) -// 2) each step +// 2) each step (where size is 1 for coeff access or PacketSize for packet access) // base += [size*step, ..., size*step] +// +// TODO: Perhaps it's better to initialize lazily (so not in the constructor but in packetOp) +// in order to avoid the padd() in operator() ? template struct linspaced_op_impl { typedef typename packet_traits::type Packet; - linspaced_op_impl(Scalar low, Scalar step) : + linspaced_op_impl(const Scalar& low, const Scalar& step) : m_low(low), m_step(step), m_packetStep(pset1(packet_traits::size*step)), - m_base(padd(pset1(low),pmul(pset1(step),plset(-packet_traits::size)))) {} + m_base(padd(pset1(low), pmul(pset1(step),plset(-packet_traits::size)))) {} template - EIGEN_STRONG_INLINE const Scalar operator() (Index i) const { return m_low+i*m_step; } + EIGEN_STRONG_INLINE const Scalar operator() (Index i) const + { + m_base = padd(m_base, pset1(m_step)); + return m_low+Scalar(i)*m_step; + } + template EIGEN_STRONG_INLINE const Packet packetOp(Index) const { return m_base = padd(m_base,m_packetStep); } @@ -564,7 +580,7 @@ struct linspaced_op_impl { typedef typename packet_traits::type Packet; - linspaced_op_impl(Scalar low, Scalar step) : + linspaced_op_impl(const Scalar& low, const Scalar& step) : m_low(low), m_step(step), m_lowPacket(pset1(m_low)), m_stepPacket(pset1(m_step)), m_interPacket(plset(0)) {} @@ -593,7 +609,7 @@ template struct functor_traits< linspaced_o template struct linspaced_op { typedef typename packet_traits::type Packet; - linspaced_op(Scalar low, Scalar high, int 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)/(num_steps-1))) {} template EIGEN_STRONG_INLINE const Scalar operator() (Index i) const { return impl(i); } @@ -632,12 +648,14 @@ template struct linspaced_op template struct functor_has_linear_access { enum { ret = 1 }; }; template struct functor_has_linear_access > { enum { ret = 0 }; }; -// in CwiseBinaryOp, we require the Lhs and Rhs to have the same scalar type, except for multiplication -// where we only require them to have the same _real_ scalar type so one may multiply, say, float by complex. +// In Eigen, any binary op (Product, CwiseBinaryOp) require the Lhs and Rhs to have the same scalar type, except for multiplication +// where the mixing of different types is handled by scalar_product_traits +// In particular, real * complex is allowed. // FIXME move this to functor_traits adding a functor_default -template struct functor_allows_mixing_real_and_complex { enum { ret = 0 }; }; -template struct functor_allows_mixing_real_and_complex > { enum { ret = 1 }; }; -template struct functor_allows_mixing_real_and_complex > { enum { ret = 1 }; }; +template struct functor_is_product_like { enum { ret = 0 }; }; +template struct functor_is_product_like > { enum { ret = 1 }; }; +template struct functor_is_product_like > { enum { ret = 1 }; }; +template struct functor_is_product_like > { enum { ret = 1 }; }; /** \internal @@ -666,7 +684,7 @@ struct functor_traits > */ template struct scalar_sqrt_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_sqrt_op) - inline const Scalar operator() (const Scalar& a) const { return internal::sqrt(a); } + inline const Scalar operator() (const Scalar& a) const { using std::sqrt; return sqrt(a); } typedef typename packet_traits::type Packet; inline Packet packetOp(const Packet& a) const { return internal::psqrt(a); } }; @@ -684,7 +702,7 @@ struct functor_traits > */ template struct scalar_cos_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_cos_op) - inline Scalar operator() (const Scalar& a) const { return internal::cos(a); } + inline Scalar operator() (const Scalar& a) const { using std::cos; return cos(a); } typedef typename packet_traits::type Packet; inline Packet packetOp(const Packet& a) const { return internal::pcos(a); } }; @@ -703,7 +721,7 @@ struct functor_traits > */ template struct scalar_sin_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_sin_op) - inline const Scalar operator() (const Scalar& a) const { return internal::sin(a); } + inline const Scalar operator() (const Scalar& a) const { using std::sin; return sin(a); } typedef typename packet_traits::type Packet; inline Packet packetOp(const Packet& a) const { return internal::psin(a); } }; @@ -723,7 +741,7 @@ struct functor_traits > */ template struct scalar_tan_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_tan_op) - inline const Scalar operator() (const Scalar& a) const { return internal::tan(a); } + inline const Scalar operator() (const Scalar& a) const { using std::tan; return tan(a); } typedef typename packet_traits::type Packet; inline Packet packetOp(const Packet& a) const { return internal::ptan(a); } }; @@ -742,7 +760,7 @@ struct functor_traits > */ template struct scalar_acos_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_acos_op) - inline const Scalar operator() (const Scalar& a) const { return internal::acos(a); } + inline const Scalar operator() (const Scalar& a) const { using std::acos; return acos(a); } typedef typename packet_traits::type Packet; inline Packet packetOp(const Packet& a) const { return internal::pacos(a); } }; @@ -761,7 +779,7 @@ struct functor_traits > */ template struct scalar_asin_op { EIGEN_EMPTY_STRUCT_CTOR(scalar_asin_op) - inline const Scalar operator() (const Scalar& a) const { return internal::asin(a); } + inline const Scalar operator() (const Scalar& a) const { using std::asin; return asin(a); } typedef typename packet_traits::type Packet; inline Packet packetOp(const Packet& a) const { return internal::pasin(a); } }; @@ -783,7 +801,7 @@ struct scalar_pow_op { // FIXME default copy constructors seems bugged with std::complex<> inline scalar_pow_op(const scalar_pow_op& other) : m_exponent(other.m_exponent) { } inline scalar_pow_op(const Scalar& exponent) : m_exponent(exponent) {} - inline Scalar operator() (const Scalar& a) const { return internal::pow(a, m_exponent); } + inline Scalar operator() (const Scalar& a) const { return numext::pow(a, m_exponent); } const Scalar m_exponent; }; template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Fuzzy.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Fuzzy.h index d74edcfdb..fe63bd298 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Fuzzy.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Fuzzy.h @@ -19,7 +19,7 @@ namespace internal template::IsInteger> struct isApprox_selector { - static bool run(const Derived& x, const OtherDerived& y, typename Derived::RealScalar prec) + static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar& prec) { using std::min; typename internal::nested::type nested(x); @@ -31,7 +31,7 @@ struct isApprox_selector template struct isApprox_selector { - static bool run(const Derived& x, const OtherDerived& y, typename Derived::RealScalar) + static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar&) { return x.matrix() == y.matrix(); } @@ -40,16 +40,16 @@ struct isApprox_selector template::IsInteger> struct isMuchSmallerThan_object_selector { - static bool run(const Derived& x, const OtherDerived& y, typename Derived::RealScalar prec) + static bool run(const Derived& x, const OtherDerived& y, const typename Derived::RealScalar& prec) { - return x.cwiseAbs2().sum() <= abs2(prec) * y.cwiseAbs2().sum(); + return x.cwiseAbs2().sum() <= numext::abs2(prec) * y.cwiseAbs2().sum(); } }; template struct isMuchSmallerThan_object_selector { - static bool run(const Derived& x, const OtherDerived&, typename Derived::RealScalar) + static bool run(const Derived& x, const OtherDerived&, const typename Derived::RealScalar&) { return x.matrix() == Derived::Zero(x.rows(), x.cols()).matrix(); } @@ -58,16 +58,16 @@ struct isMuchSmallerThan_object_selector template::IsInteger> struct isMuchSmallerThan_scalar_selector { - static bool run(const Derived& x, const typename Derived::RealScalar& y, typename Derived::RealScalar prec) + static bool run(const Derived& x, const typename Derived::RealScalar& y, const typename Derived::RealScalar& prec) { - return x.cwiseAbs2().sum() <= abs2(prec * y); + return x.cwiseAbs2().sum() <= numext::abs2(prec * y); } }; template struct isMuchSmallerThan_scalar_selector { - static bool run(const Derived& x, const typename Derived::RealScalar&, typename Derived::RealScalar) + static bool run(const Derived& x, const typename Derived::RealScalar&, const typename Derived::RealScalar&) { return x.matrix() == Derived::Zero(x.rows(), x.cols()).matrix(); } @@ -97,7 +97,7 @@ template template bool DenseBase::isApprox( const DenseBase& other, - RealScalar prec + const RealScalar& prec ) const { return internal::isApprox_selector::run(derived(), other.derived(), prec); @@ -119,7 +119,7 @@ bool DenseBase::isApprox( template bool DenseBase::isMuchSmallerThan( const typename NumTraits::Real& other, - RealScalar prec + const RealScalar& prec ) const { return internal::isMuchSmallerThan_scalar_selector::run(derived(), other, prec); @@ -139,7 +139,7 @@ template template bool DenseBase::isMuchSmallerThan( const DenseBase& other, - RealScalar prec + const RealScalar& prec ) const { return internal::isMuchSmallerThan_object_selector::run(derived(), other.derived(), prec); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h index bfc2a67b1..2a59d9464 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h @@ -222,7 +222,29 @@ class GeneralProduct ***********************************************************************/ namespace internal { -template struct outer_product_selector; + +// Column major +template +EIGEN_DONT_INLINE void outer_product_selector_run(const ProductType& prod, Dest& dest, const Func& func, const false_type&) +{ + typedef typename Dest::Index Index; + // FIXME make sure lhs is sequentially stored + // FIXME not very good if rhs is real and lhs complex while alpha is real too + const Index cols = dest.cols(); + for (Index j=0; j +EIGEN_DONT_INLINE void outer_product_selector_run(const ProductType& prod, Dest& dest, const Func& func, const true_type&) { + typedef typename Dest::Index Index; + // FIXME make sure rhs is sequentially stored + // FIXME not very good if lhs is real and rhs complex while alpha is real too + const Index rows = dest.rows(); + for (Index i=0; i struct traits > @@ -235,6 +257,8 @@ template class GeneralProduct : public ProductBase, Lhs, Rhs> { + template struct IsRowMajor : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::type {}; + public: EIGEN_PRODUCT_PUBLIC_INTERFACE(GeneralProduct) @@ -243,41 +267,39 @@ class GeneralProduct EIGEN_STATIC_ASSERT((internal::is_same::value), YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) } + + struct set { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() = src; } }; + struct add { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() += src; } }; + struct sub { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() -= src; } }; + struct adds { + Scalar m_scale; + adds(const Scalar& s) : m_scale(s) {} + template void operator()(const Dst& dst, const Src& src) const { + dst.const_cast_derived() += m_scale * src; + } + }; + + template + inline void evalTo(Dest& dest) const { + internal::outer_product_selector_run(*this, dest, set(), IsRowMajor()); + } + + template + inline void addTo(Dest& dest) const { + internal::outer_product_selector_run(*this, dest, add(), IsRowMajor()); + } - template void scaleAndAddTo(Dest& dest, Scalar alpha) const + template + inline void subTo(Dest& dest) const { + internal::outer_product_selector_run(*this, dest, sub(), IsRowMajor()); + } + + template void scaleAndAddTo(Dest& dest, const Scalar& alpha) const { - internal::outer_product_selector<(int(Dest::Flags)&RowMajorBit) ? RowMajor : ColMajor>::run(*this, dest, alpha); + internal::outer_product_selector_run(*this, dest, adds(alpha), IsRowMajor()); } }; -namespace internal { - -template<> struct outer_product_selector { - template - static EIGEN_DONT_INLINE void run(const ProductType& prod, Dest& dest, typename ProductType::Scalar alpha) { - typedef typename Dest::Index Index; - // FIXME make sure lhs is sequentially stored - // FIXME not very good if rhs is real and lhs complex while alpha is real too - const Index cols = dest.cols(); - for (Index j=0; j struct outer_product_selector { - template - static EIGEN_DONT_INLINE void run(const ProductType& prod, Dest& dest, typename ProductType::Scalar alpha) { - typedef typename Dest::Index Index; - // FIXME make sure rhs is sequentially stored - // FIXME not very good if lhs is real and rhs complex while alpha is real too - const Index rows = dest.rows(); - for (Index i=0; i typedef typename Lhs::Scalar LhsScalar; typedef typename Rhs::Scalar RhsScalar; - GeneralProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) + GeneralProduct(const Lhs& a_lhs, const Rhs& a_rhs) : Base(a_lhs,a_rhs) { // EIGEN_STATIC_ASSERT((internal::is_same::value), // YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) @@ -320,7 +342,7 @@ class GeneralProduct enum { Side = Lhs::IsVectorAtCompileTime ? OnTheLeft : OnTheRight }; typedef typename internal::conditional::type MatrixType; - template void scaleAndAddTo(Dest& dst, Scalar alpha) const + template void scaleAndAddTo(Dest& dst, const Scalar& alpha) const { eigen_assert(m_lhs.rows() == dst.rows() && m_rhs.cols() == dst.cols()); internal::gemv_selector struct gemv_selector { template - static void run(const ProductType& prod, Dest& dest, typename ProductType::Scalar alpha) + static void run(const ProductType& prod, Dest& dest, const typename ProductType::Scalar& alpha) { Transpose destT(dest); enum { OtherStorageOrder = StorageOrder == RowMajor ? ColMajor : RowMajor }; @@ -384,7 +406,7 @@ struct gemv_static_vector_if template<> struct gemv_selector { template - static inline void run(const ProductType& prod, Dest& dest, typename ProductType::Scalar alpha) + static inline void run(const ProductType& prod, Dest& dest, const typename ProductType::Scalar& alpha) { typedef typename ProductType::Index Index; typedef typename ProductType::LhsScalar LhsScalar; @@ -413,7 +435,7 @@ template<> struct gemv_selector gemv_static_vector_if static_dest; - bool alphaIsCompatible = (!ComplexByReal) || (imag(actualAlpha)==RealScalar(0)); + bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0)); bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible; RhsScalar compatibleAlpha = get_factor::run(actualAlpha); @@ -457,7 +479,7 @@ template<> struct gemv_selector template<> struct gemv_selector { template - static void run(const ProductType& prod, Dest& dest, typename ProductType::Scalar alpha) + static void run(const ProductType& prod, Dest& dest, const typename ProductType::Scalar& alpha) { typedef typename ProductType::LhsScalar LhsScalar; typedef typename ProductType::RhsScalar RhsScalar; @@ -508,7 +530,7 @@ template<> struct gemv_selector template<> struct gemv_selector { template - static void run(const ProductType& prod, Dest& dest, typename ProductType::Scalar alpha) + static void run(const ProductType& prod, Dest& dest, const typename ProductType::Scalar& alpha) { typedef typename Dest::Index Index; // TODO makes sure dest is sequentially stored in memory, otherwise use a temp @@ -521,7 +543,7 @@ template<> struct gemv_selector template<> struct gemv_selector { template - static void run(const ProductType& prod, Dest& dest, typename ProductType::Scalar alpha) + static void run(const ProductType& prod, Dest& dest, const typename ProductType::Scalar& alpha) { typedef typename Dest::Index Index; // TODO makes sure rhs is sequentially stored in memory, otherwise use a temp diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h index 858fb243e..5f783ebee 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h @@ -106,7 +106,7 @@ pnegate(const Packet& a) { return -a; } /** \internal \returns conj(a) (coeff-wise) */ template inline Packet -pconj(const Packet& a) { return conj(a); } +pconj(const Packet& a) { return numext::conj(a); } /** \internal \returns a * b (coeff-wise) */ template inline Packet @@ -130,7 +130,7 @@ pmax(const Packet& a, /** \internal \returns the absolute value of \a a */ template inline Packet -pabs(const Packet& a) { return abs(a); } +pabs(const Packet& a) { using std::abs; return abs(a); } /** \internal \returns the bitwise and of \a a and \a b */ template inline Packet @@ -156,7 +156,11 @@ pload(const typename unpacket_traits::type* from) { return *from; } template inline Packet ploadu(const typename unpacket_traits::type* from) { return *from; } -/** \internal \returns a packet with elements of \a *from duplicated, e.g.: (from[0],from[0],from[1],from[1]) */ +/** \internal \returns a packet with elements of \a *from duplicated. + * For instance, for a packet of 8 elements, 4 scalar will be read from \a *from and + * duplicated to form: {from[0],from[0],from[1],from[1],,from[2],from[2],,from[3],from[3]} + * Currently, this function is only used for scalar * complex products. + */ template inline Packet ploaddup(const typename unpacket_traits::type* from) { return *from; } @@ -215,7 +219,12 @@ template inline Packet preverse(const Packet& a) /** \internal \returns \a a with real and imaginary part flipped (for complex type only) */ template inline Packet pcplxflip(const Packet& a) -{ return Packet(imag(a),real(a)); } +{ + // FIXME: uncomment the following in case we drop the internal imag and real functions. +// using std::imag; +// using std::real; + return Packet(imag(a),real(a)); +} /************************** * Special math functions @@ -223,35 +232,35 @@ template inline Packet pcplxflip(const Packet& a) /** \internal \returns the sine of \a a (coeff-wise) */ template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS -Packet psin(const Packet& a) { return sin(a); } +Packet psin(const Packet& a) { using std::sin; return sin(a); } /** \internal \returns the cosine of \a a (coeff-wise) */ template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS -Packet pcos(const Packet& a) { return cos(a); } +Packet pcos(const Packet& a) { using std::cos; return cos(a); } /** \internal \returns the tan of \a a (coeff-wise) */ template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS -Packet ptan(const Packet& a) { return tan(a); } +Packet ptan(const Packet& a) { using std::tan; return tan(a); } /** \internal \returns the arc sine of \a a (coeff-wise) */ template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS -Packet pasin(const Packet& a) { return asin(a); } +Packet pasin(const Packet& a) { using std::asin; return asin(a); } /** \internal \returns the arc cosine of \a a (coeff-wise) */ template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS -Packet pacos(const Packet& a) { return acos(a); } +Packet pacos(const Packet& a) { using std::acos; return acos(a); } /** \internal \returns the exp of \a a (coeff-wise) */ template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS -Packet pexp(const Packet& a) { return exp(a); } +Packet pexp(const Packet& a) { using std::exp; return exp(a); } /** \internal \returns the log of \a a (coeff-wise) */ template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS -Packet plog(const Packet& a) { return log(a); } +Packet plog(const Packet& a) { using std::log; return log(a); } /** \internal \returns the square-root of \a a (coeff-wise) */ template EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS -Packet psqrt(const Packet& a) { return sqrt(a); } +Packet psqrt(const Packet& a) { using std::sqrt; return sqrt(a); } /*************************************************************************** * The following functions might not have to be overwritten for vectorized types @@ -302,8 +311,21 @@ struct palign_impl static inline void run(PacketType&, const PacketType&) {} }; -/** \internal update \a first using the concatenation of the \a Offset last elements - * of \a first and packet_size minus \a Offset first elements of \a second */ +/** \internal update \a first using the concatenation of the packet_size minus \a Offset last elements + * of \a first and \a Offset first elements of \a second. + * + * This function is currently only used to optimize matrix-vector products on unligned matrices. + * It takes 2 packets that represent a contiguous memory array, and returns a packet starting + * at the position \a Offset. For instance, for packets of 4 elements, we have: + * Input: + * - first = {f0,f1,f2,f3} + * - second = {s0,s1,s2,s3} + * Output: + * - if Offset==0 then {f0,f1,f2,f3} + * - if Offset==1 then {f1,f2,f3,s0} + * - if Offset==2 then {f2,f3,s0,s1} + * - if Offset==3 then {f3,s0,s1,s3} + */ template inline void palign(PacketType& first, const PacketType& second) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h index e63726c47..2acf97723 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h @@ -1,7 +1,7 @@ // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // -// Copyright (C) 2010 Gael Guennebaud +// Copyright (C) 2010-2012 Gael Guennebaud // Copyright (C) 2010 Benoit Jacob // // This Source Code Form is subject to the terms of the Mozilla @@ -11,7 +11,7 @@ #ifndef EIGEN_GLOBAL_FUNCTIONS_H #define EIGEN_GLOBAL_FUNCTIONS_H -#define EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(NAME,FUNCTOR) \ +#define EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(NAME,FUNCTOR) \ template \ inline const Eigen::CwiseUnaryOp, const Derived> \ NAME(const Eigen::ArrayBase& x) { \ @@ -35,20 +35,21 @@ }; -namespace std +namespace Eigen { - EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(real,scalar_real_op) - EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(imag,scalar_imag_op) - EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(sin,scalar_sin_op) - EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(cos,scalar_cos_op) - EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(asin,scalar_asin_op) - EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(acos,scalar_acos_op) - EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(tan,scalar_tan_op) - EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(exp,scalar_exp_op) - EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(log,scalar_log_op) - EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(abs,scalar_abs_op) - EIGEN_ARRAY_DECLARE_GLOBAL_STD_UNARY(sqrt,scalar_sqrt_op) - + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(real,scalar_real_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(imag,scalar_imag_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(conj,scalar_conjugate_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sin,scalar_sin_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cos,scalar_cos_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(asin,scalar_asin_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(acos,scalar_acos_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(tan,scalar_tan_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(exp,scalar_exp_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log,scalar_log_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs,scalar_abs_op) + EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sqrt,scalar_sqrt_op) + template inline const Eigen::CwiseUnaryOp, const Derived> pow(const Eigen::ArrayBase& x, const typename Derived::Scalar& exponent) { @@ -64,16 +65,13 @@ namespace std exponents.derived() ); } -} - -namespace Eigen -{ + /** * \brief Component-wise division of a scalar by array elements. **/ template inline const Eigen::CwiseUnaryOp, const Derived> - operator/(typename Derived::Scalar s, const Eigen::ArrayBase& a) + operator/(const typename Derived::Scalar& s, const Eigen::ArrayBase& a) { return Eigen::CwiseUnaryOp, const Derived>( a.derived(), @@ -85,19 +83,10 @@ namespace Eigen { EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(real,scalar_real_op) EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(imag,scalar_imag_op) - EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(sin,scalar_sin_op) - EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(cos,scalar_cos_op) - EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(asin,scalar_asin_op) - EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(acos,scalar_acos_op) - EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(tan,scalar_tan_op) - EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(exp,scalar_exp_op) - EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(log,scalar_log_op) - EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(abs,scalar_abs_op) EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(abs2,scalar_abs2_op) - EIGEN_ARRAY_DECLARE_GLOBAL_EIGEN_UNARY(sqrt,scalar_sqrt_op) } } -// TODO: cleanly disable those functions that are not supported on Array (internal::real_ref, internal::random, internal::isApprox...) +// TODO: cleanly disable those functions that are not supported on Array (numext::real_ref, internal::random, internal::isApprox...) #endif // EIGEN_GLOBAL_FUNCTIONS_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h index cc8e18a00..c8d5f6379 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h @@ -55,9 +55,8 @@ struct IOFormat const std::string& _rowSeparator = "\n", const std::string& _rowPrefix="", const std::string& _rowSuffix="", const std::string& _matPrefix="", const std::string& _matSuffix="") : matPrefix(_matPrefix), matSuffix(_matSuffix), rowPrefix(_rowPrefix), rowSuffix(_rowSuffix), rowSeparator(_rowSeparator), - coeffSeparator(_coeffSeparator), precision(_precision), flags(_flags) + rowSpacer(""), coeffSeparator(_coeffSeparator), precision(_precision), flags(_flags) { - rowSpacer = ""; int i = int(matSuffix.length())-1; while (i>=0 && matSuffix[i]!='\n') { @@ -129,6 +128,7 @@ struct significant_decimals_default_impl static inline int run() { using std::ceil; + using std::log; return cast(ceil(-log(NumTraits::epsilon())/log(RealScalar(10)))); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h index 15a19226e..f804c89d6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h @@ -133,36 +133,36 @@ template class Ma /** Constructor in the fixed-size case. * - * \param data pointer to the array to map - * \param stride optional Stride object, passing the strides. + * \param dataPtr pointer to the array to map + * \param a_stride optional Stride object, passing the strides. */ - inline Map(PointerArgType data, const StrideType& stride = StrideType()) - : Base(cast_to_pointer_type(data)), m_stride(stride) + inline Map(PointerArgType dataPtr, const StrideType& a_stride = StrideType()) + : Base(cast_to_pointer_type(dataPtr)), m_stride(a_stride) { PlainObjectType::Base::_check_template_params(); } /** Constructor in the dynamic-size vector case. * - * \param data pointer to the array to map - * \param size the size of the vector expression - * \param stride optional Stride object, passing the strides. + * \param dataPtr pointer to the array to map + * \param a_size the size of the vector expression + * \param a_stride optional Stride object, passing the strides. */ - inline Map(PointerArgType data, Index size, const StrideType& stride = StrideType()) - : Base(cast_to_pointer_type(data), size), m_stride(stride) + inline Map(PointerArgType dataPtr, Index a_size, const StrideType& a_stride = StrideType()) + : Base(cast_to_pointer_type(dataPtr), a_size), m_stride(a_stride) { PlainObjectType::Base::_check_template_params(); } /** Constructor in the dynamic-size matrix case. * - * \param data pointer to the array to map - * \param rows the number of rows of the matrix expression - * \param cols the number of columns of the matrix expression - * \param stride optional Stride object, passing the strides. + * \param dataPtr pointer to the array to map + * \param nbRows the number of rows of the matrix expression + * \param nbCols the number of columns of the matrix expression + * \param a_stride optional Stride object, passing the strides. */ - inline Map(PointerArgType data, Index rows, Index cols, const StrideType& stride = StrideType()) - : Base(cast_to_pointer_type(data), rows, cols), m_stride(stride) + inline Map(PointerArgType dataPtr, Index nbRows, Index nbCols, const StrideType& a_stride = StrideType()) + : Base(cast_to_pointer_type(dataPtr), nbRows, nbCols), m_stride(a_stride) { PlainObjectType::Base::_check_template_params(); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index a388d61ea..6876de588 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -87,9 +87,9 @@ template class MapBase */ inline const Scalar* data() const { return m_data; } - inline const Scalar& coeff(Index row, Index col) const + inline const Scalar& coeff(Index rowId, Index colId) const { - return m_data[col * colStride() + row * rowStride()]; + return m_data[colId * colStride() + rowId * rowStride()]; } inline const Scalar& coeff(Index index) const @@ -98,9 +98,9 @@ template class MapBase return m_data[index * innerStride()]; } - inline const Scalar& coeffRef(Index row, Index col) const + inline const Scalar& coeffRef(Index rowId, Index colId) const { - return this->m_data[col * colStride() + row * rowStride()]; + return this->m_data[colId * colStride() + rowId * rowStride()]; } inline const Scalar& coeffRef(Index index) const @@ -110,10 +110,10 @@ template class MapBase } template - inline PacketScalar packet(Index row, Index col) const + inline PacketScalar packet(Index rowId, Index colId) const { return internal::ploadt - (m_data + (col * colStride() + row * rowStride())); + (m_data + (colId * colStride() + rowId * rowStride())); } template @@ -123,29 +123,29 @@ template class MapBase return internal::ploadt(m_data + index * innerStride()); } - inline MapBase(PointerType data) : m_data(data), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime) + inline MapBase(PointerType dataPtr) : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime) { EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) checkSanity(); } - inline MapBase(PointerType data, Index size) - : m_data(data), - m_rows(RowsAtCompileTime == Dynamic ? size : Index(RowsAtCompileTime)), - m_cols(ColsAtCompileTime == Dynamic ? size : Index(ColsAtCompileTime)) + inline MapBase(PointerType dataPtr, Index vecSize) + : m_data(dataPtr), + m_rows(RowsAtCompileTime == Dynamic ? vecSize : Index(RowsAtCompileTime)), + m_cols(ColsAtCompileTime == Dynamic ? vecSize : Index(ColsAtCompileTime)) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - eigen_assert(size >= 0); - eigen_assert(data == 0 || SizeAtCompileTime == Dynamic || SizeAtCompileTime == size); + eigen_assert(vecSize >= 0); + eigen_assert(dataPtr == 0 || SizeAtCompileTime == Dynamic || SizeAtCompileTime == vecSize); checkSanity(); } - inline MapBase(PointerType data, Index rows, Index cols) - : m_data(data), m_rows(rows), m_cols(cols) + inline MapBase(PointerType dataPtr, Index nbRows, Index nbCols) + : m_data(dataPtr), m_rows(nbRows), m_cols(nbCols) { - eigen_assert( (data == 0) - || ( rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) - && cols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols))); + eigen_assert( (dataPtr == 0) + || ( nbRows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == nbRows) + && nbCols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == nbCols))); checkSanity(); } @@ -210,23 +210,23 @@ template class MapBase } template - inline void writePacket(Index row, Index col, const PacketScalar& x) + inline void writePacket(Index row, Index col, const PacketScalar& val) { internal::pstoret - (this->m_data + (col * colStride() + row * rowStride()), x); + (this->m_data + (col * colStride() + row * rowStride()), val); } template - inline void writePacket(Index index, const PacketScalar& x) + inline void writePacket(Index index, const PacketScalar& val) { EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) internal::pstoret - (this->m_data + index * innerStride(), x); + (this->m_data + index * innerStride(), val); } - explicit inline MapBase(PointerType data) : Base(data) {} - inline MapBase(PointerType data, Index size) : Base(data, size) {} - inline MapBase(PointerType data, Index rows, Index cols) : Base(data, rows, cols) {} + explicit inline MapBase(PointerType dataPtr) : Base(dataPtr) {} + inline MapBase(PointerType dataPtr, Index vecSize) : Base(dataPtr, vecSize) {} + inline MapBase(PointerType dataPtr, Index nbRows, Index nbCols) : Base(dataPtr, nbRows, nbCols) {} Derived& operator=(const MapBase& other) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h index 05e913f2f..2bfc5ebd9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h @@ -51,16 +51,15 @@ struct global_math_functions_filtering_base typedef typename T::Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl type; }; -#define EIGEN_MATHFUNC_IMPL(func, scalar) func##_impl::type> -#define EIGEN_MATHFUNC_RETVAL(func, scalar) typename func##_retval::type>::type - +#define EIGEN_MATHFUNC_IMPL(func, scalar) Eigen::internal::func##_impl::type> +#define EIGEN_MATHFUNC_RETVAL(func, scalar) typename Eigen::internal::func##_retval::type>::type /**************************************************************************** * Implementation of real * ****************************************************************************/ -template -struct real_impl +template::IsComplex> +struct real_default_impl { typedef typename NumTraits::Real RealScalar; static inline RealScalar run(const Scalar& x) @@ -69,34 +68,32 @@ struct real_impl } }; -template -struct real_impl > +template +struct real_default_impl { - static inline RealScalar run(const std::complex& x) + typedef typename NumTraits::Real RealScalar; + static inline RealScalar run(const Scalar& x) { using std::real; return real(x); } }; +template struct real_impl : real_default_impl {}; + template struct real_retval { typedef typename NumTraits::Real type; }; -template -inline EIGEN_MATHFUNC_RETVAL(real, Scalar) real(const Scalar& x) -{ - return EIGEN_MATHFUNC_IMPL(real, Scalar)::run(x); -} /**************************************************************************** * Implementation of imag * ****************************************************************************/ -template -struct imag_impl +template::IsComplex> +struct imag_default_impl { typedef typename NumTraits::Real RealScalar; static inline RealScalar run(const Scalar&) @@ -105,28 +102,25 @@ struct imag_impl } }; -template -struct imag_impl > +template +struct imag_default_impl { - static inline RealScalar run(const std::complex& x) + typedef typename NumTraits::Real RealScalar; + static inline RealScalar run(const Scalar& x) { using std::imag; return imag(x); } }; +template struct imag_impl : imag_default_impl {}; + template struct imag_retval { typedef typename NumTraits::Real type; }; -template -inline EIGEN_MATHFUNC_RETVAL(imag, Scalar) imag(const Scalar& x) -{ - return EIGEN_MATHFUNC_IMPL(imag, Scalar)::run(x); -} - /**************************************************************************** * Implementation of real_ref * ****************************************************************************/ @@ -151,18 +145,6 @@ struct real_ref_retval typedef typename NumTraits::Real & type; }; -template -inline typename add_const_on_value_type< EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) >::type real_ref(const Scalar& x) -{ - return real_ref_impl::run(x); -} - -template -inline EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) real_ref(Scalar& x) -{ - return EIGEN_MATHFUNC_IMPL(real_ref, Scalar)::run(x); -} - /**************************************************************************** * Implementation of imag_ref * ****************************************************************************/ @@ -203,23 +185,11 @@ struct imag_ref_retval typedef typename NumTraits::Real & type; }; -template -inline typename add_const_on_value_type< EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar) >::type imag_ref(const Scalar& x) -{ - return imag_ref_impl::run(x); -} - -template -inline EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar) imag_ref(Scalar& x) -{ - return EIGEN_MATHFUNC_IMPL(imag_ref, Scalar)::run(x); -} - /**************************************************************************** * Implementation of conj * ****************************************************************************/ -template +template::IsComplex> struct conj_impl { static inline Scalar run(const Scalar& x) @@ -228,10 +198,10 @@ struct conj_impl } }; -template -struct conj_impl > +template +struct conj_impl { - static inline std::complex run(const std::complex& x) + static inline Scalar run(const Scalar& x) { using std::conj; return conj(x); @@ -244,39 +214,6 @@ struct conj_retval typedef Scalar type; }; -template -inline EIGEN_MATHFUNC_RETVAL(conj, Scalar) conj(const Scalar& x) -{ - return EIGEN_MATHFUNC_IMPL(conj, Scalar)::run(x); -} - -/**************************************************************************** -* Implementation of abs * -****************************************************************************/ - -template -struct abs_impl -{ - typedef typename NumTraits::Real RealScalar; - static inline RealScalar run(const Scalar& x) - { - using std::abs; - return abs(x); - } -}; - -template -struct abs_retval -{ - typedef typename NumTraits::Real type; -}; - -template -inline EIGEN_MATHFUNC_RETVAL(abs, Scalar) abs(const Scalar& x) -{ - return EIGEN_MATHFUNC_IMPL(abs, Scalar)::run(x); -} - /**************************************************************************** * Implementation of abs2 * ****************************************************************************/ @@ -306,12 +243,6 @@ struct abs2_retval typedef typename NumTraits::Real type; }; -template -inline EIGEN_MATHFUNC_RETVAL(abs2, Scalar) abs2(const Scalar& x) -{ - return EIGEN_MATHFUNC_IMPL(abs2, Scalar)::run(x); -} - /**************************************************************************** * Implementation of norm1 * ****************************************************************************/ @@ -322,6 +253,7 @@ struct norm1_default_impl typedef typename NumTraits::Real RealScalar; static inline RealScalar run(const Scalar& x) { + using std::abs; return abs(real(x)) + abs(imag(x)); } }; @@ -331,6 +263,7 @@ struct norm1_default_impl { static inline Scalar run(const Scalar& x) { + using std::abs; return abs(x); } }; @@ -344,12 +277,6 @@ struct norm1_retval typedef typename NumTraits::Real type; }; -template -inline EIGEN_MATHFUNC_RETVAL(norm1, Scalar) norm1(const Scalar& x) -{ - return EIGEN_MATHFUNC_IMPL(norm1, Scalar)::run(x); -} - /**************************************************************************** * Implementation of hypot * ****************************************************************************/ @@ -362,9 +289,12 @@ struct hypot_impl { using std::max; using std::min; + using std::abs; + using std::sqrt; RealScalar _x = abs(x); RealScalar _y = abs(y); RealScalar p = (max)(_x, _y); + if(p==RealScalar(0)) return 0; RealScalar q = (min)(_x, _y); RealScalar qp = q/p; return p * sqrt(RealScalar(1) + qp*qp); @@ -377,12 +307,6 @@ struct hypot_retval typedef typename NumTraits::Real type; }; -template -inline EIGEN_MATHFUNC_RETVAL(hypot, Scalar) hypot(const Scalar& x, const Scalar& y) -{ - return EIGEN_MATHFUNC_IMPL(hypot, Scalar)::run(x, y); -} - /**************************************************************************** * Implementation of cast * ****************************************************************************/ @@ -405,97 +329,29 @@ inline NewType cast(const OldType& x) } /**************************************************************************** -* Implementation of sqrt * +* Implementation of atanh2 * ****************************************************************************/ template -struct sqrt_default_impl -{ - static inline Scalar run(const Scalar& x) - { - using std::sqrt; - return sqrt(x); - } -}; - -template -struct sqrt_default_impl -{ - static inline Scalar run(const Scalar&) - { -#ifdef EIGEN2_SUPPORT - eigen_assert(!NumTraits::IsInteger); -#else - EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) -#endif - return Scalar(0); - } -}; - -template -struct sqrt_impl : sqrt_default_impl::IsInteger> {}; - -template -struct sqrt_retval -{ - typedef Scalar type; -}; - -template -inline EIGEN_MATHFUNC_RETVAL(sqrt, Scalar) sqrt(const Scalar& x) -{ - return EIGEN_MATHFUNC_IMPL(sqrt, Scalar)::run(x); -} - -/**************************************************************************** -* Implementation of standard unary real functions (exp, log, sin, cos, ... * -****************************************************************************/ - -// This macro instanciate all the necessary template mechanism which is common to all unary real functions. -#define EIGEN_MATHFUNC_STANDARD_REAL_UNARY(NAME) \ - template struct NAME##_default_impl { \ - static inline Scalar run(const Scalar& x) { using std::NAME; return NAME(x); } \ - }; \ - template struct NAME##_default_impl { \ - static inline Scalar run(const Scalar&) { \ - EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar) \ - return Scalar(0); \ - } \ - }; \ - template struct NAME##_impl \ - : NAME##_default_impl::IsInteger> \ - {}; \ - template struct NAME##_retval { typedef Scalar type; }; \ - template \ - inline EIGEN_MATHFUNC_RETVAL(NAME, Scalar) NAME(const Scalar& x) { \ - return EIGEN_MATHFUNC_IMPL(NAME, Scalar)::run(x); \ - } - -EIGEN_MATHFUNC_STANDARD_REAL_UNARY(exp) -EIGEN_MATHFUNC_STANDARD_REAL_UNARY(log) -EIGEN_MATHFUNC_STANDARD_REAL_UNARY(sin) -EIGEN_MATHFUNC_STANDARD_REAL_UNARY(cos) -EIGEN_MATHFUNC_STANDARD_REAL_UNARY(tan) -EIGEN_MATHFUNC_STANDARD_REAL_UNARY(asin) -EIGEN_MATHFUNC_STANDARD_REAL_UNARY(acos) - -/**************************************************************************** -* Implementation of atan2 * -****************************************************************************/ - -template -struct atan2_default_impl +struct atanh2_default_impl { typedef Scalar retval; + typedef typename NumTraits::Real RealScalar; static inline Scalar run(const Scalar& x, const Scalar& y) { - using std::atan2; - return atan2(x, y); + using std::abs; + using std::log; + using std::sqrt; + Scalar z = x / y; + if (y == Scalar(0) || abs(z) > sqrt(NumTraits::epsilon())) + return RealScalar(0.5) * log((y + x) / (y - x)); + else + return z + z*z*z / RealScalar(3); } }; template -struct atan2_default_impl +struct atanh2_default_impl { static inline Scalar run(const Scalar&, const Scalar&) { @@ -505,20 +361,14 @@ struct atan2_default_impl }; template -struct atan2_impl : atan2_default_impl::IsInteger> {}; +struct atanh2_impl : atanh2_default_impl::IsInteger> {}; template -struct atan2_retval +struct atanh2_retval { typedef Scalar type; }; -template -inline EIGEN_MATHFUNC_RETVAL(atan2, Scalar) atan2(const Scalar& x, const Scalar& y) -{ - return EIGEN_MATHFUNC_IMPL(atan2, Scalar)::run(x, y); -} - /**************************************************************************** * Implementation of pow * ****************************************************************************/ @@ -562,12 +412,6 @@ struct pow_retval typedef Scalar type; }; -template -inline EIGEN_MATHFUNC_RETVAL(pow, Scalar) pow(const Scalar& x, const Scalar& y) -{ - return EIGEN_MATHFUNC_IMPL(pow, Scalar)::run(x, y); -} - /**************************************************************************** * Implementation of random * ****************************************************************************/ @@ -666,11 +510,10 @@ struct random_default_impl #else enum { rand_bits = floor_log2<(unsigned int)(RAND_MAX)+1>::value, scalar_bits = sizeof(Scalar) * CHAR_BIT, - shift = EIGEN_PLAIN_ENUM_MAX(0, int(rand_bits) - int(scalar_bits)) + shift = EIGEN_PLAIN_ENUM_MAX(0, int(rand_bits) - int(scalar_bits)), + offset = NumTraits::IsSigned ? (1 << (EIGEN_PLAIN_ENUM_MIN(rand_bits,scalar_bits)-1)) : 0 }; - Scalar x = Scalar(std::rand() >> shift); - Scalar offset = NumTraits::IsSigned ? Scalar(1 << (rand_bits-1)) : Scalar(0); - return x - offset; + return Scalar((std::rand() >> shift) - offset); #endif } }; @@ -702,6 +545,97 @@ inline EIGEN_MATHFUNC_RETVAL(random, Scalar) random() return EIGEN_MATHFUNC_IMPL(random, Scalar)::run(); } +} // end namespace internal + +/**************************************************************************** +* Generic math function * +****************************************************************************/ + +namespace numext { + +template +inline EIGEN_MATHFUNC_RETVAL(real, Scalar) real(const Scalar& x) +{ + return EIGEN_MATHFUNC_IMPL(real, Scalar)::run(x); +} + +template +inline typename internal::add_const_on_value_type< EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) >::type real_ref(const Scalar& x) +{ + return internal::real_ref_impl::run(x); +} + +template +inline EIGEN_MATHFUNC_RETVAL(real_ref, Scalar) real_ref(Scalar& x) +{ + return EIGEN_MATHFUNC_IMPL(real_ref, Scalar)::run(x); +} + +template +inline EIGEN_MATHFUNC_RETVAL(imag, Scalar) imag(const Scalar& x) +{ + return EIGEN_MATHFUNC_IMPL(imag, Scalar)::run(x); +} + +template +inline typename internal::add_const_on_value_type< EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar) >::type imag_ref(const Scalar& x) +{ + return internal::imag_ref_impl::run(x); +} + +template +inline EIGEN_MATHFUNC_RETVAL(imag_ref, Scalar) imag_ref(Scalar& x) +{ + return EIGEN_MATHFUNC_IMPL(imag_ref, Scalar)::run(x); +} + +template +inline EIGEN_MATHFUNC_RETVAL(conj, Scalar) conj(const Scalar& x) +{ + return EIGEN_MATHFUNC_IMPL(conj, Scalar)::run(x); +} + +template +inline EIGEN_MATHFUNC_RETVAL(abs2, Scalar) abs2(const Scalar& x) +{ + return EIGEN_MATHFUNC_IMPL(abs2, Scalar)::run(x); +} + +template +inline EIGEN_MATHFUNC_RETVAL(norm1, Scalar) norm1(const Scalar& x) +{ + return EIGEN_MATHFUNC_IMPL(norm1, Scalar)::run(x); +} + +template +inline EIGEN_MATHFUNC_RETVAL(hypot, Scalar) hypot(const Scalar& x, const Scalar& y) +{ + return EIGEN_MATHFUNC_IMPL(hypot, Scalar)::run(x, y); +} + +template +inline EIGEN_MATHFUNC_RETVAL(atanh2, Scalar) atanh2(const Scalar& x, const Scalar& y) +{ + return EIGEN_MATHFUNC_IMPL(atanh2, Scalar)::run(x, y); +} + +template +inline EIGEN_MATHFUNC_RETVAL(pow, Scalar) pow(const Scalar& x, const Scalar& y) +{ + return EIGEN_MATHFUNC_IMPL(pow, Scalar)::run(x, y); +} + +// std::isfinite is non standard, so let's define our own version, +// even though it is not very efficient. +template bool (isfinite)(const T& x) +{ + return x::highest() && x>NumTraits::lowest(); +} + +} // end namespace numext + +namespace internal { + /**************************************************************************** * Implementation of fuzzy comparisons * ****************************************************************************/ @@ -718,11 +652,13 @@ struct scalar_fuzzy_default_impl template static inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y, const RealScalar& prec) { + using std::abs; return abs(x) <= abs(y) * prec; } static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar& prec) { using std::min; + using std::abs; return abs(x - y) <= (min)(abs(x), abs(y)) * prec; } static inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y, const RealScalar& prec) @@ -757,12 +693,12 @@ struct scalar_fuzzy_default_impl template static inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y, const RealScalar& prec) { - return abs2(x) <= abs2(y) * prec * prec; + return numext::abs2(x) <= numext::abs2(y) * prec * prec; } static inline bool isApprox(const Scalar& x, const Scalar& y, const RealScalar& prec) { using std::min; - return abs2(x - y) <= (min)(abs2(x), abs2(y)) * prec * prec; + return numext::abs2(x - y) <= (min)(numext::abs2(x), numext::abs2(y)) * prec * prec; } }; @@ -824,17 +760,7 @@ template<> struct scalar_fuzzy_impl }; -/**************************************************************************** -* Special functions * -****************************************************************************/ - -// std::isfinite is non standard, so let's define our own version, -// even though it is not very efficient. -template bool (isfinite)(const T& x) -{ - return x::highest() && x>NumTraits::lowest(); -} - + } // end namespace internal } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h index 99160b591..0ba5d90cc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h @@ -200,16 +200,16 @@ class Matrix * * \sa resize(Index,Index) */ - EIGEN_STRONG_INLINE explicit Matrix() : Base() + EIGEN_STRONG_INLINE Matrix() : Base() { Base::_check_template_params(); - EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED + EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } // FIXME is it still needed Matrix(internal::constructor_without_unaligned_array_assert) : Base(internal::constructor_without_unaligned_array_assert()) - { Base::_check_template_params(); EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED } + { Base::_check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } /** \brief Constructs a vector or row-vector with given dimension. \only_for_vectors * @@ -224,7 +224,7 @@ class Matrix EIGEN_STATIC_ASSERT_VECTOR_ONLY(Matrix) eigen_assert(dim >= 0); eigen_assert(SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim); - EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED + EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } #ifndef EIGEN_PARSED_BY_DOXYGEN diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h index 36ea2cee8..9193b6abb 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h @@ -162,6 +162,9 @@ template class MatrixBase #ifndef EIGEN_PARSED_BY_DOXYGEN template Derived& lazyAssign(const ProductBase& other); + + template + Derived& lazyAssign(const MatrixPowerProduct& other); #endif // not EIGEN_PARSED_BY_DOXYGEN template @@ -212,8 +215,8 @@ template class MatrixBase typedef Diagonal DiagonalReturnType; DiagonalReturnType diagonal(); - typedef const Diagonal ConstDiagonalReturnType; - const ConstDiagonalReturnType diagonal() const; + typedef typename internal::add_const >::type ConstDiagonalReturnType; + ConstDiagonalReturnType diagonal() const; template struct DiagonalIndexReturnType { typedef Diagonal Type; }; template struct ConstDiagonalIndexReturnType { typedef const Diagonal Type; }; @@ -224,11 +227,11 @@ template class MatrixBase // Note: The "MatrixBase::" prefixes are added to help MSVC9 to match these declarations with the later implementations. // On the other hand they confuse MSVC8... #if (defined _MSC_VER) && (_MSC_VER >= 1500) // 2008 or later - typename MatrixBase::template DiagonalIndexReturnType::Type diagonal(Index index); - typename MatrixBase::template ConstDiagonalIndexReturnType::Type diagonal(Index index) const; + typename MatrixBase::template DiagonalIndexReturnType::Type diagonal(Index index); + typename MatrixBase::template ConstDiagonalIndexReturnType::Type diagonal(Index index) const; #else - typename DiagonalIndexReturnType::Type diagonal(Index index); - typename ConstDiagonalIndexReturnType::Type diagonal(Index index) const; + typename DiagonalIndexReturnType::Type diagonal(Index index); + typename ConstDiagonalIndexReturnType::Type diagonal(Index index) const; #endif #ifdef EIGEN2_SUPPORT @@ -255,7 +258,7 @@ template class MatrixBase template typename ConstSelfAdjointViewReturnType::Type selfadjointView() const; const SparseView sparseView(const Scalar& m_reference = Scalar(0), - typename NumTraits::Real m_epsilon = NumTraits::dummy_precision()) const; + const typename NumTraits::Real& m_epsilon = NumTraits::dummy_precision()) const; static const IdentityReturnType Identity(); static const IdentityReturnType Identity(Index rows, Index cols); static const BasisReturnType Unit(Index size, Index i); @@ -271,16 +274,16 @@ template class MatrixBase Derived& setIdentity(); Derived& setIdentity(Index rows, Index cols); - bool isIdentity(RealScalar prec = NumTraits::dummy_precision()) const; - bool isDiagonal(RealScalar prec = NumTraits::dummy_precision()) const; + bool isIdentity(const RealScalar& prec = NumTraits::dummy_precision()) const; + bool isDiagonal(const RealScalar& prec = NumTraits::dummy_precision()) const; - bool isUpperTriangular(RealScalar prec = NumTraits::dummy_precision()) const; - bool isLowerTriangular(RealScalar prec = NumTraits::dummy_precision()) const; + bool isUpperTriangular(const RealScalar& prec = NumTraits::dummy_precision()) const; + bool isLowerTriangular(const RealScalar& prec = NumTraits::dummy_precision()) const; template bool isOrthogonal(const MatrixBase& other, - RealScalar prec = NumTraits::dummy_precision()) const; - bool isUnitary(RealScalar prec = NumTraits::dummy_precision()) const; + const RealScalar& prec = NumTraits::dummy_precision()) const; + bool isUnitary(const RealScalar& prec = NumTraits::dummy_precision()) const; /** \returns true if each coefficients of \c *this and \a other are all exactly equal. * \warning When using floating point scalar values you probably should rather use a @@ -314,7 +317,7 @@ template class MatrixBase MatrixBase& matrix() { return *this; } const MatrixBase& matrix() const { return *this; } - /** \returns an \link ArrayBase Array \endlink expression of this matrix + /** \returns an \link Eigen::ArrayBase Array \endlink expression of this matrix * \sa ArrayBase::matrix() */ ArrayWrapper array() { return derived(); } const ArrayWrapper array() const { return derived(); } @@ -454,6 +457,7 @@ template class MatrixBase const MatrixFunctionReturnValue sin() const; const MatrixSquareRootReturnValue sqrt() const; const MatrixLogarithmReturnValue log() const; + const MatrixPowerReturnValue pow(const RealScalar& p) const; #ifdef EIGEN2_SUPPORT template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/NoAlias.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/NoAlias.h index ecb3fa285..768bfb18c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/NoAlias.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/NoAlias.h @@ -80,8 +80,17 @@ class NoAlias template EIGEN_STRONG_INLINE ExpressionType& operator-=(const CoeffBasedProduct& other) { return m_expression.derived() -= CoeffBasedProduct(other.lhs(), other.rhs()); } + + template + ExpressionType& operator=(const ReturnByValue& func) + { return m_expression = func; } #endif + ExpressionType& expression() const + { + return m_expression; + } + protected: ExpressionType& m_expression; }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/NumTraits.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/NumTraits.h index c94ef026b..bac9e50b8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/NumTraits.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/NumTraits.h @@ -140,6 +140,9 @@ struct NumTraits > AddCost = ArrayType::SizeAtCompileTime==Dynamic ? Dynamic : ArrayType::SizeAtCompileTime * NumTraits::AddCost, MulCost = ArrayType::SizeAtCompileTime==Dynamic ? Dynamic : ArrayType::SizeAtCompileTime * NumTraits::MulCost }; + + static inline RealScalar epsilon() { return NumTraits::epsilon(); } + static inline RealScalar dummy_precision() { return NumTraits::dummy_precision(); } }; } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index bc29f8142..4fc5dd318 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -105,13 +105,13 @@ class PermutationBase : public EigenBase #endif /** \returns the number of rows */ - inline Index rows() const { return indices().size(); } + inline Index rows() const { return Index(indices().size()); } /** \returns the number of columns */ - inline Index cols() const { return indices().size(); } + inline Index cols() const { return Index(indices().size()); } /** \returns the size of a side of the respective square matrix, i.e., the number of indices */ - inline Index size() const { return indices().size(); } + inline Index size() const { return Index(indices().size()); } #ifndef EIGEN_PARSED_BY_DOXYGEN template @@ -139,9 +139,9 @@ class PermutationBase : public EigenBase /** Resizes to given size. */ - inline void resize(Index size) + inline void resize(Index newSize) { - indices().resize(size); + indices().resize(newSize); } /** Sets *this to be the identity permutation matrix */ @@ -153,9 +153,9 @@ class PermutationBase : public EigenBase /** Sets *this to be the identity permutation matrix of given size. */ - void setIdentity(Index size) + void setIdentity(Index newSize) { - resize(size); + resize(newSize); setIdentity(); } @@ -317,7 +317,7 @@ class PermutationMatrix : public PermutationBase - explicit inline PermutationMatrix(const MatrixBase& indices) : m_indices(indices) + explicit inline PermutationMatrix(const MatrixBase& a_indices) : m_indices(a_indices) {} /** Convert the Transpositions \a tr to a permutation matrix */ @@ -406,12 +406,12 @@ class Map, typedef typename IndicesType::Scalar Index; #endif - inline Map(const Index* indices) - : m_indices(indices) + inline Map(const Index* indicesPtr) + : m_indices(indicesPtr) {} - inline Map(const Index* indices, Index size) - : m_indices(indices,size) + inline Map(const Index* indicesPtr, Index size) + : m_indices(indicesPtr,size) {} /** Copies the other permutation into *this */ @@ -490,8 +490,8 @@ class PermutationWrapper : public PermutationBase > { typedef typename remove_all::type MatrixTypeNestedCleaned; + typedef typename MatrixType::Index Index; permut_matrix_product_retval(const PermutationType& perm, const MatrixType& matrix) : m_permutation(perm), m_matrix(matrix) {} - inline int rows() const { return m_matrix.rows(); } - inline int cols() const { return m_matrix.cols(); } + inline Index rows() const { return m_matrix.rows(); } + inline Index cols() const { return m_matrix.cols(); } template inline void evalTo(Dest& dst) const { - const int n = Side==OnTheLeft ? rows() : cols(); + const Index n = Side==OnTheLeft ? rows() : cols(); if(is_same::value && extract_data(dst) == extract_data(m_matrix)) { // apply the permutation inplace Matrix mask(m_permutation.size()); mask.fill(false); - int r = 0; + Index r = 0; while(r < m_permutation.size()) { // search for the next seed @@ -566,10 +567,10 @@ struct permut_matrix_product_retval if(r>=m_permutation.size()) break; // we got one, let's follow it until we are back to the seed - int k0 = r++; - int kPrev = k0; + Index k0 = r++; + Index kPrev = k0; mask.coeffRef(k0) = true; - for(int k=m_permutation.indices().coeff(k0); k!=k0; k=m_permutation.indices().coeff(k)) + for(Index k=m_permutation.indices().coeff(k0); k!=k0; k=m_permutation.indices().coeff(k)) { Block(dst, k) .swap(Block diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h index 71c74309a..af0a479c7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h @@ -11,28 +11,41 @@ #ifndef EIGEN_DENSESTORAGEBASE_H #define EIGEN_DENSESTORAGEBASE_H -#ifdef EIGEN_INITIALIZE_MATRICES_BY_ZERO -# define EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED for(int i=0;i::quiet_NaN(); #else -# define EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED +# undef EIGEN_INITIALIZE_COEFFS +# define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED #endif namespace Eigen { namespace internal { -template -EIGEN_ALWAYS_INLINE void check_rows_cols_for_overflow(Index rows, Index cols) -{ - // http://hg.mozilla.org/mozilla-central/file/6c8a909977d3/xpcom/ds/CheckedInt.h#l242 - // we assume Index is signed - Index max_index = (size_t(1) << (8 * sizeof(Index) - 1)) - 1; // assume Index is signed - bool error = (rows < 0 || cols < 0) ? true - : (rows == 0 || cols == 0) ? false - : (rows > max_index / cols); - if (error) - throw_std_bad_alloc(); -} +template struct check_rows_cols_for_overflow { + template + static EIGEN_ALWAYS_INLINE void run(Index, Index) + { + } +}; + +template<> struct check_rows_cols_for_overflow { + template + static EIGEN_ALWAYS_INLINE void run(Index rows, Index cols) + { + // http://hg.mozilla.org/mozilla-central/file/6c8a909977d3/xpcom/ds/CheckedInt.h#l242 + // we assume Index is signed + Index max_index = (size_t(1) << (8 * sizeof(Index) - 1)) - 1; // assume Index is signed + bool error = (rows == 0 || cols == 0) ? false + : (rows > max_index / cols); + if (error) + throw_std_bad_alloc(); + } +}; template struct conservative_resize_like_impl; @@ -119,12 +132,12 @@ class PlainObjectBase : public internal::dense_xpr_base::type EIGEN_STRONG_INLINE Index rows() const { return m_storage.rows(); } EIGEN_STRONG_INLINE Index cols() const { return m_storage.cols(); } - EIGEN_STRONG_INLINE const Scalar& coeff(Index row, Index col) const + EIGEN_STRONG_INLINE const Scalar& coeff(Index rowId, Index colId) const { if(Flags & RowMajorBit) - return m_storage.data()[col + row * m_storage.cols()]; + return m_storage.data()[colId + rowId * m_storage.cols()]; else // column-major - return m_storage.data()[row + col * m_storage.rows()]; + return m_storage.data()[rowId + colId * m_storage.rows()]; } EIGEN_STRONG_INLINE const Scalar& coeff(Index index) const @@ -132,12 +145,12 @@ class PlainObjectBase : public internal::dense_xpr_base::type return m_storage.data()[index]; } - EIGEN_STRONG_INLINE Scalar& coeffRef(Index row, Index col) + EIGEN_STRONG_INLINE Scalar& coeffRef(Index rowId, Index colId) { if(Flags & RowMajorBit) - return m_storage.data()[col + row * m_storage.cols()]; + return m_storage.data()[colId + rowId * m_storage.cols()]; else // column-major - return m_storage.data()[row + col * m_storage.rows()]; + return m_storage.data()[rowId + colId * m_storage.rows()]; } EIGEN_STRONG_INLINE Scalar& coeffRef(Index index) @@ -145,12 +158,12 @@ class PlainObjectBase : public internal::dense_xpr_base::type return m_storage.data()[index]; } - EIGEN_STRONG_INLINE const Scalar& coeffRef(Index row, Index col) const + EIGEN_STRONG_INLINE const Scalar& coeffRef(Index rowId, Index colId) const { if(Flags & RowMajorBit) - return m_storage.data()[col + row * m_storage.cols()]; + return m_storage.data()[colId + rowId * m_storage.cols()]; else // column-major - return m_storage.data()[row + col * m_storage.rows()]; + return m_storage.data()[rowId + colId * m_storage.rows()]; } EIGEN_STRONG_INLINE const Scalar& coeffRef(Index index) const @@ -160,12 +173,12 @@ class PlainObjectBase : public internal::dense_xpr_base::type /** \internal */ template - EIGEN_STRONG_INLINE PacketScalar packet(Index row, Index col) const + EIGEN_STRONG_INLINE PacketScalar packet(Index rowId, Index colId) const { return internal::ploadt (m_storage.data() + (Flags & RowMajorBit - ? col + row * m_storage.cols() - : row + col * m_storage.rows())); + ? colId + rowId * m_storage.cols() + : rowId + colId * m_storage.rows())); } /** \internal */ @@ -177,19 +190,19 @@ class PlainObjectBase : public internal::dense_xpr_base::type /** \internal */ template - EIGEN_STRONG_INLINE void writePacket(Index row, Index col, const PacketScalar& x) + EIGEN_STRONG_INLINE void writePacket(Index rowId, Index colId, const PacketScalar& val) { internal::pstoret (m_storage.data() + (Flags & RowMajorBit - ? col + row * m_storage.cols() - : row + col * m_storage.rows()), x); + ? colId + rowId * m_storage.cols() + : rowId + colId * m_storage.rows()), val); } /** \internal */ template - EIGEN_STRONG_INLINE void writePacket(Index index, const PacketScalar& x) + EIGEN_STRONG_INLINE void writePacket(Index index, const PacketScalar& val) { - internal::pstoret(m_storage.data() + index, x); + internal::pstoret(m_storage.data() + index, val); } /** \returns a const pointer to the data array of this matrix */ @@ -216,17 +229,22 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * \sa resize(Index) for vectors, resize(NoChange_t, Index), resize(Index, NoChange_t) */ - EIGEN_STRONG_INLINE void resize(Index rows, Index cols) + EIGEN_STRONG_INLINE void resize(Index nbRows, Index nbCols) { - #ifdef EIGEN_INITIALIZE_MATRICES_BY_ZERO - internal::check_rows_cols_for_overflow(rows, cols); - Index size = rows*cols; + eigen_assert( EIGEN_IMPLIES(RowsAtCompileTime!=Dynamic,nbRows==RowsAtCompileTime) + && EIGEN_IMPLIES(ColsAtCompileTime!=Dynamic,nbCols==ColsAtCompileTime) + && EIGEN_IMPLIES(RowsAtCompileTime==Dynamic && MaxRowsAtCompileTime!=Dynamic,nbRows<=MaxRowsAtCompileTime) + && EIGEN_IMPLIES(ColsAtCompileTime==Dynamic && MaxColsAtCompileTime!=Dynamic,nbCols<=MaxColsAtCompileTime) + && nbRows>=0 && nbCols>=0 && "Invalid sizes when resizing a matrix or array."); + internal::check_rows_cols_for_overflow::run(nbRows, nbCols); + #ifdef EIGEN_INITIALIZE_COEFFS + Index size = nbRows*nbCols; bool size_changed = size != this->size(); - m_storage.resize(size, rows, cols); - if(size_changed) EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED + m_storage.resize(size, nbRows, nbCols); + if(size_changed) EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED #else - internal::check_rows_cols_for_overflow(rows, cols); - m_storage.resize(rows*cols, rows, cols); + internal::check_rows_cols_for_overflow::run(nbRows, nbCols); + m_storage.resize(nbRows*nbCols, nbRows, nbCols); #endif } @@ -244,16 +262,16 @@ class PlainObjectBase : public internal::dense_xpr_base::type inline void resize(Index size) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(PlainObjectBase) - eigen_assert(SizeAtCompileTime == Dynamic || SizeAtCompileTime == size); - #ifdef EIGEN_INITIALIZE_MATRICES_BY_ZERO + eigen_assert(((SizeAtCompileTime == Dynamic && (MaxSizeAtCompileTime==Dynamic || size<=MaxSizeAtCompileTime)) || SizeAtCompileTime == size) && size>=0); + #ifdef EIGEN_INITIALIZE_COEFFS bool size_changed = size != this->size(); #endif if(RowsAtCompileTime == 1) m_storage.resize(size, 1, size); else m_storage.resize(size, size, 1); - #ifdef EIGEN_INITIALIZE_MATRICES_BY_ZERO - if(size_changed) EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED + #ifdef EIGEN_INITIALIZE_COEFFS + if(size_changed) EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED #endif } @@ -265,9 +283,9 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * \sa resize(Index,Index) */ - inline void resize(NoChange_t, Index cols) + inline void resize(NoChange_t, Index nbCols) { - resize(rows(), cols); + resize(rows(), nbCols); } /** Resizes the matrix, changing only the number of rows. For the parameter of type NoChange_t, just pass the special value \c NoChange @@ -278,9 +296,9 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * \sa resize(Index,Index) */ - inline void resize(Index rows, NoChange_t) + inline void resize(Index nbRows, NoChange_t) { - resize(rows, cols()); + resize(nbRows, cols()); } /** Resizes \c *this to have the same dimensions as \a other. @@ -294,7 +312,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type EIGEN_STRONG_INLINE void resizeLike(const EigenBase& _other) { const OtherDerived& other = _other.derived(); - internal::check_rows_cols_for_overflow(other.rows(), other.cols()); + internal::check_rows_cols_for_overflow::run(other.rows(), other.cols()); const Index othersize = other.rows()*other.cols(); if(RowsAtCompileTime == 1) { @@ -318,9 +336,9 @@ class PlainObjectBase : public internal::dense_xpr_base::type * Matrices are resized relative to the top-left element. In case values need to be * appended to the matrix they will be uninitialized. */ - EIGEN_STRONG_INLINE void conservativeResize(Index rows, Index cols) + EIGEN_STRONG_INLINE void conservativeResize(Index nbRows, Index nbCols) { - internal::conservative_resize_like_impl::run(*this, rows, cols); + internal::conservative_resize_like_impl::run(*this, nbRows, nbCols); } /** Resizes the matrix to \a rows x \a cols while leaving old values untouched. @@ -330,10 +348,10 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * In case the matrix is growing, new rows will be uninitialized. */ - EIGEN_STRONG_INLINE void conservativeResize(Index rows, NoChange_t) + EIGEN_STRONG_INLINE void conservativeResize(Index nbRows, NoChange_t) { // Note: see the comment in conservativeResize(Index,Index) - conservativeResize(rows, cols()); + conservativeResize(nbRows, cols()); } /** Resizes the matrix to \a rows x \a cols while leaving old values untouched. @@ -343,10 +361,10 @@ class PlainObjectBase : public internal::dense_xpr_base::type * * In case the matrix is growing, new columns will be uninitialized. */ - EIGEN_STRONG_INLINE void conservativeResize(NoChange_t, Index cols) + EIGEN_STRONG_INLINE void conservativeResize(NoChange_t, Index nbCols) { // Note: see the comment in conservativeResize(Index,Index) - conservativeResize(rows(), cols); + conservativeResize(rows(), nbCols); } /** Resizes the vector to \a size while retaining old values. @@ -400,10 +418,10 @@ class PlainObjectBase : public internal::dense_xpr_base::type return Base::operator=(func); } - EIGEN_STRONG_INLINE explicit PlainObjectBase() : m_storage() + EIGEN_STRONG_INLINE PlainObjectBase() : m_storage() { // _check_template_params(); -// EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED +// EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } #ifndef EIGEN_PARSED_BY_DOXYGEN @@ -412,15 +430,15 @@ class PlainObjectBase : public internal::dense_xpr_base::type PlainObjectBase(internal::constructor_without_unaligned_array_assert) : m_storage(internal::constructor_without_unaligned_array_assert()) { -// _check_template_params(); EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED +// _check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } #endif - EIGEN_STRONG_INLINE PlainObjectBase(Index size, Index rows, Index cols) - : m_storage(size, rows, cols) + EIGEN_STRONG_INLINE PlainObjectBase(Index a_size, Index nbRows, Index nbCols) + : m_storage(a_size, nbRows, nbCols) { // _check_template_params(); -// EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED +// EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED } /** \copydoc MatrixBase::operator=(const EigenBase&) @@ -439,7 +457,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type : m_storage(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) { _check_template_params(); - internal::check_rows_cols_for_overflow(other.derived().rows(), other.derived().cols()); + internal::check_rows_cols_for_overflow::run(other.derived().rows(), other.derived().cols()); Base::operator=(other.derived()); } @@ -551,6 +569,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type eigen_assert((this->size()==0 || (IsVectorAtCompileTime ? (this->size() == other.size()) : (rows() == other.rows() && cols() == other.cols()))) && "Size mismatch. Automatic resizing is disabled because EIGEN_NO_AUTOMATIC_RESIZING is defined"); + EIGEN_ONLY_USED_FOR_DEBUG(other); #else resizeLike(other); #endif @@ -600,23 +619,19 @@ class PlainObjectBase : public internal::dense_xpr_base::type } template - EIGEN_STRONG_INLINE void _init2(Index rows, Index cols, typename internal::enable_if::type* = 0) + EIGEN_STRONG_INLINE void _init2(Index nbRows, Index nbCols, typename internal::enable_if::type* = 0) { EIGEN_STATIC_ASSERT(bool(NumTraits::IsInteger) && bool(NumTraits::IsInteger), FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED) - eigen_assert(rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) - && cols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)); - internal::check_rows_cols_for_overflow(rows, cols); - m_storage.resize(rows*cols,rows,cols); - EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED + resize(nbRows,nbCols); } template - EIGEN_STRONG_INLINE void _init2(const Scalar& x, const Scalar& y, typename internal::enable_if::type* = 0) + EIGEN_STRONG_INLINE void _init2(const Scalar& val0, const Scalar& val1, typename internal::enable_if::type* = 0) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 2) - m_storage.data()[0] = x; - m_storage.data()[1] = y; + m_storage.data()[0] = val0; + m_storage.data()[1] = val1; } template @@ -665,7 +680,7 @@ struct internal::conservative_resize_like_impl if ( ( Derived::IsRowMajor && _this.cols() == cols) || // row-major and we change only the number of rows (!Derived::IsRowMajor && _this.rows() == rows) ) // column-major and we change only the number of columns { - internal::check_rows_cols_for_overflow(rows, cols); + internal::check_rows_cols_for_overflow::run(rows, cols); _this.derived().m_storage.conservativeResize(rows*cols,rows,cols); } else diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h deleted file mode 100644 index 30aa8943b..000000000 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h +++ /dev/null @@ -1,98 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008-2011 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 with this -// file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_PRODUCT_H -#define EIGEN_PRODUCT_H - -template class Product; -template class ProductImpl; - -/** \class Product - * \ingroup Core_Module - * - * \brief Expression of the product of two arbitrary matrices or vectors - * - * \param Lhs the type of the left-hand side expression - * \param Rhs the type of the right-hand side expression - * - * This class represents an expression of the product of two arbitrary matrices. - * - */ - -namespace internal { -template -struct traits > -{ - typedef MatrixXpr XprKind; - typedef typename remove_all::type LhsCleaned; - typedef typename remove_all::type RhsCleaned; - typedef typename scalar_product_traits::Scalar, typename traits::Scalar>::ReturnType Scalar; - typedef typename promote_storage_type::StorageKind, - typename traits::StorageKind>::ret StorageKind; - typedef typename promote_index_type::Index, - typename traits::Index>::type Index; - enum { - RowsAtCompileTime = LhsCleaned::RowsAtCompileTime, - ColsAtCompileTime = RhsCleaned::ColsAtCompileTime, - MaxRowsAtCompileTime = LhsCleaned::MaxRowsAtCompileTime, - MaxColsAtCompileTime = RhsCleaned::MaxColsAtCompileTime, - Flags = (MaxRowsAtCompileTime==1 ? RowMajorBit : 0), // TODO should be no storage order - CoeffReadCost = 0 // TODO CoeffReadCost should not be part of the expression traits - }; -}; -} // end namespace internal - - -template -class Product : public ProductImpl::StorageKind, - typename internal::traits::StorageKind>::ret> -{ - public: - - typedef typename ProductImpl< - Lhs, Rhs, - typename internal::promote_storage_type::ret>::Base Base; - EIGEN_GENERIC_PUBLIC_INTERFACE(Product) - - typedef typename Lhs::Nested LhsNested; - typedef typename Rhs::Nested RhsNested; - typedef typename internal::remove_all::type LhsNestedCleaned; - typedef typename internal::remove_all::type RhsNestedCleaned; - - Product(const Lhs& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs) - { - eigen_assert(lhs.cols() == rhs.rows() - && "invalid matrix product" - && "if you wanted a coeff-wise or a dot product use the respective explicit functions"); - } - - inline Index rows() const { return m_lhs.rows(); } - inline Index cols() const { return m_rhs.cols(); } - - const LhsNestedCleaned& lhs() const { return m_lhs; } - const RhsNestedCleaned& rhs() const { return m_rhs; } - - protected: - - const LhsNested m_lhs; - const RhsNested m_rhs; -}; - -template -class ProductImpl : public internal::dense_xpr_base >::type -{ - typedef Product Derived; - public: - - typedef typename internal::dense_xpr_base >::type Base; - EIGEN_DENSE_PUBLIC_INTERFACE(Derived) -}; - -#endif // EIGEN_PRODUCT_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h index ec12e5c9f..a494b5f87 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h @@ -87,10 +87,10 @@ class ProductBase : public MatrixBase typedef typename Base::PlainObject PlainObject; - ProductBase(const Lhs& lhs, const Rhs& rhs) - : m_lhs(lhs), m_rhs(rhs) + ProductBase(const Lhs& a_lhs, const Rhs& a_rhs) + : m_lhs(a_lhs), m_rhs(a_rhs) { - eigen_assert(lhs.cols() == rhs.rows() + eigen_assert(a_lhs.cols() == a_rhs.rows() && "invalid matrix product" && "if you wanted a coeff-wise or a dot product use the respective explicit functions"); } @@ -108,7 +108,7 @@ class ProductBase : public MatrixBase inline void subTo(Dest& dst) const { scaleAndAddTo(dst,Scalar(-1)); } template - inline void scaleAndAddTo(Dest& dst,Scalar alpha) const { derived().scaleAndAddTo(dst,alpha); } + inline void scaleAndAddTo(Dest& dst, const Scalar& alpha) const { derived().scaleAndAddTo(dst,alpha); } const _LhsNested& lhs() const { return m_lhs; } const _RhsNested& rhs() const { return m_rhs; } @@ -195,25 +195,25 @@ class ScaledProduct; // Also note that here we accept any compatible scalar types template const ScaledProduct -operator*(const ProductBase& prod, typename Derived::Scalar x) +operator*(const ProductBase& prod, const typename Derived::Scalar& x) { return ScaledProduct(prod.derived(), x); } template typename internal::enable_if::value, const ScaledProduct >::type -operator*(const ProductBase& prod, typename Derived::RealScalar x) +operator*(const ProductBase& prod, const typename Derived::RealScalar& x) { return ScaledProduct(prod.derived(), x); } template const ScaledProduct -operator*(typename Derived::Scalar x,const ProductBase& prod) +operator*(const typename Derived::Scalar& x,const ProductBase& prod) { return ScaledProduct(prod.derived(), x); } template typename internal::enable_if::value, const ScaledProduct >::type -operator*(typename Derived::RealScalar x,const ProductBase& prod) +operator*(const typename Derived::RealScalar& x,const ProductBase& prod) { return ScaledProduct(prod.derived(), x); } namespace internal { @@ -241,7 +241,7 @@ class ScaledProduct typedef typename Base::PlainObject PlainObject; // EIGEN_PRODUCT_PUBLIC_INTERFACE(ScaledProduct) - ScaledProduct(const NestedProduct& prod, Scalar x) + ScaledProduct(const NestedProduct& prod, const Scalar& x) : Base(prod.lhs(),prod.rhs()), m_prod(prod), m_alpha(x) {} template @@ -254,7 +254,7 @@ class ScaledProduct inline void subTo(Dest& dst) const { scaleAndAddTo(dst, Scalar(-1)); } template - inline void scaleAndAddTo(Dest& dst,Scalar alpha) const { m_prod.derived().scaleAndAddTo(dst,alpha * m_alpha); } + inline void scaleAndAddTo(Dest& dst, const Scalar& a_alpha) const { m_prod.derived().scaleAndAddTo(dst,a_alpha * m_alpha); } const Scalar& alpha() const { return m_alpha; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Random.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Random.h index a9f7f4346..480fea408 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Random.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Random.h @@ -112,7 +112,7 @@ inline Derived& DenseBase::setRandom() return *this = Random(rows(), cols()); } -/** Resizes to the given \a size, and sets all coefficients in this expression to random values. +/** Resizes to the given \a newSize, and sets all coefficients in this expression to random values. * * \only_for_vectors * @@ -123,16 +123,16 @@ inline Derived& DenseBase::setRandom() */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setRandom(Index size) +PlainObjectBase::setRandom(Index newSize) { - resize(size); + resize(newSize); return setRandom(); } /** Resizes to the given size, and sets all coefficients in this expression to random values. * - * \param rows the new number of rows - * \param cols the new number of columns + * \param nbRows the new number of rows + * \param nbCols the new number of columns * * Example: \include Matrix_setRandom_int_int.cpp * Output: \verbinclude Matrix_setRandom_int_int.out @@ -141,9 +141,9 @@ PlainObjectBase::setRandom(Index size) */ template EIGEN_STRONG_INLINE Derived& -PlainObjectBase::setRandom(Index rows, Index cols) +PlainObjectBase::setRandom(Index nbRows, Index nbCols) { - resize(rows, cols); + resize(nbRows, nbCols); return setRandom(); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h index b7ce7c658..50548fa9a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h @@ -330,7 +330,8 @@ DenseBase::redux(const Func& func) const ::run(derived(), func); } -/** \returns the minimum of all coefficients of *this +/** \returns the minimum of all coefficients of \c *this. + * \warning the result is undefined if \c *this contains NaN. */ template EIGEN_STRONG_INLINE typename internal::traits::Scalar @@ -339,7 +340,8 @@ DenseBase::minCoeff() const return this->redux(Eigen::internal::scalar_min_op()); } -/** \returns the maximum of all coefficients of *this +/** \returns the maximum of all coefficients of \c *this. + * \warning the result is undefined if \c *this contains NaN. */ template EIGEN_STRONG_INLINE typename internal::traits::Scalar diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h new file mode 100644 index 000000000..aba795bdb --- /dev/null +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -0,0 +1,255 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2012 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 +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_REF_H +#define EIGEN_REF_H + +namespace Eigen { + +template class RefBase; +template,OuterStride<> >::type > class Ref; + +/** \class Ref + * \ingroup Core_Module + * + * \brief A matrix or vector expression mapping an existing expressions + * + * \tparam PlainObjectType the equivalent matrix type of the mapped data + * \tparam Options specifies whether the pointer is \c #Aligned, or \c #Unaligned. + * The default is \c #Unaligned. + * \tparam StrideType optionally specifies strides. By default, Ref implies a contiguous storage along the inner dimension (inner stride==1), + * but accept a variable outer stride (leading dimension). + * This can be overridden by specifying strides. + * The type passed here must be a specialization of the Stride template, see examples below. + * + * This class permits to write non template functions taking Eigen's object as parameters while limiting the number of copies. + * A Ref<> object can represent either a const expression or a l-value: + * \code + * // in-out argument: + * void foo1(Ref x); + * + * // read-only const argument: + * void foo2(const Ref& x); + * \endcode + * + * In the in-out case, the input argument must satisfies the constraints of the actual Ref<> type, otherwise a compilation issue will be triggered. + * By default, a Ref can reference any dense vector expression of float having a contiguous memory layout. + * Likewise, a Ref can reference any column major dense matrix expression of float whose column's elements are contiguously stored with + * the possibility to have a constant space inbetween each column, i.e.: the inner stride mmust be equal to 1, but the outer-stride (or leading dimension), + * can be greater than the number of rows. + * + * In the const case, if the input expression does not match the above requirement, then it is evaluated into a temporary before being passed to the function. + * Here are some examples: + * \code + * MatrixXf A; + * VectorXf a; + * foo1(a.head()); // OK + * foo1(A.col()); // OK + * foo1(A.row()); // compilation error because here innerstride!=1 + * foo2(A.row()); // The row is copied into a contiguous temporary + * foo2(2*a); // The expression is evaluated into a temporary + * foo2(A.col().segment(2,4)); // No temporary + * \endcode + * + * The range of inputs that can be referenced without temporary can be enlarged using the last two template parameter. + * Here is an example accepting an innerstride!=1: + * \code + * // in-out argument: + * void foo3(Ref > x); + * foo3(A.row()); // OK + * \endcode + * The downside here is that the function foo3 might be significantly slower than foo1 because it won't be able to exploit vectorization, and will involved more + * expensive address computations even if the input is contiguously stored in memory. To overcome this issue, one might propose to overloads internally calling a + * template function, e.g.: + * \code + * // in the .h: + * void foo(const Ref& A); + * void foo(const Ref >& A); + * + * // in the .cpp: + * template void foo_impl(const TypeOfA& A) { + * ... // crazy code goes here + * } + * void foo(const Ref& A) { foo_impl(A); } + * void foo(const Ref >& A) { foo_impl(A); } + * \endcode + * + * + * \sa PlainObjectBase::Map(), \ref TopicStorageOrders + */ + +namespace internal { + +template +struct traits > + : public traits > +{ + typedef _PlainObjectType PlainObjectType; + typedef _StrideType StrideType; + enum { + Options = _Options + }; + + template struct match { + enum { + HasDirectAccess = internal::has_direct_access::ret, + StorageOrderMatch = PlainObjectType::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), + OuterStrideMatch = Derived::IsVectorAtCompileTime + || int(StrideType::OuterStrideAtCompileTime)==int(Dynamic) || int(StrideType::OuterStrideAtCompileTime)==int(Derived::OuterStrideAtCompileTime), + AlignmentMatch = (_Options!=Aligned) || ((PlainObjectType::Flags&AlignedBit)==0) || ((traits::Flags&AlignedBit)==AlignedBit), + MatchAtCompileTime = HasDirectAccess && StorageOrderMatch && InnerStrideMatch && OuterStrideMatch && AlignmentMatch + }; + typedef typename internal::conditional::type type; + }; + +}; + +template +struct traits > : public traits {}; + +} + +template class RefBase + : public MapBase +{ + typedef typename internal::traits::PlainObjectType PlainObjectType; + typedef typename internal::traits::StrideType StrideType; + +public: + + typedef MapBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(RefBase) + + inline Index innerStride() const + { + return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1; + } + + inline Index outerStride() const + { + return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer() + : IsVectorAtCompileTime ? this->size() + : int(Flags)&RowMajorBit ? this->cols() + : this->rows(); + } + + RefBase() + : Base(0,RowsAtCompileTime==Dynamic?0:RowsAtCompileTime,ColsAtCompileTime==Dynamic?0:ColsAtCompileTime), + // Stride<> does not allow default ctor for Dynamic strides, so let' initialize it with dummy values: + m_stride(StrideType::OuterStrideAtCompileTime==Dynamic?0:StrideType::OuterStrideAtCompileTime, + StrideType::InnerStrideAtCompileTime==Dynamic?0:StrideType::InnerStrideAtCompileTime) + {} + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(RefBase) + +protected: + + typedef Stride StrideBase; + + template + void construct(Expression& expr) + { + if(PlainObjectType::RowsAtCompileTime==1) + { + eigen_assert(expr.rows()==1 || expr.cols()==1); + ::new (static_cast(this)) Base(expr.data(), 1, expr.size()); + } + else if(PlainObjectType::ColsAtCompileTime==1) + { + eigen_assert(expr.rows()==1 || expr.cols()==1); + ::new (static_cast(this)) Base(expr.data(), expr.size(), 1); + } + 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()); + } + + StrideBase m_stride; +}; + + +template class Ref + : public RefBase > +{ + typedef internal::traits Traits; + public: + + typedef RefBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Ref) + + + #ifndef EIGEN_PARSED_BY_DOXYGEN + template + inline Ref(PlainObjectBase& expr, + typename internal::enable_if::MatchAtCompileTime),Derived>::type* = 0) + { + Base::construct(expr); + } + template + inline Ref(const DenseBase& expr, + typename internal::enable_if::value&&bool(Traits::template match::MatchAtCompileTime)),Derived>::type* = 0, + int = Derived::ThisConstantIsPrivateInPlainObjectBase) + #else + template + inline Ref(DenseBase& expr) + #endif + { + Base::construct(expr.const_cast_derived()); + } + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Ref) + +}; + +// this is the const ref version +template class Ref + : public RefBase > +{ + typedef internal::traits Traits; + public: + + typedef RefBase Base; + EIGEN_DENSE_PUBLIC_INTERFACE(Ref) + + template + inline Ref(const DenseBase& expr) + { +// std::cout << match_helper::HasDirectAccess << "," << match_helper::OuterStrideMatch << "," << match_helper::InnerStrideMatch << "\n"; +// std::cout << int(StrideType::OuterStrideAtCompileTime) << " - " << int(Derived::OuterStrideAtCompileTime) << "\n"; +// std::cout << int(StrideType::InnerStrideAtCompileTime) << " - " << int(Derived::InnerStrideAtCompileTime) << "\n"; + construct(expr.derived(), typename Traits::template match::type()); + } + + protected: + + template + void construct(const Expression& expr,internal::true_type) + { + Base::construct(expr); + } + + template + void construct(const Expression& expr, internal::false_type) + { + m_object.lazyAssign(expr); + Base::construct(m_object); + } + + protected: + TPlainObjectType m_object; +}; + +} // end namespace Eigen + +#endif // EIGEN_REF_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h index b61fdc29e..dde86a834 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h @@ -70,8 +70,8 @@ template class Replicate EIGEN_DENSE_PUBLIC_INTERFACE(Replicate) template - inline explicit Replicate(const OriginalMatrixType& matrix) - : m_matrix(matrix), m_rowFactor(RowFactor), m_colFactor(ColFactor) + inline explicit Replicate(const OriginalMatrixType& a_matrix) + : m_matrix(a_matrix), m_rowFactor(RowFactor), m_colFactor(ColFactor) { EIGEN_STATIC_ASSERT((internal::is_same::type,OriginalMatrixType>::value), THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE) @@ -79,8 +79,8 @@ template class Replicate } template - inline Replicate(const OriginalMatrixType& matrix, Index rowFactor, Index colFactor) - : m_matrix(matrix), m_rowFactor(rowFactor), m_colFactor(colFactor) + inline Replicate(const OriginalMatrixType& a_matrix, Index rowFactor, Index colFactor) + : m_matrix(a_matrix), m_rowFactor(rowFactor), m_colFactor(colFactor) { EIGEN_STATIC_ASSERT((internal::is_same::type,OriginalMatrixType>::value), THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE) @@ -89,27 +89,27 @@ template class Replicate inline Index rows() const { return m_matrix.rows() * m_rowFactor.value(); } inline Index cols() const { return m_matrix.cols() * m_colFactor.value(); } - inline Scalar coeff(Index row, Index col) const + inline Scalar coeff(Index rowId, Index colId) const { // try to avoid using modulo; this is a pure optimization strategy const Index actual_row = internal::traits::RowsAtCompileTime==1 ? 0 - : RowFactor==1 ? row - : row%m_matrix.rows(); + : RowFactor==1 ? rowId + : rowId%m_matrix.rows(); const Index actual_col = internal::traits::ColsAtCompileTime==1 ? 0 - : ColFactor==1 ? col - : col%m_matrix.cols(); + : ColFactor==1 ? colId + : colId%m_matrix.cols(); return m_matrix.coeff(actual_row, actual_col); } template - inline PacketScalar packet(Index row, Index col) const + inline PacketScalar packet(Index rowId, Index colId) const { const Index actual_row = internal::traits::RowsAtCompileTime==1 ? 0 - : RowFactor==1 ? row - : row%m_matrix.rows(); + : RowFactor==1 ? rowId + : rowId%m_matrix.rows(); const Index actual_col = internal::traits::ColsAtCompileTime==1 ? 0 - : ColFactor==1 ? col - : col%m_matrix.cols(); + : ColFactor==1 ? colId + : colId%m_matrix.cols(); return m_matrix.template packet(actual_row, actual_col); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h index 613912ffa..d66c24ba0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h @@ -48,7 +48,7 @@ struct nested, n, PlainObject> } // end namespace internal template class ReturnByValue - : public internal::dense_xpr_base< ReturnByValue >::type + : internal::no_assignment_operator, public internal::dense_xpr_base< ReturnByValue >::type { public: typedef typename internal::traits::ReturnType ReturnType; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Select.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Select.h index 2bf6e91d0..87993bbb5 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Select.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Select.h @@ -60,10 +60,10 @@ class Select : internal::no_assignment_operator, typedef typename internal::dense_xpr_base