From 23f5c8c331ef58b6535a5aa16de106d6ad6b4e92 Mon Sep 17 00:00:00 2001 From: Michael Bosse Date: Tue, 3 Dec 2019 09:01:43 -0800 Subject: [PATCH 01/71] Make variable names consistent with math Rename mEstimator residual to loss so that it is not confused with lsq residual vector Ax-b. Rename distance to squaredDistance when appropriate. Removed erroneous 0.5 factor from NoiseModelFactor::weight() Fix bug in mEstimator::Null::loss() --- gtsam.h | 18 ++-- gtsam/linear/LossFunctions.cpp | 16 ++-- gtsam/linear/LossFunctions.h | 30 +++---- gtsam/linear/NoiseModel.cpp | 2 +- gtsam/linear/NoiseModel.h | 16 ++-- gtsam/linear/tests/testNoiseModel.cpp | 88 ++++++++++---------- gtsam/nonlinear/NonlinearFactor.cpp | 4 +- matlab/gtsam_examples/VisualizeMEstimators.m | 2 +- 8 files changed, 87 insertions(+), 89 deletions(-) diff --git a/gtsam.h b/gtsam.h index 070a3c42f..878f41385 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1364,7 +1364,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { @@ -1375,7 +1375,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Huber: gtsam::noiseModel::mEstimator::Base { @@ -1386,7 +1386,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { @@ -1397,7 +1397,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { @@ -1408,7 +1408,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { @@ -1419,7 +1419,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { @@ -1430,7 +1430,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { @@ -1441,7 +1441,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { @@ -1452,7 +1452,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; }///\namespace mEstimator diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 6bc737e2c..8bb670a92 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -141,7 +141,7 @@ double Fair::weight(double error) const { return 1.0 / (1.0 + std::abs(error) / c_); } -double Fair::residual(double error) const { +double Fair::loss(double error) const { const double absError = std::abs(error); const double normalizedError = absError / c_; const double c_2 = c_ * c_; @@ -175,7 +175,7 @@ double Huber::weight(double error) const { return (absError <= k_) ? (1.0) : (k_ / absError); } -double Huber::residual(double error) const { +double Huber::loss(double error) const { const double absError = std::abs(error); if (absError <= k_) { // |x| <= k return error*error / 2; @@ -212,7 +212,7 @@ double Cauchy::weight(double error) const { return ksquared_ / (ksquared_ + error*error); } -double Cauchy::residual(double error) const { +double Cauchy::loss(double error) const { const double val = std::log1p(error * error / ksquared_); return ksquared_ * val * 0.5; } @@ -249,7 +249,7 @@ double Tukey::weight(double error) const { return 0.0; } -double Tukey::residual(double error) const { +double Tukey::loss(double error) const { double absError = std::abs(error); if (absError <= c_) { const double one_minus_xc2 = 1.0 - error*error/csquared_; @@ -285,7 +285,7 @@ double Welsch::weight(double error) const { return std::exp(-xc2); } -double Welsch::residual(double error) const { +double Welsch::loss(double error) const { const double xc2 = (error*error)/csquared_; return csquared_ * 0.5 * -std::expm1(-xc2); } @@ -318,7 +318,7 @@ double GemanMcClure::weight(double error) const { return c4/(c2error*c2error); } -double GemanMcClure::residual(double error) const { +double GemanMcClure::loss(double error) const { const double c2 = c_*c_; const double error2 = error*error; return 0.5 * (c2 * error2) / (c2 + error2); @@ -356,7 +356,7 @@ double DCS::weight(double error) const { return 1.0; } -double DCS::residual(double error) const { +double DCS::loss(double error) const { // This is the simplified version of Eq 9 from (Agarwal13icra) // after you simplify and cancel terms. const double e2 = error*error; @@ -400,7 +400,7 @@ double L2WithDeadZone::weight(double error) const { else return (k_+error)/error; } -double L2WithDeadZone::residual(double error) const { +double L2WithDeadZone::loss(double error) const { const double abs_error = std::abs(error); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 1f7cc1377..93a159350 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -36,12 +36,12 @@ namespace noiseModel { * The mEstimator name space contains all robust error functions. * It mirrors the exposition at * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf - * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. + * which talks about minimizing \sum \rho(r_i), where \rho is a loss function of choice. * * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: * * Name Symbol Least-Squares L1-norm Huber - * Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x| shared_ptr; @@ -131,7 +131,7 @@ class GTSAM_EXPORT Null : public Base { Null(const ReweightScheme reweight = Block) : Base(reweight) {} ~Null() {} double weight(double /*error*/) const { return 1.0; } - double residual(double error) const { return error; } + double loss(double error) const { return 0.5 * error * error; } void print(const std::string &s) const; bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create(); @@ -155,7 +155,7 @@ class GTSAM_EXPORT Fair : public Base { Fair(double c = 1.3998, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double c, const ReweightScheme reweight = Block); @@ -180,7 +180,7 @@ class GTSAM_EXPORT Huber : public Base { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -210,7 +210,7 @@ class GTSAM_EXPORT Cauchy : public Base { Cauchy(double k = 0.1, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -235,7 +235,7 @@ class GTSAM_EXPORT Tukey : public Base { Tukey(double c = 4.6851, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -260,7 +260,7 @@ class GTSAM_EXPORT Welsch : public Base { Welsch(double c = 2.9846, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -296,7 +296,7 @@ class GTSAM_EXPORT GemanMcClure : public Base { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); ~GemanMcClure() {} double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -326,7 +326,7 @@ class GTSAM_EXPORT DCS : public Base { DCS(double c = 1.0, const ReweightScheme reweight = Block); ~DCS() {} double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -359,7 +359,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 4e599d45f..8297a8a6a 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -371,7 +371,7 @@ Vector Constrained::whiten(const Vector& v) const { } /* ************************************************************************* */ -double Constrained::distance(const Vector& v) const { +double Constrained::squaredDistance(const Vector& v) const { Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements for (size_t i=0; i& A, Vector& b) const = 0; virtual void WhitenSystem(Matrix& A, Vector& b) const = 0; @@ -211,7 +211,7 @@ namespace gtsam { */ virtual double Mahalanobis(const Vector& v) const; - inline virtual double distance(const Vector& v) const { + inline virtual double squaredDistance(const Vector& v) const { return Mahalanobis(v); } @@ -460,11 +460,11 @@ namespace gtsam { } /** - * The distance function for a constrained noisemodel, + * The squaredDistance function for a constrained noisemodel, * for non-constrained versions, uses sigmas, otherwise * uses the penalty function with mu */ - virtual double distance(const Vector& v) const; + virtual double squaredDistance(const Vector& v) const; /** Fully constrained variations */ static shared_ptr All(size_t dim) { @@ -692,11 +692,9 @@ namespace gtsam { { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } - // Fold the use of the m-estimator residual(...) function into distance(...) - inline virtual double distance(const Vector& v) const - { return robust_->residual(this->unweightedWhiten(v).norm()); } - inline virtual double distance_non_whitened(const Vector& v) const - { return robust_->residual(v.norm()); } + // Fold the use of the m-estimator loss(...) function into squaredDistance(...) + inline virtual double squaredDistance(const Vector& v) const + { return 2.0 * robust_->loss(this->unweightedWhiten(v).norm()); } // TODO: these are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; virtual void WhitenSystem(std::vector& A, Vector& b) const; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 10578627f..f981ad728 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -68,10 +68,10 @@ TEST(NoiseModel, constructors) for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened))); - // test Mahalanobis distance - double distance = 5*5+10*10+15*15; + // test Mahalanobis squaredDistance + double squaredDistance = 5*5+10*10+15*15; for(Gaussian::shared_ptr mi: m) - DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); + DOUBLES_EQUAL(squaredDistance,mi->Mahalanobis(unwhitened),1e-9); // test R matrix for(Gaussian::shared_ptr mi: m) @@ -182,8 +182,8 @@ TEST(NoiseModel, ConstrainedMixed ) EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible))); EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible))); - DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9); - DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9); + DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->squaredDistance(infeasible),1e-9); + DOUBLES_EQUAL(0.5,d->squaredDistance(feasible),1e-9); } /* ************************************************************************* */ @@ -197,8 +197,8 @@ TEST(NoiseModel, ConstrainedAll ) EXPECT(assert_equal(Vector3(1.0, 1.0, 1.0),i->whiten(infeasible))); EXPECT(assert_equal(Vector3(0.0, 0.0, 0.0),i->whiten(feasible))); - DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9); - DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9); + DOUBLES_EQUAL(1000.0 * 3.0,i->squaredDistance(infeasible),1e-9); + DOUBLES_EQUAL(0.0,i->squaredDistance(feasible),1e-9); } /* ************************************************************************* */ @@ -467,10 +467,10 @@ TEST(NoiseModel, robustFunctionFair) DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8); DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->loss(error1), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->loss(error2), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->loss(error3), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionHuber) @@ -483,10 +483,10 @@ TEST(NoiseModel, robustFunctionHuber) DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8); DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); - DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8); - DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8); - DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8); - DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8); + DOUBLES_EQUAL(0.5000, huber->loss(error1), 1e-8); + DOUBLES_EQUAL(37.5000, huber->loss(error2), 1e-8); + DOUBLES_EQUAL(37.5000, huber->loss(error3), 1e-8); + DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionCauchy) @@ -499,10 +499,10 @@ TEST(NoiseModel, robustFunctionCauchy) DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8); DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error3), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionGemanMcClure) @@ -514,10 +514,10 @@ TEST(NoiseModel, robustFunctionGemanMcClure) DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8); DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->loss(error1), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->loss(error2), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->loss(error3), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionWelsch) @@ -530,10 +530,10 @@ TEST(NoiseModel, robustFunctionWelsch) DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8); DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->loss(error1), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->loss(error2), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->loss(error3), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionTukey) @@ -546,10 +546,10 @@ TEST(NoiseModel, robustFunctionTukey) DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8); DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->loss(error3), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionDCS) @@ -560,8 +560,8 @@ TEST(NoiseModel, robustFunctionDCS) DOUBLES_EQUAL(1.0 , dcs->weight(error1), 1e-8); DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8); - DOUBLES_EQUAL(0.5 , dcs->residual(error1), 1e-8); - DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8); + DOUBLES_EQUAL(0.5 , dcs->loss(error1), 1e-8); + DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8); } TEST(NoiseModel, robustFunctionL2WithDeadZone) @@ -576,12 +576,12 @@ TEST(NoiseModel, robustFunctionL2WithDeadZone) DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8); DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->residual(e0), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->residual(e2), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->residual(e3), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->residual(e5), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->loss(e0), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->loss(e1), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->loss(e2), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->loss(e3), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->loss(e4), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->loss(e5), 1e-8); } /* ************************************************************************* */ @@ -673,12 +673,12 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone) * evaluating the total penalty. This bug causes the test below to fail, so I'm leaving it * commented out until the underlying bug in GTSAM is fixed. * - * for (int i = 0; i < 5; i++) { - * Vector3 error = Vector3(i, 0, 0); - * DOUBLES_EQUAL(0.5*max(0,i-1)*max(0,i-1), robust->distance(error), 1e-8); - * } */ + for (int i = 0; i < 5; i++) { + Vector3 error = Vector3(i, 0, 0); + DOUBLES_EQUAL(0.5*max(0,i-1)*max(0,i-1), robust->squaredDistance(error), 1e-8); + } } /* ************************************************************************* */ @@ -687,7 +687,7 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone) EQUALITY(cov, gaussian->covariance());\ EXPECT(assert_equal(white, gaussian->whiten(e)));\ EXPECT(assert_equal(e, gaussian->unwhiten(white)));\ - EXPECT_DOUBLES_EQUAL(251, gaussian->distance(e), 1e-9);\ + EXPECT_DOUBLES_EQUAL(251, gaussian->squaredDistance(e), 1e-9);\ Matrix A = R.inverse(); Vector b = e;\ gaussian->WhitenSystem(A, b);\ EXPECT(assert_equal(I, A));\ diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index ee14e8073..4bce91f1e 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -106,7 +106,7 @@ double NoiseModelFactor::weight(const Values& c) const { if (noiseModel_) { const Vector b = unwhitenedError(c); check(noiseModel_, b.size()); - return 0.5 * noiseModel_->weight(b); + return noiseModel_->weight(b); } else return 1.0; @@ -121,7 +121,7 @@ double NoiseModelFactor::error(const Values& c) const { const Vector b = unwhitenedError(c); check(noiseModel_, b.size()); if (noiseModel_) - return 0.5 * noiseModel_->distance(b); + return 0.5 * noiseModel_->squaredDistance(b); else return 0.5 * b.squaredNorm(); } else { diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index 8a0485334..ce505df5d 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -48,7 +48,7 @@ function plot_m_estimator(x, model, plot_title, fig_id, filename) rho = zeros(size(x)); for i = 1:size(x, 2) w(i) = model.weight(x(i)); - rho(i) = model.residual(x(i)); + rho(i) = model.loss(x(i)); end psi = w .* x; From a5ff7505aca70278fdb31be52cf6c60a5868724a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Jan 2021 11:46:28 -0500 Subject: [PATCH 02/71] update completed TODOs --- gtsam/linear/NoiseModel.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index de79ac12b..907c78e0c 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -687,7 +687,7 @@ namespace gtsam { /// Return the contained noise model const NoiseModel::shared_ptr& noise() const { return noise_; } - // TODO: functions below are dummy but necessary for the noiseModel::Base + // Functions below are dummy but necessary for the noiseModel::Base inline Vector whiten(const Vector& v) const override { Vector r = v; this->WhitenSystem(r); return r; } inline Matrix Whiten(const Matrix& A) const override @@ -699,7 +699,7 @@ namespace gtsam { return robust_->loss(std::sqrt(squared_distance)); } - // TODO: these are really robust iterated re-weighting support functions + // These are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; void WhitenSystem(std::vector& A, Vector& b) const override; void WhitenSystem(Matrix& A, Vector& b) const override; @@ -710,7 +710,6 @@ namespace gtsam { return noise_->unweightedWhiten(v); } double weight(const Vector& v) const override { - // Todo(mikebosse): make the robust weight function input a vector. return robust_->weight(v.norm()); } From 6e46b727423956af98a64663218be232fdbc6c65 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Jan 2021 11:46:49 -0500 Subject: [PATCH 03/71] add unit test for NonlinearFactor weight with different noise models --- tests/testNonlinearFactor.cpp | 76 +++++++++++++++++++++++++++++++++++ 1 file changed, 76 insertions(+) diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 84bba850b..67a23355d 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -101,6 +101,82 @@ TEST( NonlinearFactor, NonlinearFactor ) DOUBLES_EQUAL(expected,actual,0.00000001); } +/* ************************************************************************* */ +TEST(NonlinearFactor, Weight) { + // create a values structure for the non linear factor graph + Values values; + + // Instantiate a concrete class version of a NoiseModelFactor + PriorFactor factor1(X(1), Point2(0, 0)); + values.insert(X(1), Point2(0.1, 0.1)); + + CHECK(assert_equal(1.0, factor1.weight(values))); + + // Factor with noise model + auto noise = noiseModel::Isotropic::Sigma(2, 0.2); + PriorFactor factor2(X(2), Point2(1, 1), noise); + values.insert(X(2), Point2(1.1, 1.1)); + + CHECK(assert_equal(1.0, factor2.weight(values))); + + Point2 estimate(3, 3), prior(1, 1); + double distance = (estimate - prior).norm(); + + auto gaussian = noiseModel::Isotropic::Sigma(2, 0.2); + + PriorFactor factor; + + // vector to store all the robust models in so we can test iteratively. + vector robust_models; + + // Fair noise model + auto fair = noiseModel::Robust::Create( + noiseModel::mEstimator::Fair::Create(1.3998), gaussian); + robust_models.push_back(fair); + + // Huber noise model + auto huber = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), gaussian); + robust_models.push_back(huber); + + // Cauchy noise model + auto cauchy = noiseModel::Robust::Create( + noiseModel::mEstimator::Cauchy::Create(0.1), gaussian); + robust_models.push_back(cauchy); + + // Tukey noise model + auto tukey = noiseModel::Robust::Create( + noiseModel::mEstimator::Tukey::Create(4.6851), gaussian); + robust_models.push_back(tukey); + + // Welsch noise model + auto welsch = noiseModel::Robust::Create( + noiseModel::mEstimator::Welsch::Create(2.9846), gaussian); + robust_models.push_back(welsch); + + // Geman-McClure noise model + auto gm = noiseModel::Robust::Create( + noiseModel::mEstimator::GemanMcClure::Create(1.0), gaussian); + robust_models.push_back(gm); + + // DCS noise model + auto dcs = noiseModel::Robust::Create( + noiseModel::mEstimator::DCS::Create(1.0), gaussian); + robust_models.push_back(dcs); + + // L2WithDeadZone noise model + auto l2 = noiseModel::Robust::Create( + noiseModel::mEstimator::L2WithDeadZone::Create(1.0), gaussian); + robust_models.push_back(l2); + + for(auto&& model: robust_models) { + factor = PriorFactor(X(3), prior, model); + values.clear(); + values.insert(X(3), estimate); + CHECK(assert_equal(model->robust()->weight(distance), factor.weight(values))); + } +} + /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f1 ) { From d9c770d2296c41536241c9683b39604ed0e67e82 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 14:23:46 +0200 Subject: [PATCH 04/71] Forward declaration of PinholeCameraCal3Fisheye Forward declaration of PinholeCameraCal3Fisheye needed by python wrapper. --- gtsam/geometry/SimpleCamera.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index aa00222c7..119e9d1a3 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -33,6 +34,7 @@ namespace gtsam { using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; + using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** From 0a1fead551fcf7bd8790414d5091c6f9bbd118c8 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 22 Oct 2021 19:33:06 +0200 Subject: [PATCH 05/71] Test of jacobian of Cal3Fisheye for on-axis point --- python/gtsam/tests/test_Cal3Fisheye.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 298c6e57b..326c8682f 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -105,6 +105,28 @@ class TestCal3Fisheye(GtsamTestCase): score = graph.error(state) self.assertAlmostEqual(score, 0) + def test_jacobian(self): + """Evaluate jacobian at optical axis""" + obj_point_on_axis = np.array([0, 0, 1]) + img_point = np.array([0.0, 0.0]) + pose = gtsam.Pose3() + camera = gtsam.Cal3Fisheye() + state = gtsam.Values() + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + state.insert_point3(landmark_key, obj_point_on_axis) + state.insert_pose3(pose_key, pose) + state.insert_cal3fisheye(camera_key, camera) + g = gtsam.NonlinearFactorGraph() + noise_model = gtsam.noiseModel.Unit.Create(2) + factor = gtsam.GeneralSFMFactor2Cal3Fisheye(img_point, noise_model, pose_key, landmark_key, camera_key) + g.add(factor) + f = g.error(state) + gaussian_factor_graph = g.linearize(state) + H, z = gaussian_factor_graph.jacobian() + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 4*np.eye(2)) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation_skipped(self): """Estimate spatial point from image measurements""" From 1640f172e6ad56d107506daf4c46947a8908d7f5 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 22 Oct 2021 19:34:27 +0200 Subject: [PATCH 06/71] Test jacobian of Cal3Unified for on-axis point --- python/gtsam/tests/test_Cal3Unified.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index dab1ae446..bafbacfa4 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -117,6 +117,28 @@ class TestCal3Unified(GtsamTestCase): score = graph.error(state) self.assertAlmostEqual(score, 0) + def test_jacobian(self): + """Evaluate jacobian at optical axis""" + obj_point_on_axis = np.array([0, 0, 1]) + img_point = np.array([0.0, 0.0]) + pose = gtsam.Pose3() + camera = gtsam.Cal3Unified() + state = gtsam.Values() + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + state.insert_cal3unified(camera_key, camera) + state.insert_point3(landmark_key, obj_point_on_axis) + state.insert_pose3(pose_key, pose) + g = gtsam.NonlinearFactorGraph() + noise_model = gtsam.noiseModel.Unit.Create(2) + factor = gtsam.GeneralSFMFactor2Cal3Unified(img_point, noise_model, pose_key, landmark_key, camera_key) + g.add(factor) + f = g.error(state) + gaussian_factor_graph = g.linearize(state) + H, z = gaussian_factor_graph.jacobian() + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 4*np.eye(2)) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation(self): """Estimate spatial point from image measurements""" From 8df2c7086641892c88124f3b0a7af9ee6fabefaf Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 22 Oct 2021 19:39:09 +0200 Subject: [PATCH 07/71] Avoid division by zero in jacobian calculation --- gtsam/geometry/Cal3Fisheye.cpp | 42 +++++++++++++++++++--------------- 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 52d475d5d..f889342b6 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -76,28 +76,32 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, // Derivative for points in intrinsic coords (2 by 2) if (H2) { - const double dtd_dt = - 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; - const double dt_dr = 1 / (1 + r2); - const double rinv = 1 / r; - const double dr_dxi = xi * rinv; - const double dr_dyi = yi * rinv; - const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; - const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; + if (r2==0) { + *H2 = DK; + } else { + const double dtd_dt = + 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; + const double dt_dr = 1 / (1 + r2); + const double rinv = 1 / r; + const double dr_dxi = xi * rinv; + const double dr_dyi = yi * rinv; + const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; + const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; - const double td = t * K.dot(T); - const double rrinv = 1 / r2; - const double dxd_dxi = - dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi; - const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi; - const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi; - const double dyd_dyi = - dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi; + const double td = t * K.dot(T); + const double rrinv = 1 / r2; + const double dxd_dxi = + dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi; + const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi; + const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi; + const double dyd_dyi = + dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi; - Matrix2 DR; - DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; + Matrix2 DR; + DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; - *H2 = DK * DR; + *H2 = DK * DR; + } } return uv; From 91103d5f477b09b063f50cd0620b716a60b80a8d Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 11:20:12 +0200 Subject: [PATCH 08/71] Check numeric stability close to optical axis --- python/gtsam/tests/test_Cal3Fisheye.py | 63 ++++++++++++++++++++++---- 1 file changed, 54 insertions(+), 9 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 326c8682f..81ba33631 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -17,6 +17,15 @@ import gtsam from gtsam.utils.test_case import GtsamTestCase from gtsam.symbol_shorthand import K, L, P + +def ulp(ftype=np.float64): + """ + Unit in the last place of floating point datatypes + """ + f = np.finfo(ftype) + return f.tiny / ftype(1 << f.nmant) + + class TestCal3Fisheye(GtsamTestCase): @classmethod @@ -105,27 +114,63 @@ class TestCal3Fisheye(GtsamTestCase): score = graph.error(state) self.assertAlmostEqual(score, 0) - def test_jacobian(self): - """Evaluate jacobian at optical axis""" + def test_jacobian_on_axis(self): + """Check of jacobian at optical axis""" obj_point_on_axis = np.array([0, 0, 1]) - img_point = np.array([0.0, 0.0]) + img_point = np.array([0, 0]) + f, z, H = self.evaluate_jacobian(obj_point_on_axis, img_point) + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) + + def test_jacobian_convergence(self): + """Test stability of jacobian close to optical axis""" + t = ulp(np.float64) + obj_point_close_to_axis = np.array([t, 0, 1]) + img_point = np.array([np.sqrt(t), 0]) + f, z, H = self.evaluate_jacobian(obj_point_close_to_axis, img_point) + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) + + # With a height of sqrt(ulp), this may cause an overflow + t = ulp(np.float64) + obj_point_close_to_axis = np.array([np.sqrt(t), 0, 1]) + img_point = np.array([np.sqrt(t), 0]) + f, z, H = self.evaluate_jacobian(obj_point_close_to_axis, img_point) + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) + + def test_scaling_factor(self): + "Check convergence of atan(r, z)/r for small r" + r = ulp(np.float64) + s = np.arctan(r) / r + self.assertEqual(s, 1.0) + z = 1 + s = np.arctan2(r, z) / r + self.assertEqual(s, 1.0) + z = 2 + s = np.arctan2(r, z) / r if r/z != 0 else 1.0 + self.assertEqual(s, 1.0) + + @staticmethod + def evaluate_jacobian(obj_point, img_point): + """Evaluate jacobian at given object point""" pose = gtsam.Pose3() camera = gtsam.Cal3Fisheye() state = gtsam.Values() camera_key, pose_key, landmark_key = K(0), P(0), L(0) - state.insert_point3(landmark_key, obj_point_on_axis) + state.insert_point3(landmark_key, obj_point) state.insert_pose3(pose_key, pose) - state.insert_cal3fisheye(camera_key, camera) g = gtsam.NonlinearFactorGraph() noise_model = gtsam.noiseModel.Unit.Create(2) - factor = gtsam.GeneralSFMFactor2Cal3Fisheye(img_point, noise_model, pose_key, landmark_key, camera_key) + factor = gtsam.GenericProjectionFactorCal3Fisheye(img_point, noise_model, pose_key, landmark_key, camera) g.add(factor) f = g.error(state) gaussian_factor_graph = g.linearize(state) H, z = gaussian_factor_graph.jacobian() - self.assertAlmostEqual(f, 0) - self.gtsamAssertEquals(z, np.zeros(2)) - self.gtsamAssertEquals(H @ H.T, 4*np.eye(2)) + return f, z, H @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation_skipped(self): From c0219c1ad018e2732943c6b775aa6ae7fb3b1d99 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 11:56:42 +0200 Subject: [PATCH 09/71] Numerically stable refactoring of fisheye jacobian --- gtsam/geometry/Cal3Fisheye.cpp | 36 ++++++++++++++++++++++------------ 1 file changed, 24 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index f889342b6..e95165c9a 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -46,7 +46,7 @@ double Cal3Fisheye::Scaling(double r) { /* ************************************************************************* */ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, OptionalJacobian<2, 2> H2) const { - const double xi = p.x(), yi = p.y(); + const double xi = p.x(), yi = p.y(), zi = 1; const double r2 = xi * xi + yi * yi, r = sqrt(r2); const double t = atan(r); const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4; @@ -81,22 +81,34 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, } else { const double dtd_dt = 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; - const double dt_dr = 1 / (1 + r2); + const double R2 = r2 + zi*zi + const double dt_dr = zi / R2; const double rinv = 1 / r; const double dr_dxi = xi * rinv; const double dr_dyi = yi * rinv; - const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; - const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; + const double dtd_dr = dtd_dt * dt_dr + // const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; + // const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; + + const double c2 = dr_dxi * dr_dxi + const double s2 = dr_dyi * dr_dyi + const double cs = dr_dxi * dr_dyi - const double td = t * K.dot(T); - const double rrinv = 1 / r2; - const double dxd_dxi = - dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi; - const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi; - const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi; - const double dyd_dyi = - dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi; + // Following refactoring is numerically stable, even for unnormalized radial + // values by avoiding division with the square radius. + // + // const double td = t * K.dot(T); - note: s = td/r + // const double rrinv = 1 / r2; - division by r2 may cause overflow + const double dxd_dxi = dtd_dr * c2 + s * (1 - c2); + const double dxd_dyi = (dtd_dr - s) * cs; + const double dyd_dxi = dxd_dyi; + const double dyd_dyi = dtd_dr * c2 + s * (1 - s2); + // Derivatives by depth, for future use to support incident + // angles above 90 deg. + // const double dxd_dzi = -dtd_dt * x / R2 + // const double dyd_dzi = -dtd_dt * y / R2 + Matrix2 DR; DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; From 0d01e4844f2974d838d2b43272f423e1e8ed7577 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 13:23:18 +0200 Subject: [PATCH 10/71] Fix missing semicolons --- gtsam/geometry/Cal3Fisheye.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index e95165c9a..ee2e66080 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -32,7 +32,7 @@ Vector9 Cal3Fisheye::vector() const { } /* ************************************************************************* */ -double Cal3Fisheye::Scaling(double r) { +double Cal3Fisheye::Scaling(double r, double zi) { static constexpr double threshold = 1e-8; if (r > threshold || r < -threshold) { return atan(r) / r; @@ -48,12 +48,12 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, OptionalJacobian<2, 2> H2) const { const double xi = p.x(), yi = p.y(), zi = 1; const double r2 = xi * xi + yi * yi, r = sqrt(r2); - const double t = atan(r); + const double t = atan2(r, zi); const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4; Vector5 K, T; K << 1, k1_, k2_, k3_, k4_; T << 1, t2, t4, t6, t8; - const double scaling = Scaling(r); + const double scaling = Scaling(r, zi); const double s = scaling * K.dot(T); const double xd = s * xi, yd = s * yi; Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_); @@ -81,18 +81,18 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, } else { const double dtd_dt = 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; - const double R2 = r2 + zi*zi + const double R2 = r2 + zi*zi; const double dt_dr = zi / R2; const double rinv = 1 / r; const double dr_dxi = xi * rinv; const double dr_dyi = yi * rinv; - const double dtd_dr = dtd_dt * dt_dr + const double dtd_dr = dtd_dt * dt_dr; // const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; // const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; - const double c2 = dr_dxi * dr_dxi - const double s2 = dr_dyi * dr_dyi - const double cs = dr_dxi * dr_dyi + const double c2 = dr_dxi * dr_dxi; + const double s2 = dr_dyi * dr_dyi; + const double cs = dr_dxi * dr_dyi; // Following refactoring is numerically stable, even for unnormalized radial // values by avoiding division with the square radius. From 8c2ea78b1aede2581fecacccb56526597aeb117b Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 13:27:05 +0200 Subject: [PATCH 11/71] Undo change in scaling function --- gtsam/geometry/Cal3Fisheye.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index ee2e66080..c4ce151ba 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -32,7 +32,7 @@ Vector9 Cal3Fisheye::vector() const { } /* ************************************************************************* */ -double Cal3Fisheye::Scaling(double r, double zi) { +double Cal3Fisheye::Scaling(double r) { static constexpr double threshold = 1e-8; if (r > threshold || r < -threshold) { return atan(r) / r; @@ -53,7 +53,7 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, Vector5 K, T; K << 1, k1_, k2_, k3_, k4_; T << 1, t2, t4, t6, t8; - const double scaling = Scaling(r, zi); + const double scaling = Scaling(r); const double s = scaling * K.dot(T); const double xd = s * xi, yd = s * yi; Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_); From e1db2be5bdad8ac02320a9488014db55d252ca1c Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 13:54:38 +0200 Subject: [PATCH 12/71] Fix type in extression for dyd_dyi --- gtsam/geometry/Cal3Fisheye.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index c4ce151ba..7bf9ea300 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -102,7 +102,7 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, const double dxd_dxi = dtd_dr * c2 + s * (1 - c2); const double dxd_dyi = (dtd_dr - s) * cs; const double dyd_dxi = dxd_dyi; - const double dyd_dyi = dtd_dr * c2 + s * (1 - s2); + const double dyd_dyi = dtd_dr * s2 + s * (1 - s2); // Derivatives by depth, for future use to support incident // angles above 90 deg. From 2763bd89686d95a70e40349c20d0324a254dc202 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 14:34:04 +0200 Subject: [PATCH 13/71] Convergence of equidistant scaling utilizing atan2 --- python/gtsam/tests/test_Cal3Fisheye.py | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 81ba33631..6bdaa5a12 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -143,16 +143,23 @@ class TestCal3Fisheye(GtsamTestCase): self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) def test_scaling_factor(self): - "Check convergence of atan(r, z)/r for small r" + """Check convergence of atan2(r, z)/r ~ 1/z for small r""" r = ulp(np.float64) s = np.arctan(r) / r self.assertEqual(s, 1.0) z = 1 - s = np.arctan2(r, z) / r - self.assertEqual(s, 1.0) + s = scaling_factor(r, z) + self.assertEqual(s, 1.0/z) z = 2 - s = np.arctan2(r, z) / r if r/z != 0 else 1.0 - self.assertEqual(s, 1.0) + s = scaling_factor(r, z) + self.assertEqual(s, 1.0/z) + s = scaling_factor(2*r, z) + self.assertEqual(s, 1.0/z) + + @staticmethod + def scaling_factor(r, z): + """Projection factor theta/r for equidistant fisheye lens model""" + return np.arctan2(r, z) / r if r/z != 0 else 1.0/z @staticmethod def evaluate_jacobian(obj_point, img_point): From 296c937ca83af7ced0301da06d38a09b1ec19a65 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 15:55:25 +0200 Subject: [PATCH 14/71] Fix calling scaling_factor static method. --- python/gtsam/tests/test_Cal3Fisheye.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 6bdaa5a12..e54afc757 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -148,12 +148,12 @@ class TestCal3Fisheye(GtsamTestCase): s = np.arctan(r) / r self.assertEqual(s, 1.0) z = 1 - s = scaling_factor(r, z) + s = self.scaling_factor(r, z) self.assertEqual(s, 1.0/z) z = 2 - s = scaling_factor(r, z) + s = self.scaling_factor(r, z) self.assertEqual(s, 1.0/z) - s = scaling_factor(2*r, z) + s = self.scaling_factor(2*r, z) self.assertEqual(s, 1.0/z) @staticmethod From 508db60f74a8c3c24da9aa2e0720f0c603a7a520 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 2 Nov 2021 12:04:04 -0400 Subject: [PATCH 15/71] add jacobian of second argument to adjoint and adjointTranpsose --- gtsam/geometry/Pose3.cpp | 12 ++++++++---- gtsam/geometry/Pose3.h | 10 ++++++---- gtsam/geometry/tests/testPose3.cpp | 27 +++++++++++++++++---------- 3 files changed, 31 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 4bfb574b1..59594680a 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -116,7 +116,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { /* ************************************************************************* */ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi) { + OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { if (Hxi) { Hxi->setZero(); for (int i = 0; i < 6; ++i) { @@ -127,12 +127,14 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, Hxi->col(i) = Gi * y; } } - return adjointMap(xi) * y; + const Matrix6& ad_xi = adjointMap(xi); + if (H_y) *H_y = ad_xi; + return ad_xi * y; } /* ************************************************************************* */ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi) { + OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { if (Hxi) { Hxi->setZero(); for (int i = 0; i < 6; ++i) { @@ -143,7 +145,9 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, Hxi->col(i) = GTi * y; } } - return adjointMap(xi).transpose() * y; + const Matrix6& adT_xi = adjointMap(xi).transpose(); + if (H_y) *H_y = adT_xi; + return adT_xi * y; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index b6107120e..884d0760c 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -177,13 +177,14 @@ public: * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. * */ - static Matrix6 adjointMap(const Vector6 &xi); + static Matrix6 adjointMap(const Vector6& xi); /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ - static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, - OptionalJacobian<6, 6> Hxi = boost::none); + static Vector6 adjoint(const Vector6& xi, const Vector6& y, + OptionalJacobian<6, 6> Hxi = boost::none, + OptionalJacobian<6, 6> H_y = boost::none); // temporary fix for wrappers until case issue is resolved static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} @@ -193,7 +194,8 @@ public: * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi = boost::none); + OptionalJacobian<6, 6> Hxi = boost::none, + OptionalJacobian<6, 6> H_y = boost::none); /// Derivative of Expmap static Matrix6 ExpmapDerivative(const Vector6& xi); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index f0f2c0ccd..e1d3d5ea2 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -912,16 +912,20 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) { } TEST( Pose3, adjoint) { - Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi); + Vector6 v = (Vector6() << 1, 2, 3, 4, 5, 6).finished(); + Vector expected = testDerivAdjoint(screwPose3::xi, v); - Matrix actualH; - Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH); + Matrix actualH1, actualH2; + Vector actual = Pose3::adjoint(screwPose3::xi, v, actualH1, actualH2); - Matrix numericalH = numericalDerivative21( - testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5); + Matrix numericalH1 = numericalDerivative21( + testDerivAdjoint, screwPose3::xi, v, 1e-5); + Matrix numericalH2 = numericalDerivative22( + testDerivAdjoint, screwPose3::xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-5)); - EXPECT(assert_equal(numericalH,actualH,1e-5)); + EXPECT(assert_equal(numericalH1,actualH1,1e-5)); + EXPECT(assert_equal(numericalH2,actualH2,1e-5)); } /* ************************************************************************* */ @@ -934,14 +938,17 @@ TEST( Pose3, adjointTranspose) { Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished(); Vector expected = testDerivAdjointTranspose(xi, v); - Matrix actualH; - Vector actual = Pose3::adjointTranspose(xi, v, actualH); + Matrix actualH1, actualH2; + Vector actual = Pose3::adjointTranspose(xi, v, actualH1, actualH2); - Matrix numericalH = numericalDerivative21( + Matrix numericalH1 = numericalDerivative21( + testDerivAdjointTranspose, xi, v, 1e-5); + Matrix numericalH2 = numericalDerivative22( testDerivAdjointTranspose, xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-15)); - EXPECT(assert_equal(numericalH,actualH,1e-5)); + EXPECT(assert_equal(numericalH1,actualH1,1e-5)); + EXPECT(assert_equal(numericalH2,actualH2,1e-5)); } /* ************************************************************************* */ From a61cbdc4d12e8fad03e98ed703ca40ebb72ec61e Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 3 Nov 2021 17:14:37 +0100 Subject: [PATCH 16/71] Fix warnings raised by GCC -Wpedactic --- gtsam/base/Manifold.h | 2 +- gtsam/base/Matrix.h | 22 +++++++++---------- gtsam/base/Testable.h | 2 +- gtsam/base/Vector.h | 4 ++-- gtsam/geometry/concepts.h | 4 ++-- gtsam/inference/FactorGraph.h | 2 +- gtsam/navigation/MagPoseFactor.h | 4 ++-- gtsam/nonlinear/CustomFactor.h | 2 +- gtsam/nonlinear/NonlinearEquality.h | 6 ++--- gtsam/nonlinear/PriorFactor.h | 2 +- gtsam_unstable/slam/BetweenFactorEM.h | 3 ++- gtsam_unstable/slam/PoseBetweenFactor.h | 4 ++-- gtsam_unstable/slam/PosePriorFactor.h | 4 ++-- .../slam/TransformBtwRobotsUnaryFactor.h | 4 ++-- 14 files changed, 33 insertions(+), 32 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index dbe497005..a14404268 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -178,4 +178,4 @@ struct FixedDimension { // * the gtsam namespace to be more easily enforced as testable // */ #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold; -#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold _gtsam_IsManifold_##T; +#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 013947bbd..26f03f3e3 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -46,18 +46,18 @@ typedef Eigen::Matrix M // Create handy typedefs and constants for square-size matrices // MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 #define GTSAM_MAKE_MATRIX_DEFS(N) \ -typedef Eigen::Matrix Matrix##N; \ -typedef Eigen::Matrix Matrix1##N; \ -typedef Eigen::Matrix Matrix2##N; \ -typedef Eigen::Matrix Matrix3##N; \ -typedef Eigen::Matrix Matrix4##N; \ -typedef Eigen::Matrix Matrix5##N; \ -typedef Eigen::Matrix Matrix6##N; \ -typedef Eigen::Matrix Matrix7##N; \ -typedef Eigen::Matrix Matrix8##N; \ -typedef Eigen::Matrix Matrix9##N; \ +using Matrix##N = Eigen::Matrix; \ +using Matrix1##N = Eigen::Matrix; \ +using Matrix2##N = Eigen::Matrix; \ +using Matrix3##N = Eigen::Matrix; \ +using Matrix4##N = Eigen::Matrix; \ +using Matrix5##N = Eigen::Matrix; \ +using Matrix6##N = Eigen::Matrix; \ +using Matrix7##N = Eigen::Matrix; \ +using Matrix8##N = Eigen::Matrix; \ +using Matrix9##N = Eigen::Matrix; \ static const Eigen::MatrixBase::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \ -static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = Matrix##N::Zero(); +static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = Matrix##N::Zero() GTSAM_MAKE_MATRIX_DEFS(1); GTSAM_MAKE_MATRIX_DEFS(2); diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 6062c7ae1..74e237699 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -173,4 +173,4 @@ namespace gtsam { * @deprecated please use BOOST_CONCEPT_ASSERT and */ #define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable; -#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::IsTestable _gtsam_Testable_##T; +#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index ed90a7126..8a7eb1d55 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -48,8 +48,8 @@ static const Eigen::MatrixBase::ConstantReturnType Z_3x1 = Vector3::Zer // Create handy typedefs and constants for vectors with N>3 // VectorN and Z_Nx1, for N=1..9 #define GTSAM_MAKE_VECTOR_DEFS(N) \ - typedef Eigen::Matrix Vector##N; \ - static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = Vector##N::Zero(); + using Vector##N = Eigen::Matrix; \ + static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = Vector##N::Zero() GTSAM_MAKE_VECTOR_DEFS(4); GTSAM_MAKE_VECTOR_DEFS(5); diff --git a/gtsam/geometry/concepts.h b/gtsam/geometry/concepts.h index 207b48f56..a313d4448 100644 --- a/gtsam/geometry/concepts.h +++ b/gtsam/geometry/concepts.h @@ -71,6 +71,6 @@ private: */ /** Pose Concept macros */ -#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept; -#define GTSAM_CONCEPT_POSE_TYPE(T) typedef gtsam::PoseConcept _gtsam_PoseConcept##T; +#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept +#define GTSAM_CONCEPT_POSE_TYPE(T) using _gtsam_PoseConcept##T = gtsam::PoseConcept; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index e337e3249..0a0c89f50 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -121,7 +121,7 @@ class FactorGraph { protected: /** concept check, makes sure FACTOR defines print and equals */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) + GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR); /** Collection of factors */ FastVector factors_; diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index da2a54ce9..feb6e0e19 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -45,8 +45,8 @@ class MagPoseFactor: public NoiseModelFactor1 { using shared_ptr = boost::shared_ptr>; /// Concept check by type. - GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE) + GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE); public: ~MagPoseFactor() override {} diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index dbaf31898..615b5418e 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -101,4 +101,4 @@ private: } }; -}; +} diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 47083d5d7..1d7be99fd 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -218,9 +218,9 @@ protected: X value_; /// fixed value for variable - GTSAM_CONCEPT_MANIFOLD_TYPE(X) + GTSAM_CONCEPT_MANIFOLD_TYPE(X); - GTSAM_CONCEPT_TESTABLE_TYPE(X) + GTSAM_CONCEPT_TESTABLE_TYPE(X); public: @@ -296,7 +296,7 @@ class NonlinearEquality2 : public NoiseModelFactor2 { using Base = NoiseModelFactor2; using This = NonlinearEquality2; - GTSAM_CONCEPT_MANIFOLD_TYPE(T) + GTSAM_CONCEPT_MANIFOLD_TYPE(T); /// Default constructor to allow for serialization NonlinearEquality2() {} diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index c745f7bd9..71c204ad0 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -39,7 +39,7 @@ namespace gtsam { VALUE prior_; /** The measurement */ /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(T) + GTSAM_CONCEPT_TESTABLE_TYPE(T); public: diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 98ec59fe9..f78f9b334 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -56,7 +56,8 @@ private: bool flag_bump_up_near_zero_probs_; /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T)GTSAM_CONCEPT_TESTABLE_TYPE(T) + GTSAM_CONCEPT_LIE_TYPE(T); + GTSAM_CONCEPT_TESTABLE_TYPE(T); public: diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index a6c583418..624fd4a54 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -40,8 +40,8 @@ namespace gtsam { boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE) + GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE); public: // shorthand for a smart pointer to a factor diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index f657fc443..bea849c00 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -37,8 +37,8 @@ namespace gtsam { boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE) + GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE); public: /// shorthand for a smart pointer to a factor diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 956c75999..3f2e02a78 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -55,8 +55,8 @@ namespace gtsam { SharedGaussian model_; /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T) - GTSAM_CONCEPT_TESTABLE_TYPE(T) + GTSAM_CONCEPT_LIE_TYPE(T); + GTSAM_CONCEPT_TESTABLE_TYPE(T); public: From 89ce766269b0fece98cdb47c3c47a4096d437164 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Thu, 4 Nov 2021 07:11:28 +0100 Subject: [PATCH 17/71] more consistent notation of macros --- gtsam/base/Lie.h | 2 +- gtsam/base/Manifold.h | 2 +- gtsam/base/Testable.h | 2 +- gtsam/base/chartTesting.h | 2 +- gtsam/geometry/PinholePose.h | 2 +- gtsam/inference/ClusterTree.h | 2 +- gtsam/inference/EliminationTree.h | 2 +- gtsam/inference/FactorGraph.h | 2 +- gtsam/navigation/MagPoseFactor.h | 2 +- gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h | 2 +- gtsam/nonlinear/NonlinearEquality.h | 7 +++---- gtsam/nonlinear/PriorFactor.h | 2 +- gtsam/slam/GeneralSFMFactor.h | 6 +++--- gtsam_unstable/slam/BetweenFactorEM.h | 4 ++-- gtsam_unstable/slam/PoseBetweenFactor.h | 2 +- gtsam_unstable/slam/PosePriorFactor.h | 2 +- gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h | 4 ++-- 17 files changed, 23 insertions(+), 24 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index ac7c2a9a5..cb8e7d017 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -370,4 +370,4 @@ public: * the gtsam namespace to be more easily enforced as testable */ #define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup; -#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup _gtsam_IsLieGroup_##T; +#define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam::IsLieGroup; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index a14404268..962dc8269 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -178,4 +178,4 @@ struct FixedDimension { // * the gtsam namespace to be more easily enforced as testable // */ #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold; -#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold +#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold; diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 74e237699..d50d62c1f 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -173,4 +173,4 @@ namespace gtsam { * @deprecated please use BOOST_CONCEPT_ASSERT and */ #define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable; -#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable +#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable; diff --git a/gtsam/base/chartTesting.h b/gtsam/base/chartTesting.h index f63054a5b..8f5213f91 100644 --- a/gtsam/base/chartTesting.h +++ b/gtsam/base/chartTesting.h @@ -32,7 +32,7 @@ void testDefaultChart(TestResult& result_, const std::string& name_, const T& value) { - GTSAM_CONCEPT_TESTABLE_TYPE(T); + GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef typename gtsam::DefaultChart Chart; typedef typename Chart::vector Vector; diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index cc6435a58..7a0b08227 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -35,7 +35,7 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase { private: - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) // Get dimensions of calibration type at compile time static const int DimK = FixedDimension::value; diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index e225bac5f..7dd414193 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -110,7 +110,7 @@ class ClusterTree { typedef sharedCluster sharedNode; /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType) protected: FastVector roots_; diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index e4a64c589..70e10b3bd 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -81,7 +81,7 @@ namespace gtsam { protected: /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType) FastVector roots_; FastVector remainingFactors_; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 0a0c89f50..e337e3249 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -121,7 +121,7 @@ class FactorGraph { protected: /** concept check, makes sure FACTOR defines print and equals */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR); + GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) /** Collection of factors */ FastVector factors_; diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index feb6e0e19..c0a6a7ece 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -45,7 +45,7 @@ class MagPoseFactor: public NoiseModelFactor1 { using shared_ptr = boost::shared_ptr>; /// Concept check by type. - GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_TESTABLE_TYPE(POSE) GTSAM_CONCEPT_POSE_TYPE(POSE); public: diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index fd9e49a62..a7a0d724b 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -149,7 +149,7 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { - // GTSAM_CONCEPT_MANIFOLD_TYPE(V); + // GTSAM_CONCEPT_MANIFOLD_TYPE(V) size_t iteration = 0; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 1d7be99fd..43d30254e 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -218,9 +218,8 @@ protected: X value_; /// fixed value for variable - GTSAM_CONCEPT_MANIFOLD_TYPE(X); - - GTSAM_CONCEPT_TESTABLE_TYPE(X); + GTSAM_CONCEPT_MANIFOLD_TYPE(X) + GTSAM_CONCEPT_TESTABLE_TYPE(X) public: @@ -296,7 +295,7 @@ class NonlinearEquality2 : public NoiseModelFactor2 { using Base = NoiseModelFactor2; using This = NonlinearEquality2; - GTSAM_CONCEPT_MANIFOLD_TYPE(T); + GTSAM_CONCEPT_MANIFOLD_TYPE(T) /// Default constructor to allow for serialization NonlinearEquality2() {} diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 71c204ad0..c745f7bd9 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -39,7 +39,7 @@ namespace gtsam { VALUE prior_; /** The measurement */ /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(T); + GTSAM_CONCEPT_TESTABLE_TYPE(T) public: diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 2e4543177..bfc3a0f78 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -59,8 +59,8 @@ namespace gtsam { template class GeneralSFMFactor: public NoiseModelFactor2 { - GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA); - GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK); + GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) + GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) static const int DimC = FixedDimension::value; static const int DimL = FixedDimension::value; @@ -202,7 +202,7 @@ struct traits > : Testable< template class GeneralSFMFactor2: public NoiseModelFactor3 { - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) static const int DimK = FixedDimension::value; protected: diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index f78f9b334..572935da3 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -56,8 +56,8 @@ private: bool flag_bump_up_near_zero_probs_; /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T); - GTSAM_CONCEPT_TESTABLE_TYPE(T); + GTSAM_CONCEPT_LIE_TYPE(T) + GTSAM_CONCEPT_TESTABLE_TYPE(T) public: diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 624fd4a54..444da275d 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -40,7 +40,7 @@ namespace gtsam { boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_TESTABLE_TYPE(POSE) GTSAM_CONCEPT_POSE_TYPE(POSE); public: diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index bea849c00..665bb4680 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -37,7 +37,7 @@ namespace gtsam { boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_TESTABLE_TYPE(POSE) GTSAM_CONCEPT_POSE_TYPE(POSE); public: diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 3f2e02a78..956c75999 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -55,8 +55,8 @@ namespace gtsam { SharedGaussian model_; /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T); - GTSAM_CONCEPT_TESTABLE_TYPE(T); + GTSAM_CONCEPT_LIE_TYPE(T) + GTSAM_CONCEPT_TESTABLE_TYPE(T) public: From 94aa96e00a9bb21e7cccb75219c614d1a91653ff Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 8 Nov 2021 22:41:59 +0100 Subject: [PATCH 18/71] prefer semicolon in definitions --- gtsam/base/Matrix.h | 20 ++++++++++---------- gtsam/base/Vector.h | 20 ++++++++++---------- gtsam/geometry/Pose2.cpp | 2 +- gtsam/geometry/Pose3.cpp | 2 +- gtsam/geometry/concepts.h | 2 +- gtsam/navigation/MagPoseFactor.h | 2 +- gtsam_unstable/slam/PoseBetweenFactor.h | 2 +- gtsam_unstable/slam/PosePriorFactor.h | 2 +- 8 files changed, 26 insertions(+), 26 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 26f03f3e3..61c61a5af 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -57,17 +57,17 @@ using Matrix7##N = Eigen::Matrix; \ using Matrix8##N = Eigen::Matrix; \ using Matrix9##N = Eigen::Matrix; \ static const Eigen::MatrixBase::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \ -static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = Matrix##N::Zero() +static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = Matrix##N::Zero(); -GTSAM_MAKE_MATRIX_DEFS(1); -GTSAM_MAKE_MATRIX_DEFS(2); -GTSAM_MAKE_MATRIX_DEFS(3); -GTSAM_MAKE_MATRIX_DEFS(4); -GTSAM_MAKE_MATRIX_DEFS(5); -GTSAM_MAKE_MATRIX_DEFS(6); -GTSAM_MAKE_MATRIX_DEFS(7); -GTSAM_MAKE_MATRIX_DEFS(8); -GTSAM_MAKE_MATRIX_DEFS(9); +GTSAM_MAKE_MATRIX_DEFS(1) +GTSAM_MAKE_MATRIX_DEFS(2) +GTSAM_MAKE_MATRIX_DEFS(3) +GTSAM_MAKE_MATRIX_DEFS(4) +GTSAM_MAKE_MATRIX_DEFS(5) +GTSAM_MAKE_MATRIX_DEFS(6) +GTSAM_MAKE_MATRIX_DEFS(7) +GTSAM_MAKE_MATRIX_DEFS(8) +GTSAM_MAKE_MATRIX_DEFS(9) // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 8a7eb1d55..35c68c4b4 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -49,17 +49,17 @@ static const Eigen::MatrixBase::ConstantReturnType Z_3x1 = Vector3::Zer // VectorN and Z_Nx1, for N=1..9 #define GTSAM_MAKE_VECTOR_DEFS(N) \ using Vector##N = Eigen::Matrix; \ - static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = Vector##N::Zero() + static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = Vector##N::Zero(); -GTSAM_MAKE_VECTOR_DEFS(4); -GTSAM_MAKE_VECTOR_DEFS(5); -GTSAM_MAKE_VECTOR_DEFS(6); -GTSAM_MAKE_VECTOR_DEFS(7); -GTSAM_MAKE_VECTOR_DEFS(8); -GTSAM_MAKE_VECTOR_DEFS(9); -GTSAM_MAKE_VECTOR_DEFS(10); -GTSAM_MAKE_VECTOR_DEFS(11); -GTSAM_MAKE_VECTOR_DEFS(12); +GTSAM_MAKE_VECTOR_DEFS(4) +GTSAM_MAKE_VECTOR_DEFS(5) +GTSAM_MAKE_VECTOR_DEFS(6) +GTSAM_MAKE_VECTOR_DEFS(7) +GTSAM_MAKE_VECTOR_DEFS(8) +GTSAM_MAKE_VECTOR_DEFS(9) +GTSAM_MAKE_VECTOR_DEFS(10) +GTSAM_MAKE_VECTOR_DEFS(11) +GTSAM_MAKE_VECTOR_DEFS(12) typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 8dafffee8..a0b3d502e 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -28,7 +28,7 @@ using namespace std; namespace gtsam { /** instantiate concept checks */ -GTSAM_CONCEPT_POSE_INST(Pose2); +GTSAM_CONCEPT_POSE_INST(Pose2) static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 4bfb574b1..f8c61fd59 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -30,7 +30,7 @@ using std::vector; using Point3Pairs = vector; /** instantiate concept checks */ -GTSAM_CONCEPT_POSE_INST(Pose3); +GTSAM_CONCEPT_POSE_INST(Pose3) /* ************************************************************************* */ Pose3::Pose3(const Pose2& pose2) : diff --git a/gtsam/geometry/concepts.h b/gtsam/geometry/concepts.h index a313d4448..bafb62418 100644 --- a/gtsam/geometry/concepts.h +++ b/gtsam/geometry/concepts.h @@ -71,6 +71,6 @@ private: */ /** Pose Concept macros */ -#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept +#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept; #define GTSAM_CONCEPT_POSE_TYPE(T) using _gtsam_PoseConcept##T = gtsam::PoseConcept; diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index c0a6a7ece..da2a54ce9 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -46,7 +46,7 @@ class MagPoseFactor: public NoiseModelFactor1 { /// Concept check by type. GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE) public: ~MagPoseFactor() override {} diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 444da275d..a6c583418 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -41,7 +41,7 @@ namespace gtsam { /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE) public: // shorthand for a smart pointer to a factor diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 665bb4680..f657fc443 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -38,7 +38,7 @@ namespace gtsam { /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE) public: /// shorthand for a smart pointer to a factor From 582f6914cd8491a83bb40fcc4091a4dd83d69c20 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 8 Nov 2021 23:07:05 +0100 Subject: [PATCH 19/71] more extra semicolon warnings fixed --- .../linear/tests/testSerializationLinear.cpp | 20 +-- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/CombinedImuFactor.h | 2 +- gtsam/navigation/tests/testAttitudeFactor.cpp | 2 +- .../tests/testImuFactorSerialization.cpp | 14 +- .../tests/testSerializationNonlinear.cpp | 48 +++--- gtsam/slam/tests/testSmartFactorBase.cpp | 14 +- .../slam/tests/testSmartProjectionFactor.cpp | 14 +- .../tests/testSmartProjectionPoseFactor.cpp | 14 +- gtsam_unstable/linear/tests/testQPSolver.cpp | 2 +- tests/testExpressionFactor.cpp | 2 +- tests/testSerializationSLAM.cpp | 142 +++++++++--------- tests/testSubgraphPreconditioner.cpp | 2 +- 13 files changed, 139 insertions(+), 139 deletions(-) diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index c5b3dab37..881b2830e 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -39,14 +39,14 @@ using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ // Export Noisemodels // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") /* ************************************************************************* */ // example noise models @@ -129,9 +129,9 @@ TEST (Serialization, SharedDiagonal_noiseModels) { /* Create GUIDs for factors */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional") /* ************************************************************************* */ TEST (Serialization, linear_factors) { diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index ca1c5b93a..7d086eb57 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -257,5 +257,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) { /// namespace gtsam /// Boost serialization export definition for derived class -BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams); +BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index ed94750b7..89e8b574f 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -353,4 +353,4 @@ struct traits : public Testable {}; } // namespace gtsam /// Add Boost serialization export key (declaration) for derived class -BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams); +BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams) diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index d49907cbf..9304e8412 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -66,7 +66,7 @@ TEST( Rot3AttitudeFactor, Constructor ) { /* ************************************************************************* */ // Export Noisemodels // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); +BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic) /* ************************************************************************* */ TEST(Rot3AttitudeFactor, Serialization) { diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index ed72e18e9..e72b1fb5b 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -31,16 +31,16 @@ using namespace gtsam; using namespace gtsam::serializationTestHelpers; BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, - "gtsam_noiseModel_Constrained"); + "gtsam_noiseModel_Constrained") BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, - "gtsam_noiseModel_Diagonal"); + "gtsam_noiseModel_Diagonal") BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, - "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); + "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, - "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") template P getPreintegratedMeasurements() { diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index f4bb5f4f6..4a73cbb0b 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -35,37 +35,37 @@ using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ // Create GUIDs for Noisemodels -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") /* ************************************************************************* */ // Create GUIDs for factors -BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); +BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional") /* ************************************************************************* */ // Export all classes derived from Value -GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); -GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler); -GTSAM_VALUE_EXPORT(gtsam::Point3); -GTSAM_VALUE_EXPORT(gtsam::Pose3); -GTSAM_VALUE_EXPORT(gtsam::Rot3); -GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); -GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); -GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2) +GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler) +GTSAM_VALUE_EXPORT(gtsam::Point3) +GTSAM_VALUE_EXPORT(gtsam::Pose3) +GTSAM_VALUE_EXPORT(gtsam::Rot3) +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera) +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera) +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera) namespace detail { template struct pack { diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index 951cbf8f4..ba46dce8d 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -127,13 +127,13 @@ TEST(SmartFactorBase, Stereo) { } /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") TEST(SmartFactorBase, serialize) { using namespace gtsam::serializationTestHelpers; diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 1fd06cc9f..133f81511 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -811,13 +811,13 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) { } /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") TEST(SmartProjectionFactor, serialize) { using namespace vanilla; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 997c33846..f3706fa02 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -1333,13 +1333,13 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { } /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") TEST(SmartProjectionPoseFactor, serialize) { using namespace vanillaPose; diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 67a0c971e..12bd93416 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -226,7 +226,7 @@ pair testParser(QPSParser parser) { expected.inequalities.add(X1, -I_1x1, 0, 2); // x >= 0 expected.inequalities.add(X2, -I_1x1, 0, 3); // y > = 0 return {expected, exampleqp}; -}; +} TEST(QPSolver, ParserSyntaticTest) { auto result = testParser(QPSParser("QPExample.QPS")); diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 66dbed1eb..75425a0cd 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -606,7 +606,7 @@ Vector3 f(const Point2& a, const Vector3& b, OptionalJacobian<3, 2> H1, if (H1) *H1 << b.y(), b.z(), b.x(), 0, 0, 0; if (H2) *H2 = A; return A * b; -}; +} } TEST(ExpressionFactor, MultiplyWithInverseFunction) { diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 2e99aff71..496122419 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -114,94 +114,94 @@ using symbol_shorthand::L; /* Create GUIDs for Noisemodels */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") /* Create GUIDs for geometry */ /* ************************************************************************* */ -GTSAM_VALUE_EXPORT(gtsam::Point2); -GTSAM_VALUE_EXPORT(gtsam::StereoPoint2); -GTSAM_VALUE_EXPORT(gtsam::Point3); -GTSAM_VALUE_EXPORT(gtsam::Rot2); -GTSAM_VALUE_EXPORT(gtsam::Rot3); -GTSAM_VALUE_EXPORT(gtsam::Pose2); -GTSAM_VALUE_EXPORT(gtsam::Pose3); -GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); -GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); -GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); -GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); -GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); -GTSAM_VALUE_EXPORT(gtsam::StereoCamera); +GTSAM_VALUE_EXPORT(gtsam::Point2) +GTSAM_VALUE_EXPORT(gtsam::StereoPoint2) +GTSAM_VALUE_EXPORT(gtsam::Point3) +GTSAM_VALUE_EXPORT(gtsam::Rot2) +GTSAM_VALUE_EXPORT(gtsam::Rot3) +GTSAM_VALUE_EXPORT(gtsam::Pose2) +GTSAM_VALUE_EXPORT(gtsam::Pose3) +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2) +GTSAM_VALUE_EXPORT(gtsam::Cal3DS2) +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo) +GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera) +GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2) +GTSAM_VALUE_EXPORT(gtsam::StereoCamera) /* Create GUIDs for factors */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor") -BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3"); -BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3"); -BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3"); -BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2") +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2") +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3") +BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2") +BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3") +BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2") +BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3") +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2") +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2") +BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera") +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera") -BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2") +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3") +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2") +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3") +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2") +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3") -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera") -BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D"); -BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); -BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point"); -BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2"); +BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D") +BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D") +BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2") +BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3") +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint") +BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point") +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera") +BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2") -BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); +BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D") -BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); -BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2") +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2") -BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); -BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2") +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2") -BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2") -BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); +BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D") /* ************************************************************************* */ diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index fb9f7a5a2..84ccc131a 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -200,7 +200,7 @@ TEST(SubgraphPreconditioner, system) { } /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor") // Read from XML file static GaussianFactorGraph read(const string& name) { From 7d468e98a04a581caeafb674e0ac4af024552f59 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 14:50:48 -0500 Subject: [PATCH 20/71] fix warning --- gtsam/nonlinear/GncOptimizer.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3ddaf4820..3025d2468 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -142,8 +142,9 @@ class GTSAM_EXPORT GncOptimizer { * provides an extra interface for the user to initialize the weightst * */ void setWeights(const Vector w) { - if(w.size() != nfg_.size()){ - throw std::runtime_error("GncOptimizer::setWeights: the number of specified weights" + if (size_t(w.size()) != nfg_.size()) { + throw std::runtime_error( + "GncOptimizer::setWeights: the number of specified weights" " does not match the size of the factor graph."); } weights_ = w; From bfb21c2faad20e2731cd4d98c56c9b78d4e60ea1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 16:50:14 -0500 Subject: [PATCH 21/71] reduce call stack --- gtsam/inference/EliminateableFactorGraph-inst.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 81f4047a1..4157336d1 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -36,17 +36,17 @@ namespace gtsam { // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex // before creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(function, computedVariableIndex, orderingType); + return eliminateSequential(orderingType, function, computedVariableIndex); } else { // Compute an ordering and call this function again. We are guaranteed to have a // VariableIndex already here because we computed one if needed in the previous 'if' block. if (orderingType == Ordering::METIS) { Ordering computedOrdering = Ordering::Metis(asDerived()); - return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + return eliminateSequential(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = Ordering::Colamd(*variableIndex); - return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + return eliminateSequential(computedOrdering, function, variableIndex); } } } From 916504129990b40683427b81453cacbc787a82da Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 17:56:03 -0500 Subject: [PATCH 22/71] use safer eigen indexing syntax --- gtsam/nonlinear/factorTesting.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index 74ef87737..3a9b6fb11 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -35,8 +35,7 @@ namespace gtsam { * zero errors anyway. However, it means that below will only be exact for the correct measurement. */ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, - const Values& values, double delta = 1e-5) { - + const Values& values, double delta = 1e-5) { // We will fill a vector of key/Jacobians pairs (a map would sort) std::vector > jacobians; @@ -46,24 +45,24 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, // Loop over all variables const double one_over_2delta = 1.0 / (2.0 * delta); - for(Key key: factor) { + for (Key key : factor) { // Compute central differences using the values struct. VectorValues dX = values.zeroVectors(); const size_t cols = dX.dim(key); Matrix J = Matrix::Zero(rows, cols); for (size_t col = 0; col < cols; ++col) { Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); - dx[col] = delta; + dx(col) = delta; dX[key] = dx; Values eval_values = values.retract(dX); const Eigen::VectorXd left = factor.whitenedError(eval_values); - dx[col] = -delta; + dx(col) = -delta; dX[key] = dx; eval_values = values.retract(dX); const Eigen::VectorXd right = factor.whitenedError(eval_values); J.col(col) = (left - right) * one_over_2delta; } - jacobians.push_back(std::make_pair(key,J)); + jacobians.push_back(std::make_pair(key, J)); } // Next step...return JacobianFactor From a634a91c1a7233d1afaa016d3e909ae7077d4b73 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 18:14:01 -0500 Subject: [PATCH 23/71] wrap Colamd function --- gtsam/nonlinear/nonlinear.i | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index d068bd7ee..ecf63094d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -115,6 +115,10 @@ class Ordering { Ordering(); Ordering(const gtsam::Ordering& other); + template + static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); + // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; From 1bcb44784a1dc7f5239001b1cec6a6383a058ed9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 18:19:47 -0500 Subject: [PATCH 24/71] format and refactor the SFM BAL example --- python/gtsam/examples/SFMExample_bal.py | 107 +++++++++++++----------- 1 file changed, 59 insertions(+), 48 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index dfe8b523c..65b9e1334 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -7,49 +7,58 @@ See LICENSE for the license information Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file - Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert) + Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert, Varun Agrawal) """ import argparse import logging import sys -import matplotlib.pyplot as plt -import numpy as np - import gtsam -from gtsam import ( - GeneralSFMFactorCal3Bundler, - PinholeCameraCal3Bundler, - PriorFactorPinholeCameraCal3Bundler, - readBal, - symbol_shorthand -) +from gtsam import (GeneralSFMFactorCal3Bundler, + PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3, + readBal) +from gtsam.symbol_shorthand import C, P +from gtsam.utils.plot import plot_3d_points, plot_trajectory -C = symbol_shorthand.C -P = symbol_shorthand.P +logging.basicConfig(stream=sys.stdout, level=logging.INFO) -logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) +def plot(scene_data: gtsam.SfmData, result: gtsam.Values): + """Plot the trajectory.""" + plot_vals = gtsam.Values() + for cam_idx in range(scene_data.number_cameras()): + plot_vals.insert(C(cam_idx), + result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) + for t_idx in range(scene_data.number_tracks()): + plot_vals.insert(P(t_idx), result.atPoint3(P(t_idx))) -def run(args): + plot_3d_points(0, plot_vals, linespec="g.") + plot_trajectory(0, plot_vals, show=True) + + +def run(args: argparse.Namespace): """ Run LM optimization with BAL input data and report resulting error """ - input_file = gtsam.findExampleDataFile(args.input_file) + if args.input_file: + input_file = args.input_file + else: + input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") # Load the SfM data from file scene_data = readBal(input_file) - logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n") + logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(), + scene_data.number_cameras()) # Create a factor graph graph = gtsam.NonlinearFactorGraph() # We share *one* noiseModel between all projection factors - noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Add measurements to the factor graph j = 0 for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) # SfmTrack + track = scene_data.track(t_idx) # SfmTrack # retrieve the SfmMeasurement objects for m_idx in range(track.number_measurements()): # i represents the camera index, and uv is the 2d measurement @@ -60,20 +69,18 @@ def run(args): # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( - gtsam.PriorFactorPinholeCameraCal3Bundler( - C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) - ) - ) + PriorFactorPinholeCameraCal3Bundler( + C(0), scene_data.camera(0), + gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) # Also add a prior on the position of the first landmark to fix the scale graph.push_back( - gtsam.PriorFactorPoint3( - P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - ) - ) + PriorFactorPoint3(P(0), + scene_data.track(0).point3(), + gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) # Create initial estimate initial = gtsam.Values() - + i = 0 # add each PinholeCameraCal3Bundler for cam_idx in range(scene_data.number_cameras()): @@ -81,12 +88,10 @@ def run(args): initial.insert(C(i), camera) i += 1 - j = 0 # add each SfmTrack for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) - initial.insert(P(j), track.point3()) - j += 1 + track = scene_data.track(t_idx) + initial.insert(P(t_idx), track.point3()) # Optimize the graph and print results try: @@ -94,25 +99,31 @@ def run(args): params.setVerbosityLM("ERROR") lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = lm.optimize() - except Exception as e: + except RuntimeError: logging.exception("LM Optimization failed") return + # Error drops from ~2764.22 to ~0.046 - logging.info(f"final error: {graph.error(result)}") + logging.info("initial error: %f", graph.error(initial)) + logging.info("final error: %f", graph.error(result)) + + plot(scene_data, result) + + +def main(): + """Main runner.""" + parser = argparse.ArgumentParser() + parser.add_argument('-i', + '--input_file', + type=str, + default="", + help="""Read SFM data from the specified BAL file. + The data format is described here: https://grail.cs.washington.edu/projects/bal/. + BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, + then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector + and (x,y,z) 3d point initializations.""") + run(parser.parse_args()) if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument( - '-i', - '--input_file', - type=str, - default="dubrovnik-3-7-pre", - help='Read SFM data from the specified BAL file' - 'The data format is described here: https://grail.cs.washington.edu/projects/bal/.' - 'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, ' - 'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector' - 'and (x,y,z) 3d point initializations.' - ) - run(parser.parse_args()) - + main() From 5051f19f307783420365e1e263ac664710d94a33 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 18:25:42 -0500 Subject: [PATCH 25/71] properly deprecate eliminate functions --- .../inference/EliminateableFactorGraph-inst.h | 42 ++++++++++--------- gtsam/inference/EliminateableFactorGraph.h | 2 + gtsam/linear/GaussianFactorGraph.cpp | 5 ++- gtsam/nonlinear/Marginals.cpp | 11 +++-- gtsam/nonlinear/NonlinearOptimizer.cpp | 10 +++-- 5 files changed, 40 insertions(+), 30 deletions(-) diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 4157336d1..35e7505c9 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -78,29 +78,31 @@ namespace gtsam { } /* ************************************************************************* */ - template - boost::shared_ptr::BayesTreeType> - EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrderingType orderingType, const Eliminate& function, - OptionalVariableIndex variableIndex) const - { - if(!variableIndex) { - // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check - // for no variable index first so that it's always computed if we need to call COLAMD because - // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex - // before creating ordering. + template + boost::shared_ptr< + typename EliminateableFactorGraph::BayesTreeType> + EliminateableFactorGraph::eliminateMultifrontal( + OptionalOrderingType orderingType, const Eliminate& function, + OptionalVariableIndex variableIndex) const { + if (!variableIndex) { + // If no VariableIndex provided, compute one and call this function again + // IMPORTANT: we check for no variable index first so that it's always + // computed if we need to call COLAMD because no Ordering is provided. + // When removing optional from VariableIndex, create VariableIndex before + // creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(function, computedVariableIndex, orderingType); - } - else { - // Compute an ordering and call this function again. We are guaranteed to have a - // VariableIndex already here because we computed one if needed in the previous 'if' block. + return eliminateMultifrontal(orderingType, function, + computedVariableIndex); + } else { + // Compute an ordering and call this function again. We are guaranteed to + // have a VariableIndex already here because we computed one if needed in + // the previous 'if' block. if (orderingType == Ordering::METIS) { Ordering computedOrdering = Ordering::Metis(asDerived()); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + return eliminateMultifrontal(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = Ordering::Colamd(*variableIndex); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + return eliminateMultifrontal(computedOrdering, function, variableIndex); } } } @@ -273,7 +275,7 @@ namespace gtsam { else { // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateSequential(function); + return factorGraph->eliminateSequential(Ordering::COLAMD, function); } } } @@ -340,7 +342,7 @@ namespace gtsam { else { // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateMultifrontal(function); + return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function); } } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index edc4883e7..3c51d8f84 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -288,6 +288,7 @@ namespace gtsam { FactorGraphType& asDerived() { return static_cast(*this); } public: + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated ordering and orderingType shouldn't both be specified */ boost::shared_ptr eliminateSequential( const Ordering& ordering, @@ -339,6 +340,7 @@ namespace gtsam { OptionalVariableIndex variableIndex = boost::none) const { return marginalMultifrontalBayesTree(variables, function, variableIndex); } + #endif }; } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 24c4b9a0d..dc2eb8dd6 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -290,10 +290,11 @@ namespace gtsam { return blocks; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const { gttic(GaussianFactorGraph_optimize); - return BaseEliminateable::eliminateMultifrontal(function)->optimize(); + return BaseEliminateable::eliminateMultifrontal(Ordering::COLAMD, function) + ->optimize(); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index c29a79623..2a3dddc37 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -80,11 +80,14 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut /* ************************************************************************* */ void Marginals::computeBayesTree() { + // The default ordering to use. + const Ordering ordering = Ordering::COLAMND; // Compute BayesTree - if(factorization_ == CHOLESKY) - bayesTree_ = *graph_.eliminateMultifrontal(EliminatePreferCholesky); - else if(factorization_ == QR) - bayesTree_ = *graph_.eliminateMultifrontal(EliminateQR); + if (factorization_ == CHOLESKY) + bayesTree_ = + *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); + else if (factorization_ == QR) + bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminateQR); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 0d7e9e17f..3ce6db4af 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -147,11 +147,13 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) if (params.ordering) - delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), - boost::none, params.orderingType)->optimize(); + delta = gfg.eliminateSequential(*params.ordering, + params.getEliminationFunction()) + ->optimize(); else - delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none, - params.orderingType)->optimize(); + delta = gfg.eliminateSequential(params.orderingType, + params.getEliminationFunction()) + ->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams if (!params.iterativeParams) From 5c9c60a0be9e0ac556e0b6801412814ecb7434bd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Nov 2021 13:57:31 -0500 Subject: [PATCH 26/71] address reviewer comments --- python/gtsam/examples/SFMExample_bal.py | 45 ++++++++++++------------- 1 file changed, 22 insertions(+), 23 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 65b9e1334..77c186bd3 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -16,36 +16,37 @@ import sys import gtsam from gtsam import (GeneralSFMFactorCal3Bundler, - PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3, - readBal) + PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3) from gtsam.symbol_shorthand import C, P -from gtsam.utils.plot import plot_3d_points, plot_trajectory +from gtsam.utils import plot +from matplotlib import pyplot as plt logging.basicConfig(stream=sys.stdout, level=logging.INFO) +DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre" -def plot(scene_data: gtsam.SfmData, result: gtsam.Values): - """Plot the trajectory.""" + +def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values): + """Plot the SFM results.""" plot_vals = gtsam.Values() for cam_idx in range(scene_data.number_cameras()): plot_vals.insert(C(cam_idx), result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) - for t_idx in range(scene_data.number_tracks()): - plot_vals.insert(P(t_idx), result.atPoint3(P(t_idx))) + for j in range(scene_data.number_tracks()): + plot_vals.insert(P(j), result.atPoint3(P(j))) - plot_3d_points(0, plot_vals, linespec="g.") - plot_trajectory(0, plot_vals, show=True) + plot.plot_3d_points(0, plot_vals, linespec="g.") + plot.plot_trajectory(0, plot_vals, title="SFM results") + + plt.show() def run(args: argparse.Namespace): """ Run LM optimization with BAL input data and report resulting error """ - if args.input_file: - input_file = args.input_file - else: - input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") + input_file = args.input_file # Load the SfM data from file - scene_data = readBal(input_file) + scene_data = gtsam.readBal(input_file) logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(), scene_data.number_cameras()) @@ -56,16 +57,14 @@ def run(args: argparse.Namespace): noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Add measurements to the factor graph - j = 0 - for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) # SfmTrack + for j in range(scene_data.number_tracks()): + track = scene_data.track(j) # SfmTrack # retrieve the SfmMeasurement objects for m_idx in range(track.number_measurements()): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) # note use of shorthand symbols C and P graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) - j += 1 # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( @@ -89,9 +88,9 @@ def run(args: argparse.Namespace): i += 1 # add each SfmTrack - for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) - initial.insert(P(t_idx), track.point3()) + for j in range(scene_data.number_tracks()): + track = scene_data.track(j) + initial.insert(P(j), track.point3()) # Optimize the graph and print results try: @@ -107,7 +106,7 @@ def run(args: argparse.Namespace): logging.info("initial error: %f", graph.error(initial)) logging.info("final error: %f", graph.error(result)) - plot(scene_data, result) + plot_scene(scene_data, result) def main(): @@ -116,7 +115,7 @@ def main(): parser.add_argument('-i', '--input_file', type=str, - default="", + default=gtsam.findExampleDataFile(DEFAULT_BAL_DATASET), help="""Read SFM data from the specified BAL file. The data format is described here: https://grail.cs.washington.edu/projects/bal/. BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, From 0ccb18b055074777593f6169e2c5110037cd2cab Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Nov 2021 14:27:28 -0500 Subject: [PATCH 27/71] add return type definitions Because my time is more valuable than a reviewer's pedanticness --- python/gtsam/examples/SFMExample_bal.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 77c186bd3..f3e48c3c3 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -17,8 +17,8 @@ import sys import gtsam from gtsam import (GeneralSFMFactorCal3Bundler, PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3) -from gtsam.symbol_shorthand import C, P -from gtsam.utils import plot +from gtsam.symbol_shorthand import C, P # type: ignore +from gtsam.utils import plot # type: ignore from matplotlib import pyplot as plt logging.basicConfig(stream=sys.stdout, level=logging.INFO) @@ -26,7 +26,7 @@ logging.basicConfig(stream=sys.stdout, level=logging.INFO) DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre" -def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values): +def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None: """Plot the SFM results.""" plot_vals = gtsam.Values() for cam_idx in range(scene_data.number_cameras()): @@ -41,7 +41,7 @@ def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values): plt.show() -def run(args: argparse.Namespace): +def run(args: argparse.Namespace) -> None: """ Run LM optimization with BAL input data and report resulting error """ input_file = args.input_file @@ -109,7 +109,7 @@ def run(args: argparse.Namespace): plot_scene(scene_data, result) -def main(): +def main() -> None: """Main runner.""" parser = argparse.ArgumentParser() parser.add_argument('-i', From 57d7103a4717ea854d6c10cab74fca6a920582e2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Nov 2021 18:22:54 -0500 Subject: [PATCH 28/71] fix OrderingType declaration --- gtsam/nonlinear/Marginals.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 2a3dddc37..41212ed76 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -81,13 +81,14 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut /* ************************************************************************* */ void Marginals::computeBayesTree() { // The default ordering to use. - const Ordering ordering = Ordering::COLAMND; + const Ordering::OrderingType defaultOrderingType = Ordering::COLAMD; // Compute BayesTree if (factorization_ == CHOLESKY) - bayesTree_ = - *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); + bayesTree_ = *graph_.eliminateMultifrontal(defaultOrderingType, + EliminatePreferCholesky); else if (factorization_ == QR) - bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminateQR); + bayesTree_ = + *graph_.eliminateMultifrontal(defaultOrderingType, EliminateQR); } /* ************************************************************************* */ From 0cbec6736aed04361df4495ef670062b4e1eadef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Nov 2021 18:23:38 -0500 Subject: [PATCH 29/71] update cmake to copy python tests whenever they are updated --- python/CMakeLists.txt | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index b703f5900..e2444a51a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -89,12 +89,19 @@ set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name ) +# Set the path for the GTSAM python module set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) -# Symlink all tests .py files to build folder. +# Copy all python files to build folder. copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" "${GTSAM_MODULE_PATH}") +# Hack to get python test files copied every time they are modified +file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py") +foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) + configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY) +endforeach() + # Common directory for data/datasets stored with the package. # This will store the data in the Python site package directly. file(COPY "${GTSAM_SOURCE_DIR}/examples/Data" DESTINATION "${GTSAM_MODULE_PATH}") @@ -147,10 +154,16 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable) - # Symlink all tests .py files to build folder. + # Copy all python files to build folder. copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable" "${GTSAM_UNSTABLE_MODULE_PATH}") + # Hack to get python test files copied every time they are modified + file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py") + foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) + configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY) + endforeach() + # Add gtsam_unstable to the install target list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_UNSTABLE_TARGET}) From d86fc9870688e3652416a927495691742018f7de Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 10 Nov 2021 19:01:31 -0500 Subject: [PATCH 30/71] update python wrapper --- gtsam/geometry/geometry.i | 3 +++ python/gtsam/tests/test_Pose3.py | 10 +++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 9baa49e8e..a40951d3e 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -473,6 +473,9 @@ class Pose3 { Vector logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; + Vector AdjointTranspose(Vector xi) const; + static Matrix adjointMap(Vector xi); + static Vector adjoint(Vector xi, Vector y); static Matrix adjointMap_(Vector xi); static Vector adjoint_(Vector xi, Vector y); static Vector adjointTranspose(Vector xi, Vector y); diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index e07b904a9..411828890 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -59,8 +59,16 @@ class TestPose3(GtsamTestCase): self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2)) def test_adjoint(self): - """Test adjoint method.""" + """Test adjoint methods.""" + T = Pose3() xi = np.array([1, 2, 3, 4, 5, 6]) + # test calling functions + T.AdjointMap() + T.Adjoint(xi) + T.AdjointTranspose(xi) + Pose3.adjointMap(xi) + Pose3.adjoint(xi, xi) + # test correctness of adjoint(x, y) expected = np.dot(Pose3.adjointMap_(xi), xi) actual = Pose3.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) From b82acc133b46cdd2b4a57ca327757079a37c7124 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 Nov 2021 14:24:15 -0500 Subject: [PATCH 31/71] properly deprecate additional methods to fully finish deprecation --- gtsam/linear/GaussianFactorGraph.cpp | 7 ------- gtsam/linear/GaussianFactorGraph.h | 11 ++++++++--- gtsam/nonlinear/Marginals.h | 2 ++ gtsam/nonlinear/NonlinearFactorGraph.h | 2 ++ 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index dc2eb8dd6..36db9f5d7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -504,13 +504,6 @@ namespace gtsam { return e; } - /* ************************************************************************* */ - /** \deprecated */ - VectorValues GaussianFactorGraph::optimize(boost::none_t, - const Eliminate& function) const { - return optimize(function); - } - /* ************************************************************************* */ void GaussianFactorGraph::printErrors( const VectorValues& values, const std::string& str, diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index d41374854..2dc5747cd 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -395,9 +395,14 @@ namespace gtsam { public: - /** \deprecated */ - VectorValues optimize(boost::none_t, - const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 + /** \deprecated */ + VectorValues optimize(boost::none_t, + const Eliminate& function = + EliminationTraitsType::DefaultEliminate) const { + return optimize(function); + } +#endif }; diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 9935bafdd..e73038db0 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -131,6 +131,7 @@ protected: void computeBayesTree(const Ordering& ordering); public: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated argument order changed due to removing boost::optional */ Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} @@ -142,6 +143,7 @@ public: /** \deprecated argument order changed due to removing boost::optional */ Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} +#endif }; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 4d321f8ab..5b9ed4bb1 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -265,6 +265,7 @@ namespace gtsam { public: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated */ boost::shared_ptr linearizeToHessianFactor( const Values& values, boost::none_t, const Dampen& dampen = nullptr) const @@ -274,6 +275,7 @@ namespace gtsam { Values updateCholesky(const Values& values, boost::none_t, const Dampen& dampen = nullptr) const {return updateCholesky(values, dampen);} +#endif }; From cf0c8d2fee04c14a6fba59a814988f3fd4ff082f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 Nov 2021 15:45:27 -0500 Subject: [PATCH 32/71] fix VectorValues include --- gtsam/linear/GaussianFactorGraph.cpp | 1 - gtsam/linear/GaussianFactorGraph.h | 7 ++++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 36db9f5d7..664aeff6d 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -19,7 +19,6 @@ */ #include -#include #include #include #include diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 2dc5747cd..5ccb0ce91 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -21,12 +21,13 @@ #pragma once -#include #include +#include +#include // Included here instead of fw-declared so we can use Errors::iterator #include -#include #include -#include // Included here instead of fw-declared so we can use Errors::iterator +#include +#include namespace gtsam { From fe6eb433e0472090198f311755ce4b0d25d59d3c Mon Sep 17 00:00:00 2001 From: duembgen Date: Mon, 15 Nov 2021 11:10:29 +0100 Subject: [PATCH 33/71] Add namespace to ambiguous placeholders --- gtsam/navigation/tests/testGPSFactor.cpp | 4 ++-- gtsam/navigation/tests/testMagFactor.cpp | 16 ++++++++-------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index b784c0c94..c94e1d3d5 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -72,7 +72,7 @@ TEST( GPSFactor, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; @@ -101,7 +101,7 @@ TEST( GPSFactor2, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 5107b3b6b..85447facd 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -64,7 +64,7 @@ TEST( MagFactor, unrotate ) { Point3 expected(22735.5, 314.502, 44202.5); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); + (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6)); } // ************************************************************************* @@ -76,35 +76,35 @@ TEST( MagFactor, Factors ) { MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal((Matrix)numericalDerivative11 // - (std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); + (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); + (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// + (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),// H1, 1e-7)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// + (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),// H2, 1e-7)); // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// + (std::bind(&MagFactor3::evaluateError, &f3, std::placeholders::_1, dir, bias, none, none, none), s),// H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// + (std::bind(&MagFactor3::evaluateError, &f3, s, std::placeholders::_1, bias, none, none, none), dir),// H2, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// + (std::bind(&MagFactor3::evaluateError, &f3, s, dir, std::placeholders::_1, none, none, none), bias),// H3, 1e-7)); } From d27d6b60a78278b02a153eb7073c2a201c5e8d7e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Nov 2021 10:54:00 -0500 Subject: [PATCH 34/71] Formatting with Google style --- gtsam_unstable/discrete/AllDiff.cpp | 172 +++--- gtsam_unstable/discrete/AllDiff.h | 120 ++--- gtsam_unstable/discrete/BinaryAllDiff.h | 157 +++--- gtsam_unstable/discrete/CSP.cpp | 161 +++--- gtsam_unstable/discrete/CSP.h | 145 +++-- gtsam_unstable/discrete/Constraint.h | 117 ++-- gtsam_unstable/discrete/Domain.cpp | 161 +++--- gtsam_unstable/discrete/Domain.h | 184 +++---- gtsam_unstable/discrete/Scheduler.cpp | 510 +++++++++--------- gtsam_unstable/discrete/Scheduler.h | 253 ++++----- gtsam_unstable/discrete/SingleValue.cpp | 129 +++-- gtsam_unstable/discrete/SingleValue.h | 127 +++-- gtsam_unstable/discrete/tests/testCSP.cpp | 150 +++--- .../discrete/tests/testLoopyBelief.cpp | 123 +++-- .../discrete/tests/testScheduler.cpp | 51 +- gtsam_unstable/discrete/tests/testSudoku.cpp | 127 ++--- 16 files changed, 1301 insertions(+), 1386 deletions(-) diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 9e124954f..d6e1c6453 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -5,107 +5,105 @@ * @author Frank Dellaert */ -#include -#include #include +#include +#include + #include namespace gtsam { - /* ************************************************************************* */ - AllDiff::AllDiff(const DiscreteKeys& dkeys) : - Constraint(dkeys.indices()) { - for(const DiscreteKey& dkey: dkeys) - cardinalities_.insert(dkey); - } +/* ************************************************************************* */ +AllDiff::AllDiff(const DiscreteKeys& dkeys) : Constraint(dkeys.indices()) { + for (const DiscreteKey& dkey : dkeys) cardinalities_.insert(dkey); +} - /* ************************************************************************* */ - void AllDiff::print(const std::string& s, - const KeyFormatter& formatter) const { - std::cout << s << "AllDiff on "; - for (Key dkey: keys_) - std::cout << formatter(dkey) << " "; - std::cout << std::endl; - } +/* ************************************************************************* */ +void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const { + std::cout << s << "AllDiff on "; + for (Key dkey : keys_) std::cout << formatter(dkey) << " "; + std::cout << std::endl; +} - /* ************************************************************************* */ - double AllDiff::operator()(const Values& values) const { - std::set < size_t > taken; // record values taken by keys - for(Key dkey: keys_) { - size_t value = values.at(dkey); // get the value for that key - if (taken.count(value)) return 0.0;// check if value alreday taken - taken.insert(value);// if not, record it as taken and keep checking +/* ************************************************************************* */ +double AllDiff::operator()(const Values& values) const { + std::set taken; // record values taken by keys + for (Key dkey : keys_) { + size_t value = values.at(dkey); // get the value for that key + if (taken.count(value)) return 0.0; // check if value alreday taken + taken.insert(value); // if not, record it as taken and keep checking + } + return 1.0; +} + +/* ************************************************************************* */ +DecisionTreeFactor AllDiff::toDecisionTreeFactor() const { + // We will do this by converting the allDif into many BinaryAllDiff + // constraints + DecisionTreeFactor converted; + size_t nrKeys = keys_.size(); + for (size_t i1 = 0; i1 < nrKeys; i1++) + for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) { + BinaryAllDiff binary12(discreteKey(i1), discreteKey(i2)); + converted = converted * binary12.toDecisionTreeFactor(); } - return 1.0; - } + return converted; +} - /* ************************************************************************* */ - DecisionTreeFactor AllDiff::toDecisionTreeFactor() const { - // We will do this by converting the allDif into many BinaryAllDiff constraints - DecisionTreeFactor converted; - size_t nrKeys = keys_.size(); - for (size_t i1 = 0; i1 < nrKeys; i1++) - for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) { - BinaryAllDiff binary12(discreteKey(i1),discreteKey(i2)); - converted = converted * binary12.toDecisionTreeFactor(); - } - return converted; - } +/* ************************************************************************* */ +DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} - /* ************************************************************************* */ - DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } +/* ************************************************************************* */ +bool AllDiff::ensureArcConsistency(size_t j, + std::vector& domains) const { + // Though strictly not part of allDiff, we check for + // a value in domains[j] that does not occur in any other connected domain. + // If found, we make this a singleton... + // TODO: make a new constraint where this really is true + Domain& Dj = domains[j]; + if (Dj.checkAllDiff(keys_, domains)) return true; - /* ************************************************************************* */ - bool AllDiff::ensureArcConsistency(size_t j, std::vector& domains) const { - // Though strictly not part of allDiff, we check for - // a value in domains[j] that does not occur in any other connected domain. - // If found, we make this a singleton... - // TODO: make a new constraint where this really is true - Domain& Dj = domains[j]; - if (Dj.checkAllDiff(keys_, domains)) return true; - - // Check all other domains for singletons and erase corresponding values - // This is the same as arc-consistency on the equivalent binary constraints - bool changed = false; - for(Key k: keys_) - if (k != j) { - const Domain& Dk = domains[k]; - if (Dk.isSingleton()) { // check if singleton - size_t value = Dk.firstValue(); - if (Dj.contains(value)) { - Dj.erase(value); // erase value if true - changed = true; - } + // Check all other domains for singletons and erase corresponding values + // This is the same as arc-consistency on the equivalent binary constraints + bool changed = false; + for (Key k : keys_) + if (k != j) { + const Domain& Dk = domains[k]; + if (Dk.isSingleton()) { // check if singleton + size_t value = Dk.firstValue(); + if (Dj.contains(value)) { + Dj.erase(value); // erase value if true + changed = true; } } - return changed; - } + } + return changed; +} - /* ************************************************************************* */ - Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { - DiscreteKeys newKeys; - // loop over keys and add them only if they do not appear in values - for(Key k: keys_) - if (values.find(k) == values.end()) { - newKeys.push_back(DiscreteKey(k,cardinalities_.at(k))); - } - return boost::make_shared(newKeys); - } +/* ************************************************************************* */ +Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { + DiscreteKeys newKeys; + // loop over keys and add them only if they do not appear in values + for (Key k : keys_) + if (values.find(k) == values.end()) { + newKeys.push_back(DiscreteKey(k, cardinalities_.at(k))); + } + return boost::make_shared(newKeys); +} - /* ************************************************************************* */ - Constraint::shared_ptr AllDiff::partiallyApply( - const std::vector& domains) const { - DiscreteFactor::Values known; - for(Key k: keys_) { - const Domain& Dk = domains[k]; - if (Dk.isSingleton()) - known[k] = Dk.firstValue(); - } - return partiallyApply(known); +/* ************************************************************************* */ +Constraint::shared_ptr AllDiff::partiallyApply( + const std::vector& domains) const { + DiscreteFactor::Values known; + for (Key k : keys_) { + const Domain& Dk = domains[k]; + if (Dk.isSingleton()) known[k] = Dk.firstValue(); } + return partiallyApply(known); +} - /* ************************************************************************* */ -} // namespace gtsam +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 80e700b29..b0fd1d631 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -7,71 +7,71 @@ #pragma once -#include #include +#include namespace gtsam { - /** - * General AllDiff constraint - * Returns 1 if values for all keys are different, 0 otherwise - * DiscreteFactors are all awkward in that they have to store two types of keys: - * for each variable we have a Key and an Key. In this factor, we - * keep the Indices locally, and the Indices are stored in IndexFactor. +/** + * General AllDiff constraint + * Returns 1 if values for all keys are different, 0 otherwise + * DiscreteFactors are all awkward in that they have to store two types of keys: + * for each variable we have a Key and an Key. In this factor, we + * keep the Indices locally, and the Indices are stored in IndexFactor. + */ +class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { + std::map cardinalities_; + + DiscreteKey discreteKey(size_t i) const { + Key j = keys_[i]; + return DiscreteKey(j, cardinalities_.at(j)); + } + + public: + /// Constructor + AllDiff(const DiscreteKeys& dkeys); + + // print + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) + return false; + else { + const AllDiff& f(static_cast(other)); + return cardinalities_.size() == f.cardinalities_.size() && + std::equal(cardinalities_.begin(), cardinalities_.end(), + f.cardinalities_.begin()); + } + } + + /// Calculate value = expensive ! + double operator()(const Values& values) const override; + + /// Convert into a decisiontree, can be *very* expensive ! + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency + * Arc-consistency involves creating binaryAllDiff constraints + * In which case the combinatorial hyper-arc explosion disappears. + * @param j domain to be checked + * @param domains all other domains */ - class GTSAM_UNSTABLE_EXPORT AllDiff: public Constraint { + bool ensureArcConsistency(size_t j, + std::vector& domains) const override; - std::map cardinalities_; + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values&) const override; - DiscreteKey discreteKey(size_t i) const { - Key j = keys_[i]; - return DiscreteKey(j,cardinalities_.at(j)); - } + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector&) const override; +}; - public: - - /// Constructor - AllDiff(const DiscreteKeys& dkeys); - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const AllDiff& f(static_cast(other)); - return cardinalities_.size() == f.cardinalities_.size() - && std::equal(cardinalities_.begin(), cardinalities_.end(), - f.cardinalities_.begin()); - } - } - - /// Calculate value = expensive ! - double operator()(const Values& values) const override; - - /// Convert into a decisiontree, can be *very* expensive ! - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * Arc-consistency involves creating binaryAllDiff constraints - * In which case the combinatorial hyper-arc explosion disappears. - * @param j domain to be checked - * @param domains all other domains - */ - bool ensureArcConsistency(size_t j, std::vector& domains) const override; - - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values&) const override; - - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply(const std::vector&) const override; - }; - -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index bbb60e2f1..d8e1a590a 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -7,94 +7,93 @@ #pragma once -#include -#include #include +#include +#include namespace gtsam { - /** - * Binary AllDiff constraint - * Returns 1 if values for two keys are different, 0 otherwise - * DiscreteFactors are all awkward in that they have to store two types of keys: - * for each variable we have a Index and an Index. In this factor, we - * keep the Indices locally, and the Indices are stored in IndexFactor. - */ - class BinaryAllDiff: public Constraint { +/** + * Binary AllDiff constraint + * Returns 1 if values for two keys are different, 0 otherwise + * DiscreteFactors are all awkward in that they have to store two types of keys: + * for each variable we have a Index and an Index. In this factor, we + * keep the Indices locally, and the Indices are stored in IndexFactor. + */ +class BinaryAllDiff : public Constraint { + size_t cardinality0_, cardinality1_; /// cardinality - size_t cardinality0_, cardinality1_; /// cardinality + public: + /// Constructor + BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) + : Constraint(key1.first, key2.first), + cardinality0_(key1.second), + cardinality1_(key2.second) {} - public: + // print + void print( + const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " + << formatter(keys_[1]) << std::endl; + } - /// Constructor - BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) : - Constraint(key1.first, key2.first), - cardinality0_(key1.second), cardinality1_(key2.second) { - } - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override { - std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " - << formatter(keys_[1]) << std::endl; - } - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const BinaryAllDiff& f(static_cast(other)); - return (cardinality0_==f.cardinality0_) && (cardinality1_==f.cardinality1_); - } - } - - /// Calculate value - double operator()(const Values& values) const override { - return (double) (values.at(keys_[0]) != values.at(keys_[1])); - } - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override { - DiscreteKeys keys; - keys.push_back(DiscreteKey(keys_[0],cardinality0_)); - keys.push_back(DiscreteKey(keys_[1],cardinality1_)); - std::vector table; - for (size_t i1 = 0; i1 < cardinality0_; i1++) - for (size_t i2 = 0; i2 < cardinality1_; i2++) - table.push_back(i1 != i2); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - /// - bool ensureArcConsistency(size_t j, std::vector& domains) const override { -// throw std::runtime_error( -// "BinaryAllDiff::ensureArcConsistency not implemented"); + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) return false; + else { + const BinaryAllDiff& f(static_cast(other)); + return (cardinality0_ == f.cardinality0_) && + (cardinality1_ == f.cardinality1_); } + } - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values&) const override { - throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); - } + /// Calculate value + double operator()(const Values& values) const override { + return (double)(values.at(keys_[0]) != values.at(keys_[1])); + } - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector&) const override { - throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); - } - }; + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override { + DiscreteKeys keys; + keys.push_back(DiscreteKey(keys_[0], cardinality0_)); + keys.push_back(DiscreteKey(keys_[1], cardinality1_)); + std::vector table; + for (size_t i1 = 0; i1 < cardinality0_; i1++) + for (size_t i2 = 0; i2 < cardinality1_; i2++) table.push_back(i1 != i2); + DecisionTreeFactor converted(keys, table); + return converted; + } -} // namespace gtsam + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; + } + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains + */ + bool ensureArcConsistency(size_t j, + std::vector& domains) const override { + // throw std::runtime_error( + // "BinaryAllDiff::ensureArcConsistency not implemented"); + return false; + } + + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values&) const override { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } + + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector&) const override { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } +}; + +} // namespace gtsam diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index 525abd098..b1d70dc6e 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -5,99 +5,104 @@ * @author Frank Dellaert */ -#include -#include #include +#include +#include using namespace std; namespace gtsam { - /// Find the best total assignment - can be expensive - CSP::sharedValues CSP::optimalAssignment() const { - DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(); - sharedValues mpe = chordal->optimize(); - return mpe; - } +/// Find the best total assignment - can be expensive +CSP::sharedValues CSP::optimalAssignment() const { + DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(); + sharedValues mpe = chordal->optimize(); + return mpe; +} - /// Find the best total assignment - can be expensive - CSP::sharedValues CSP::optimalAssignment(const Ordering& ordering) const { - DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering); - sharedValues mpe = chordal->optimize(); - return mpe; - } +/// Find the best total assignment - can be expensive +CSP::sharedValues CSP::optimalAssignment(const Ordering& ordering) const { + DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering); + sharedValues mpe = chordal->optimize(); + return mpe; +} - void CSP::runArcConsistency(size_t cardinality, size_t nrIterations, bool print) const { - // Create VariableIndex - VariableIndex index(*this); - // index.print(); +void CSP::runArcConsistency(size_t cardinality, size_t nrIterations, + bool print) const { + // Create VariableIndex + VariableIndex index(*this); + // index.print(); - size_t n = index.size(); + size_t n = index.size(); - // Initialize domains - std::vector < Domain > domains; - for (size_t j = 0; j < n; j++) - domains.push_back(Domain(DiscreteKey(j,cardinality))); + // Initialize domains + std::vector domains; + for (size_t j = 0; j < n; j++) + domains.push_back(Domain(DiscreteKey(j, cardinality))); - // Create array of flags indicating a domain changed or not - std::vector changed(n); + // Create array of flags indicating a domain changed or not + std::vector changed(n); - // iterate nrIterations over entire grid - for (size_t it = 0; it < nrIterations; it++) { - bool anyChange = false; - // iterate over all cells - for (size_t v = 0; v < n; v++) { - // keep track of which domains changed - changed[v] = false; - // loop over all factors/constraints for variable v - const FactorIndices& factors = index[v]; - for(size_t f: factors) { - // if not already a singleton - if (!domains[v].isSingleton()) { - // get the constraint and call its ensureArcConsistency method - Constraint::shared_ptr constraint = boost::dynamic_pointer_cast((*this)[f]); - if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); - changed[v] = constraint->ensureArcConsistency(v,domains) || changed[v]; - } - } // f - if (changed[v]) anyChange = true; - } // v - if (!anyChange) break; - // TODO: Sudoku specific hack - if (print) { - if (cardinality == 9 && n == 81) { - for (size_t i = 0, v = 0; i < (size_t)std::sqrt((double)n); i++) { - for (size_t j = 0; j < (size_t)std::sqrt((double)n); j++, v++) { - if (changed[v]) cout << "*"; - domains[v].print(); - cout << "\t"; - } // i - cout << endl; - } // j - } else { - for (size_t v = 0; v < n; v++) { + // iterate nrIterations over entire grid + for (size_t it = 0; it < nrIterations; it++) { + bool anyChange = false; + // iterate over all cells + for (size_t v = 0; v < n; v++) { + // keep track of which domains changed + changed[v] = false; + // loop over all factors/constraints for variable v + const FactorIndices& factors = index[v]; + for (size_t f : factors) { + // if not already a singleton + if (!domains[v].isSingleton()) { + // get the constraint and call its ensureArcConsistency method + Constraint::shared_ptr constraint = + boost::dynamic_pointer_cast((*this)[f]); + if (!constraint) + throw runtime_error("CSP:runArcConsistency: non-constraint factor"); + changed[v] = + constraint->ensureArcConsistency(v, domains) || changed[v]; + } + } // f + if (changed[v]) anyChange = true; + } // v + if (!anyChange) break; + // TODO: Sudoku specific hack + if (print) { + if (cardinality == 9 && n == 81) { + for (size_t i = 0, v = 0; i < (size_t)std::sqrt((double)n); i++) { + for (size_t j = 0; j < (size_t)std::sqrt((double)n); j++, v++) { if (changed[v]) cout << "*"; domains[v].print(); cout << "\t"; - } // v - } - cout << endl; - } // print - } // it + } // i + cout << endl; + } // j + } else { + for (size_t v = 0; v < n; v++) { + if (changed[v]) cout << "*"; + domains[v].print(); + cout << "\t"; + } // v + } + cout << endl; + } // print + } // it #ifndef INPROGRESS - // Now create new problem with all singleton variables removed - // We do this by adding simplifying all factors using parial application - // TODO: create a new ordering as we go, to ensure a connected graph - // KeyOrdering ordering; - // vector dkeys; - for(const DiscreteFactor::shared_ptr& f: factors_) { - Constraint::shared_ptr constraint = boost::dynamic_pointer_cast(f); - if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); - Constraint::shared_ptr reduced = constraint->partiallyApply(domains); - if (print) reduced->print(); - } -#endif + // Now create new problem with all singleton variables removed + // We do this by adding simplifying all factors using parial application + // TODO: create a new ordering as we go, to ensure a connected graph + // KeyOrdering ordering; + // vector dkeys; + for (const DiscreteFactor::shared_ptr& f : factors_) { + Constraint::shared_ptr constraint = + boost::dynamic_pointer_cast(f); + if (!constraint) + throw runtime_error("CSP:runArcConsistency: non-constraint factor"); + Constraint::shared_ptr reduced = constraint->partiallyApply(domains); + if (print) reduced->print(); } -} // gtsam - +#endif +} +} // namespace gtsam diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index 9e843f667..544cdf0c9 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -7,84 +7,81 @@ #pragma once +#include #include #include -#include namespace gtsam { - /** - * Constraint Satisfaction Problem class - * A specialization of a DiscreteFactorGraph. - * It knows about CSP-specific constraints and algorithms +/** + * Constraint Satisfaction Problem class + * A specialization of a DiscreteFactorGraph. + * It knows about CSP-specific constraints and algorithms + */ +class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { + public: + /** A map from keys to values */ + typedef KeyVector Indices; + typedef Assignment Values; + typedef boost::shared_ptr sharedValues; + + public: + // /// Constructor + // CSP() { + // } + + /// Add a unary constraint, allowing only a single value + void addSingleValue(const DiscreteKey& dkey, size_t value) { + boost::shared_ptr factor(new SingleValue(dkey, value)); + push_back(factor); + } + + /// Add a binary AllDiff constraint + void addAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) { + boost::shared_ptr factor(new BinaryAllDiff(key1, key2)); + push_back(factor); + } + + /// Add a general AllDiff constraint + void addAllDiff(const DiscreteKeys& dkeys) { + boost::shared_ptr factor(new AllDiff(dkeys)); + push_back(factor); + } + + // /** return product of all factors as a single factor */ + // DecisionTreeFactor product() const { + // DecisionTreeFactor result; + // for(const sharedFactor& factor: *this) + // if (factor) result = (*factor) * result; + // return result; + // } + + /// Find the best total assignment - can be expensive + sharedValues optimalAssignment() const; + + /// Find the best total assignment - can be expensive + sharedValues optimalAssignment(const Ordering& ordering) const; + + // /* + // * Perform loopy belief propagation + // * True belief propagation would check for each value in domain + // * whether any satisfying separator assignment can be found. + // * This corresponds to hyper-arc consistency in CSP speak. + // * This can be done by creating a mini-factor graph and search. + // * For a nine-by-nine Sudoku, the search tree will be 8+6+6=20 levels + // deep. + // * It will be very expensive to exclude values that way. + // */ + // void applyBeliefPropagation(size_t nrIterations = 10) const; + + /* + * Apply arc-consistency ~ Approximate loopy belief propagation + * We need to give the domains to a constraint, and it returns + * a domain whose values don't conflict in the arc-consistency way. + * TODO: should get cardinality from Indices */ - class GTSAM_UNSTABLE_EXPORT CSP: public DiscreteFactorGraph { - public: - - /** A map from keys to values */ - typedef KeyVector Indices; - typedef Assignment Values; - typedef boost::shared_ptr sharedValues; - - public: - -// /// Constructor -// CSP() { -// } - - /// Add a unary constraint, allowing only a single value - void addSingleValue(const DiscreteKey& dkey, size_t value) { - boost::shared_ptr factor(new SingleValue(dkey, value)); - push_back(factor); - } - - /// Add a binary AllDiff constraint - void addAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) { - boost::shared_ptr factor( - new BinaryAllDiff(key1, key2)); - push_back(factor); - } - - /// Add a general AllDiff constraint - void addAllDiff(const DiscreteKeys& dkeys) { - boost::shared_ptr factor(new AllDiff(dkeys)); - push_back(factor); - } - -// /** return product of all factors as a single factor */ -// DecisionTreeFactor product() const { -// DecisionTreeFactor result; -// for(const sharedFactor& factor: *this) -// if (factor) result = (*factor) * result; -// return result; -// } - - /// Find the best total assignment - can be expensive - sharedValues optimalAssignment() const; - - /// Find the best total assignment - can be expensive - sharedValues optimalAssignment(const Ordering& ordering) const; - -// /* -// * Perform loopy belief propagation -// * True belief propagation would check for each value in domain -// * whether any satisfying separator assignment can be found. -// * This corresponds to hyper-arc consistency in CSP speak. -// * This can be done by creating a mini-factor graph and search. -// * For a nine-by-nine Sudoku, the search tree will be 8+6+6=20 levels deep. -// * It will be very expensive to exclude values that way. -// */ -// void applyBeliefPropagation(size_t nrIterations = 10) const; - - /* - * Apply arc-consistency ~ Approximate loopy belief propagation - * We need to give the domains to a constraint, and it returns - * a domain whose values don't conflict in the arc-consistency way. - * TODO: should get cardinality from Indices - */ - void runArcConsistency(size_t cardinality, size_t nrIterations = 10, - bool print = false) const; - }; // CSP - -} // gtsam + void runArcConsistency(size_t cardinality, size_t nrIterations = 10, + bool print = false) const; +}; // CSP +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index c3a26de68..b8baccff9 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -17,77 +17,68 @@ #pragma once -#include #include +#include + #include namespace gtsam { - class Domain; +class Domain; - /** - * Base class for discrete probabilistic factors - * The most general one is the derived DecisionTreeFactor +/** + * Base class for discrete probabilistic factors + * The most general one is the derived DecisionTreeFactor + */ +class Constraint : public DiscreteFactor { + public: + typedef boost::shared_ptr shared_ptr; + + protected: + /// Construct n-way factor + Constraint(const KeyVector& js) : DiscreteFactor(js) {} + + /// Construct unary factor + Constraint(Key j) : DiscreteFactor(boost::assign::cref_list_of<1>(j)) {} + + /// Construct binary factor + Constraint(Key j1, Key j2) + : DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) {} + + /// construct from container + template + Constraint(KeyIterator beginKey, KeyIterator endKey) + : DiscreteFactor(beginKey, endKey) {} + + public: + /// @name Standard Constructors + /// @{ + + /// Default constructor for I/O + Constraint(); + + /// Virtual destructor + ~Constraint() override {} + + /// @} + /// @name Standard Interface + /// @{ + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains */ - class Constraint : public DiscreteFactor { + virtual bool ensureArcConsistency(size_t j, + std::vector& domains) const = 0; - public: + /// Partially apply known values + virtual shared_ptr partiallyApply(const Values&) const = 0; - typedef boost::shared_ptr shared_ptr; - - protected: - - /// Construct n-way factor - Constraint(const KeyVector& js) : - DiscreteFactor(js) { - } - - /// Construct unary factor - Constraint(Key j) : - DiscreteFactor(boost::assign::cref_list_of<1>(j)) { - } - - /// Construct binary factor - Constraint(Key j1, Key j2) : - DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) { - } - - /// construct from container - template - Constraint(KeyIterator beginKey, KeyIterator endKey) : - DiscreteFactor(beginKey, endKey) { - } - - public: - - /// @name Standard Constructors - /// @{ - - /// Default constructor for I/O - Constraint(); - - /// Virtual destructor - ~Constraint() override {} - - /// @} - /// @name Standard Interface - /// @{ - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - virtual bool ensureArcConsistency(size_t j, std::vector& domains) const = 0; - - /// Partially apply known values - virtual shared_ptr partiallyApply(const Values&) const = 0; - - - /// Partially apply known values, domain version - virtual shared_ptr partiallyApply(const std::vector&) const = 0; - /// @} - }; + /// Partially apply known values, domain version + virtual shared_ptr partiallyApply(const std::vector&) const = 0; + /// @} +}; // DiscreteFactor -}// namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index 740ef067c..a81b1d1ad 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -5,92 +5,89 @@ * @author Frank Dellaert */ -#include -#include #include +#include +#include + #include namespace gtsam { - using namespace std; - - /* ************************************************************************* */ - void Domain::print(const string& s, - const KeyFormatter& formatter) const { -// cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << -// formatter(keys_[0]) << ") with values"; -// for (size_t v: values_) cout << " " << v; -// cout << endl; - for (size_t v: values_) cout << v; - } - - /* ************************************************************************* */ - double Domain::operator()(const Values& values) const { - return contains(values.at(keys_[0])); - } - - /* ************************************************************************* */ - DecisionTreeFactor Domain::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(keys_[0],cardinality_); - vector table; - for (size_t i1 = 0; i1 < cardinality_; ++i1) - table.push_back(contains(i1)); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /* ************************************************************************* */ - DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* ************************************************************************* */ - bool Domain::ensureArcConsistency(size_t j, vector& domains) const { - if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); - Domain& D = domains[j]; - for(size_t value: values_) - if (!D.contains(value)) throw runtime_error("Unsatisfiable"); - D = *this; - return true; - } - - /* ************************************************************************* */ - bool Domain::checkAllDiff(const KeyVector keys, vector& domains) { - Key j = keys_[0]; - // for all values in this domain - for(size_t value: values_) { - // for all connected domains - for(Key k: keys) - // if any domain contains the value we cannot make this domain singleton - if (k!=j && domains[k].contains(value)) - goto found; - values_.clear(); - values_.insert(value); - return true; // we changed it - found:; - } - return false; // we did not change it - } - - /* ************************************************************************* */ - Constraint::shared_ptr Domain::partiallyApply( - const Values& values) const { - Values::const_iterator it = values.find(keys_[0]); - if (it != values.end() && !contains(it->second)) throw runtime_error( - "Domain::partiallyApply: unsatisfiable"); - return boost::make_shared < Domain > (*this); - } - - /* ************************************************************************* */ - Constraint::shared_ptr Domain::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; - if (Dk.isSingleton() && !contains(*Dk.begin())) throw runtime_error( - "Domain::partiallyApply: unsatisfiable"); - return boost::make_shared < Domain > (Dk); - } +using namespace std; /* ************************************************************************* */ -} // namespace gtsam +void Domain::print(const string& s, const KeyFormatter& formatter) const { + // cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << + // formatter(keys_[0]) << ") with values"; + // for (size_t v: values_) cout << " " << v; + // cout << endl; + for (size_t v : values_) cout << v; +} + +/* ************************************************************************* */ +double Domain::operator()(const Values& values) const { + return contains(values.at(keys_[0])); +} + +/* ************************************************************************* */ +DecisionTreeFactor Domain::toDecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(keys_[0], cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; ++i1) table.push_back(contains(i1)); + DecisionTreeFactor converted(keys, table); + return converted; +} + +/* ************************************************************************* */ +DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} + +/* ************************************************************************* */ +bool Domain::ensureArcConsistency(size_t j, vector& domains) const { + if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); + Domain& D = domains[j]; + for (size_t value : values_) + if (!D.contains(value)) throw runtime_error("Unsatisfiable"); + D = *this; + return true; +} + +/* ************************************************************************* */ +bool Domain::checkAllDiff(const KeyVector keys, vector& domains) { + Key j = keys_[0]; + // for all values in this domain + for (size_t value : values_) { + // for all connected domains + for (Key k : keys) + // if any domain contains the value we cannot make this domain singleton + if (k != j && domains[k].contains(value)) goto found; + values_.clear(); + values_.insert(value); + return true; // we changed it + found:; + } + return false; // we did not change it +} + +/* ************************************************************************* */ +Constraint::shared_ptr Domain::partiallyApply(const Values& values) const { + Values::const_iterator it = values.find(keys_[0]); + if (it != values.end() && !contains(it->second)) + throw runtime_error("Domain::partiallyApply: unsatisfiable"); + return boost::make_shared(*this); +} + +/* ************************************************************************* */ +Constraint::shared_ptr Domain::partiallyApply( + const vector& domains) const { + const Domain& Dk = domains[keys_[0]]; + if (Dk.isSingleton() && !contains(*Dk.begin())) + throw runtime_error("Domain::partiallyApply: unsatisfiable"); + return boost::make_shared(Dk); +} + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 5acc5a08f..15828b653 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -7,111 +7,97 @@ #pragma once -#include #include +#include namespace gtsam { - /** - * Domain restriction constraint +/** + * Domain restriction constraint + */ +class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { + size_t cardinality_; /// Cardinality + std::set values_; /// allowed values + + public: + typedef boost::shared_ptr shared_ptr; + + // Constructor on Discrete Key initializes an "all-allowed" domain + Domain(const DiscreteKey& dkey) + : Constraint(dkey.first), cardinality_(dkey.second) { + for (size_t v = 0; v < cardinality_; v++) values_.insert(v); + } + + // Constructor on Discrete Key with single allowed value + // Consider SingleValue constraint + Domain(const DiscreteKey& dkey, size_t v) + : Constraint(dkey.first), cardinality_(dkey.second) { + values_.insert(v); + } + + /// Constructor + Domain(const Domain& other) + : Constraint(other.keys_[0]), values_(other.values_) {} + + /// insert a value, non const :-( + void insert(size_t value) { values_.insert(value); } + + /// erase a value, non const :-( + void erase(size_t value) { values_.erase(value); } + + size_t nrValues() const { return values_.size(); } + + bool isSingleton() const { return nrValues() == 1; } + + size_t firstValue() const { return *values_.begin(); } + + // print + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) + return false; + else { + const Domain& f(static_cast(other)); + return (cardinality_ == f.cardinality_) && (values_ == f.values_); + } + } + + bool contains(size_t value) const { return values_.count(value) > 0; } + + /// Calculate value + double operator()(const Values& values) const override; + + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains */ - class GTSAM_UNSTABLE_EXPORT Domain: public Constraint { + bool ensureArcConsistency(size_t j, + std::vector& domains) const override; - size_t cardinality_; /// Cardinality - std::set values_; /// allowed values + /** + * Check for a value in domain that does not occur in any other connected + * domain. If found, we make this a singleton... Called in + * AllDiff::ensureArcConsistency + * @param keys connected domains through alldiff + */ + bool checkAllDiff(const KeyVector keys, std::vector& domains); - public: + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values& values) const override; - typedef boost::shared_ptr shared_ptr; + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector& domains) const override; +}; - // Constructor on Discrete Key initializes an "all-allowed" domain - Domain(const DiscreteKey& dkey) : - Constraint(dkey.first), cardinality_(dkey.second) { - for (size_t v = 0; v < cardinality_; v++) - values_.insert(v); - } - - // Constructor on Discrete Key with single allowed value - // Consider SingleValue constraint - Domain(const DiscreteKey& dkey, size_t v) : - Constraint(dkey.first), cardinality_(dkey.second) { - values_.insert(v); - } - - /// Constructor - Domain(const Domain& other) : - Constraint(other.keys_[0]), values_(other.values_) { - } - - /// insert a value, non const :-( - void insert(size_t value) { - values_.insert(value); - } - - /// erase a value, non const :-( - void erase(size_t value) { - values_.erase(value); - } - - size_t nrValues() const { - return values_.size(); - } - - bool isSingleton() const { - return nrValues() == 1; - } - - size_t firstValue() const { - return *values_.begin(); - } - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const Domain& f(static_cast(other)); - return (cardinality_==f.cardinality_) && (values_==f.values_); - } - } - - bool contains(size_t value) const { - return values_.count(value)>0; - } - - /// Calculate value - double operator()(const Values& values) const override; - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - bool ensureArcConsistency(size_t j, std::vector& domains) const override; - - /** - * Check for a value in domain that does not occur in any other connected domain. - * If found, we make this a singleton... Called in AllDiff::ensureArcConsistency - * @param keys connected domains through alldiff - */ - bool checkAllDiff(const KeyVector keys, std::vector& domains); - - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values& values) const override; - - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector& domains) const override; - }; - -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 3273778c4..415f92e62 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -5,298 +5,286 @@ * @author Frank Dellaert */ -#include -#include #include #include +#include +#include #include - +#include #include #include -#include namespace gtsam { - using namespace std; +using namespace std; - Scheduler::Scheduler(size_t maxNrStudents, const string& filename): - maxNrStudents_(maxNrStudents) - { - typedef boost::tokenizer > Tokenizer; +Scheduler::Scheduler(size_t maxNrStudents, const string& filename) + : maxNrStudents_(maxNrStudents) { + typedef boost::tokenizer > Tokenizer; - // open file - ifstream is(filename.c_str()); - if (!is) { - cerr << "Scheduler: could not open file " << filename << endl; - throw runtime_error("Scheduler: could not open file " + filename); - } - - string line; // buffer - - // process first line with faculty - if (getline(is, line, '\r')) { - Tokenizer tok(line); - Tokenizer::iterator it = tok.begin(); - for (++it; it != tok.end(); ++it) - addFaculty(*it); - } - - // for all remaining lines - size_t count = 0; - while (getline(is, line, '\r')) { - if (count++ > 100) throw runtime_error("reached 100 lines, exiting"); - Tokenizer tok(line); - Tokenizer::iterator it = tok.begin(); - addSlot(*it++); // add slot - // add availability - for (; it != tok.end(); ++it) - available_ += (it->empty()) ? "0 " : "1 "; - available_ += '\n'; - } - } // constructor - - /** addStudent has to be called after adding slots and faculty */ - void Scheduler::addStudent(const string& studentName, - const string& area1, const string& area2, - const string& area3, const string& advisor) { - assert(nrStudents() area) const { - return area ? students_[s].keys_[*area] : students_[s].key_; + // open file + ifstream is(filename.c_str()); + if (!is) { + cerr << "Scheduler: could not open file " << filename << endl; + throw runtime_error("Scheduler: could not open file " + filename); } - const string& Scheduler::studentName(size_t i) const { - assert(i 100) throw runtime_error("reached 100 lines, exiting"); + Tokenizer tok(line); + Tokenizer::iterator it = tok.begin(); + addSlot(*it++); // add slot + // add availability + for (; it != tok.end(); ++it) available_ += (it->empty()) ? "0 " : "1 "; + available_ += '\n'; + } +} // constructor + +/** addStudent has to be called after adding slots and faculty */ +void Scheduler::addStudent(const string& studentName, const string& area1, + const string& area2, const string& area3, + const string& advisor) { + assert(nrStudents() < maxNrStudents_); + assert(facultyInArea_.count(area1)); + assert(facultyInArea_.count(area2)); + assert(facultyInArea_.count(area3)); + size_t advisorIndex = facultyIndex_[advisor]; + Student student(nrFaculty(), advisorIndex); + student.name_ = studentName; + // We fix the ordering by assigning a higher index to the student + // and numbering the areas lower + Key j = 3 * maxNrStudents_ + nrStudents(); + student.key_ = DiscreteKey(j, nrTimeSlots()); + Key base = 3 * nrStudents(); + student.keys_[0] = DiscreteKey(base + 0, nrFaculty()); + student.keys_[1] = DiscreteKey(base + 1, nrFaculty()); + student.keys_[2] = DiscreteKey(base + 2, nrFaculty()); + student.areaName_[0] = area1; + student.areaName_[1] = area2; + student.areaName_[2] = area3; + students_.push_back(student); +} + +/** get key for student and area, 0 is time slot itself */ +const DiscreteKey& Scheduler::key(size_t s, + boost::optional area) const { + return area ? students_[s].keys_[*area] : students_[s].key_; +} + +const string& Scheduler::studentName(size_t i) const { + assert(i < nrStudents()); + return students_[i].name_; +} + +const DiscreteKey& Scheduler::studentKey(size_t i) const { + assert(i < nrStudents()); + return students_[i].key_; +} + +const string& Scheduler::studentArea(size_t i, size_t area) const { + assert(i < nrStudents()); + return students_[i].areaName_[area]; +} + +/** Add student-specific constraints to the graph */ +void Scheduler::addStudentSpecificConstraints(size_t i, + boost::optional slot) { + bool debug = ISDEBUG("Scheduler::buildGraph"); + + assert(i < nrStudents()); + const Student& s = students_[i]; + + if (!slot && !slotsAvailable_.empty()) { + if (debug) cout << "Adding availability of slots" << endl; + assert(slotsAvailable_.size() == s.key_.second); + CSP::add(s.key_, slotsAvailable_); } - const string& Scheduler::studentArea(size_t i, size_t area) const { - assert(i slot) { - bool debug = ISDEBUG("Scheduler::buildGraph"); + if (debug) cout << "Area constraints " << areaName << endl; + assert(facultyInArea_[areaName].size() == areaKey.second); + CSP::add(areaKey, facultyInArea_[areaName]); - assert(iat(j); - cout << studentName(s) << " slot: " << slotName_[slot] << endl; - Key base = 3*s; - for (size_t area = 0; area < 3; area++) { - size_t faculty = assignment->at(base+area); - cout << setw(12) << studentArea(s,area) << ": " << facultyName_[faculty] - << endl; - } - cout << endl; - } } +} // buildGraph - /** Special print for single-student case */ - void Scheduler::printSpecial(sharedValues assignment) const { - Values::const_iterator it = assignment->begin(); - for (size_t area = 0; area < 3; area++, it++) { - size_t f = it->second; - cout << setw(12) << studentArea(0,area) << ": " << facultyName_[f] << endl; +/** print */ +void Scheduler::print(const string& s, const KeyFormatter& formatter) const { + cout << s << " Faculty:" << endl; + for (const string& name : facultyName_) cout << name << '\n'; + cout << endl; + + cout << s << " Slots:\n"; + size_t i = 0; + for (const string& name : slotName_) cout << i++ << " " << name << endl; + cout << endl; + + cout << "Availability:\n" << available_ << '\n'; + + cout << s << " Area constraints:\n"; + for (const FacultyInArea::value_type& it : facultyInArea_) { + cout << setw(12) << it.first << ": "; + for (double v : it.second) cout << v << " "; + cout << '\n'; + } + cout << endl; + + cout << s << " Students:\n"; + for (const Student& student : students_) student.print(); + cout << endl; + + CSP::print(s + " Factor graph"); + cout << endl; +} // print + +/** Print readable form of assignment */ +void Scheduler::printAssignment(sharedValues assignment) const { + // Not intended to be general! Assumes very particular ordering ! + cout << endl; + for (size_t s = 0; s < nrStudents(); s++) { + Key j = 3 * maxNrStudents_ + s; + size_t slot = assignment->at(j); + cout << studentName(s) << " slot: " << slotName_[slot] << endl; + Key base = 3 * s; + for (size_t area = 0; area < 3; area++) { + size_t faculty = assignment->at(base + area); + cout << setw(12) << studentArea(s, area) << ": " << facultyName_[faculty] + << endl; } cout << endl; } +} - /** Accumulate faculty stats */ - void Scheduler::accumulateStats(sharedValues assignment, vector< - size_t>& stats) const { - for (size_t s = 0; s < nrStudents(); s++) { - Key base = 3*s; - for (size_t area = 0; area < 3; area++) { - size_t f = assignment->at(base+area); - assert(fbegin(); + for (size_t area = 0; area < 3; area++, it++) { + size_t f = it->second; + cout << setw(12) << studentArea(0, area) << ": " << facultyName_[f] << endl; + } + cout << endl; +} + +/** Accumulate faculty stats */ +void Scheduler::accumulateStats(sharedValues assignment, + vector& stats) const { + for (size_t s = 0; s < nrStudents(); s++) { + Key base = 3 * s; + for (size_t area = 0; area < 3; area++) { + size_t f = assignment->at(base + area); + assert(f < stats.size()); + stats[f]++; + } // area + } // s +} + +/** Eliminate, return a Bayes net */ +DiscreteBayesNet::shared_ptr Scheduler::eliminate() const { + gttic(my_eliminate); + // TODO: fix this!! + size_t maxKey = keys().size(); + Ordering defaultKeyOrdering; + for (size_t i = 0; i < maxKey; ++i) defaultKeyOrdering += Key(i); + DiscreteBayesNet::shared_ptr chordal = + this->eliminateSequential(defaultKeyOrdering); + gttoc(my_eliminate); + return chordal; +} + +/** Find the best total assignment - can be expensive */ +Scheduler::sharedValues Scheduler::optimalAssignment() const { + DiscreteBayesNet::shared_ptr chordal = eliminate(); + + if (ISDEBUG("Scheduler::optimalAssignment")) { + DiscreteBayesNet::const_iterator it = chordal->end() - 1; + const Student& student = students_.front(); + cout << endl; + (*it)->print(student.name_); } - /** Eliminate, return a Bayes net */ - DiscreteBayesNet::shared_ptr Scheduler::eliminate() const { - gttic(my_eliminate); - // TODO: fix this!! - size_t maxKey = keys().size(); - Ordering defaultKeyOrdering; - for (size_t i = 0; ieliminateSequential(defaultKeyOrdering); - gttoc(my_eliminate); - return chordal; - } + gttic(my_optimize); + sharedValues mpe = chordal->optimize(); + gttoc(my_optimize); + return mpe; +} - /** Find the best total assignment - can be expensive */ - Scheduler::sharedValues Scheduler::optimalAssignment() const { - DiscreteBayesNet::shared_ptr chordal = eliminate(); - - if (ISDEBUG("Scheduler::optimalAssignment")) { - DiscreteBayesNet::const_iterator it = chordal->end()-1; - const Student & student = students_.front(); - cout << endl; - (*it)->print(student.name_); - } - - gttic(my_optimize); - sharedValues mpe = chordal->optimize(); - gttoc(my_optimize); - return mpe; - } - - /** find the assignment of students to slots with most possible committees */ - Scheduler::sharedValues Scheduler::bestSchedule() const { - sharedValues best; - throw runtime_error("bestSchedule not implemented"); - return best; - } - - /** find the corresponding most desirable committee assignment */ - Scheduler::sharedValues Scheduler::bestAssignment( - sharedValues bestSchedule) const { - sharedValues best; - throw runtime_error("bestAssignment not implemented"); - return best; - } - -} // gtsam +/** find the assignment of students to slots with most possible committees */ +Scheduler::sharedValues Scheduler::bestSchedule() const { + sharedValues best; + throw runtime_error("bestSchedule not implemented"); + return best; +} +/** find the corresponding most desirable committee assignment */ +Scheduler::sharedValues Scheduler::bestAssignment( + sharedValues bestSchedule) const { + sharedValues best; + throw runtime_error("bestAssignment not implemented"); + return best; +} +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Scheduler.h b/gtsam_unstable/discrete/Scheduler.h index 6faf9956f..faf131f5c 100644 --- a/gtsam_unstable/discrete/Scheduler.h +++ b/gtsam_unstable/discrete/Scheduler.h @@ -11,165 +11,150 @@ namespace gtsam { +/** + * Scheduler class + * Creates one variable for each student, and three variables for each + * of the student's areas, for a total of 4*nrStudents variables. + * The "student" variable will determine when the student takes the qual. + * The "area" variables determine which faculty are on his/her committee. + */ +class GTSAM_UNSTABLE_EXPORT Scheduler : public CSP { + private: + /** Internal data structure for students */ + struct Student { + std::string name_; + DiscreteKey key_; // key for student + std::vector keys_; // key for areas + std::vector areaName_; + std::vector advisor_; + Student(size_t nrFaculty, size_t advisorIndex) + : keys_(3), areaName_(3), advisor_(nrFaculty, 1.0) { + advisor_[advisorIndex] = 0.0; + } + void print() const { + using std::cout; + cout << name_ << ": "; + for (size_t area = 0; area < 3; area++) cout << areaName_[area] << " "; + cout << std::endl; + } + }; + + /** Maximum number of students */ + size_t maxNrStudents_; + + /** discrete keys, indexed by student and area index */ + std::vector students_; + + /** faculty identifiers */ + std::map facultyIndex_; + std::vector facultyName_, slotName_, areaName_; + + /** area constraints */ + typedef std::map > FacultyInArea; + FacultyInArea facultyInArea_; + + /** nrTimeSlots * nrFaculty availability constraints */ + std::string available_; + + /** which slots are good */ + std::vector slotsAvailable_; + + public: /** - * Scheduler class - * Creates one variable for each student, and three variables for each - * of the student's areas, for a total of 4*nrStudents variables. - * The "student" variable will determine when the student takes the qual. - * The "area" variables determine which faculty are on his/her committee. + * Constructor + * We need to know the number of students in advance for ordering keys. + * then add faculty, slots, areas, availability, students, in that order */ - class GTSAM_UNSTABLE_EXPORT Scheduler : public CSP { + Scheduler(size_t maxNrStudents) : maxNrStudents_(maxNrStudents) {} - private: + /// Destructor + virtual ~Scheduler() {} - /** Internal data structure for students */ - struct Student { - std::string name_; - DiscreteKey key_; // key for student - std::vector keys_; // key for areas - std::vector areaName_; - std::vector advisor_; - Student(size_t nrFaculty, size_t advisorIndex) : - keys_(3), areaName_(3), advisor_(nrFaculty, 1.0) { - advisor_[advisorIndex] = 0.0; - } - void print() const { - using std::cout; - cout << name_ << ": "; - for (size_t area = 0; area < 3; area++) - cout << areaName_[area] << " "; - cout << std::endl; - } - }; + void addFaculty(const std::string& facultyName) { + facultyIndex_[facultyName] = nrFaculty(); + facultyName_.push_back(facultyName); + } - /** Maximum number of students */ - size_t maxNrStudents_; + size_t nrFaculty() const { return facultyName_.size(); } - /** discrete keys, indexed by student and area index */ - std::vector students_; + /** boolean std::string of nrTimeSlots * nrFaculty */ + void setAvailability(const std::string& available) { available_ = available; } - /** faculty identifiers */ - std::map facultyIndex_; - std::vector facultyName_, slotName_, areaName_; + void addSlot(const std::string& slotName) { slotName_.push_back(slotName); } - /** area constraints */ - typedef std::map > FacultyInArea; - FacultyInArea facultyInArea_; + size_t nrTimeSlots() const { return slotName_.size(); } - /** nrTimeSlots * nrFaculty availability constraints */ - std::string available_; + const std::string& slotName(size_t s) const { return slotName_[s]; } - /** which slots are good */ - std::vector slotsAvailable_; + /** slots available, boolean */ + void setSlotsAvailable(const std::vector& slotsAvailable) { + slotsAvailable_ = slotsAvailable; + } - public: + void addArea(const std::string& facultyName, const std::string& areaName) { + areaName_.push_back(areaName); + std::vector& table = + facultyInArea_[areaName]; // will create if needed + if (table.empty()) table.resize(nrFaculty(), 0); + table[facultyIndex_[facultyName]] = 1; + } - /** - * Constructor - * We need to know the number of students in advance for ordering keys. - * then add faculty, slots, areas, availability, students, in that order - */ - Scheduler(size_t maxNrStudents) : maxNrStudents_(maxNrStudents) {} + /** + * Constructor that reads in faculty, slots, availibility. + * Still need to add areas and students after this + */ + Scheduler(size_t maxNrStudents, const std::string& filename); - /// Destructor - virtual ~Scheduler() {} + /** get key for student and area, 0 is time slot itself */ + const DiscreteKey& key(size_t s, + boost::optional area = boost::none) const; - void addFaculty(const std::string& facultyName) { - facultyIndex_[facultyName] = nrFaculty(); - facultyName_.push_back(facultyName); - } + /** addStudent has to be called after adding slots and faculty */ + void addStudent(const std::string& studentName, const std::string& area1, + const std::string& area2, const std::string& area3, + const std::string& advisor); - size_t nrFaculty() const { - return facultyName_.size(); - } + /// current number of students + size_t nrStudents() const { return students_.size(); } - /** boolean std::string of nrTimeSlots * nrFaculty */ - void setAvailability(const std::string& available) { - available_ = available; - } + const std::string& studentName(size_t i) const; + const DiscreteKey& studentKey(size_t i) const; + const std::string& studentArea(size_t i, size_t area) const; - void addSlot(const std::string& slotName) { - slotName_.push_back(slotName); - } + /** Add student-specific constraints to the graph */ + void addStudentSpecificConstraints( + size_t i, boost::optional slot = boost::none); - size_t nrTimeSlots() const { - return slotName_.size(); - } + /** Main routine that builds factor graph */ + void buildGraph(size_t mutexBound = 7); - const std::string& slotName(size_t s) const { - return slotName_[s]; - } + /** print */ + void print( + const std::string& s = "Scheduler", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; - /** slots available, boolean */ - void setSlotsAvailable(const std::vector& slotsAvailable) { - slotsAvailable_ = slotsAvailable; - } + /** Print readable form of assignment */ + void printAssignment(sharedValues assignment) const; - void addArea(const std::string& facultyName, const std::string& areaName) { - areaName_.push_back(areaName); - std::vector& table = facultyInArea_[areaName]; // will create if needed - if (table.empty()) table.resize(nrFaculty(), 0); - table[facultyIndex_[facultyName]] = 1; - } + /** Special print for single-student case */ + void printSpecial(sharedValues assignment) const; - /** - * Constructor that reads in faculty, slots, availibility. - * Still need to add areas and students after this - */ - Scheduler(size_t maxNrStudents, const std::string& filename); + /** Accumulate faculty stats */ + void accumulateStats(sharedValues assignment, + std::vector& stats) const; - /** get key for student and area, 0 is time slot itself */ - const DiscreteKey& key(size_t s, boost::optional area = boost::none) const; + /** Eliminate, return a Bayes net */ + DiscreteBayesNet::shared_ptr eliminate() const; - /** addStudent has to be called after adding slots and faculty */ - void addStudent(const std::string& studentName, const std::string& area1, - const std::string& area2, const std::string& area3, - const std::string& advisor); + /** Find the best total assignment - can be expensive */ + sharedValues optimalAssignment() const; - /// current number of students - size_t nrStudents() const { - return students_.size(); - } + /** find the assignment of students to slots with most possible committees */ + sharedValues bestSchedule() const; - const std::string& studentName(size_t i) const; - const DiscreteKey& studentKey(size_t i) const; - const std::string& studentArea(size_t i, size_t area) const; - - /** Add student-specific constraints to the graph */ - void addStudentSpecificConstraints(size_t i, boost::optional slot = boost::none); - - /** Main routine that builds factor graph */ - void buildGraph(size_t mutexBound = 7); - - /** print */ - void print( - const std::string& s = "Scheduler", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /** Print readable form of assignment */ - void printAssignment(sharedValues assignment) const; - - /** Special print for single-student case */ - void printSpecial(sharedValues assignment) const; - - /** Accumulate faculty stats */ - void accumulateStats(sharedValues assignment, - std::vector& stats) const; - - /** Eliminate, return a Bayes net */ - DiscreteBayesNet::shared_ptr eliminate() const; - - /** Find the best total assignment - can be expensive */ - sharedValues optimalAssignment() const; - - /** find the assignment of students to slots with most possible committees */ - sharedValues bestSchedule() const; - - /** find the corresponding most desirable committee assignment */ - sharedValues bestAssignment(sharedValues bestSchedule) const; - - }; // Scheduler - -} // gtsam + /** find the corresponding most desirable committee assignment */ + sharedValues bestAssignment(sharedValues bestSchedule) const; +}; // Scheduler +} // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 6324f14cd..105887dc9 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -5,75 +5,74 @@ * @author Frank Dellaert */ -#include -#include -#include #include +#include +#include +#include + #include namespace gtsam { - using namespace std; - - /* ************************************************************************* */ - void SingleValue::print(const string& s, - const KeyFormatter& formatter) const { - cout << s << "SingleValue on " << "j=" << formatter(keys_[0]) - << " with value " << value_ << endl; - } - - /* ************************************************************************* */ - double SingleValue::operator()(const Values& values) const { - return (double) (values.at(keys_[0]) == value_); - } - - /* ************************************************************************* */ - DecisionTreeFactor SingleValue::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(keys_[0],cardinality_); - vector table; - for (size_t i1 = 0; i1 < cardinality_; i1++) - table.push_back(i1 == value_); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /* ************************************************************************* */ - DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* ************************************************************************* */ - bool SingleValue::ensureArcConsistency(size_t j, - vector& domains) const { - if (j != keys_[0]) throw invalid_argument( - "SingleValue check on wrong domain"); - Domain& D = domains[j]; - if (D.isSingleton()) { - if (D.firstValue() != value_) throw runtime_error("Unsatisfiable"); - return false; - } - D = Domain(discreteKey(),value_); - return true; - } - - /* ************************************************************************* */ - Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const { - Values::const_iterator it = values.find(keys_[0]); - if (it != values.end() && it->second != value_) throw runtime_error( - "SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared < SingleValue > (keys_[0], cardinality_, value_); - } - - /* ************************************************************************* */ - Constraint::shared_ptr SingleValue::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; - if (Dk.isSingleton() && !Dk.contains(value_)) throw runtime_error( - "SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared < SingleValue > (discreteKey(), value_); - } +using namespace std; /* ************************************************************************* */ -} // namespace gtsam +void SingleValue::print(const string& s, const KeyFormatter& formatter) const { + cout << s << "SingleValue on " + << "j=" << formatter(keys_[0]) << " with value " << value_ << endl; +} + +/* ************************************************************************* */ +double SingleValue::operator()(const Values& values) const { + return (double)(values.at(keys_[0]) == value_); +} + +/* ************************************************************************* */ +DecisionTreeFactor SingleValue::toDecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(keys_[0], cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; i1++) table.push_back(i1 == value_); + DecisionTreeFactor converted(keys, table); + return converted; +} + +/* ************************************************************************* */ +DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} + +/* ************************************************************************* */ +bool SingleValue::ensureArcConsistency(size_t j, + vector& domains) const { + if (j != keys_[0]) + throw invalid_argument("SingleValue check on wrong domain"); + Domain& D = domains[j]; + if (D.isSingleton()) { + if (D.firstValue() != value_) throw runtime_error("Unsatisfiable"); + return false; + } + D = Domain(discreteKey(), value_); + return true; +} + +/* ************************************************************************* */ +Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const { + Values::const_iterator it = values.find(keys_[0]); + if (it != values.end() && it->second != value_) + throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared(keys_[0], cardinality_, value_); +} + +/* ************************************************************************* */ +Constraint::shared_ptr SingleValue::partiallyApply( + const vector& domains) const { + const Domain& Dk = domains[keys_[0]]; + if (Dk.isSingleton() && !Dk.contains(value_)) + throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared(discreteKey(), value_); +} + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index c4d2addec..a2aec338c 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -7,76 +7,73 @@ #pragma once -#include #include +#include namespace gtsam { - /** - * SingleValue constraint +/** + * SingleValue constraint + */ +class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { + /// Number of values + size_t cardinality_; + + /// allowed value + size_t value_; + + DiscreteKey discreteKey() const { + return DiscreteKey(keys_[0], cardinality_); + } + + public: + typedef boost::shared_ptr shared_ptr; + + /// Constructor + SingleValue(Key key, size_t n, size_t value) + : Constraint(key), cardinality_(n), value_(value) {} + + /// Constructor + SingleValue(const DiscreteKey& dkey, size_t value) + : Constraint(dkey.first), cardinality_(dkey.second), value_(value) {} + + // print + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) + return false; + else { + const SingleValue& f(static_cast(other)); + return (cardinality_ == f.cardinality_) && (value_ == f.value_); + } + } + + /// Calculate value + double operator()(const Values& values) const override; + + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains */ - class GTSAM_UNSTABLE_EXPORT SingleValue: public Constraint { + bool ensureArcConsistency(size_t j, + std::vector& domains) const override; - /// Number of values - size_t cardinality_; + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values& values) const override; - /// allowed value - size_t value_; + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector& domains) const override; +}; - DiscreteKey discreteKey() const { - return DiscreteKey(keys_[0],cardinality_); - } - - public: - - typedef boost::shared_ptr shared_ptr; - - /// Constructor - SingleValue(Key key, size_t n, size_t value) : - Constraint(key), cardinality_(n), value_(value) { - } - - /// Constructor - SingleValue(const DiscreteKey& dkey, size_t value) : - Constraint(dkey.first), cardinality_(dkey.second), value_(value) { - } - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const SingleValue& f(static_cast(other)); - return (cardinality_==f.cardinality_) && (value_==f.value_); - } - } - - /// Calculate value - double operator()(const Values& values) const override; - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - bool ensureArcConsistency(size_t j, std::vector& domains) const override; - - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values& values) const override; - - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector& domains) const override; - }; - -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index 3dd493b1b..1552fcbf1 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -7,49 +7,50 @@ #include #include + #include using boost::assign::insert; #include -#include + #include +#include using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST_UNSAFE( BinaryAllDif, allInOne) -{ +TEST_UNSAFE(BinaryAllDif, allInOne) { // Create keys and ordering size_t nrColors = 2; -// DiscreteKey ID("Idaho", nrColors), UT("Utah", nrColors), AZ("Arizona", nrColors); + // DiscreteKey ID("Idaho", nrColors), UT("Utah", nrColors), AZ("Arizona", + // nrColors); DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); // Check construction and conversion BinaryAllDiff c1(ID, UT); DecisionTreeFactor f1(ID & UT, "0 1 1 0"); - EXPECT(assert_equal(f1,c1.toDecisionTreeFactor())); + EXPECT(assert_equal(f1, c1.toDecisionTreeFactor())); // Check construction and conversion BinaryAllDiff c2(UT, AZ); DecisionTreeFactor f2(UT & AZ, "0 1 1 0"); - EXPECT(assert_equal(f2,c2.toDecisionTreeFactor())); + EXPECT(assert_equal(f2, c2.toDecisionTreeFactor())); - DecisionTreeFactor f3 = f1*f2; - EXPECT(assert_equal(f3,c1*f2)); - EXPECT(assert_equal(f3,c2*f1)); + DecisionTreeFactor f3 = f1 * f2; + EXPECT(assert_equal(f3, c1 * f2)); + EXPECT(assert_equal(f3, c2 * f1)); } /* ************************************************************************* */ -TEST_UNSAFE( CSP, allInOne) -{ +TEST_UNSAFE(CSP, allInOne) { // Create keys and ordering size_t nrColors = 2; DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); // Create the CSP CSP csp; - csp.addAllDiff(ID,UT); - csp.addAllDiff(UT,AZ); + csp.addAllDiff(ID, UT); + csp.addAllDiff(UT, AZ); // Check an invalid combination, with ID==UT==AZ all same color DiscreteFactor::Values invalid; @@ -69,67 +70,67 @@ TEST_UNSAFE( CSP, allInOne) DecisionTreeFactor product = csp.product(); // product.dot("product"); DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0"); - EXPECT(assert_equal(expectedProduct,product)); + EXPECT(assert_equal(expectedProduct, product)); // Solve CSP::sharedValues mpe = csp.optimalAssignment(); CSP::Values expected; insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1); - EXPECT(assert_equal(expected,*mpe)); + EXPECT(assert_equal(expected, *mpe)); EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); } /* ************************************************************************* */ -TEST_UNSAFE( CSP, WesternUS) -{ +TEST_UNSAFE(CSP, WesternUS) { // Create keys size_t nrColors = 4; DiscreteKey - // Create ordering according to example in ND-CSP.lyx - WA(0, nrColors), OR(3, nrColors), CA(1, nrColors),NV(2, nrColors), - ID(8, nrColors), UT(9, nrColors), AZ(10, nrColors), - MT(4, nrColors), WY(5, nrColors), CO(7, nrColors), NM(6, nrColors); + // Create ordering according to example in ND-CSP.lyx + WA(0, nrColors), + OR(3, nrColors), CA(1, nrColors), NV(2, nrColors), ID(8, nrColors), + UT(9, nrColors), AZ(10, nrColors), MT(4, nrColors), WY(5, nrColors), + CO(7, nrColors), NM(6, nrColors); // Create the CSP CSP csp; - csp.addAllDiff(WA,ID); - csp.addAllDiff(WA,OR); - csp.addAllDiff(OR,ID); - csp.addAllDiff(OR,CA); - csp.addAllDiff(OR,NV); - csp.addAllDiff(CA,NV); - csp.addAllDiff(CA,AZ); - csp.addAllDiff(ID,MT); - csp.addAllDiff(ID,WY); - csp.addAllDiff(ID,UT); - csp.addAllDiff(ID,NV); - csp.addAllDiff(NV,UT); - csp.addAllDiff(NV,AZ); - csp.addAllDiff(UT,WY); - csp.addAllDiff(UT,CO); - csp.addAllDiff(UT,NM); - csp.addAllDiff(UT,AZ); - csp.addAllDiff(AZ,CO); - csp.addAllDiff(AZ,NM); - csp.addAllDiff(MT,WY); - csp.addAllDiff(WY,CO); - csp.addAllDiff(CO,NM); + csp.addAllDiff(WA, ID); + csp.addAllDiff(WA, OR); + csp.addAllDiff(OR, ID); + csp.addAllDiff(OR, CA); + csp.addAllDiff(OR, NV); + csp.addAllDiff(CA, NV); + csp.addAllDiff(CA, AZ); + csp.addAllDiff(ID, MT); + csp.addAllDiff(ID, WY); + csp.addAllDiff(ID, UT); + csp.addAllDiff(ID, NV); + csp.addAllDiff(NV, UT); + csp.addAllDiff(NV, AZ); + csp.addAllDiff(UT, WY); + csp.addAllDiff(UT, CO); + csp.addAllDiff(UT, NM); + csp.addAllDiff(UT, AZ); + csp.addAllDiff(AZ, CO); + csp.addAllDiff(AZ, NM); + csp.addAllDiff(MT, WY); + csp.addAllDiff(WY, CO); + csp.addAllDiff(CO, NM); // Solve Ordering ordering; - ordering += Key(0),Key(1),Key(2),Key(3),Key(4),Key(5),Key(6),Key(7),Key(8),Key(9),Key(10); + ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7), + Key(8), Key(9), Key(10); CSP::sharedValues mpe = csp.optimalAssignment(ordering); // GTSAM_PRINT(*mpe); CSP::Values expected; - insert(expected) - (WA.first,1)(CA.first,1)(NV.first,3)(OR.first,0) - (MT.first,1)(WY.first,0)(NM.first,3)(CO.first,2) - (ID.first,2)(UT.first,1)(AZ.first,0); + insert(expected)(WA.first, 1)(CA.first, 1)(NV.first, 3)(OR.first, 0)( + MT.first, 1)(WY.first, 0)(NM.first, 3)(CO.first, 2)(ID.first, 2)( + UT.first, 1)(AZ.first, 0); // TODO: Fix me! mpe result seems to be right. (See the printing) // It has the same prob as the expected solution. // Is mpe another solution, or the expected solution is unique??? - EXPECT(assert_equal(expected,*mpe)); + EXPECT(assert_equal(expected, *mpe)); EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // Write out the dual graph for hmetis @@ -142,8 +143,7 @@ TEST_UNSAFE( CSP, WesternUS) } /* ************************************************************************* */ -TEST_UNSAFE( CSP, AllDiff) -{ +TEST_UNSAFE(CSP, AllDiff) { // Create keys and ordering size_t nrColors = 3; DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); @@ -151,24 +151,25 @@ TEST_UNSAFE( CSP, AllDiff) // Create the CSP CSP csp; vector dkeys; - dkeys += ID,UT,AZ; + dkeys += ID, UT, AZ; csp.addAllDiff(dkeys); - csp.addSingleValue(AZ,2); -// GTSAM_PRINT(csp); + csp.addSingleValue(AZ, 2); + // GTSAM_PRINT(csp); // Check construction and conversion - SingleValue s(AZ,2); - DecisionTreeFactor f1(AZ,"0 0 1"); - EXPECT(assert_equal(f1,s.toDecisionTreeFactor())); + SingleValue s(AZ, 2); + DecisionTreeFactor f1(AZ, "0 0 1"); + EXPECT(assert_equal(f1, s.toDecisionTreeFactor())); // Check construction and conversion AllDiff alldiff(dkeys); DecisionTreeFactor actual = alldiff.toDecisionTreeFactor(); -// GTSAM_PRINT(actual); -// actual.dot("actual"); - DecisionTreeFactor f2(ID & AZ & UT, + // GTSAM_PRINT(actual); + // actual.dot("actual"); + DecisionTreeFactor f2( + ID & AZ & UT, "0 0 0 0 0 1 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 1 0 0 0 0 0"); - EXPECT(assert_equal(f2,actual)); + EXPECT(assert_equal(f2, actual)); // Check an invalid combination, with ID==UT==AZ all same color DiscreteFactor::Values invalid; @@ -188,36 +189,36 @@ TEST_UNSAFE( CSP, AllDiff) CSP::sharedValues mpe = csp.optimalAssignment(); CSP::Values expected; insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2); - EXPECT(assert_equal(expected,*mpe)); + EXPECT(assert_equal(expected, *mpe)); EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // Arc-consistency vector domains; domains += Domain(ID), Domain(AZ), Domain(UT); - SingleValue singleValue(AZ,2); - EXPECT(singleValue.ensureArcConsistency(1,domains)); - EXPECT(alldiff.ensureArcConsistency(0,domains)); - EXPECT(!alldiff.ensureArcConsistency(1,domains)); - EXPECT(alldiff.ensureArcConsistency(2,domains)); - LONGS_EQUAL(2,domains[0].nrValues()); - LONGS_EQUAL(1,domains[1].nrValues()); - LONGS_EQUAL(2,domains[2].nrValues()); + SingleValue singleValue(AZ, 2); + EXPECT(singleValue.ensureArcConsistency(1, domains)); + EXPECT(alldiff.ensureArcConsistency(0, domains)); + EXPECT(!alldiff.ensureArcConsistency(1, domains)); + EXPECT(alldiff.ensureArcConsistency(2, domains)); + LONGS_EQUAL(2, domains[0].nrValues()); + LONGS_EQUAL(1, domains[1].nrValues()); + LONGS_EQUAL(2, domains[2].nrValues()); // Parial application, version 1 DiscreteFactor::Values known; known[AZ.first] = 2; DiscreteFactor::shared_ptr reduced1 = alldiff.partiallyApply(known); DecisionTreeFactor f3(ID & UT, "0 1 1 1 0 1 1 1 0"); - EXPECT(assert_equal(f3,reduced1->toDecisionTreeFactor())); + EXPECT(assert_equal(f3, reduced1->toDecisionTreeFactor())); DiscreteFactor::shared_ptr reduced2 = singleValue.partiallyApply(known); DecisionTreeFactor f4(AZ, "0 0 1"); - EXPECT(assert_equal(f4,reduced2->toDecisionTreeFactor())); + EXPECT(assert_equal(f4, reduced2->toDecisionTreeFactor())); // Parial application, version 2 DiscreteFactor::shared_ptr reduced3 = alldiff.partiallyApply(domains); - EXPECT(assert_equal(f3,reduced3->toDecisionTreeFactor())); + EXPECT(assert_equal(f3, reduced3->toDecisionTreeFactor())); DiscreteFactor::shared_ptr reduced4 = singleValue.partiallyApply(domains); - EXPECT(assert_equal(f4,reduced4->toDecisionTreeFactor())); + EXPECT(assert_equal(f4, reduced4->toDecisionTreeFactor())); // full arc-consistency test csp.runArcConsistency(nrColors); @@ -229,4 +230,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index 9929938d5..c48d7639d 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -5,14 +5,15 @@ * @date Oct 11, 2013 */ -#include +#include #include #include -#include -#include +#include + #include -#include +#include #include +#include using namespace std; using namespace boost; @@ -23,11 +24,12 @@ using namespace gtsam; * Loopy belief solver for graphs with only binary and unary factors */ class LoopyBelief { - /** Star graph struct for each node, containing * - the star graph itself - * - the product of original unary factors so we don't have to recompute it later, and - * - the factor indices of the corrected belief factors of the neighboring nodes + * - the product of original unary factors so we don't have to recompute it + * later, and + * - the factor indices of the corrected belief factors of the neighboring + * nodes */ typedef std::map CorrectedBeliefIndices; struct StarGraph { @@ -36,41 +38,41 @@ class LoopyBelief { DecisionTreeFactor::shared_ptr unary; VariableIndex varIndex_; StarGraph(const DiscreteFactorGraph::shared_ptr& _star, - const CorrectedBeliefIndices& _beliefIndices, - const DecisionTreeFactor::shared_ptr& _unary) : - star(_star), correctedBeliefIndices(_beliefIndices), unary(_unary), varIndex_( - *_star) { - } + const CorrectedBeliefIndices& _beliefIndices, + const DecisionTreeFactor::shared_ptr& _unary) + : star(_star), + correctedBeliefIndices(_beliefIndices), + unary(_unary), + varIndex_(*_star) {} void print(const std::string& s = "") const { cout << s << ":" << endl; star->print("Star graph: "); - for(Key key: correctedBeliefIndices | boost::adaptors::map_keys) { + for (Key key : correctedBeliefIndices | boost::adaptors::map_keys) { cout << "Belief factor index for " << key << ": " - << correctedBeliefIndices.at(key) << endl; + << correctedBeliefIndices.at(key) << endl; } - if (unary) - unary->print("Unary: "); + if (unary) unary->print("Unary: "); } }; typedef std::map StarGraphs; - StarGraphs starGraphs_; ///< star graph at each variable + StarGraphs starGraphs_; ///< star graph at each variable -public: + public: /** Constructor - * Need all discrete keys to access node's cardinality for creating belief factors + * Need all discrete keys to access node's cardinality for creating belief + * factors * TODO: so troublesome!! */ LoopyBelief(const DiscreteFactorGraph& graph, - const std::map& allDiscreteKeys) : - starGraphs_(buildStarGraphs(graph, allDiscreteKeys)) { - } + const std::map& allDiscreteKeys) + : starGraphs_(buildStarGraphs(graph, allDiscreteKeys)) {} /// print void print(const std::string& s = "") const { cout << s << ":" << endl; - for(Key key: starGraphs_ | boost::adaptors::map_keys) { + for (Key key : starGraphs_ | boost::adaptors::map_keys) { starGraphs_.at(key).print((boost::format("Node %d:") % key).str()); } } @@ -79,12 +81,13 @@ public: DiscreteFactorGraph::shared_ptr iterate( const std::map& allDiscreteKeys) { static const bool debug = false; - static DiscreteConditional::shared_ptr dummyCond; // unused by-product of elimination + static DiscreteConditional::shared_ptr + dummyCond; // unused by-product of elimination DiscreteFactorGraph::shared_ptr beliefs(new DiscreteFactorGraph()); std::map > allMessages; // Eliminate each star graph - for(Key key: starGraphs_ | boost::adaptors::map_keys) { -// cout << "***** Node " << key << "*****" << endl; + for (Key key : starGraphs_ | boost::adaptors::map_keys) { + // cout << "***** Node " << key << "*****" << endl; // initialize belief to the unary factor from the original graph DecisionTreeFactor::shared_ptr beliefAtKey; @@ -92,15 +95,16 @@ public: std::map messages; // eliminate each neighbor in this star graph one by one - for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { + for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices | + boost::adaptors::map_keys) { DiscreteFactorGraph subGraph; - for(size_t factor: starGraphs_.at(key).varIndex_[neighbor]) { + for (size_t factor : starGraphs_.at(key).varIndex_[neighbor]) { subGraph.push_back(starGraphs_.at(key).star->at(factor)); } if (debug) subGraph.print("------- Subgraph:"); DiscreteFactor::shared_ptr message; - boost::tie(dummyCond, message) = EliminateDiscrete(subGraph, - Ordering(list_of(neighbor))); + boost::tie(dummyCond, message) = + EliminateDiscrete(subGraph, Ordering(list_of(neighbor))); // store the new factor into messages messages.insert(make_pair(neighbor, message)); if (debug) message->print("------- Message: "); @@ -108,14 +112,12 @@ public: // Belief is the product of all messages and the unary factor // Incorporate new the factor to belief if (!beliefAtKey) - beliefAtKey = boost::dynamic_pointer_cast( - message); - else beliefAtKey = - boost::make_shared( - (*beliefAtKey) - * (*boost::dynamic_pointer_cast( - message))); + boost::dynamic_pointer_cast(message); + else + beliefAtKey = boost::make_shared( + (*beliefAtKey) * + (*boost::dynamic_pointer_cast(message))); } if (starGraphs_.at(key).unary) beliefAtKey = boost::make_shared( @@ -133,7 +135,8 @@ public: sumFactorTable = (boost::format("%s %f") % sumFactorTable % sum).str(); DecisionTreeFactor sumFactor(allDiscreteKeys.at(key), sumFactorTable); if (debug) sumFactor.print("denomFactor: "); - beliefAtKey = boost::make_shared((*beliefAtKey) / sumFactor); + beliefAtKey = + boost::make_shared((*beliefAtKey) / sumFactor); if (debug) beliefAtKey->print("New belief at key normalized: "); beliefs->push_back(beliefAtKey); allMessages[key] = messages; @@ -141,17 +144,20 @@ public: // Update corrected beliefs VariableIndex beliefFactors(*beliefs); - for(Key key: starGraphs_ | boost::adaptors::map_keys) { + for (Key key : starGraphs_ | boost::adaptors::map_keys) { std::map messages = allMessages[key]; - for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { - DecisionTreeFactor correctedBelief = (*boost::dynamic_pointer_cast< - DecisionTreeFactor>(beliefs->at(beliefFactors[key].front()))) - / (*boost::dynamic_pointer_cast( + for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices | + boost::adaptors::map_keys) { + DecisionTreeFactor correctedBelief = + (*boost::dynamic_pointer_cast( + beliefs->at(beliefFactors[key].front()))) / + (*boost::dynamic_pointer_cast( messages.at(neighbor))); if (debug) correctedBelief.print("correctedBelief: "); - size_t beliefIndex = starGraphs_.at(neighbor).correctedBeliefIndices.at( - key); - starGraphs_.at(neighbor).star->replace(beliefIndex, + size_t beliefIndex = + starGraphs_.at(neighbor).correctedBeliefIndices.at(key); + starGraphs_.at(neighbor).star->replace( + beliefIndex, boost::make_shared(correctedBelief)); } } @@ -161,21 +167,22 @@ public: return beliefs; } -private: + private: /** * Build star graphs for each node. */ - StarGraphs buildStarGraphs(const DiscreteFactorGraph& graph, + StarGraphs buildStarGraphs( + const DiscreteFactorGraph& graph, const std::map& allDiscreteKeys) const { StarGraphs starGraphs; - VariableIndex varIndex(graph); ///< access to all factors of each node - for(Key key: varIndex | boost::adaptors::map_keys) { + VariableIndex varIndex(graph); ///< access to all factors of each node + for (Key key : varIndex | boost::adaptors::map_keys) { // initialize to multiply with other unary factors later DecisionTreeFactor::shared_ptr prodOfUnaries; // collect all factors involving this key in the original graph DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph()); - for(size_t factorIndex: varIndex[key]) { + for (size_t factorIndex : varIndex[key]) { star->push_back(graph.at(factorIndex)); // accumulate unary factors @@ -185,9 +192,9 @@ private: graph.at(factorIndex)); else prodOfUnaries = boost::make_shared( - *prodOfUnaries - * (*boost::dynamic_pointer_cast( - graph.at(factorIndex)))); + *prodOfUnaries * + (*boost::dynamic_pointer_cast( + graph.at(factorIndex)))); } } @@ -196,7 +203,7 @@ private: KeySet neighbors = star->keys(); neighbors.erase(key); CorrectedBeliefIndices correctedBeliefIndices; - for(Key neighbor: neighbors) { + for (Key neighbor : neighbors) { // TODO: default table for keys with more than 2 values? string initialBelief; for (size_t v = 0; v < allDiscreteKeys.at(neighbor).second - 1; ++v) { @@ -207,9 +214,8 @@ private: DecisionTreeFactor(allDiscreteKeys.at(neighbor), initialBelief)); correctedBeliefIndices.insert(make_pair(neighbor, star->size() - 1)); } - starGraphs.insert( - make_pair(key, - StarGraph(star, correctedBeliefIndices, prodOfUnaries))); + starGraphs.insert(make_pair( + key, StarGraph(star, correctedBeliefIndices, prodOfUnaries))); } return starGraphs; } @@ -249,7 +255,6 @@ TEST_UNSAFE(LoopyBelief, construction) { DiscreteFactorGraph::shared_ptr beliefs = solver.iterate(allKeys); beliefs->print(); } - } /* ************************************************************************* */ diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index 3f6c6a1e0..4eb86fe1f 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -5,14 +5,13 @@ */ //#define ENABLE_TIMING -#include +#include #include #include +#include -#include - -#include #include +#include #include using namespace boost::assign; @@ -22,7 +21,6 @@ using namespace gtsam; /* ************************************************************************* */ // Create the expected graph of constraints DiscreteFactorGraph createExpected() { - // Start building size_t nrFaculty = 4, nrTimeSlots = 3; @@ -47,27 +45,27 @@ DiscreteFactorGraph createExpected() { string available = "1 1 1 0 1 1 1 1 0 1 1 1"; // Akansel - expected.add(A1, faculty_in_A); // Area 1 - expected.add(A1, "1 1 1 0"); // Advisor + expected.add(A1, faculty_in_A); // Area 1 + expected.add(A1, "1 1 1 0"); // Advisor expected.add(A & A1, available); - expected.add(A2, faculty_in_M); // Area 2 - expected.add(A2, "1 1 1 0"); // Advisor + expected.add(A2, faculty_in_M); // Area 2 + expected.add(A2, "1 1 1 0"); // Advisor expected.add(A & A2, available); - expected.add(A3, faculty_in_P); // Area 3 - expected.add(A3, "1 1 1 0"); // Advisor + expected.add(A3, faculty_in_P); // Area 3 + expected.add(A3, "1 1 1 0"); // Advisor expected.add(A & A3, available); // Mutual exclusion for faculty expected.addAllDiff(A1 & A2 & A3); // Jake - expected.add(J1, faculty_in_H); // Area 1 - expected.add(J1, "1 0 1 1"); // Advisor + expected.add(J1, faculty_in_H); // Area 1 + expected.add(J1, "1 0 1 1"); // Advisor expected.add(J & J1, available); - expected.add(J2, faculty_in_C); // Area 2 - expected.add(J2, "1 0 1 1"); // Advisor + expected.add(J2, faculty_in_C); // Area 2 + expected.add(J2, "1 0 1 1"); // Advisor expected.add(J & J2, available); - expected.add(J3, faculty_in_A); // Area 3 - expected.add(J3, "1 0 1 1"); // Advisor + expected.add(J3, faculty_in_A); // Area 3 + expected.add(J3, "1 0 1 1"); // Advisor expected.add(J & J3, available); // Mutual exclusion for faculty expected.addAllDiff(J1 & J2 & J3); @@ -79,8 +77,7 @@ DiscreteFactorGraph createExpected() { } /* ************************************************************************* */ -TEST( schedulingExample, test) -{ +TEST(schedulingExample, test) { Scheduler s(2); // add faculty @@ -121,7 +118,7 @@ TEST( schedulingExample, test) // Do brute force product and output that to file DecisionTreeFactor product = s.product(); - //product.dot("scheduling", false); + // product.dot("scheduling", false); // Do exact inference gttic(small); @@ -129,25 +126,24 @@ TEST( schedulingExample, test) gttoc(small); // print MPE, commented out as unit tests don't print -// s.printAssignment(MPE); + // s.printAssignment(MPE); // Commented out as does not work yet // s.runArcConsistency(8,10,true); // find the assignment of students to slots with most possible committees // Commented out as not implemented yet -// sharedValues bestSchedule = s.bestSchedule(); -// GTSAM_PRINT(*bestSchedule); + // sharedValues bestSchedule = s.bestSchedule(); + // GTSAM_PRINT(*bestSchedule); // find the corresponding most desirable committee assignment // Commented out as not implemented yet -// sharedValues bestAssignment = s.bestAssignment(bestSchedule); -// GTSAM_PRINT(*bestAssignment); + // sharedValues bestAssignment = s.bestAssignment(bestSchedule); + // GTSAM_PRINT(*bestAssignment); } /* ************************************************************************* */ -TEST( schedulingExample, smallFromFile) -{ +TEST(schedulingExample, smallFromFile) { string path(TOPSRCDIR "/gtsam_unstable/discrete/examples/"); Scheduler s(2, path + "small.csv"); @@ -179,4 +175,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index e2115e8bc..78f159251 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -5,21 +5,22 @@ * @author Frank Dellaert */ -#include #include +#include + #include using boost::assign::insert; +#include + #include #include -#include using namespace std; using namespace gtsam; #define PRINT false -class Sudoku: public CSP { - +class Sudoku : public CSP { /// sudoku size size_t n_; @@ -27,25 +28,21 @@ class Sudoku: public CSP { typedef std::pair IJ; std::map dkeys_; -public: - + public: /// return DiscreteKey for cell(i,j) const DiscreteKey& dkey(size_t i, size_t j) const { return dkeys_.at(IJ(i, j)); } /// return Key for cell(i,j) - Key key(size_t i, size_t j) const { - return dkey(i, j).first; - } + Key key(size_t i, size_t j) const { return dkey(i, j).first; } /// Constructor - Sudoku(size_t n, ...) : - n_(n) { + Sudoku(size_t n, ...) : n_(n) { // Create variables, ordering, and unary constraints va_list ap; va_start(ap, n); - Key k=0; + Key k = 0; for (size_t i = 0; i < n; ++i) { for (size_t j = 0; j < n; ++j, ++k) { // create the key @@ -56,23 +53,21 @@ public: // cout << value << " "; if (value != 0) addSingleValue(dkeys_[ij], value - 1); } - //cout << endl; + // cout << endl; } va_end(ap); // add row constraints for (size_t i = 0; i < n; i++) { DiscreteKeys dkeys; - for (size_t j = 0; j < n; j++) - dkeys += dkey(i, j); + for (size_t j = 0; j < n; j++) dkeys += dkey(i, j); addAllDiff(dkeys); } // add col constraints for (size_t j = 0; j < n; j++) { DiscreteKeys dkeys; - for (size_t i = 0; i < n; i++) - dkeys += dkey(i, j); + for (size_t i = 0; i < n; i++) dkeys += dkey(i, j); addAllDiff(dkeys); } @@ -84,8 +79,7 @@ public: // Box I,J DiscreteKeys dkeys; for (size_t i = i0; i < i0 + N; i++) - for (size_t j = j0; j < j0 + N; j++) - dkeys += dkey(i, j); + for (size_t j = j0; j < j0 + N; j++) dkeys += dkey(i, j); addAllDiff(dkeys); j0 += N; } @@ -109,74 +103,59 @@ public: DiscreteFactor::sharedValues MPE = optimalAssignment(); printAssignment(MPE); } - }; /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, small) -{ - Sudoku csp(4, - 1,0, 0,4, - 0,0, 0,0, +TEST_UNSAFE(Sudoku, small) { + Sudoku csp(4, 1, 0, 0, 4, 0, 0, 0, 0, - 4,0, 2,0, - 0,1, 0,0); + 4, 0, 2, 0, 0, 1, 0, 0); // Do BP - csp.runArcConsistency(4,10,PRINT); + csp.runArcConsistency(4, 10, PRINT); // optimize and check CSP::sharedValues solution = csp.optimalAssignment(); CSP::Values expected; - insert(expected) - (csp.key(0,0), 0)(csp.key(0,1), 1)(csp.key(0,2), 2)(csp.key(0,3), 3) - (csp.key(1,0), 2)(csp.key(1,1), 3)(csp.key(1,2), 0)(csp.key(1,3), 1) - (csp.key(2,0), 3)(csp.key(2,1), 2)(csp.key(2,2), 1)(csp.key(2,3), 0) - (csp.key(3,0), 1)(csp.key(3,1), 0)(csp.key(3,2), 3)(csp.key(3,3), 2); - EXPECT(assert_equal(expected,*solution)); - //csp.printAssignment(solution); + insert(expected)(csp.key(0, 0), 0)(csp.key(0, 1), 1)(csp.key(0, 2), 2)( + csp.key(0, 3), 3)(csp.key(1, 0), 2)(csp.key(1, 1), 3)(csp.key(1, 2), 0)( + csp.key(1, 3), 1)(csp.key(2, 0), 3)(csp.key(2, 1), 2)(csp.key(2, 2), 1)( + csp.key(2, 3), 0)(csp.key(3, 0), 1)(csp.key(3, 1), 0)(csp.key(3, 2), 3)( + csp.key(3, 3), 2); + EXPECT(assert_equal(expected, *solution)); + // csp.printAssignment(solution); } /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, easy) -{ - Sudoku sudoku(9, - 0,0,5, 0,9,0, 0,0,1, - 0,0,0, 0,0,2, 0,7,3, - 7,6,0, 0,0,8, 2,0,0, +TEST_UNSAFE(Sudoku, easy) { + Sudoku sudoku(9, 0, 0, 5, 0, 9, 0, 0, 0, 1, 0, 0, 0, 0, 0, 2, 0, 7, 3, 7, 6, + 0, 0, 0, 8, 2, 0, 0, - 0,1,2, 0,0,9, 0,0,4, - 0,0,0, 2,0,3, 0,0,0, - 3,0,0, 1,0,0, 9,6,0, + 0, 1, 2, 0, 0, 9, 0, 0, 4, 0, 0, 0, 2, 0, 3, 0, 0, 0, 3, 0, 0, + 1, 0, 0, 9, 6, 0, - 0,0,1, 9,0,0, 0,5,8, - 9,7,0, 5,0,0, 0,0,0, - 5,0,0, 0,3,0, 7,0,0); + 0, 0, 1, 9, 0, 0, 0, 5, 8, 9, 7, 0, 5, 0, 0, 0, 0, 0, 5, 0, 0, + 0, 3, 0, 7, 0, 0); // Do BP - sudoku.runArcConsistency(4,10,PRINT); + sudoku.runArcConsistency(4, 10, PRINT); // sudoku.printSolution(); // don't do it } /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, extreme) -{ - Sudoku sudoku(9, - 0,0,9, 7,4,8, 0,0,0, - 7,0,0, 0,0,0, 0,0,0, - 0,2,0, 1,0,9, 0,0,0, +TEST_UNSAFE(Sudoku, extreme) { + Sudoku sudoku(9, 0, 0, 9, 7, 4, 8, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, + 0, 1, 0, 9, 0, 0, 0, - 0,0,7, 0,0,0, 2,4,0, - 0,6,4, 0,1,0, 5,9,0, - 0,9,8, 0,0,0, 3,0,0, + 0, 0, 7, 0, 0, 0, 2, 4, 0, 0, 6, 4, 0, 1, 0, 5, 9, 0, 0, 9, 8, + 0, 0, 0, 3, 0, 0, - 0,0,0, 8,0,3, 0,2,0, - 0,0,0, 0,0,0, 0,0,6, - 0,0,0, 2,7,5, 9,0,0); + 0, 0, 0, 8, 0, 3, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 0, 0, 0, + 2, 7, 5, 9, 0, 0); // Do BP - sudoku.runArcConsistency(9,10,PRINT); + sudoku.runArcConsistency(9, 10, PRINT); #ifdef METIS VariableIndexOrdered index(sudoku); @@ -185,29 +164,24 @@ TEST_UNSAFE( Sudoku, extreme) index.outputMetisFormat(os); #endif - //sudoku.printSolution(); // don't do it + // sudoku.printSolution(); // don't do it } /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, AJC_3star_Feb8_2012) -{ - Sudoku sudoku(9, - 9,5,0, 0,0,6, 0,0,0, - 0,8,4, 0,7,0, 0,0,0, - 6,2,0, 5,0,0, 4,0,0, +TEST_UNSAFE(Sudoku, AJC_3star_Feb8_2012) { + Sudoku sudoku(9, 9, 5, 0, 0, 0, 6, 0, 0, 0, 0, 8, 4, 0, 7, 0, 0, 0, 0, 6, 2, + 0, 5, 0, 0, 4, 0, 0, - 0,0,0, 2,9,0, 6,0,0, - 0,9,0, 0,0,0, 0,2,0, - 0,0,2, 0,6,3, 0,0,0, + 0, 0, 0, 2, 9, 0, 6, 0, 0, 0, 9, 0, 0, 0, 0, 0, 2, 0, 0, 0, 2, + 0, 6, 3, 0, 0, 0, - 0,0,9, 0,0,7, 0,6,8, - 0,0,0, 0,3,0, 2,9,0, - 0,0,0, 1,0,0, 0,3,7); + 0, 0, 9, 0, 0, 7, 0, 6, 8, 0, 0, 0, 0, 3, 0, 2, 9, 0, 0, 0, 0, + 1, 0, 0, 0, 3, 7); // Do BP - sudoku.runArcConsistency(9,10,PRINT); + sudoku.runArcConsistency(9, 10, PRINT); - //sudoku.printSolution(); // don't do it + // sudoku.printSolution(); // don't do it } /* ************************************************************************* */ @@ -216,4 +190,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From aebcf07ab5a1ace4741edbed3e721f867062fa09 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Nov 2021 11:31:11 -0500 Subject: [PATCH 35/71] Formatted sudokus better --- gtsam_unstable/discrete/tests/testSudoku.cpp | 57 ++++++++++++-------- 1 file changed, 34 insertions(+), 23 deletions(-) diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index 78f159251..4843ae269 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -107,9 +107,11 @@ class Sudoku : public CSP { /* ************************************************************************* */ TEST_UNSAFE(Sudoku, small) { - Sudoku csp(4, 1, 0, 0, 4, 0, 0, 0, 0, - - 4, 0, 2, 0, 0, 1, 0, 0); + Sudoku csp(4, // + 1, 0, 0, 4, // + 0, 0, 0, 0, // + 4, 0, 2, 0, // + 0, 1, 0, 0); // Do BP csp.runArcConsistency(4, 10, PRINT); @@ -128,14 +130,18 @@ TEST_UNSAFE(Sudoku, small) { /* ************************************************************************* */ TEST_UNSAFE(Sudoku, easy) { - Sudoku sudoku(9, 0, 0, 5, 0, 9, 0, 0, 0, 1, 0, 0, 0, 0, 0, 2, 0, 7, 3, 7, 6, - 0, 0, 0, 8, 2, 0, 0, + Sudoku sudoku(9, // + 0, 0, 5, 0, 9, 0, 0, 0, 1, // + 0, 0, 0, 0, 0, 2, 0, 7, 3, // + 7, 6, 0, 0, 0, 8, 2, 0, 0, // - 0, 1, 2, 0, 0, 9, 0, 0, 4, 0, 0, 0, 2, 0, 3, 0, 0, 0, 3, 0, 0, - 1, 0, 0, 9, 6, 0, + 0, 1, 2, 0, 0, 9, 0, 0, 4, // + 0, 0, 0, 2, 0, 3, 0, 0, 0, // + 3, 0, 0, 1, 0, 0, 9, 6, 0, // - 0, 0, 1, 9, 0, 0, 0, 5, 8, 9, 7, 0, 5, 0, 0, 0, 0, 0, 5, 0, 0, - 0, 3, 0, 7, 0, 0); + 0, 0, 1, 9, 0, 0, 0, 5, 8, // + 9, 7, 0, 5, 0, 0, 0, 0, 0, // + 5, 0, 0, 0, 3, 0, 7, 0, 0); // Do BP sudoku.runArcConsistency(4, 10, PRINT); @@ -145,14 +151,15 @@ TEST_UNSAFE(Sudoku, easy) { /* ************************************************************************* */ TEST_UNSAFE(Sudoku, extreme) { - Sudoku sudoku(9, 0, 0, 9, 7, 4, 8, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, - 0, 1, 0, 9, 0, 0, 0, - - 0, 0, 7, 0, 0, 0, 2, 4, 0, 0, 6, 4, 0, 1, 0, 5, 9, 0, 0, 9, 8, - 0, 0, 0, 3, 0, 0, - - 0, 0, 0, 8, 0, 3, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 0, 0, 0, - 2, 7, 5, 9, 0, 0); + Sudoku sudoku(9, // + 0, 0, 9, 7, 4, 8, 0, 0, 0, 7, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, // + 0, 1, 0, 9, 0, 0, 0, 0, 0, 7, // + 0, 0, 0, 2, 4, 0, 0, 6, 4, 0, // + 1, 0, 5, 9, 0, 0, 9, 8, 0, 0, // + 0, 3, 0, 0, 0, 0, 0, 8, 0, 3, // + 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 6, 0, 0, 0, 2, 7, 5, 9, 0, 0); // Do BP sudoku.runArcConsistency(9, 10, PRINT); @@ -169,14 +176,18 @@ TEST_UNSAFE(Sudoku, extreme) { /* ************************************************************************* */ TEST_UNSAFE(Sudoku, AJC_3star_Feb8_2012) { - Sudoku sudoku(9, 9, 5, 0, 0, 0, 6, 0, 0, 0, 0, 8, 4, 0, 7, 0, 0, 0, 0, 6, 2, - 0, 5, 0, 0, 4, 0, 0, + Sudoku sudoku(9, // + 9, 5, 0, 0, 0, 6, 0, 0, 0, // + 0, 8, 4, 0, 7, 0, 0, 0, 0, // + 6, 2, 0, 5, 0, 0, 4, 0, 0, // - 0, 0, 0, 2, 9, 0, 6, 0, 0, 0, 9, 0, 0, 0, 0, 0, 2, 0, 0, 0, 2, - 0, 6, 3, 0, 0, 0, + 0, 0, 0, 2, 9, 0, 6, 0, 0, // + 0, 9, 0, 0, 0, 0, 0, 2, 0, // + 0, 0, 2, 0, 6, 3, 0, 0, 0, // - 0, 0, 9, 0, 0, 7, 0, 6, 8, 0, 0, 0, 0, 3, 0, 2, 9, 0, 0, 0, 0, - 1, 0, 0, 0, 3, 7); + 0, 0, 9, 0, 0, 7, 0, 6, 8, // + 0, 0, 0, 0, 3, 0, 2, 9, 0, // + 0, 0, 0, 1, 0, 0, 0, 3, 7); // Do BP sudoku.runArcConsistency(9, 10, PRINT); From dd509756685469413eb6e964b291b0a6b35dd1ec Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Nov 2021 10:17:49 -0500 Subject: [PATCH 36/71] Revamped arc consistency --- gtsam_unstable/discrete/AllDiff.cpp | 184 ++++++++++++---------- gtsam_unstable/discrete/AllDiff.h | 119 +++++++------- gtsam_unstable/discrete/BinaryAllDiff.h | 163 ++++++++++--------- gtsam_unstable/discrete/CSP.cpp | 5 +- gtsam_unstable/discrete/Constraint.h | 119 +++++++------- gtsam_unstable/discrete/Domain.cpp | 155 +++++++++--------- gtsam_unstable/discrete/Domain.h | 35 ++-- gtsam_unstable/discrete/SingleValue.cpp | 125 +++++++-------- gtsam_unstable/discrete/SingleValue.h | 125 +++++++-------- gtsam_unstable/discrete/tests/testCSP.cpp | 129 +++++++++------ 10 files changed, 612 insertions(+), 547 deletions(-) diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index d6e1c6453..ebc789ec2 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -5,105 +5,115 @@ * @author Frank Dellaert */ -#include -#include #include - +#include +#include #include namespace gtsam { -/* ************************************************************************* */ -AllDiff::AllDiff(const DiscreteKeys& dkeys) : Constraint(dkeys.indices()) { - for (const DiscreteKey& dkey : dkeys) cardinalities_.insert(dkey); -} - -/* ************************************************************************* */ -void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const { - std::cout << s << "AllDiff on "; - for (Key dkey : keys_) std::cout << formatter(dkey) << " "; - std::cout << std::endl; -} - -/* ************************************************************************* */ -double AllDiff::operator()(const Values& values) const { - std::set taken; // record values taken by keys - for (Key dkey : keys_) { - size_t value = values.at(dkey); // get the value for that key - if (taken.count(value)) return 0.0; // check if value alreday taken - taken.insert(value); // if not, record it as taken and keep checking + /* ************************************************************************* */ + AllDiff::AllDiff(const DiscreteKeys& dkeys) : + Constraint(dkeys.indices()) { + for(const DiscreteKey& dkey: dkeys) + cardinalities_.insert(dkey); } - return 1.0; -} -/* ************************************************************************* */ -DecisionTreeFactor AllDiff::toDecisionTreeFactor() const { - // We will do this by converting the allDif into many BinaryAllDiff - // constraints - DecisionTreeFactor converted; - size_t nrKeys = keys_.size(); - for (size_t i1 = 0; i1 < nrKeys; i1++) - for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) { - BinaryAllDiff binary12(discreteKey(i1), discreteKey(i2)); - converted = converted * binary12.toDecisionTreeFactor(); + /* ************************************************************************* */ + void AllDiff::print(const std::string& s, + const KeyFormatter& formatter) const { + std::cout << s << "AllDiff on "; + for (Key dkey: keys_) + std::cout << formatter(dkey) << " "; + std::cout << std::endl; + } + + /* ************************************************************************* */ + double AllDiff::operator()(const Values& values) const { + std::set < size_t > taken; // record values taken by keys + for(Key dkey: keys_) { + size_t value = values.at(dkey); // get the value for that key + if (taken.count(value)) return 0.0;// check if value alreday taken + taken.insert(value);// if not, record it as taken and keep checking } - return converted; -} + return 1.0; + } -/* ************************************************************************* */ -DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; -} + /* ************************************************************************* */ + DecisionTreeFactor AllDiff::toDecisionTreeFactor() const { + // We will do this by converting the allDif into many BinaryAllDiff constraints + DecisionTreeFactor converted; + size_t nrKeys = keys_.size(); + for (size_t i1 = 0; i1 < nrKeys; i1++) + for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) { + BinaryAllDiff binary12(discreteKey(i1),discreteKey(i2)); + converted = converted * binary12.toDecisionTreeFactor(); + } + return converted; + } -/* ************************************************************************* */ -bool AllDiff::ensureArcConsistency(size_t j, - std::vector& domains) const { - // Though strictly not part of allDiff, we check for - // a value in domains[j] that does not occur in any other connected domain. - // If found, we make this a singleton... - // TODO: make a new constraint where this really is true - Domain& Dj = domains[j]; - if (Dj.checkAllDiff(keys_, domains)) return true; + /* ************************************************************************* */ + DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; + } - // Check all other domains for singletons and erase corresponding values - // This is the same as arc-consistency on the equivalent binary constraints - bool changed = false; - for (Key k : keys_) - if (k != j) { - const Domain& Dk = domains[k]; - if (Dk.isSingleton()) { // check if singleton - size_t value = Dk.firstValue(); - if (Dj.contains(value)) { - Dj.erase(value); // erase value if true - changed = true; + /* ************************************************************************* */ + bool AllDiff::ensureArcConsistency(size_t j, + std::vector* domains) const { + // We are changing the domain of variable j. + // TODO(dellaert): confusing, I thought we were changing others... + Domain& Dj = domains->at(j); + + // Though strictly not part of allDiff, we check for + // a value in domains[j] that does not occur in any other connected domain. + // If found, we make this a singleton... + // TODO: make a new constraint where this really is true + boost::optional maybeChanged = Dj.checkAllDiff(keys_, *domains); + if (maybeChanged) { + Dj = *maybeChanged; + return true; + } + + // Check all other domains for singletons and erase corresponding values. + // This is the same as arc-consistency on the equivalent binary constraints + bool changed = false; + for (Key k : keys_) + if (k != j) { + const Domain& Dk = domains->at(k); + if (Dk.isSingleton()) { // check if singleton + size_t value = Dk.firstValue(); + if (Dj.contains(value)) { + Dj.erase(value); // erase value if true + changed = true; + } } } - } - return changed; -} - -/* ************************************************************************* */ -Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { - DiscreteKeys newKeys; - // loop over keys and add them only if they do not appear in values - for (Key k : keys_) - if (values.find(k) == values.end()) { - newKeys.push_back(DiscreteKey(k, cardinalities_.at(k))); - } - return boost::make_shared(newKeys); -} - -/* ************************************************************************* */ -Constraint::shared_ptr AllDiff::partiallyApply( - const std::vector& domains) const { - DiscreteFactor::Values known; - for (Key k : keys_) { - const Domain& Dk = domains[k]; - if (Dk.isSingleton()) known[k] = Dk.firstValue(); + return changed; } - return partiallyApply(known); -} -/* ************************************************************************* */ -} // namespace gtsam + /* ************************************************************************* */ + Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { + DiscreteKeys newKeys; + // loop over keys and add them only if they do not appear in values + for(Key k: keys_) + if (values.find(k) == values.end()) { + newKeys.push_back(DiscreteKey(k,cardinalities_.at(k))); + } + return boost::make_shared(newKeys); + } + + /* ************************************************************************* */ + Constraint::shared_ptr AllDiff::partiallyApply( + const std::vector& domains) const { + DiscreteFactor::Values known; + for(Key k: keys_) { + const Domain& Dk = domains[k]; + if (Dk.isSingleton()) + known[k] = Dk.firstValue(); + } + return partiallyApply(known); + } + + /* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index b0fd1d631..8c83e5ba1 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -7,71 +7,70 @@ #pragma once -#include #include +#include namespace gtsam { -/** - * General AllDiff constraint - * Returns 1 if values for all keys are different, 0 otherwise - * DiscreteFactors are all awkward in that they have to store two types of keys: - * for each variable we have a Key and an Key. In this factor, we - * keep the Indices locally, and the Indices are stored in IndexFactor. - */ -class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { - std::map cardinalities_; - - DiscreteKey discreteKey(size_t i) const { - Key j = keys_[i]; - return DiscreteKey(j, cardinalities_.at(j)); - } - - public: - /// Constructor - AllDiff(const DiscreteKeys& dkeys); - - // print - void print(const std::string& s = "", const KeyFormatter& formatter = - DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if (!dynamic_cast(&other)) - return false; - else { - const AllDiff& f(static_cast(other)); - return cardinalities_.size() == f.cardinalities_.size() && - std::equal(cardinalities_.begin(), cardinalities_.end(), - f.cardinalities_.begin()); - } - } - - /// Calculate value = expensive ! - double operator()(const Values& values) const override; - - /// Convert into a decisiontree, can be *very* expensive ! - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * Arc-consistency involves creating binaryAllDiff constraints - * In which case the combinatorial hyper-arc explosion disappears. - * @param j domain to be checked - * @param domains all other domains + /** + * General AllDiff constraint. + * Returns 1 if values for all keys are different, 0 otherwise. */ - bool ensureArcConsistency(size_t j, - std::vector& domains) const override; + class GTSAM_UNSTABLE_EXPORT AllDiff: public Constraint { - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values&) const override; + std::map cardinalities_; - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector&) const override; -}; + DiscreteKey discreteKey(size_t i) const { + Key j = keys_[i]; + return DiscreteKey(j,cardinalities_.at(j)); + } -} // namespace gtsam + public: + + /// Construct from keys. + AllDiff(const DiscreteKeys& dkeys); + + // print + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if(!dynamic_cast(&other)) + return false; + else { + const AllDiff& f(static_cast(other)); + return cardinalities_.size() == f.cardinalities_.size() + && std::equal(cardinalities_.begin(), cardinalities_.end(), + f.cardinalities_.begin()); + } + } + + /// Calculate value = expensive ! + double operator()(const Values& values) const override; + + /// Convert into a decisiontree, can be *very* expensive ! + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency + * Arc-consistency involves creating binaryAllDiff constraints + * In which case the combinatorial hyper-arc explosion disappears. + * @param j domain to be checked + * @param (in/out) domains all other domains + */ + bool ensureArcConsistency(size_t j, + std::vector* domains) const override; + + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values&) const override; + + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector&) const override; + }; + +} // namespace gtsam diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index d8e1a590a..acc3cc421 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -7,93 +7,92 @@ #pragma once -#include -#include #include +#include +#include namespace gtsam { -/** - * Binary AllDiff constraint - * Returns 1 if values for two keys are different, 0 otherwise - * DiscreteFactors are all awkward in that they have to store two types of keys: - * for each variable we have a Index and an Index. In this factor, we - * keep the Indices locally, and the Indices are stored in IndexFactor. - */ -class BinaryAllDiff : public Constraint { - size_t cardinality0_, cardinality1_; /// cardinality - - public: - /// Constructor - BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) - : Constraint(key1.first, key2.first), - cardinality0_(key1.second), - cardinality1_(key2.second) {} - - // print - void print( - const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override { - std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " - << formatter(keys_[1]) << std::endl; - } - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if (!dynamic_cast(&other)) - return false; - else { - const BinaryAllDiff& f(static_cast(other)); - return (cardinality0_ == f.cardinality0_) && - (cardinality1_ == f.cardinality1_); - } - } - - /// Calculate value - double operator()(const Values& values) const override { - return (double)(values.at(keys_[0]) != values.at(keys_[1])); - } - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override { - DiscreteKeys keys; - keys.push_back(DiscreteKey(keys_[0], cardinality0_)); - keys.push_back(DiscreteKey(keys_[1], cardinality1_)); - std::vector table; - for (size_t i1 = 0; i1 < cardinality0_; i1++) - for (size_t i2 = 0; i2 < cardinality1_; i2++) table.push_back(i1 != i2); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains + /** + * Binary AllDiff constraint + * Returns 1 if values for two keys are different, 0 otherwise. */ - bool ensureArcConsistency(size_t j, - std::vector& domains) const override { - // throw std::runtime_error( - // "BinaryAllDiff::ensureArcConsistency not implemented"); - return false; - } + class BinaryAllDiff: public Constraint { - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values&) const override { - throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); - } + size_t cardinality0_, cardinality1_; /// cardinality - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector&) const override { - throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); - } -}; + public: -} // namespace gtsam + /// Constructor + BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) : + Constraint(key1.first, key2.first), + cardinality0_(key1.second), cardinality1_(key2.second) { + } + + // print + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " + << formatter(keys_[1]) << std::endl; + } + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if(!dynamic_cast(&other)) + return false; + else { + const BinaryAllDiff& f(static_cast(other)); + return (cardinality0_==f.cardinality0_) && (cardinality1_==f.cardinality1_); + } + } + + /// Calculate value + double operator()(const Values& values) const override { + return (double) (values.at(keys_[0]) != values.at(keys_[1])); + } + + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override { + DiscreteKeys keys; + keys.push_back(DiscreteKey(keys_[0],cardinality0_)); + keys.push_back(DiscreteKey(keys_[1],cardinality1_)); + std::vector table; + for (size_t i1 = 0; i1 < cardinality0_; i1++) + for (size_t i2 = 0; i2 < cardinality1_; i2++) + table.push_back(i1 != i2); + DecisionTreeFactor converted(keys, table); + return converted; + } + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; + } + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains + */ + /// + bool ensureArcConsistency(size_t j, + std::vector* domains) const override { + throw std::runtime_error( + "BinaryAllDiff::ensureArcConsistency not implemented"); + return false; + } + + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values&) const override { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } + + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector&) const override { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } + }; + +} // namespace gtsam diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index b1d70dc6e..bab1ac3c8 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -56,12 +56,11 @@ void CSP::runArcConsistency(size_t cardinality, size_t nrIterations, // if not already a singleton if (!domains[v].isSingleton()) { // get the constraint and call its ensureArcConsistency method - Constraint::shared_ptr constraint = - boost::dynamic_pointer_cast((*this)[f]); + auto constraint = boost::dynamic_pointer_cast((*this)[f]); if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); changed[v] = - constraint->ensureArcConsistency(v, domains) || changed[v]; + constraint->ensureArcConsistency(v, &domains) || changed[v]; } } // f if (changed[v]) anyChange = true; diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index b8baccff9..ff6f3834e 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -17,68 +17,79 @@ #pragma once -#include #include - +#include #include namespace gtsam { -class Domain; + class Domain; -/** - * Base class for discrete probabilistic factors - * The most general one is the derived DecisionTreeFactor - */ -class Constraint : public DiscreteFactor { - public: - typedef boost::shared_ptr shared_ptr; - - protected: - /// Construct n-way factor - Constraint(const KeyVector& js) : DiscreteFactor(js) {} - - /// Construct unary factor - Constraint(Key j) : DiscreteFactor(boost::assign::cref_list_of<1>(j)) {} - - /// Construct binary factor - Constraint(Key j1, Key j2) - : DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) {} - - /// construct from container - template - Constraint(KeyIterator beginKey, KeyIterator endKey) - : DiscreteFactor(beginKey, endKey) {} - - public: - /// @name Standard Constructors - /// @{ - - /// Default constructor for I/O - Constraint(); - - /// Virtual destructor - ~Constraint() override {} - - /// @} - /// @name Standard Interface - /// @{ - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains + /** + * Base class for constraint factors + * Derived classes include SingleValue, BinaryAllDiff, and AllDiff. */ - virtual bool ensureArcConsistency(size_t j, - std::vector& domains) const = 0; + class GTSAM_EXPORT Constraint : public DiscreteFactor { - /// Partially apply known values - virtual shared_ptr partiallyApply(const Values&) const = 0; + public: - /// Partially apply known values, domain version - virtual shared_ptr partiallyApply(const std::vector&) const = 0; - /// @} -}; + typedef boost::shared_ptr shared_ptr; + + protected: + + /// Construct unary constraint factor. + Constraint(Key j) : + DiscreteFactor(boost::assign::cref_list_of<1>(j)) { + } + + /// Construct binary constraint factor. + Constraint(Key j1, Key j2) : + DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) { + } + + /// Construct n-way constraint factor. + Constraint(const KeyVector& js) : + DiscreteFactor(js) { + } + + /// construct from container + template + Constraint(KeyIterator beginKey, KeyIterator endKey) : + DiscreteFactor(beginKey, endKey) { + } + + public: + + /// @name Standard Constructors + /// @{ + + /// Default constructor for I/O + Constraint(); + + /// Virtual destructor + ~Constraint() override {} + + /// @} + /// @name Standard Interface + /// @{ + + /* + * Ensure Arc-consistency, possibly changing domains of connected variables. + * @param j domain to be checked + * @param (in/out) domains all other domains + * @return true if domains were changed, false otherwise. + */ + virtual bool ensureArcConsistency(size_t j, + std::vector* domains) const = 0; + + /// Partially apply known values + virtual shared_ptr partiallyApply(const Values&) const = 0; + + + /// Partially apply known values, domain version + virtual shared_ptr partiallyApply(const std::vector&) const = 0; + /// @} + }; // DiscreteFactor -} // namespace gtsam +}// namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index a81b1d1ad..c2ba1c7f9 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -5,89 +5,90 @@ * @author Frank Dellaert */ -#include -#include #include - +#include +#include #include namespace gtsam { -using namespace std; + using namespace std; -/* ************************************************************************* */ -void Domain::print(const string& s, const KeyFormatter& formatter) const { - // cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << - // formatter(keys_[0]) << ") with values"; - // for (size_t v: values_) cout << " " << v; - // cout << endl; - for (size_t v : values_) cout << v; -} - -/* ************************************************************************* */ -double Domain::operator()(const Values& values) const { - return contains(values.at(keys_[0])); -} - -/* ************************************************************************* */ -DecisionTreeFactor Domain::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(keys_[0], cardinality_); - vector table; - for (size_t i1 = 0; i1 < cardinality_; ++i1) table.push_back(contains(i1)); - DecisionTreeFactor converted(keys, table); - return converted; -} - -/* ************************************************************************* */ -DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; -} - -/* ************************************************************************* */ -bool Domain::ensureArcConsistency(size_t j, vector& domains) const { - if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); - Domain& D = domains[j]; - for (size_t value : values_) - if (!D.contains(value)) throw runtime_error("Unsatisfiable"); - D = *this; - return true; -} - -/* ************************************************************************* */ -bool Domain::checkAllDiff(const KeyVector keys, vector& domains) { - Key j = keys_[0]; - // for all values in this domain - for (size_t value : values_) { - // for all connected domains - for (Key k : keys) - // if any domain contains the value we cannot make this domain singleton - if (k != j && domains[k].contains(value)) goto found; - values_.clear(); - values_.insert(value); - return true; // we changed it - found:; + /* ************************************************************************* */ + void Domain::print(const string& s, + const KeyFormatter& formatter) const { + cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << + formatter(keys_[0]) << ") with values"; + for (size_t v: values_) cout << " " << v; + cout << endl; + } + + /* ************************************************************************* */ + double Domain::operator()(const Values& values) const { + return contains(values.at(keys_[0])); + } + + /* ************************************************************************* */ + DecisionTreeFactor Domain::toDecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(keys_[0],cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; ++i1) + table.push_back(contains(i1)); + DecisionTreeFactor converted(keys, table); + return converted; + } + + /* ************************************************************************* */ + DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; + } + + /* ************************************************************************* */ + bool Domain::ensureArcConsistency(size_t j, vector* domains) const { + if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); + Domain& D = domains->at(j); + for(size_t value: values_) + if (!D.contains(value)) throw runtime_error("Unsatisfiable"); + D = *this; + return true; + } + + /* ************************************************************************* */ + boost::optional Domain::checkAllDiff( + const KeyVector keys, const vector& domains) const { + Key j = keys_[0]; + // for all values in this domain + for (const size_t value : values_) { + // for all connected domains + for (const Key k : keys) + // if any domain contains the value we cannot make this domain singleton + if (k != j && domains[k].contains(value)) goto found; + // Otherwise: return a singleton: + return Domain(this->discreteKey(), value); + found:; + } + return boost::none; // we did not change it + } + + /* ************************************************************************* */ + Constraint::shared_ptr Domain::partiallyApply( + const Values& values) const { + Values::const_iterator it = values.find(keys_[0]); + if (it != values.end() && !contains(it->second)) throw runtime_error( + "Domain::partiallyApply: unsatisfiable"); + return boost::make_shared < Domain > (*this); + } + + /* ************************************************************************* */ + Constraint::shared_ptr Domain::partiallyApply( + const vector& domains) const { + const Domain& Dk = domains[keys_[0]]; + if (Dk.isSingleton() && !contains(*Dk.begin())) throw runtime_error( + "Domain::partiallyApply: unsatisfiable"); + return boost::make_shared < Domain > (Dk); } - return false; // we did not change it -} /* ************************************************************************* */ -Constraint::shared_ptr Domain::partiallyApply(const Values& values) const { - Values::const_iterator it = values.find(keys_[0]); - if (it != values.end() && !contains(it->second)) - throw runtime_error("Domain::partiallyApply: unsatisfiable"); - return boost::make_shared(*this); -} - -/* ************************************************************************* */ -Constraint::shared_ptr Domain::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; - if (Dk.isSingleton() && !contains(*Dk.begin())) - throw runtime_error("Domain::partiallyApply: unsatisfiable"); - return boost::make_shared(Dk); -} - -/* ************************************************************************* */ -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 15828b653..d06966081 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -7,18 +7,23 @@ #pragma once -#include #include +#include namespace gtsam { /** - * Domain restriction constraint + * The Domain class represents a constraint that restricts the possible values a + * particular variable, with given key, can take on. */ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { size_t cardinality_; /// Cardinality std::set values_; /// allowed values + DiscreteKey discreteKey() const { + return DiscreteKey(keys_[0], cardinality_); + } + public: typedef boost::shared_ptr shared_ptr; @@ -35,14 +40,10 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { values_.insert(v); } - /// Constructor - Domain(const Domain& other) - : Constraint(other.keys_[0]), values_(other.values_) {} - - /// insert a value, non const :-( + /// Insert a value, non const :-( void insert(size_t value) { values_.insert(value); } - /// erase a value, non const :-( + /// Erase a value, non const :-( void erase(size_t value) { values_.erase(value); } size_t nrValues() const { return values_.size(); } @@ -82,15 +83,17 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { * @param domains all other domains */ bool ensureArcConsistency(size_t j, - std::vector& domains) const override; + std::vector* domains) const override; /** - * Check for a value in domain that does not occur in any other connected - * domain. If found, we make this a singleton... Called in - * AllDiff::ensureArcConsistency - * @param keys connected domains through alldiff + * Check for a value in domain that does not occur in any other connected + * domain. If found, return a a new singleton domain... + * Called in AllDiff::ensureArcConsistency + * @param keys connected domains through alldiff + * @param keys other domains */ - bool checkAllDiff(const KeyVector keys, std::vector& domains); + boost::optional checkAllDiff( + const KeyVector keys, const std::vector& domains) const; /// Partially apply known values Constraint::shared_ptr partiallyApply(const Values& values) const override; @@ -98,6 +101,6 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { /// Partially apply known values, domain version Constraint::shared_ptr partiallyApply( const std::vector& domains) const override; -}; + }; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 105887dc9..e042e550c 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -5,74 +5,75 @@ * @author Frank Dellaert */ -#include -#include -#include #include - +#include +#include +#include #include namespace gtsam { -using namespace std; + using namespace std; -/* ************************************************************************* */ -void SingleValue::print(const string& s, const KeyFormatter& formatter) const { - cout << s << "SingleValue on " - << "j=" << formatter(keys_[0]) << " with value " << value_ << endl; -} - -/* ************************************************************************* */ -double SingleValue::operator()(const Values& values) const { - return (double)(values.at(keys_[0]) == value_); -} - -/* ************************************************************************* */ -DecisionTreeFactor SingleValue::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(keys_[0], cardinality_); - vector table; - for (size_t i1 = 0; i1 < cardinality_; i1++) table.push_back(i1 == value_); - DecisionTreeFactor converted(keys, table); - return converted; -} - -/* ************************************************************************* */ -DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; -} - -/* ************************************************************************* */ -bool SingleValue::ensureArcConsistency(size_t j, - vector& domains) const { - if (j != keys_[0]) - throw invalid_argument("SingleValue check on wrong domain"); - Domain& D = domains[j]; - if (D.isSingleton()) { - if (D.firstValue() != value_) throw runtime_error("Unsatisfiable"); - return false; + /* ************************************************************************* */ + void SingleValue::print(const string& s, + const KeyFormatter& formatter) const { + cout << s << "SingleValue on " << "j=" << formatter(keys_[0]) + << " with value " << value_ << endl; + } + + /* ************************************************************************* */ + double SingleValue::operator()(const Values& values) const { + return (double) (values.at(keys_[0]) == value_); + } + + /* ************************************************************************* */ + DecisionTreeFactor SingleValue::toDecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(keys_[0],cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; i1++) + table.push_back(i1 == value_); + DecisionTreeFactor converted(keys, table); + return converted; + } + + /* ************************************************************************* */ + DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; + } + + /* ************************************************************************* */ + bool SingleValue::ensureArcConsistency(size_t j, + vector* domains) const { + if (j != keys_[0]) + throw invalid_argument("SingleValue check on wrong domain"); + Domain& D = domains->at(j); + if (D.isSingleton()) { + if (D.firstValue() != value_) throw runtime_error("Unsatisfiable"); + return false; + } + D = Domain(discreteKey(), value_); + return true; + } + + /* ************************************************************************* */ + Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const { + Values::const_iterator it = values.find(keys_[0]); + if (it != values.end() && it->second != value_) throw runtime_error( + "SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared(keys_[0], cardinality_, value_); + } + + /* ************************************************************************* */ + Constraint::shared_ptr SingleValue::partiallyApply( + const vector& domains) const { + const Domain& Dk = domains[keys_[0]]; + if (Dk.isSingleton() && !Dk.contains(value_)) throw runtime_error( + "SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared(discreteKey(), value_); } - D = Domain(discreteKey(), value_); - return true; -} /* ************************************************************************* */ -Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const { - Values::const_iterator it = values.find(keys_[0]); - if (it != values.end() && it->second != value_) - throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared(keys_[0], cardinality_, value_); -} - -/* ************************************************************************* */ -Constraint::shared_ptr SingleValue::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; - if (Dk.isSingleton() && !Dk.contains(value_)) - throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared(discreteKey(), value_); -} - -/* ************************************************************************* */ -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index a2aec338c..0f9a8fb0f 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -7,73 +7,74 @@ #pragma once -#include #include namespace gtsam { -/** - * SingleValue constraint - */ -class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { - /// Number of values - size_t cardinality_; - - /// allowed value - size_t value_; - - DiscreteKey discreteKey() const { - return DiscreteKey(keys_[0], cardinality_); - } - - public: - typedef boost::shared_ptr shared_ptr; - - /// Constructor - SingleValue(Key key, size_t n, size_t value) - : Constraint(key), cardinality_(n), value_(value) {} - - /// Constructor - SingleValue(const DiscreteKey& dkey, size_t value) - : Constraint(dkey.first), cardinality_(dkey.second), value_(value) {} - - // print - void print(const std::string& s = "", const KeyFormatter& formatter = - DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if (!dynamic_cast(&other)) - return false; - else { - const SingleValue& f(static_cast(other)); - return (cardinality_ == f.cardinality_) && (value_ == f.value_); - } - } - - /// Calculate value - double operator()(const Values& values) const override; - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains + /** + * SingleValue constraint: ensures a variable takes on a certain value. + * This could of course also be implemented by changing its `Domain`. */ - bool ensureArcConsistency(size_t j, - std::vector& domains) const override; + class GTSAM_UNSTABLE_EXPORT SingleValue: public Constraint { + + size_t cardinality_; /// < Number of values + size_t value_; ///< allowed value - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values& values) const override; + DiscreteKey discreteKey() const { + return DiscreteKey(keys_[0],cardinality_); + } - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector& domains) const override; -}; + public: -} // namespace gtsam + typedef boost::shared_ptr shared_ptr; + + /// Construct from key, cardinality, and given value. + SingleValue(Key key, size_t n, size_t value) : + Constraint(key), cardinality_(n), value_(value) { + } + + /// Construct from DiscreteKey and given value. + SingleValue(const DiscreteKey& dkey, size_t value) : + Constraint(dkey.first), cardinality_(dkey.second), value_(value) { + } + + // print + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if(!dynamic_cast(&other)) + return false; + else { + const SingleValue& f(static_cast(other)); + return (cardinality_==f.cardinality_) && (value_==f.value_); + } + } + + /// Calculate value + double operator()(const Values& values) const override; + + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency: just sets domain[j] to {value_} + * @param j domain to be checked + * @param domains all other domains + */ + bool ensureArcConsistency(size_t j, + std::vector* domains) const override; + + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values& values) const override; + + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector& domains) const override; + }; + +} // namespace gtsam diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index 1552fcbf1..b1aaab303 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -19,12 +19,33 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST_UNSAFE(BinaryAllDif, allInOne) { - // Create keys and ordering +TEST(CSP, SingleValue) { + // Create keys for Idaho, Arizona, and Utah, allowing two colors for each: + size_t nrColors = 3; + DiscreteKey ID(0, nrColors), AZ(1, nrColors), UT(2, nrColors); + + // Check that a single value is equal to a decision stump with only one "1": + SingleValue singleValue(AZ, 2); + DecisionTreeFactor f1(AZ, "0 0 1"); + EXPECT(assert_equal(f1, singleValue.toDecisionTreeFactor())); + + // Create domains, laid out as a vector. + // TODO(dellaert): should be map?? + vector domains; + domains += Domain(ID), Domain(AZ), Domain(UT); + + // Ensure arc-consistency: just wipes out values in AZ domain: + EXPECT(singleValue.ensureArcConsistency(1, &domains)); + LONGS_EQUAL(3, domains[0].nrValues()); + LONGS_EQUAL(1, domains[1].nrValues()); + LONGS_EQUAL(3, domains[2].nrValues()); +} + +/* ************************************************************************* */ +TEST(CSP, BinaryAllDif) { + // Create keys for Idaho, Arizona, and Utah, allowing 2 colors for each: size_t nrColors = 2; - // DiscreteKey ID("Idaho", nrColors), UT("Utah", nrColors), AZ("Arizona", - // nrColors); - DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); + DiscreteKey ID(0, nrColors), AZ(1, nrColors), UT(2, nrColors); // Check construction and conversion BinaryAllDiff c1(ID, UT); @@ -36,16 +57,51 @@ TEST_UNSAFE(BinaryAllDif, allInOne) { DecisionTreeFactor f2(UT & AZ, "0 1 1 0"); EXPECT(assert_equal(f2, c2.toDecisionTreeFactor())); + // Check multiplication of factors with constraint: DecisionTreeFactor f3 = f1 * f2; EXPECT(assert_equal(f3, c1 * f2)); EXPECT(assert_equal(f3, c2 * f1)); } /* ************************************************************************* */ -TEST_UNSAFE(CSP, allInOne) { - // Create keys and ordering +TEST(CSP, AllDiff) { + // Create keys for Idaho, Arizona, and Utah, allowing two colors for each: + size_t nrColors = 3; + DiscreteKey ID(0, nrColors), AZ(1, nrColors), UT(2, nrColors); + + // Check construction and conversion + vector dkeys{ID, UT, AZ}; + AllDiff alldiff(dkeys); + DecisionTreeFactor actual = alldiff.toDecisionTreeFactor(); + // GTSAM_PRINT(actual); + actual.dot("actual"); + DecisionTreeFactor f2( + ID & AZ & UT, + "0 0 0 0 0 1 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 1 0 0 0 0 0"); + EXPECT(assert_equal(f2, actual)); + + // Create domains. + vector domains; + domains += Domain(ID), Domain(AZ), Domain(UT); + + // First constrict AZ domain: + SingleValue singleValue(AZ, 2); + EXPECT(singleValue.ensureArcConsistency(1, &domains)); + + // Arc-consistency + EXPECT(alldiff.ensureArcConsistency(0, &domains)); + EXPECT(!alldiff.ensureArcConsistency(1, &domains)); + EXPECT(alldiff.ensureArcConsistency(2, &domains)); + LONGS_EQUAL(2, domains[0].nrValues()); + LONGS_EQUAL(1, domains[1].nrValues()); + LONGS_EQUAL(2, domains[2].nrValues()); +} + +/* ************************************************************************* */ +TEST(CSP, allInOne) { + // Create keys for Idaho, Arizona, and Utah, allowing 3 colors for each: size_t nrColors = 2; - DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); + DiscreteKey ID(0, nrColors), AZ(1, nrColors), UT(2, nrColors); // Create the CSP CSP csp; @@ -81,15 +137,12 @@ TEST_UNSAFE(CSP, allInOne) { } /* ************************************************************************* */ -TEST_UNSAFE(CSP, WesternUS) { - // Create keys +TEST(CSP, WesternUS) { + // Create keys for all states in Western US, with 4 color possibilities. size_t nrColors = 4; - DiscreteKey - // Create ordering according to example in ND-CSP.lyx - WA(0, nrColors), - OR(3, nrColors), CA(1, nrColors), NV(2, nrColors), ID(8, nrColors), - UT(9, nrColors), AZ(10, nrColors), MT(4, nrColors), WY(5, nrColors), - CO(7, nrColors), NM(6, nrColors); + DiscreteKey WA(0, nrColors), OR(3, nrColors), CA(1, nrColors), + NV(2, nrColors), ID(8, nrColors), UT(9, nrColors), AZ(10, nrColors), + MT(4, nrColors), WY(5, nrColors), CO(7, nrColors), NM(6, nrColors); // Create the CSP CSP csp; @@ -116,10 +169,12 @@ TEST_UNSAFE(CSP, WesternUS) { csp.addAllDiff(WY, CO); csp.addAllDiff(CO, NM); - // Solve + // Create ordering according to example in ND-CSP.lyx Ordering ordering; ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7), Key(8), Key(9), Key(10); + + // Solve using that ordering: CSP::sharedValues mpe = csp.optimalAssignment(ordering); // GTSAM_PRINT(*mpe); CSP::Values expected; @@ -143,33 +198,17 @@ TEST_UNSAFE(CSP, WesternUS) { } /* ************************************************************************* */ -TEST_UNSAFE(CSP, AllDiff) { - // Create keys and ordering +TEST(CSP, ArcConsistency) { + // Create keys for Idaho, Arizona, and Utah, allowing three colors for each: size_t nrColors = 3; - DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); + DiscreteKey ID(0, nrColors), AZ(1, nrColors), UT(2, nrColors); - // Create the CSP + // Create the CSP using just one all-diff constraint, plus constrain Arizona. CSP csp; - vector dkeys; - dkeys += ID, UT, AZ; + vector dkeys{ID, UT, AZ}; csp.addAllDiff(dkeys); csp.addSingleValue(AZ, 2); - // GTSAM_PRINT(csp); - - // Check construction and conversion - SingleValue s(AZ, 2); - DecisionTreeFactor f1(AZ, "0 0 1"); - EXPECT(assert_equal(f1, s.toDecisionTreeFactor())); - - // Check construction and conversion - AllDiff alldiff(dkeys); - DecisionTreeFactor actual = alldiff.toDecisionTreeFactor(); - // GTSAM_PRINT(actual); - // actual.dot("actual"); - DecisionTreeFactor f2( - ID & AZ & UT, - "0 0 0 0 0 1 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 1 0 0 0 0 0"); - EXPECT(assert_equal(f2, actual)); + // GTSAM_PRINT(csp); // Check an invalid combination, with ID==UT==AZ all same color DiscreteFactor::Values invalid; @@ -192,14 +231,15 @@ TEST_UNSAFE(CSP, AllDiff) { EXPECT(assert_equal(expected, *mpe)); EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); - // Arc-consistency + // ensure arc-consistency, i.e., narrow domains... vector domains; domains += Domain(ID), Domain(AZ), Domain(UT); SingleValue singleValue(AZ, 2); - EXPECT(singleValue.ensureArcConsistency(1, domains)); - EXPECT(alldiff.ensureArcConsistency(0, domains)); - EXPECT(!alldiff.ensureArcConsistency(1, domains)); - EXPECT(alldiff.ensureArcConsistency(2, domains)); + AllDiff alldiff(dkeys); + EXPECT(singleValue.ensureArcConsistency(1, &domains)); + EXPECT(alldiff.ensureArcConsistency(0, &domains)); + EXPECT(!alldiff.ensureArcConsistency(1, &domains)); + EXPECT(alldiff.ensureArcConsistency(2, &domains)); LONGS_EQUAL(2, domains[0].nrValues()); LONGS_EQUAL(1, domains[1].nrValues()); LONGS_EQUAL(2, domains[2].nrValues()); @@ -222,6 +262,7 @@ TEST_UNSAFE(CSP, AllDiff) { // full arc-consistency test csp.runArcConsistency(nrColors); + // GTSAM_PRINT(csp); } /* ************************************************************************* */ From b7f43906bc3fc136a14c8b62b240399d39544e5d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Nov 2021 15:08:01 -0500 Subject: [PATCH 37/71] Formatting only --- gtsam_unstable/discrete/AllDiff.cpp | 189 +++++++++++----------- gtsam_unstable/discrete/AllDiff.h | 116 +++++++------ gtsam_unstable/discrete/BinaryAllDiff.h | 153 +++++++++--------- gtsam_unstable/discrete/Constraint.h | 120 +++++++------- gtsam_unstable/discrete/Domain.cpp | 158 +++++++++--------- gtsam_unstable/discrete/Domain.h | 6 +- gtsam_unstable/discrete/SingleValue.cpp | 129 ++++++++------- gtsam_unstable/discrete/SingleValue.h | 123 +++++++------- gtsam_unstable/discrete/tests/testCSP.cpp | 2 +- 9 files changed, 487 insertions(+), 509 deletions(-) diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index ebc789ec2..ef18053a4 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -5,115 +5,112 @@ * @author Frank Dellaert */ -#include -#include #include +#include +#include + #include namespace gtsam { - /* ************************************************************************* */ - AllDiff::AllDiff(const DiscreteKeys& dkeys) : - Constraint(dkeys.indices()) { - for(const DiscreteKey& dkey: dkeys) - cardinalities_.insert(dkey); - } +/* ************************************************************************* */ +AllDiff::AllDiff(const DiscreteKeys& dkeys) : Constraint(dkeys.indices()) { + for (const DiscreteKey& dkey : dkeys) cardinalities_.insert(dkey); +} - /* ************************************************************************* */ - void AllDiff::print(const std::string& s, - const KeyFormatter& formatter) const { - std::cout << s << "AllDiff on "; - for (Key dkey: keys_) - std::cout << formatter(dkey) << " "; - std::cout << std::endl; - } +/* ************************************************************************* */ +void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const { + std::cout << s << "AllDiff on "; + for (Key dkey : keys_) std::cout << formatter(dkey) << " "; + std::cout << std::endl; +} - /* ************************************************************************* */ - double AllDiff::operator()(const Values& values) const { - std::set < size_t > taken; // record values taken by keys - for(Key dkey: keys_) { - size_t value = values.at(dkey); // get the value for that key - if (taken.count(value)) return 0.0;// check if value alreday taken - taken.insert(value);// if not, record it as taken and keep checking +/* ************************************************************************* */ +double AllDiff::operator()(const Values& values) const { + std::set taken; // record values taken by keys + for (Key dkey : keys_) { + size_t value = values.at(dkey); // get the value for that key + if (taken.count(value)) return 0.0; // check if value alreday taken + taken.insert(value); // if not, record it as taken and keep checking + } + return 1.0; +} + +/* ************************************************************************* */ +DecisionTreeFactor AllDiff::toDecisionTreeFactor() const { + // We will do this by converting the allDif into many BinaryAllDiff + // constraints + DecisionTreeFactor converted; + size_t nrKeys = keys_.size(); + for (size_t i1 = 0; i1 < nrKeys; i1++) + for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) { + BinaryAllDiff binary12(discreteKey(i1), discreteKey(i2)); + converted = converted * binary12.toDecisionTreeFactor(); } - return 1.0; + return converted; +} + +/* ************************************************************************* */ +DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} + +/* ************************************************************************* */ +bool AllDiff::ensureArcConsistency(size_t j, + std::vector* domains) const { + // We are changing the domain of variable j. + // TODO(dellaert): confusing, I thought we were changing others... + Domain& Dj = domains->at(j); + + // Though strictly not part of allDiff, we check for + // a value in domains[j] that does not occur in any other connected domain. + // If found, we make this a singleton... + // TODO: make a new constraint where this really is true + boost::optional maybeChanged = Dj.checkAllDiff(keys_, *domains); + if (maybeChanged) { + Dj = *maybeChanged; + return true; } - /* ************************************************************************* */ - DecisionTreeFactor AllDiff::toDecisionTreeFactor() const { - // We will do this by converting the allDif into many BinaryAllDiff constraints - DecisionTreeFactor converted; - size_t nrKeys = keys_.size(); - for (size_t i1 = 0; i1 < nrKeys; i1++) - for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) { - BinaryAllDiff binary12(discreteKey(i1),discreteKey(i2)); - converted = converted * binary12.toDecisionTreeFactor(); - } - return converted; - } - - /* ************************************************************************* */ - DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* ************************************************************************* */ - bool AllDiff::ensureArcConsistency(size_t j, - std::vector* domains) const { - // We are changing the domain of variable j. - // TODO(dellaert): confusing, I thought we were changing others... - Domain& Dj = domains->at(j); - - // Though strictly not part of allDiff, we check for - // a value in domains[j] that does not occur in any other connected domain. - // If found, we make this a singleton... - // TODO: make a new constraint where this really is true - boost::optional maybeChanged = Dj.checkAllDiff(keys_, *domains); - if (maybeChanged) { - Dj = *maybeChanged; - return true; - } - - // Check all other domains for singletons and erase corresponding values. - // This is the same as arc-consistency on the equivalent binary constraints - bool changed = false; - for (Key k : keys_) - if (k != j) { - const Domain& Dk = domains->at(k); - if (Dk.isSingleton()) { // check if singleton - size_t value = Dk.firstValue(); - if (Dj.contains(value)) { - Dj.erase(value); // erase value if true - changed = true; - } + // Check all other domains for singletons and erase corresponding values. + // This is the same as arc-consistency on the equivalent binary constraints + bool changed = false; + for (Key k : keys_) + if (k != j) { + const Domain& Dk = domains->at(k); + if (Dk.isSingleton()) { // check if singleton + size_t value = Dk.firstValue(); + if (Dj.contains(value)) { + Dj.erase(value); // erase value if true + changed = true; } } - return changed; - } + } + return changed; +} - /* ************************************************************************* */ - Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { - DiscreteKeys newKeys; - // loop over keys and add them only if they do not appear in values - for(Key k: keys_) - if (values.find(k) == values.end()) { - newKeys.push_back(DiscreteKey(k,cardinalities_.at(k))); - } - return boost::make_shared(newKeys); - } +/* ************************************************************************* */ +Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { + DiscreteKeys newKeys; + // loop over keys and add them only if they do not appear in values + for (Key k : keys_) + if (values.find(k) == values.end()) { + newKeys.push_back(DiscreteKey(k, cardinalities_.at(k))); + } + return boost::make_shared(newKeys); +} - /* ************************************************************************* */ - Constraint::shared_ptr AllDiff::partiallyApply( - const std::vector& domains) const { - DiscreteFactor::Values known; - for(Key k: keys_) { - const Domain& Dk = domains[k]; - if (Dk.isSingleton()) - known[k] = Dk.firstValue(); - } - return partiallyApply(known); +/* ************************************************************************* */ +Constraint::shared_ptr AllDiff::partiallyApply( + const std::vector& domains) const { + DiscreteFactor::Values known; + for (Key k : keys_) { + const Domain& Dk = domains[k]; + if (Dk.isSingleton()) known[k] = Dk.firstValue(); } + return partiallyApply(known); +} - /* ************************************************************************* */ -} // namespace gtsam +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 8c83e5ba1..4deabda94 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -7,70 +7,68 @@ #pragma once -#include #include +#include namespace gtsam { - /** - * General AllDiff constraint. - * Returns 1 if values for all keys are different, 0 otherwise. +/** + * General AllDiff constraint. + * Returns 1 if values for all keys are different, 0 otherwise. + */ +class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { + std::map cardinalities_; + + DiscreteKey discreteKey(size_t i) const { + Key j = keys_[i]; + return DiscreteKey(j, cardinalities_.at(j)); + } + + public: + /// Construct from keys. + AllDiff(const DiscreteKeys& dkeys); + + // print + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) + return false; + else { + const AllDiff& f(static_cast(other)); + return cardinalities_.size() == f.cardinalities_.size() && + std::equal(cardinalities_.begin(), cardinalities_.end(), + f.cardinalities_.begin()); + } + } + + /// Calculate value = expensive ! + double operator()(const Values& values) const override; + + /// Convert into a decisiontree, can be *very* expensive ! + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency + * Arc-consistency involves creating binaryAllDiff constraints + * In which case the combinatorial hyper-arc explosion disappears. + * @param j domain to be checked + * @param (in/out) domains all other domains */ - class GTSAM_UNSTABLE_EXPORT AllDiff: public Constraint { + bool ensureArcConsistency(size_t j, + std::vector* domains) const override; - std::map cardinalities_; + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values&) const override; - DiscreteKey discreteKey(size_t i) const { - Key j = keys_[i]; - return DiscreteKey(j,cardinalities_.at(j)); - } + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector&) const override; +}; - public: - - /// Construct from keys. - AllDiff(const DiscreteKeys& dkeys); - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const AllDiff& f(static_cast(other)); - return cardinalities_.size() == f.cardinalities_.size() - && std::equal(cardinalities_.begin(), cardinalities_.end(), - f.cardinalities_.begin()); - } - } - - /// Calculate value = expensive ! - double operator()(const Values& values) const override; - - /// Convert into a decisiontree, can be *very* expensive ! - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * Arc-consistency involves creating binaryAllDiff constraints - * In which case the combinatorial hyper-arc explosion disappears. - * @param j domain to be checked - * @param (in/out) domains all other domains - */ - bool ensureArcConsistency(size_t j, - std::vector* domains) const override; - - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values&) const override; - - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector&) const override; - }; - -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index acc3cc421..21cfb18f2 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -7,92 +7,91 @@ #pragma once -#include -#include #include +#include +#include namespace gtsam { - /** - * Binary AllDiff constraint - * Returns 1 if values for two keys are different, 0 otherwise. - */ - class BinaryAllDiff: public Constraint { +/** + * Binary AllDiff constraint + * Returns 1 if values for two keys are different, 0 otherwise. + */ +class BinaryAllDiff : public Constraint { + size_t cardinality0_, cardinality1_; /// cardinality - size_t cardinality0_, cardinality1_; /// cardinality + public: + /// Constructor + BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) + : Constraint(key1.first, key2.first), + cardinality0_(key1.second), + cardinality1_(key2.second) {} - public: + // print + void print( + const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " + << formatter(keys_[1]) << std::endl; + } - /// Constructor - BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) : - Constraint(key1.first, key2.first), - cardinality0_(key1.second), cardinality1_(key2.second) { - } - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override { - std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " - << formatter(keys_[1]) << std::endl; - } - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const BinaryAllDiff& f(static_cast(other)); - return (cardinality0_==f.cardinality0_) && (cardinality1_==f.cardinality1_); - } - } - - /// Calculate value - double operator()(const Values& values) const override { - return (double) (values.at(keys_[0]) != values.at(keys_[1])); - } - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override { - DiscreteKeys keys; - keys.push_back(DiscreteKey(keys_[0],cardinality0_)); - keys.push_back(DiscreteKey(keys_[1],cardinality1_)); - std::vector table; - for (size_t i1 = 0; i1 < cardinality0_; i1++) - for (size_t i2 = 0; i2 < cardinality1_; i2++) - table.push_back(i1 != i2); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - /// - bool ensureArcConsistency(size_t j, - std::vector* domains) const override { - throw std::runtime_error( - "BinaryAllDiff::ensureArcConsistency not implemented"); + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) return false; + else { + const BinaryAllDiff& f(static_cast(other)); + return (cardinality0_ == f.cardinality0_) && + (cardinality1_ == f.cardinality1_); } + } - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values&) const override { - throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); - } + /// Calculate value + double operator()(const Values& values) const override { + return (double)(values.at(keys_[0]) != values.at(keys_[1])); + } - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector&) const override { - throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); - } - }; + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override { + DiscreteKeys keys; + keys.push_back(DiscreteKey(keys_[0], cardinality0_)); + keys.push_back(DiscreteKey(keys_[1], cardinality1_)); + std::vector table; + for (size_t i1 = 0; i1 < cardinality0_; i1++) + for (size_t i2 = 0; i2 < cardinality1_; i2++) table.push_back(i1 != i2); + DecisionTreeFactor converted(keys, table); + return converted; + } -} // namespace gtsam + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; + } + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains + */ + /// + bool ensureArcConsistency(size_t j, + std::vector* domains) const override { + throw std::runtime_error( + "BinaryAllDiff::ensureArcConsistency not implemented"); + return false; + } + + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values&) const override { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } + + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector&) const override { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } +}; + +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index ff6f3834e..e9714d6b4 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -17,79 +17,69 @@ #pragma once -#include #include +#include + #include namespace gtsam { - class Domain; +class Domain; - /** - * Base class for constraint factors - * Derived classes include SingleValue, BinaryAllDiff, and AllDiff. +/** + * Base class for constraint factors + * Derived classes include SingleValue, BinaryAllDiff, and AllDiff. + */ +class GTSAM_EXPORT Constraint : public DiscreteFactor { + public: + typedef boost::shared_ptr shared_ptr; + + protected: + /// Construct unary constraint factor. + Constraint(Key j) : DiscreteFactor(boost::assign::cref_list_of<1>(j)) {} + + /// Construct binary constraint factor. + Constraint(Key j1, Key j2) + : DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) {} + + /// Construct n-way constraint factor. + Constraint(const KeyVector& js) : DiscreteFactor(js) {} + + /// construct from container + template + Constraint(KeyIterator beginKey, KeyIterator endKey) + : DiscreteFactor(beginKey, endKey) {} + + public: + /// @name Standard Constructors + /// @{ + + /// Default constructor for I/O + Constraint(); + + /// Virtual destructor + ~Constraint() override {} + + /// @} + /// @name Standard Interface + /// @{ + + /* + * Ensure Arc-consistency, possibly changing domains of connected variables. + * @param j domain to be checked + * @param (in/out) domains all other domains + * @return true if domains were changed, false otherwise. */ - class GTSAM_EXPORT Constraint : public DiscreteFactor { + virtual bool ensureArcConsistency(size_t j, + std::vector* domains) const = 0; - public: + /// Partially apply known values + virtual shared_ptr partiallyApply(const Values&) const = 0; - typedef boost::shared_ptr shared_ptr; - - protected: - - /// Construct unary constraint factor. - Constraint(Key j) : - DiscreteFactor(boost::assign::cref_list_of<1>(j)) { - } - - /// Construct binary constraint factor. - Constraint(Key j1, Key j2) : - DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) { - } - - /// Construct n-way constraint factor. - Constraint(const KeyVector& js) : - DiscreteFactor(js) { - } - - /// construct from container - template - Constraint(KeyIterator beginKey, KeyIterator endKey) : - DiscreteFactor(beginKey, endKey) { - } - - public: - - /// @name Standard Constructors - /// @{ - - /// Default constructor for I/O - Constraint(); - - /// Virtual destructor - ~Constraint() override {} - - /// @} - /// @name Standard Interface - /// @{ - - /* - * Ensure Arc-consistency, possibly changing domains of connected variables. - * @param j domain to be checked - * @param (in/out) domains all other domains - * @return true if domains were changed, false otherwise. - */ - virtual bool ensureArcConsistency(size_t j, - std::vector* domains) const = 0; - - /// Partially apply known values - virtual shared_ptr partiallyApply(const Values&) const = 0; - - - /// Partially apply known values, domain version - virtual shared_ptr partiallyApply(const std::vector&) const = 0; - /// @} - }; + /// Partially apply known values, domain version + virtual shared_ptr partiallyApply(const std::vector&) const = 0; + /// @} +}; // DiscreteFactor -}// namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index c2ba1c7f9..da23717f6 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -5,90 +5,88 @@ * @author Frank Dellaert */ -#include -#include #include +#include +#include + #include namespace gtsam { - using namespace std; - - /* ************************************************************************* */ - void Domain::print(const string& s, - const KeyFormatter& formatter) const { - cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << - formatter(keys_[0]) << ") with values"; - for (size_t v: values_) cout << " " << v; - cout << endl; - } - - /* ************************************************************************* */ - double Domain::operator()(const Values& values) const { - return contains(values.at(keys_[0])); - } - - /* ************************************************************************* */ - DecisionTreeFactor Domain::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(keys_[0],cardinality_); - vector table; - for (size_t i1 = 0; i1 < cardinality_; ++i1) - table.push_back(contains(i1)); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /* ************************************************************************* */ - DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* ************************************************************************* */ - bool Domain::ensureArcConsistency(size_t j, vector* domains) const { - if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); - Domain& D = domains->at(j); - for(size_t value: values_) - if (!D.contains(value)) throw runtime_error("Unsatisfiable"); - D = *this; - return true; - } - - /* ************************************************************************* */ - boost::optional Domain::checkAllDiff( - const KeyVector keys, const vector& domains) const { - Key j = keys_[0]; - // for all values in this domain - for (const size_t value : values_) { - // for all connected domains - for (const Key k : keys) - // if any domain contains the value we cannot make this domain singleton - if (k != j && domains[k].contains(value)) goto found; - // Otherwise: return a singleton: - return Domain(this->discreteKey(), value); - found:; - } - return boost::none; // we did not change it - } - - /* ************************************************************************* */ - Constraint::shared_ptr Domain::partiallyApply( - const Values& values) const { - Values::const_iterator it = values.find(keys_[0]); - if (it != values.end() && !contains(it->second)) throw runtime_error( - "Domain::partiallyApply: unsatisfiable"); - return boost::make_shared < Domain > (*this); - } - - /* ************************************************************************* */ - Constraint::shared_ptr Domain::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; - if (Dk.isSingleton() && !contains(*Dk.begin())) throw runtime_error( - "Domain::partiallyApply: unsatisfiable"); - return boost::make_shared < Domain > (Dk); - } +using namespace std; /* ************************************************************************* */ -} // namespace gtsam +void Domain::print(const string& s, const KeyFormatter& formatter) const { + cout << s << ": Domain on " << formatter(keys_[0]) + << " (j=" << formatter(keys_[0]) << ") with values"; + for (size_t v : values_) cout << " " << v; + cout << endl; +} + +/* ************************************************************************* */ +double Domain::operator()(const Values& values) const { + return contains(values.at(keys_[0])); +} + +/* ************************************************************************* */ +DecisionTreeFactor Domain::toDecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(keys_[0], cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; ++i1) table.push_back(contains(i1)); + DecisionTreeFactor converted(keys, table); + return converted; +} + +/* ************************************************************************* */ +DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} + +/* ************************************************************************* */ +bool Domain::ensureArcConsistency(size_t j, vector* domains) const { + if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); + Domain& D = domains->at(j); + for (size_t value : values_) + if (!D.contains(value)) throw runtime_error("Unsatisfiable"); + D = *this; + return true; +} + +/* ************************************************************************* */ +boost::optional Domain::checkAllDiff( + const KeyVector keys, const vector& domains) const { + Key j = keys_[0]; + // for all values in this domain + for (const size_t value : values_) { + // for all connected domains + for (const Key k : keys) + // if any domain contains the value we cannot make this domain singleton + if (k != j && domains[k].contains(value)) goto found; + // Otherwise: return a singleton: + return Domain(this->discreteKey(), value); + found:; + } + return boost::none; // we did not change it +} + +/* ************************************************************************* */ +Constraint::shared_ptr Domain::partiallyApply(const Values& values) const { + Values::const_iterator it = values.find(keys_[0]); + if (it != values.end() && !contains(it->second)) + throw runtime_error("Domain::partiallyApply: unsatisfiable"); + return boost::make_shared(*this); +} + +/* ************************************************************************* */ +Constraint::shared_ptr Domain::partiallyApply( + const vector& domains) const { + const Domain& Dk = domains[keys_[0]]; + if (Dk.isSingleton() && !contains(*Dk.begin())) + throw runtime_error("Domain::partiallyApply: unsatisfiable"); + return boost::make_shared(Dk); +} + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index d06966081..9fa22175a 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -7,8 +7,8 @@ #pragma once -#include #include +#include namespace gtsam { @@ -101,6 +101,6 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { /// Partially apply known values, domain version Constraint::shared_ptr partiallyApply( const std::vector& domains) const override; - }; +}; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index e042e550c..753d46cff 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -5,75 +5,74 @@ * @author Frank Dellaert */ -#include -#include -#include #include +#include +#include +#include + #include namespace gtsam { - using namespace std; - - /* ************************************************************************* */ - void SingleValue::print(const string& s, - const KeyFormatter& formatter) const { - cout << s << "SingleValue on " << "j=" << formatter(keys_[0]) - << " with value " << value_ << endl; - } - - /* ************************************************************************* */ - double SingleValue::operator()(const Values& values) const { - return (double) (values.at(keys_[0]) == value_); - } - - /* ************************************************************************* */ - DecisionTreeFactor SingleValue::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(keys_[0],cardinality_); - vector table; - for (size_t i1 = 0; i1 < cardinality_; i1++) - table.push_back(i1 == value_); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /* ************************************************************************* */ - DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* ************************************************************************* */ - bool SingleValue::ensureArcConsistency(size_t j, - vector* domains) const { - if (j != keys_[0]) - throw invalid_argument("SingleValue check on wrong domain"); - Domain& D = domains->at(j); - if (D.isSingleton()) { - if (D.firstValue() != value_) throw runtime_error("Unsatisfiable"); - return false; - } - D = Domain(discreteKey(), value_); - return true; - } - - /* ************************************************************************* */ - Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const { - Values::const_iterator it = values.find(keys_[0]); - if (it != values.end() && it->second != value_) throw runtime_error( - "SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared(keys_[0], cardinality_, value_); - } - - /* ************************************************************************* */ - Constraint::shared_ptr SingleValue::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; - if (Dk.isSingleton() && !Dk.contains(value_)) throw runtime_error( - "SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared(discreteKey(), value_); - } +using namespace std; /* ************************************************************************* */ -} // namespace gtsam +void SingleValue::print(const string& s, const KeyFormatter& formatter) const { + cout << s << "SingleValue on " + << "j=" << formatter(keys_[0]) << " with value " << value_ << endl; +} + +/* ************************************************************************* */ +double SingleValue::operator()(const Values& values) const { + return (double)(values.at(keys_[0]) == value_); +} + +/* ************************************************************************* */ +DecisionTreeFactor SingleValue::toDecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(keys_[0], cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; i1++) table.push_back(i1 == value_); + DecisionTreeFactor converted(keys, table); + return converted; +} + +/* ************************************************************************* */ +DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} + +/* ************************************************************************* */ +bool SingleValue::ensureArcConsistency(size_t j, + vector* domains) const { + if (j != keys_[0]) + throw invalid_argument("SingleValue check on wrong domain"); + Domain& D = domains->at(j); + if (D.isSingleton()) { + if (D.firstValue() != value_) throw runtime_error("Unsatisfiable"); + return false; + } + D = Domain(discreteKey(), value_); + return true; +} + +/* ************************************************************************* */ +Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const { + Values::const_iterator it = values.find(keys_[0]); + if (it != values.end() && it->second != value_) + throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared(keys_[0], cardinality_, value_); +} + +/* ************************************************************************* */ +Constraint::shared_ptr SingleValue::partiallyApply( + const vector& domains) const { + const Domain& Dk = domains[keys_[0]]; + if (Dk.isSingleton() && !Dk.contains(value_)) + throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared(discreteKey(), value_); +} + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 0f9a8fb0f..d8a9a770b 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -7,74 +7,71 @@ #pragma once +#include #include namespace gtsam { - /** - * SingleValue constraint: ensures a variable takes on a certain value. - * This could of course also be implemented by changing its `Domain`. +/** + * SingleValue constraint: ensures a variable takes on a certain value. + * This could of course also be implemented by changing its `Domain`. + */ +class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { + size_t cardinality_; /// < Number of values + size_t value_; ///< allowed value + + DiscreteKey discreteKey() const { + return DiscreteKey(keys_[0], cardinality_); + } + + public: + typedef boost::shared_ptr shared_ptr; + + /// Construct from key, cardinality, and given value. + SingleValue(Key key, size_t n, size_t value) + : Constraint(key), cardinality_(n), value_(value) {} + + /// Construct from DiscreteKey and given value. + SingleValue(const DiscreteKey& dkey, size_t value) + : Constraint(dkey.first), cardinality_(dkey.second), value_(value) {} + + // print + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) + return false; + else { + const SingleValue& f(static_cast(other)); + return (cardinality_ == f.cardinality_) && (value_ == f.value_); + } + } + + /// Calculate value + double operator()(const Values& values) const override; + + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency: just sets domain[j] to {value_} + * @param j domain to be checked + * @param domains all other domains */ - class GTSAM_UNSTABLE_EXPORT SingleValue: public Constraint { - - size_t cardinality_; /// < Number of values - size_t value_; ///< allowed value + bool ensureArcConsistency(size_t j, + std::vector* domains) const override; - DiscreteKey discreteKey() const { - return DiscreteKey(keys_[0],cardinality_); - } + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values& values) const override; - public: + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector& domains) const override; +}; - typedef boost::shared_ptr shared_ptr; - - /// Construct from key, cardinality, and given value. - SingleValue(Key key, size_t n, size_t value) : - Constraint(key), cardinality_(n), value_(value) { - } - - /// Construct from DiscreteKey and given value. - SingleValue(const DiscreteKey& dkey, size_t value) : - Constraint(dkey.first), cardinality_(dkey.second), value_(value) { - } - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const SingleValue& f(static_cast(other)); - return (cardinality_==f.cardinality_) && (value_==f.value_); - } - } - - /// Calculate value - double operator()(const Values& values) const override; - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency: just sets domain[j] to {value_} - * @param j domain to be checked - * @param domains all other domains - */ - bool ensureArcConsistency(size_t j, - std::vector* domains) const override; - - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values& values) const override; - - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector& domains) const override; - }; - -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index b1aaab303..832175455 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -30,7 +30,7 @@ TEST(CSP, SingleValue) { EXPECT(assert_equal(f1, singleValue.toDecisionTreeFactor())); // Create domains, laid out as a vector. - // TODO(dellaert): should be map?? + // TODO(dellaert): should be map?? vector domains; domains += Domain(ID), Domain(AZ), Domain(UT); From 4a05da53af352427cd950a12dcbd0bcae82a2da3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 18 Nov 2021 23:29:10 -0500 Subject: [PATCH 38/71] wrap KeyVector methods --- gtsam/linear/linear.i | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index c74161f26..cf65b1a38 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -302,6 +302,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; void printKeys(string s) const; + gtsam::KeyVector& keys() const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; size_t size() const; Vector unweighted_error(const gtsam::VectorValues& c) const; @@ -514,9 +515,9 @@ virtual class GaussianBayesNet { size_t size() const; // FactorGraph derived interface - // size_t size() const; gtsam::GaussianConditional* at(size_t idx) const; gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; bool exists(size_t idx) const; void saveGraph(const string& s) const; From 23bcf96da4955cd9f3332634d781f528c45cc451 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Nov 2021 11:46:32 -0500 Subject: [PATCH 39/71] use emplace_shared --- gtsam/discrete/DiscreteFactorGraph.h | 16 +++++++++------- gtsam_unstable/discrete/CSP.h | 22 ++++++---------------- 2 files changed, 15 insertions(+), 23 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index f39adc9a8..3ea9c3cdd 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -101,25 +101,27 @@ public: /// @} - template + // Add single key decision-tree factor. + template void add(const DiscreteKey& j, SOURCE table) { DiscreteKeys keys; keys.push_back(j); - push_back(boost::make_shared(keys, table)); + emplace_shared(keys, table); } - template + // Add binary key decision-tree factor. + template void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) { DiscreteKeys keys; keys.push_back(j1); keys.push_back(j2); - push_back(boost::make_shared(keys, table)); + emplace_shared(keys, table); } - /** add shared discreteFactor immediately from arguments */ - template + // Add shared discreteFactor immediately from arguments. + template void add(const DiscreteKeys& keys, SOURCE table) { - push_back(boost::make_shared(keys, table)); + emplace_shared(keys, table); } /** Return the set of variables involved in the factors (set union) */ diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index 544cdf0c9..e43e53932 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -21,32 +21,22 @@ namespace gtsam { class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { public: /** A map from keys to values */ - typedef KeyVector Indices; typedef Assignment Values; typedef boost::shared_ptr sharedValues; public: - // /// Constructor - // CSP() { - // } - /// Add a unary constraint, allowing only a single value void addSingleValue(const DiscreteKey& dkey, size_t value) { - boost::shared_ptr factor(new SingleValue(dkey, value)); - push_back(factor); + emplace_shared(dkey, value); } /// Add a binary AllDiff constraint void addAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) { - boost::shared_ptr factor(new BinaryAllDiff(key1, key2)); - push_back(factor); + emplace_shared(key1, key2); } /// Add a general AllDiff constraint - void addAllDiff(const DiscreteKeys& dkeys) { - boost::shared_ptr factor(new AllDiff(dkeys)); - push_back(factor); - } + void addAllDiff(const DiscreteKeys& dkeys) { emplace_shared(dkeys); } // /** return product of all factors as a single factor */ // DecisionTreeFactor product() const { @@ -56,10 +46,10 @@ class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { // return result; // } - /// Find the best total assignment - can be expensive + /// Find the best total assignment - can be expensive. sharedValues optimalAssignment() const; - /// Find the best total assignment - can be expensive + /// Find the best total assignment, with given ordering - can be expensive. sharedValues optimalAssignment(const Ordering& ordering) const; // /* @@ -78,7 +68,7 @@ class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { * Apply arc-consistency ~ Approximate loopy belief propagation * We need to give the domains to a constraint, and it returns * a domain whose values don't conflict in the arc-consistency way. - * TODO: should get cardinality from Indices + * TODO: should get cardinality from DiscreteKeys */ void runArcConsistency(size_t cardinality, size_t nrIterations = 10, bool print = false) const; From ad3225953b53dba1d5bf09e69248ad93d53de056 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Nov 2021 15:52:12 -0500 Subject: [PATCH 40/71] Cleaned up AC1 implementation --- gtsam_unstable/discrete/AllDiff.cpp | 11 +- gtsam_unstable/discrete/AllDiff.h | 12 +- gtsam_unstable/discrete/BinaryAllDiff.h | 11 +- gtsam_unstable/discrete/CSP.cpp | 118 +++++++------- gtsam_unstable/discrete/CSP.h | 14 +- gtsam_unstable/discrete/Constraint.h | 13 +- gtsam_unstable/discrete/Domain.cpp | 36 +++-- gtsam_unstable/discrete/Domain.h | 31 ++-- gtsam_unstable/discrete/SingleValue.cpp | 7 +- gtsam_unstable/discrete/SingleValue.h | 10 +- gtsam_unstable/discrete/tests/testCSP.cpp | 40 +++-- gtsam_unstable/discrete/tests/testSudoku.cpp | 162 +++++++++++++------ 12 files changed, 270 insertions(+), 195 deletions(-) diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index ef18053a4..85cf0b472 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -57,14 +57,11 @@ DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { } /* ************************************************************************* */ -bool AllDiff::ensureArcConsistency(size_t j, - std::vector* domains) const { - // We are changing the domain of variable j. - // TODO(dellaert): confusing, I thought we were changing others... +bool AllDiff::ensureArcConsistency(Key j, Domains* domains) const { Domain& Dj = domains->at(j); // Though strictly not part of allDiff, we check for - // a value in domains[j] that does not occur in any other connected domain. + // a value in domains->at(j) that does not occur in any other connected domain. // If found, we make this a singleton... // TODO: make a new constraint where this really is true boost::optional maybeChanged = Dj.checkAllDiff(keys_, *domains); @@ -103,10 +100,10 @@ Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { /* ************************************************************************* */ Constraint::shared_ptr AllDiff::partiallyApply( - const std::vector& domains) const { + const Domains& domains) const { DiscreteFactor::Values known; for (Key k : keys_) { - const Domain& Dk = domains[k]; + const Domain& Dk = domains.at(k); if (Dk.isSingleton()) known[k] = Dk.firstValue(); } return partiallyApply(known); diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 4deabda94..57b0aeb5c 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -54,21 +54,19 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; /* - * Ensure Arc-consistency - * Arc-consistency involves creating binaryAllDiff constraints - * In which case the combinatorial hyper-arc explosion disappears. + * Ensure Arc-consistency by checking every possible value of domain j. * @param j domain to be checked - * @param (in/out) domains all other domains + * @param (in/out) domains all domains, but only domains->at(j) will be checked. + * @return true if domains->at(j) was changed, false otherwise. */ - bool ensureArcConsistency(size_t j, - std::vector* domains) const override; + bool ensureArcConsistency(Key j, Domains* domains) const override; /// Partially apply known values Constraint::shared_ptr partiallyApply(const Values&) const override; /// Partially apply known values, domain version Constraint::shared_ptr partiallyApply( - const std::vector&) const override; + const Domains&) const override; }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 21cfb18f2..a2c7ba660 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -70,13 +70,12 @@ class BinaryAllDiff : public Constraint { } /* - * Ensure Arc-consistency + * Ensure Arc-consistency by checking every possible value of domain j. * @param j domain to be checked - * @param domains all other domains + * @param (in/out) domains all domains, but only domains->at(j) will be checked. + * @return true if domains->at(j) was changed, false otherwise. */ - /// - bool ensureArcConsistency(size_t j, - std::vector* domains) const override { + bool ensureArcConsistency(Key j, Domains* domains) const override { throw std::runtime_error( "BinaryAllDiff::ensureArcConsistency not implemented"); return false; @@ -89,7 +88,7 @@ class BinaryAllDiff : public Constraint { /// Partially apply known values, domain version Constraint::shared_ptr partiallyApply( - const std::vector&) const override { + const Domains&) const override { throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); } }; diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index bab1ac3c8..8c974f4fd 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -27,81 +27,75 @@ CSP::sharedValues CSP::optimalAssignment(const Ordering& ordering) const { return mpe; } -void CSP::runArcConsistency(size_t cardinality, size_t nrIterations, - bool print) const { +bool CSP::runArcConsistency(const VariableIndex& index, + Domains* domains) const { + bool changed = false; + + // iterate over all variables in the index + for (auto entry : index) { + // Get the variable's key and associated factors: + const Key key = entry.first; + const FactorIndices& factors = entry.second; + + // If this domain is already a singleton, we do nothing. + if (domains->at(key).isSingleton()) continue; + + // Otherwise, loop over all factors/constraints for variable with given key. + for (size_t f : factors) { + // If this factor is a constraint, call its ensureArcConsistency method: + auto constraint = boost::dynamic_pointer_cast((*this)[f]); + if (constraint) { + changed = constraint->ensureArcConsistency(key, domains) || changed; + } + } + } + return changed; +} + +// TODO(dellaert): This is AC1, which is inefficient as any change will cause +// the algorithm to revisit *all* variables again. Implement AC3. +Domains CSP::runArcConsistency(size_t cardinality, size_t maxIterations) const { // Create VariableIndex VariableIndex index(*this); - // index.print(); - - size_t n = index.size(); // Initialize domains - std::vector domains; - for (size_t j = 0; j < n; j++) - domains.push_back(Domain(DiscreteKey(j, cardinality))); + Domains domains; + for (auto entry : index) { + const Key key = entry.first; + domains.emplace(key, DiscreteKey(key, cardinality)); + } - // Create array of flags indicating a domain changed or not - std::vector changed(n); + // Iterate until convergence or not a single domain changed. + for (size_t it = 0; it < maxIterations; it++) { + bool changed = runArcConsistency(index, &domains); + if (!changed) break; + } + return domains; +} - // iterate nrIterations over entire grid - for (size_t it = 0; it < nrIterations; it++) { - bool anyChange = false; - // iterate over all cells - for (size_t v = 0; v < n; v++) { - // keep track of which domains changed - changed[v] = false; - // loop over all factors/constraints for variable v - const FactorIndices& factors = index[v]; - for (size_t f : factors) { - // if not already a singleton - if (!domains[v].isSingleton()) { - // get the constraint and call its ensureArcConsistency method - auto constraint = boost::dynamic_pointer_cast((*this)[f]); - if (!constraint) - throw runtime_error("CSP:runArcConsistency: non-constraint factor"); - changed[v] = - constraint->ensureArcConsistency(v, &domains) || changed[v]; - } - } // f - if (changed[v]) anyChange = true; - } // v - if (!anyChange) break; - // TODO: Sudoku specific hack - if (print) { - if (cardinality == 9 && n == 81) { - for (size_t i = 0, v = 0; i < (size_t)std::sqrt((double)n); i++) { - for (size_t j = 0; j < (size_t)std::sqrt((double)n); j++, v++) { - if (changed[v]) cout << "*"; - domains[v].print(); - cout << "\t"; - } // i - cout << endl; - } // j - } else { - for (size_t v = 0; v < n; v++) { - if (changed[v]) cout << "*"; - domains[v].print(); - cout << "\t"; - } // v - } - cout << endl; - } // print - } // it - -#ifndef INPROGRESS - // Now create new problem with all singleton variables removed - // We do this by adding simplifying all factors using parial application +CSP CSP::partiallyApply(const Domains& domains) const { + // Create new problem with all singleton variables removed + // We do this by adding simplifying all factors using partial application. // TODO: create a new ordering as we go, to ensure a connected graph // KeyOrdering ordering; // vector dkeys; + CSP new_csp; + + // Add tightened domains as new factors: + for (auto key_domain : domains) { + new_csp.emplace_shared(key_domain.second); + } + + // Reduce all existing factors: for (const DiscreteFactor::shared_ptr& f : factors_) { - Constraint::shared_ptr constraint = - boost::dynamic_pointer_cast(f); + auto constraint = boost::dynamic_pointer_cast(f); if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); Constraint::shared_ptr reduced = constraint->partiallyApply(domains); - if (print) reduced->print(); + if (reduced->size() > 1) { + new_csp.push_back(reduced); + } } -#endif + return new_csp; } } // namespace gtsam diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index e43e53932..d94913682 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -62,7 +62,7 @@ class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { // deep. // * It will be very expensive to exclude values that way. // */ - // void applyBeliefPropagation(size_t nrIterations = 10) const; + // void applyBeliefPropagation(size_t maxIterations = 10) const; /* * Apply arc-consistency ~ Approximate loopy belief propagation @@ -70,8 +70,16 @@ class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { * a domain whose values don't conflict in the arc-consistency way. * TODO: should get cardinality from DiscreteKeys */ - void runArcConsistency(size_t cardinality, size_t nrIterations = 10, - bool print = false) const; + Domains runArcConsistency(size_t cardinality, + size_t maxIterations = 10) const; + + /// Run arc consistency for all variables, return true if any domain changed. + bool runArcConsistency(const VariableIndex& index, Domains* domains) const; + + /* + * Create a new CSP, applying the given Domain constraints. + */ + CSP partiallyApply(const Domains& domains) const; }; // CSP } // namespace gtsam diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index e9714d6b4..f0e51b723 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -21,10 +21,12 @@ #include #include +#include namespace gtsam { class Domain; +using Domains = std::map; /** * Base class for constraint factors @@ -65,19 +67,18 @@ class GTSAM_EXPORT Constraint : public DiscreteFactor { /// @{ /* - * Ensure Arc-consistency, possibly changing domains of connected variables. + * Ensure Arc-consistency by checking every possible value of domain j. * @param j domain to be checked - * @param (in/out) domains all other domains - * @return true if domains were changed, false otherwise. + * @param (in/out) domains all domains, but only domains->at(j) will be checked. + * @return true if domains->at(j) was changed, false otherwise. */ - virtual bool ensureArcConsistency(size_t j, - std::vector* domains) const = 0; + virtual bool ensureArcConsistency(Key j, Domains* domains) const = 0; /// Partially apply known values virtual shared_ptr partiallyApply(const Values&) const = 0; /// Partially apply known values, domain version - virtual shared_ptr partiallyApply(const std::vector&) const = 0; + virtual shared_ptr partiallyApply(const Domains&) const = 0; /// @} }; // DiscreteFactor diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index da23717f6..98b735c6c 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -10,28 +10,35 @@ #include #include - +#include namespace gtsam { using namespace std; /* ************************************************************************* */ void Domain::print(const string& s, const KeyFormatter& formatter) const { - cout << s << ": Domain on " << formatter(keys_[0]) - << " (j=" << formatter(keys_[0]) << ") with values"; + cout << s << ": Domain on " << formatter(key()) << " (j=" << formatter(key()) + << ") with values"; for (size_t v : values_) cout << " " << v; cout << endl; } +/* ************************************************************************* */ +string Domain::base1Str() const { + stringstream ss; + for (size_t v : values_) ss << v + 1; + return ss.str(); +} + /* ************************************************************************* */ double Domain::operator()(const Values& values) const { - return contains(values.at(keys_[0])); + return contains(values.at(key())); } /* ************************************************************************* */ DecisionTreeFactor Domain::toDecisionTreeFactor() const { DiscreteKeys keys; - keys += DiscreteKey(keys_[0], cardinality_); + keys += DiscreteKey(key(), cardinality_); vector table; for (size_t i1 = 0; i1 < cardinality_; ++i1) table.push_back(contains(i1)); DecisionTreeFactor converted(keys, table); @@ -45,8 +52,8 @@ DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { } /* ************************************************************************* */ -bool Domain::ensureArcConsistency(size_t j, vector* domains) const { - if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); +bool Domain::ensureArcConsistency(Key j, Domains* domains) const { + if (j != key()) throw invalid_argument("Domain check on wrong domain"); Domain& D = domains->at(j); for (size_t value : values_) if (!D.contains(value)) throw runtime_error("Unsatisfiable"); @@ -55,15 +62,15 @@ bool Domain::ensureArcConsistency(size_t j, vector* domains) const { } /* ************************************************************************* */ -boost::optional Domain::checkAllDiff( - const KeyVector keys, const vector& domains) const { - Key j = keys_[0]; +boost::optional Domain::checkAllDiff(const KeyVector keys, + const Domains& domains) const { + Key j = key(); // for all values in this domain for (const size_t value : values_) { // for all connected domains for (const Key k : keys) // if any domain contains the value we cannot make this domain singleton - if (k != j && domains[k].contains(value)) goto found; + if (k != j && domains.at(k).contains(value)) goto found; // Otherwise: return a singleton: return Domain(this->discreteKey(), value); found:; @@ -73,16 +80,15 @@ boost::optional Domain::checkAllDiff( /* ************************************************************************* */ Constraint::shared_ptr Domain::partiallyApply(const Values& values) const { - Values::const_iterator it = values.find(keys_[0]); + Values::const_iterator it = values.find(key()); if (it != values.end() && !contains(it->second)) throw runtime_error("Domain::partiallyApply: unsatisfiable"); return boost::make_shared(*this); } /* ************************************************************************* */ -Constraint::shared_ptr Domain::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; +Constraint::shared_ptr Domain::partiallyApply(const Domains& domains) const { + const Domain& Dk = domains.at(key()); if (Dk.isSingleton() && !contains(*Dk.begin())) throw runtime_error("Domain::partiallyApply: unsatisfiable"); return boost::make_shared(Dk); diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 9fa22175a..ae137ca33 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -20,10 +20,6 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { size_t cardinality_; /// Cardinality std::set values_; /// allowed values - DiscreteKey discreteKey() const { - return DiscreteKey(keys_[0], cardinality_); - } - public: typedef boost::shared_ptr shared_ptr; @@ -40,6 +36,12 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { values_.insert(v); } + /// The one key + Key key() const { return keys_[0]; } + + // The associated discrete key + DiscreteKey discreteKey() const { return DiscreteKey(key(), cardinality_); } + /// Insert a value, non const :-( void insert(size_t value) { values_.insert(value); } @@ -66,6 +68,11 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { } } + // Return concise string representation, mostly to debug arc consistency. + // Converts from base 0 to base1. + std::string base1Str() const; + + // Check whether domain cotains a specific value. bool contains(size_t value) const { return values_.count(value) > 0; } /// Calculate value @@ -78,12 +85,13 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; /* - * Ensure Arc-consistency + * Ensure Arc-consistency by checking every possible value of domain j. * @param j domain to be checked - * @param domains all other domains + * @param (in/out) domains all domains, but only domains->at(j) will be + * checked. + * @return true if domains->at(j) was changed, false otherwise. */ - bool ensureArcConsistency(size_t j, - std::vector* domains) const override; + bool ensureArcConsistency(Key j, Domains* domains) const override; /** * Check for a value in domain that does not occur in any other connected @@ -92,15 +100,14 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { * @param keys connected domains through alldiff * @param keys other domains */ - boost::optional checkAllDiff( - const KeyVector keys, const std::vector& domains) const; + boost::optional checkAllDiff(const KeyVector keys, + const Domains& domains) const; /// Partially apply known values Constraint::shared_ptr partiallyApply(const Values& values) const override; /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector& domains) const override; + Constraint::shared_ptr partiallyApply(const Domains& domains) const override; }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 753d46cff..162e21512 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -44,8 +44,7 @@ DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { } /* ************************************************************************* */ -bool SingleValue::ensureArcConsistency(size_t j, - vector* domains) const { +bool SingleValue::ensureArcConsistency(Key j, Domains* domains) const { if (j != keys_[0]) throw invalid_argument("SingleValue check on wrong domain"); Domain& D = domains->at(j); @@ -67,8 +66,8 @@ Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const { /* ************************************************************************* */ Constraint::shared_ptr SingleValue::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; + const Domains& domains) const { + const Domain& Dk = domains.at(keys_[0]); if (Dk.isSingleton() && !Dk.contains(value_)) throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); return boost::make_shared(discreteKey(), value_); diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index d8a9a770b..d826093df 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -59,19 +59,19 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; /* - * Ensure Arc-consistency: just sets domain[j] to {value_} + * Ensure Arc-consistency: just sets domain[j] to {value_}. * @param j domain to be checked - * @param domains all other domains + * @param (in/out) domains all domains, but only domains->at(j) will be checked. + * @return true if domains->at(j) was changed, false otherwise. */ - bool ensureArcConsistency(size_t j, - std::vector* domains) const override; + bool ensureArcConsistency(Key j, Domains* domains) const override; /// Partially apply known values Constraint::shared_ptr partiallyApply(const Values& values) const override; /// Partially apply known values, domain version Constraint::shared_ptr partiallyApply( - const std::vector& domains) const override; + const Domains& domains) const override; }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index 832175455..63069d710 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -29,16 +29,17 @@ TEST(CSP, SingleValue) { DecisionTreeFactor f1(AZ, "0 0 1"); EXPECT(assert_equal(f1, singleValue.toDecisionTreeFactor())); - // Create domains, laid out as a vector. - // TODO(dellaert): should be map?? - vector domains; - domains += Domain(ID), Domain(AZ), Domain(UT); + // Create domains + Domains domains; + domains.emplace(0, Domain(ID)); + domains.emplace(1, Domain(AZ)); + domains.emplace(2, Domain(UT)); // Ensure arc-consistency: just wipes out values in AZ domain: EXPECT(singleValue.ensureArcConsistency(1, &domains)); - LONGS_EQUAL(3, domains[0].nrValues()); - LONGS_EQUAL(1, domains[1].nrValues()); - LONGS_EQUAL(3, domains[2].nrValues()); + LONGS_EQUAL(3, domains.at(0).nrValues()); + LONGS_EQUAL(1, domains.at(1).nrValues()); + LONGS_EQUAL(3, domains.at(2).nrValues()); } /* ************************************************************************* */ @@ -81,8 +82,10 @@ TEST(CSP, AllDiff) { EXPECT(assert_equal(f2, actual)); // Create domains. - vector domains; - domains += Domain(ID), Domain(AZ), Domain(UT); + Domains domains; + domains.emplace(0, Domain(ID)); + domains.emplace(1, Domain(AZ)); + domains.emplace(2, Domain(UT)); // First constrict AZ domain: SingleValue singleValue(AZ, 2); @@ -92,9 +95,9 @@ TEST(CSP, AllDiff) { EXPECT(alldiff.ensureArcConsistency(0, &domains)); EXPECT(!alldiff.ensureArcConsistency(1, &domains)); EXPECT(alldiff.ensureArcConsistency(2, &domains)); - LONGS_EQUAL(2, domains[0].nrValues()); - LONGS_EQUAL(1, domains[1].nrValues()); - LONGS_EQUAL(2, domains[2].nrValues()); + LONGS_EQUAL(2, domains.at(0).nrValues()); + LONGS_EQUAL(1, domains.at(1).nrValues()); + LONGS_EQUAL(2, domains.at(2).nrValues()); } /* ************************************************************************* */ @@ -232,17 +235,20 @@ TEST(CSP, ArcConsistency) { EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // ensure arc-consistency, i.e., narrow domains... - vector domains; - domains += Domain(ID), Domain(AZ), Domain(UT); + Domains domains; + domains.emplace(0, Domain(ID)); + domains.emplace(1, Domain(AZ)); + domains.emplace(2, Domain(UT)); + SingleValue singleValue(AZ, 2); AllDiff alldiff(dkeys); EXPECT(singleValue.ensureArcConsistency(1, &domains)); EXPECT(alldiff.ensureArcConsistency(0, &domains)); EXPECT(!alldiff.ensureArcConsistency(1, &domains)); EXPECT(alldiff.ensureArcConsistency(2, &domains)); - LONGS_EQUAL(2, domains[0].nrValues()); - LONGS_EQUAL(1, domains[1].nrValues()); - LONGS_EQUAL(2, domains[2].nrValues()); + LONGS_EQUAL(2, domains.at(0).nrValues()); + LONGS_EQUAL(1, domains.at(1).nrValues()); + LONGS_EQUAL(2, domains.at(2).nrValues()); // Parial application, version 1 DiscreteFactor::Values known; diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index 4843ae269..ee307fd5b 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -6,6 +6,7 @@ */ #include +#include #include #include @@ -20,12 +21,12 @@ using namespace gtsam; #define PRINT false +/// A class that encodes Sudoku's as a CSP problem class Sudoku : public CSP { - /// sudoku size - size_t n_; + size_t n_; ///< Side of Sudoku, e.g. 4 or 9 - /// discrete keys - typedef std::pair IJ; + /// Mapping from base i,j coordinates to discrete keys: + using IJ = std::pair; std::map dkeys_; public: @@ -42,15 +43,14 @@ class Sudoku : public CSP { // Create variables, ordering, and unary constraints va_list ap; va_start(ap, n); - Key k = 0; for (size_t i = 0; i < n; ++i) { - for (size_t j = 0; j < n; ++j, ++k) { + for (size_t j = 0; j < n; ++j) { // create the key IJ ij(i, j); - dkeys_[ij] = DiscreteKey(k, n); + Symbol key('1' + i, j + 1); + dkeys_[ij] = DiscreteKey(key, n); // get the unary constraint, if any int value = va_arg(ap, int); - // cout << value << " "; if (value != 0) addSingleValue(dkeys_[ij], value - 1); } // cout << endl; @@ -88,7 +88,7 @@ class Sudoku : public CSP { } /// Print readable form of assignment - void printAssignment(DiscreteFactor::sharedValues assignment) const { + void printAssignment(const DiscreteFactor::sharedValues& assignment) const { for (size_t i = 0; i < n_; i++) { for (size_t j = 0; j < n_; j++) { Key k = key(i, j); @@ -99,10 +99,22 @@ class Sudoku : public CSP { } /// solve and print solution - void printSolution() { + void printSolution() const { DiscreteFactor::sharedValues MPE = optimalAssignment(); printAssignment(MPE); } + + // Print domain + void printDomains(const Domains& domains) { + for (size_t i = 0; i < n_; i++) { + for (size_t j = 0; j < n_; j++) { + Key k = key(i, j); + cout << domains.at(k).base1Str(); + cout << "\t"; + } // i + cout << endl; + } // j + } }; /* ************************************************************************* */ @@ -113,9 +125,6 @@ TEST_UNSAFE(Sudoku, small) { 4, 0, 2, 0, // 0, 1, 0, 0); - // Do BP - csp.runArcConsistency(4, 10, PRINT); - // optimize and check CSP::sharedValues solution = csp.optimalAssignment(); CSP::Values expected; @@ -126,73 +135,124 @@ TEST_UNSAFE(Sudoku, small) { csp.key(3, 3), 2); EXPECT(assert_equal(expected, *solution)); // csp.printAssignment(solution); + + // Do BP (AC1) + auto domains = csp.runArcConsistency(4, 3); + // csp.printDomains(domains); + Domain domain44 = domains.at(Symbol('4', 4)); + EXPECT_LONGS_EQUAL(1, domain44.nrValues()); + + // Test Creation of a new, simpler CSP + CSP new_csp = csp.partiallyApply(domains); + // Should only be 16 new Domains + EXPECT_LONGS_EQUAL(16, new_csp.size()); + + // Check that solution + CSP::sharedValues new_solution = new_csp.optimalAssignment(); + // csp.printAssignment(new_solution); + EXPECT(assert_equal(expected, *new_solution)); } /* ************************************************************************* */ TEST_UNSAFE(Sudoku, easy) { - Sudoku sudoku(9, // - 0, 0, 5, 0, 9, 0, 0, 0, 1, // - 0, 0, 0, 0, 0, 2, 0, 7, 3, // - 7, 6, 0, 0, 0, 8, 2, 0, 0, // + Sudoku csp(9, // + 0, 0, 5, 0, 9, 0, 0, 0, 1, // + 0, 0, 0, 0, 0, 2, 0, 7, 3, // + 7, 6, 0, 0, 0, 8, 2, 0, 0, // - 0, 1, 2, 0, 0, 9, 0, 0, 4, // - 0, 0, 0, 2, 0, 3, 0, 0, 0, // - 3, 0, 0, 1, 0, 0, 9, 6, 0, // + 0, 1, 2, 0, 0, 9, 0, 0, 4, // + 0, 0, 0, 2, 0, 3, 0, 0, 0, // + 3, 0, 0, 1, 0, 0, 9, 6, 0, // - 0, 0, 1, 9, 0, 0, 0, 5, 8, // - 9, 7, 0, 5, 0, 0, 0, 0, 0, // - 5, 0, 0, 0, 3, 0, 7, 0, 0); + 0, 0, 1, 9, 0, 0, 0, 5, 8, // + 9, 7, 0, 5, 0, 0, 0, 0, 0, // + 5, 0, 0, 0, 3, 0, 7, 0, 0); - // Do BP - sudoku.runArcConsistency(4, 10, PRINT); + // csp.printSolution(); // don't do it - // sudoku.printSolution(); // don't do it + // Do BP (AC1) + auto domains = csp.runArcConsistency(9, 10); + // csp.printDomains(domains); + Key key99 = Symbol('9', 9); + Domain domain99 = domains.at(key99); + EXPECT_LONGS_EQUAL(1, domain99.nrValues()); + + // Test Creation of a new, simpler CSP + CSP new_csp = csp.partiallyApply(domains); + // 81 new Domains, and still 26 all-diff constraints + EXPECT_LONGS_EQUAL(81 + 26, new_csp.size()); + + // csp.printSolution(); // still don't do it ! :-( } /* ************************************************************************* */ TEST_UNSAFE(Sudoku, extreme) { - Sudoku sudoku(9, // - 0, 0, 9, 7, 4, 8, 0, 0, 0, 7, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, // - 0, 1, 0, 9, 0, 0, 0, 0, 0, 7, // - 0, 0, 0, 2, 4, 0, 0, 6, 4, 0, // - 1, 0, 5, 9, 0, 0, 9, 8, 0, 0, // - 0, 3, 0, 0, 0, 0, 0, 8, 0, 3, // - 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 6, 0, 0, 0, 2, 7, 5, 9, 0, 0); + Sudoku csp(9, // + 0, 0, 9, 7, 4, 8, 0, 0, 0, 7, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, // + 0, 1, 0, 9, 0, 0, 0, 0, 0, 7, // + 0, 0, 0, 2, 4, 0, 0, 6, 4, 0, // + 1, 0, 5, 9, 0, 0, 9, 8, 0, 0, // + 0, 3, 0, 0, 0, 0, 0, 8, 0, 3, // + 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 6, 0, 0, 0, 2, 7, 5, 9, 0, 0); // Do BP - sudoku.runArcConsistency(9, 10, PRINT); + csp.runArcConsistency(9, 10); #ifdef METIS - VariableIndexOrdered index(sudoku); + VariableIndexOrdered index(csp); index.print("index"); ofstream os("/Users/dellaert/src/hmetis-1.5-osx-i686/extreme-dual.txt"); index.outputMetisFormat(os); #endif - // sudoku.printSolution(); // don't do it + // Do BP (AC1) + auto domains = csp.runArcConsistency(9, 10); + // csp.printDomains(domains); + Key key99 = Symbol('9', 9); + Domain domain99 = domains.at(key99); + EXPECT_LONGS_EQUAL(2, domain99.nrValues()); + + // Test Creation of a new, simpler CSP + CSP new_csp = csp.partiallyApply(domains); + // 81 new Domains, and still 20 all-diff constraints + EXPECT_LONGS_EQUAL(81 + 20, new_csp.size()); + + // csp.printSolution(); // still don't do it ! :-( } /* ************************************************************************* */ TEST_UNSAFE(Sudoku, AJC_3star_Feb8_2012) { - Sudoku sudoku(9, // - 9, 5, 0, 0, 0, 6, 0, 0, 0, // - 0, 8, 4, 0, 7, 0, 0, 0, 0, // - 6, 2, 0, 5, 0, 0, 4, 0, 0, // + Sudoku csp(9, // + 9, 5, 0, 0, 0, 6, 0, 0, 0, // + 0, 8, 4, 0, 7, 0, 0, 0, 0, // + 6, 2, 0, 5, 0, 0, 4, 0, 0, // - 0, 0, 0, 2, 9, 0, 6, 0, 0, // - 0, 9, 0, 0, 0, 0, 0, 2, 0, // - 0, 0, 2, 0, 6, 3, 0, 0, 0, // + 0, 0, 0, 2, 9, 0, 6, 0, 0, // + 0, 9, 0, 0, 0, 0, 0, 2, 0, // + 0, 0, 2, 0, 6, 3, 0, 0, 0, // - 0, 0, 9, 0, 0, 7, 0, 6, 8, // - 0, 0, 0, 0, 3, 0, 2, 9, 0, // - 0, 0, 0, 1, 0, 0, 0, 3, 7); + 0, 0, 9, 0, 0, 7, 0, 6, 8, // + 0, 0, 0, 0, 3, 0, 2, 9, 0, // + 0, 0, 0, 1, 0, 0, 0, 3, 7); - // Do BP - sudoku.runArcConsistency(9, 10, PRINT); + // Do BP (AC1) + auto domains = csp.runArcConsistency(9, 10); + // csp.printDomains(domains); + Key key99 = Symbol('9', 9); + Domain domain99 = domains.at(key99); + EXPECT_LONGS_EQUAL(1, domain99.nrValues()); - // sudoku.printSolution(); // don't do it + // Test Creation of a new, simpler CSP + CSP new_csp = csp.partiallyApply(domains); + // Just the 81 new Domains + EXPECT_LONGS_EQUAL(81, new_csp.size()); + + // Check that solution + CSP::sharedValues solution = new_csp.optimalAssignment(); + // csp.printAssignment(solution); + EXPECT_LONGS_EQUAL(6, solution->at(key99)); } /* ************************************************************************* */ From 0c6d5d438ff44dbb143b049f4ace02f8f10ff0f7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Nov 2021 11:46:32 -0500 Subject: [PATCH 41/71] use emplace_shared --- gtsam/discrete/DiscreteFactorGraph.h | 16 +++++++++------- gtsam_unstable/discrete/CSP.h | 22 ++++++---------------- 2 files changed, 15 insertions(+), 23 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index f39adc9a8..3ea9c3cdd 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -101,25 +101,27 @@ public: /// @} - template + // Add single key decision-tree factor. + template void add(const DiscreteKey& j, SOURCE table) { DiscreteKeys keys; keys.push_back(j); - push_back(boost::make_shared(keys, table)); + emplace_shared(keys, table); } - template + // Add binary key decision-tree factor. + template void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) { DiscreteKeys keys; keys.push_back(j1); keys.push_back(j2); - push_back(boost::make_shared(keys, table)); + emplace_shared(keys, table); } - /** add shared discreteFactor immediately from arguments */ - template + // Add shared discreteFactor immediately from arguments. + template void add(const DiscreteKeys& keys, SOURCE table) { - push_back(boost::make_shared(keys, table)); + emplace_shared(keys, table); } /** Return the set of variables involved in the factors (set union) */ diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index 544cdf0c9..e43e53932 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -21,32 +21,22 @@ namespace gtsam { class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { public: /** A map from keys to values */ - typedef KeyVector Indices; typedef Assignment Values; typedef boost::shared_ptr sharedValues; public: - // /// Constructor - // CSP() { - // } - /// Add a unary constraint, allowing only a single value void addSingleValue(const DiscreteKey& dkey, size_t value) { - boost::shared_ptr factor(new SingleValue(dkey, value)); - push_back(factor); + emplace_shared(dkey, value); } /// Add a binary AllDiff constraint void addAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) { - boost::shared_ptr factor(new BinaryAllDiff(key1, key2)); - push_back(factor); + emplace_shared(key1, key2); } /// Add a general AllDiff constraint - void addAllDiff(const DiscreteKeys& dkeys) { - boost::shared_ptr factor(new AllDiff(dkeys)); - push_back(factor); - } + void addAllDiff(const DiscreteKeys& dkeys) { emplace_shared(dkeys); } // /** return product of all factors as a single factor */ // DecisionTreeFactor product() const { @@ -56,10 +46,10 @@ class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { // return result; // } - /// Find the best total assignment - can be expensive + /// Find the best total assignment - can be expensive. sharedValues optimalAssignment() const; - /// Find the best total assignment - can be expensive + /// Find the best total assignment, with given ordering - can be expensive. sharedValues optimalAssignment(const Ordering& ordering) const; // /* @@ -78,7 +68,7 @@ class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { * Apply arc-consistency ~ Approximate loopy belief propagation * We need to give the domains to a constraint, and it returns * a domain whose values don't conflict in the arc-consistency way. - * TODO: should get cardinality from Indices + * TODO: should get cardinality from DiscreteKeys */ void runArcConsistency(size_t cardinality, size_t nrIterations = 10, bool print = false) const; From 9fe6d23d9f931988309cfa4e20e4c5f695866add Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Nov 2021 16:15:05 -0500 Subject: [PATCH 42/71] Got rid of sharedValues --- examples/DiscreteBayesNetExample.cpp | 6 ++--- examples/DiscreteBayesNet_FG.cpp | 16 +++++------ examples/HMMExample.cpp | 4 +-- examples/UGM_chain.cpp | 2 +- examples/UGM_small.cpp | 2 +- gtsam/discrete/DiscreteBayesNet.cpp | 12 ++++----- gtsam/discrete/DiscreteBayesNet.h | 4 +-- gtsam/discrete/DiscreteConditional.cpp | 12 ++++----- gtsam/discrete/DiscreteConditional.h | 4 +-- gtsam/discrete/DiscreteFactor.h | 1 - gtsam/discrete/DiscreteFactorGraph.cpp | 2 +- gtsam/discrete/DiscreteFactorGraph.h | 2 +- gtsam/discrete/tests/testDiscreteBayesNet.cpp | 12 ++++----- .../tests/testDiscreteFactorGraph.cpp | 16 +++++------ gtsam_unstable/discrete/CSP.cpp | 10 +++---- gtsam_unstable/discrete/CSP.h | 4 +-- gtsam_unstable/discrete/Scheduler.cpp | 27 +++++++++---------- gtsam_unstable/discrete/Scheduler.h | 12 ++++----- .../discrete/examples/schedulingExample.cpp | 4 +-- .../discrete/examples/schedulingQuals12.cpp | 4 +-- .../discrete/examples/schedulingQuals13.cpp | 4 +-- gtsam_unstable/discrete/tests/testCSP.cpp | 20 +++++++------- .../discrete/tests/testScheduler.cpp | 10 +++---- gtsam_unstable/discrete/tests/testSudoku.cpp | 10 +++---- 24 files changed, 98 insertions(+), 102 deletions(-) diff --git a/examples/DiscreteBayesNetExample.cpp b/examples/DiscreteBayesNetExample.cpp index 5dca116c3..25259e24e 100644 --- a/examples/DiscreteBayesNetExample.cpp +++ b/examples/DiscreteBayesNetExample.cpp @@ -56,7 +56,7 @@ int main(int argc, char **argv) { DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering); // solve - DiscreteFactor::sharedValues mpe = chordal->optimize(); + autompe = chordal->optimize(); GTSAM_PRINT(*mpe); // We can also build a Bayes tree (directed junction tree). @@ -70,13 +70,13 @@ int main(int argc, char **argv) { // solve again, now with evidence DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering); - DiscreteFactor::sharedValues mpe2 = chordal2->optimize(); + autompe2 = chordal2->optimize(); GTSAM_PRINT(*mpe2); // We can also sample from it cout << "\n10 samples:" << endl; for (size_t i = 0; i < 10; i++) { - DiscreteFactor::sharedValues sample = chordal2->sample(); + autosample = chordal2->sample(); GTSAM_PRINT(*sample); } return 0; diff --git a/examples/DiscreteBayesNet_FG.cpp b/examples/DiscreteBayesNet_FG.cpp index 121df4bef..7669e567c 100644 --- a/examples/DiscreteBayesNet_FG.cpp +++ b/examples/DiscreteBayesNet_FG.cpp @@ -33,11 +33,11 @@ using namespace gtsam; int main(int argc, char **argv) { // Define keys and a print function Key C(1), S(2), R(3), W(4); - auto print = [=](DiscreteFactor::sharedValues values) { - cout << boolalpha << "Cloudy = " << static_cast((*values)[C]) - << " Sprinkler = " << static_cast((*values)[S]) - << " Rain = " << boolalpha << static_cast((*values)[R]) - << " WetGrass = " << static_cast((*values)[W]) << endl; + auto print = [=](const DiscreteFactor::Values& values) { + cout << boolalpha << "Cloudy = " << static_cast(values[C]) + << " Sprinkler = " << static_cast(values[S]) + << " Rain = " << boolalpha << static_cast(values[R]) + << " WetGrass = " << static_cast(values[W]) << endl; }; // We assume binary state variables @@ -85,7 +85,7 @@ int main(int argc, char **argv) { } // "Most Probable Explanation", i.e., configuration with largest value - DiscreteFactor::sharedValues mpe = graph.eliminateSequential()->optimize(); + autompe = graph.eliminateSequential()->optimize(); cout << "\nMost Probable Explanation (MPE):" << endl; print(mpe); @@ -97,7 +97,7 @@ int main(int argc, char **argv) { // solve again, now with evidence DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); - DiscreteFactor::sharedValues mpe_with_evidence = chordal->optimize(); + autompe_with_evidence = chordal->optimize(); cout << "\nMPE given C=0:" << endl; print(mpe_with_evidence); @@ -113,7 +113,7 @@ int main(int argc, char **argv) { // We can also sample from it cout << "\n10 samples:" << endl; for (size_t i = 0; i < 10; i++) { - DiscreteFactor::sharedValues sample = chordal->sample(); + autosample = chordal->sample(); print(sample); } return 0; diff --git a/examples/HMMExample.cpp b/examples/HMMExample.cpp index ee861e381..e91f751ad 100644 --- a/examples/HMMExample.cpp +++ b/examples/HMMExample.cpp @@ -66,13 +66,13 @@ int main(int argc, char **argv) { chordal->print("Eliminated"); // solve - DiscreteFactor::sharedValues mpe = chordal->optimize(); + autompe = chordal->optimize(); GTSAM_PRINT(*mpe); // We can also sample from it cout << "\n10 samples:" << endl; for (size_t k = 0; k < 10; k++) { - DiscreteFactor::sharedValues sample = chordal->sample(); + autosample = chordal->sample(); GTSAM_PRINT(*sample); } diff --git a/examples/UGM_chain.cpp b/examples/UGM_chain.cpp index 3a885a844..63d58adaf 100644 --- a/examples/UGM_chain.cpp +++ b/examples/UGM_chain.cpp @@ -70,7 +70,7 @@ int main(int argc, char** argv) { // "Decoding", i.e., configuration with largest value // We use sequential variable elimination DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); - DiscreteFactor::sharedValues optimalDecoding = chordal->optimize(); + autooptimalDecoding = chordal->optimize(); optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n"); // "Inference" Computing marginals for each node diff --git a/examples/UGM_small.cpp b/examples/UGM_small.cpp index 27a6205a3..9429c2b2e 100644 --- a/examples/UGM_small.cpp +++ b/examples/UGM_small.cpp @@ -63,7 +63,7 @@ int main(int argc, char** argv) { // "Decoding", i.e., configuration with largest value (MPE) // We use sequential variable elimination DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); - DiscreteFactor::sharedValues optimalDecoding = chordal->optimize(); + autooptimalDecoding = chordal->optimize(); optimalDecoding->print("\noptimalDecoding"); // "Inference" Computing marginals diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index 84a80c565..71c50c477 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -54,20 +54,20 @@ namespace gtsam { } /* ************************************************************************* */ - DiscreteFactor::sharedValues DiscreteBayesNet::optimize() const { + DiscreteFactor::Values DiscreteBayesNet::optimize() const { // solve each node in turn in topological sort order (parents first) - DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); + DiscreteFactor::Values result; for (auto conditional: boost::adaptors::reverse(*this)) - conditional->solveInPlace(*result); + conditional->solveInPlace(&result); return result; } /* ************************************************************************* */ - DiscreteFactor::sharedValues DiscreteBayesNet::sample() const { + DiscreteFactor::Values DiscreteBayesNet::sample() const { // sample each node in turn in topological sort order (parents first) - DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); + DiscreteFactor::Values result; for (auto conditional: boost::adaptors::reverse(*this)) - conditional->sampleInPlace(*result); + conditional->sampleInPlace(&result); return result; } diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index d5ba30584..e89645658 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -83,10 +83,10 @@ namespace gtsam { /** * Solve the DiscreteBayesNet by back-substitution */ - DiscreteFactor::sharedValues optimize() const; + DiscreteFactor::Values optimize() const; /** Do ancestral sampling */ - DiscreteFactor::sharedValues sample() const; + DiscreteFactor::Values sample() const; ///@} diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index ac7c58405..e7ef4de19 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -117,9 +117,9 @@ Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const { } /* ******************************************************************************** */ -void DiscreteConditional::solveInPlace(Values& values) const { +void DiscreteConditional::solveInPlace(Values* values) const { // TODO: Abhijit asks: is this really the fastest way? He thinks it is. - ADT pFS = choose(values); // P(F|S=parentsValues) + ADT pFS = choose(*values); // P(F|S=parentsValues) // Initialize Values mpe; @@ -145,16 +145,16 @@ void DiscreteConditional::solveInPlace(Values& values) const { //set values (inPlace) to mpe for(Key j: frontals()) { - values[j] = mpe[j]; + (*values)[j] = mpe[j]; } } /* ******************************************************************************** */ -void DiscreteConditional::sampleInPlace(Values& values) const { +void DiscreteConditional::sampleInPlace(Values* values) const { assert(nrFrontals() == 1); Key j = (firstFrontalKey()); - size_t sampled = sample(values); // Sample variable - values[j] = sampled; // store result in partial solution + size_t sampled = sample(*values); // Sample variable given parents + (*values)[j] = sampled; // store result in partial solution } /* ******************************************************************************** */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 8299fab2c..40413a9e7 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -133,10 +133,10 @@ public: /// @{ /// solve a conditional, in place - void solveInPlace(Values& parentsValues) const; + void solveInPlace(Values* parentsValues) const; /// sample in place, stores result in partial solution - void sampleInPlace(Values& parentsValues) const; + void sampleInPlace(Values* parentsValues) const; /// @} diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 6b0919507..cd883c59c 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -51,7 +51,6 @@ public: * the new class DiscreteValue, as the varible's type (domain) */ typedef Assignment Values; - typedef boost::shared_ptr sharedValues; public: diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index e41968d6b..4ff0e339e 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -94,7 +94,7 @@ namespace gtsam { // } /* ************************************************************************* */ - DiscreteFactor::sharedValues DiscreteFactorGraph::optimize() const + DiscreteFactorGraph::Values DiscreteFactorGraph::optimize() const { gttic(DiscreteFactorGraph_optimize); return BaseEliminateable::eliminateSequential()->optimize(); diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 3ea9c3cdd..87820c9d6 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -142,7 +142,7 @@ public: * the dense elimination function specified in \c function, * followed by back-substitution resulting from elimination. Is equivalent * to calling graph.eliminateSequential()->optimize(). */ - DiscreteFactor::sharedValues optimize() const; + Values optimize() const; // /** Permute the variables in the factors */ diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 2b440e5a0..f6159c0c6 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -104,12 +104,12 @@ TEST(DiscreteBayesNet, Asia) { EXPECT(assert_equal(expected2, *chordal->back())); // solve - DiscreteFactor::sharedValues actualMPE = chordal->optimize(); + auto actualMPE = chordal->optimize(); DiscreteFactor::Values expectedMPE; insert(expectedMPE)(Asia.first, 0)(Dyspnea.first, 0)(XRay.first, 0)( Tuberculosis.first, 0)(Smoking.first, 0)(Either.first, 0)( LungCancer.first, 0)(Bronchitis.first, 0); - EXPECT(assert_equal(expectedMPE, *actualMPE)); + EXPECT(assert_equal(expectedMPE, actualMPE)); // add evidence, we were in Asia and we have dyspnea fg.add(Asia, "0 1"); @@ -117,12 +117,12 @@ TEST(DiscreteBayesNet, Asia) { // solve again, now with evidence DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering); - DiscreteFactor::sharedValues actualMPE2 = chordal2->optimize(); + auto actualMPE2 = chordal2->optimize(); DiscreteFactor::Values expectedMPE2; insert(expectedMPE2)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 0)( Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 0)( LungCancer.first, 0)(Bronchitis.first, 1); - EXPECT(assert_equal(expectedMPE2, *actualMPE2)); + EXPECT(assert_equal(expectedMPE2, actualMPE2)); // now sample from it DiscreteFactor::Values expectedSample; @@ -130,8 +130,8 @@ TEST(DiscreteBayesNet, Asia) { insert(expectedSample)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 1)( Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 1)( LungCancer.first, 1)(Bronchitis.first, 0); - DiscreteFactor::sharedValues actualSample = chordal2->sample(); - EXPECT(assert_equal(expectedSample, *actualSample)); + auto actualSample = chordal2->sample(); + EXPECT(assert_equal(expectedSample, actualSample)); } /* ************************************************************************* */ diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 1defd5acf..6b7a43c1c 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -169,8 +169,8 @@ TEST( DiscreteFactorGraph, test) // Test optimization DiscreteFactor::Values expectedValues; insert(expectedValues)(0, 0)(1, 0)(2, 0); - DiscreteFactor::sharedValues actualValues = graph.optimize(); - EXPECT(assert_equal(expectedValues, *actualValues)); + auto actualValues = graph.optimize(); + EXPECT(assert_equal(expectedValues, actualValues)); } /* ************************************************************************* */ @@ -186,11 +186,11 @@ TEST( DiscreteFactorGraph, testMPE) // graph.product().print(); // DiscreteSequentialSolver(graph).eliminate()->print(); - DiscreteFactor::sharedValues actualMPE = graph.optimize(); + auto actualMPE = graph.optimize(); DiscreteFactor::Values expectedMPE; insert(expectedMPE)(0, 0)(1, 1)(2, 1); - EXPECT(assert_equal(expectedMPE, *actualMPE)); + EXPECT(assert_equal(expectedMPE, actualMPE)); } /* ************************************************************************* */ @@ -216,8 +216,8 @@ TEST( DiscreteFactorGraph, testMPE_Darwiche09book_p244) // Use the solver machinery. DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); - DiscreteFactor::sharedValues actualMPE = chordal->optimize(); - EXPECT(assert_equal(expectedMPE, *actualMPE)); + auto actualMPE = chordal->optimize(); + EXPECT(assert_equal(expectedMPE, actualMPE)); // DiscreteConditional::shared_ptr root = chordal->back(); // EXPECT_DOUBLES_EQUAL(0.4, (*root)(*actualMPE), 1e-9); @@ -244,8 +244,8 @@ ETree::shared_ptr eTree = ETree::Create(graph, structure); // eliminate normally and check solution DiscreteBayesNet::shared_ptr bayesNet = eTree->eliminate(&EliminateDiscrete); // bayesNet->print(">>>>>>>>>>>>>> Bayes Net <<<<<<<<<<<<<<<<<<"); -DiscreteFactor::sharedValues actualMPE = optimize(*bayesNet); -EXPECT(assert_equal(expectedMPE, *actualMPE)); +auto actualMPE = optimize(*bayesNet); +EXPECT(assert_equal(expectedMPE, actualMPE)); // Approximate and check solution // DiscreteBayesNet::shared_ptr approximateNet = eTree->approximate(); diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index b1d70dc6e..ad88469c5 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -14,17 +14,15 @@ using namespace std; namespace gtsam { /// Find the best total assignment - can be expensive -CSP::sharedValues CSP::optimalAssignment() const { +CSP::Values CSP::optimalAssignment() const { DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(); - sharedValues mpe = chordal->optimize(); - return mpe; + return chordal->optimize(); } /// Find the best total assignment - can be expensive -CSP::sharedValues CSP::optimalAssignment(const Ordering& ordering) const { +CSP::Values CSP::optimalAssignment(const Ordering& ordering) const { DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering); - sharedValues mpe = chordal->optimize(); - return mpe; + return chordal->optimize(); } void CSP::runArcConsistency(size_t cardinality, size_t nrIterations, diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index e43e53932..8b0af617f 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -47,10 +47,10 @@ class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { // } /// Find the best total assignment - can be expensive. - sharedValues optimalAssignment() const; + Values optimalAssignment() const; /// Find the best total assignment, with given ordering - can be expensive. - sharedValues optimalAssignment(const Ordering& ordering) const; + Values optimalAssignment(const Ordering& ordering) const; // /* // * Perform loopy belief propagation diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 415f92e62..6210f8037 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -202,16 +202,16 @@ void Scheduler::print(const string& s, const KeyFormatter& formatter) const { } // print /** Print readable form of assignment */ -void Scheduler::printAssignment(sharedValues assignment) const { +void Scheduler::printAssignment(const Values& assignment) const { // Not intended to be general! Assumes very particular ordering ! cout << endl; for (size_t s = 0; s < nrStudents(); s++) { Key j = 3 * maxNrStudents_ + s; - size_t slot = assignment->at(j); + size_t slot = assignment.at(j); cout << studentName(s) << " slot: " << slotName_[slot] << endl; Key base = 3 * s; for (size_t area = 0; area < 3; area++) { - size_t faculty = assignment->at(base + area); + size_t faculty = assignment.at(base + area); cout << setw(12) << studentArea(s, area) << ": " << facultyName_[faculty] << endl; } @@ -220,8 +220,8 @@ void Scheduler::printAssignment(sharedValues assignment) const { } /** Special print for single-student case */ -void Scheduler::printSpecial(sharedValues assignment) const { - Values::const_iterator it = assignment->begin(); +void Scheduler::printSpecial(const Values& assignment) const { + Values::const_iterator it = assignment.begin(); for (size_t area = 0; area < 3; area++, it++) { size_t f = it->second; cout << setw(12) << studentArea(0, area) << ": " << facultyName_[f] << endl; @@ -230,12 +230,12 @@ void Scheduler::printSpecial(sharedValues assignment) const { } /** Accumulate faculty stats */ -void Scheduler::accumulateStats(sharedValues assignment, +void Scheduler::accumulateStats(const Values& assignment, vector& stats) const { for (size_t s = 0; s < nrStudents(); s++) { Key base = 3 * s; for (size_t area = 0; area < 3; area++) { - size_t f = assignment->at(base + area); + size_t f = assignment.at(base + area); assert(f < stats.size()); stats[f]++; } // area @@ -256,7 +256,7 @@ DiscreteBayesNet::shared_ptr Scheduler::eliminate() const { } /** Find the best total assignment - can be expensive */ -Scheduler::sharedValues Scheduler::optimalAssignment() const { +Scheduler::Values Scheduler::optimalAssignment() const { DiscreteBayesNet::shared_ptr chordal = eliminate(); if (ISDEBUG("Scheduler::optimalAssignment")) { @@ -267,22 +267,21 @@ Scheduler::sharedValues Scheduler::optimalAssignment() const { } gttic(my_optimize); - sharedValues mpe = chordal->optimize(); + Values mpe = chordal->optimize(); gttoc(my_optimize); return mpe; } /** find the assignment of students to slots with most possible committees */ -Scheduler::sharedValues Scheduler::bestSchedule() const { - sharedValues best; +Scheduler::Values Scheduler::bestSchedule() const { + Values best; throw runtime_error("bestSchedule not implemented"); return best; } /** find the corresponding most desirable committee assignment */ -Scheduler::sharedValues Scheduler::bestAssignment( - sharedValues bestSchedule) const { - sharedValues best; +Scheduler::Values Scheduler::bestAssignment(const Values& bestSchedule) const { + Values best; throw runtime_error("bestAssignment not implemented"); return best; } diff --git a/gtsam_unstable/discrete/Scheduler.h b/gtsam_unstable/discrete/Scheduler.h index faf131f5c..08e866efd 100644 --- a/gtsam_unstable/discrete/Scheduler.h +++ b/gtsam_unstable/discrete/Scheduler.h @@ -134,26 +134,26 @@ class GTSAM_UNSTABLE_EXPORT Scheduler : public CSP { const KeyFormatter& formatter = DefaultKeyFormatter) const override; /** Print readable form of assignment */ - void printAssignment(sharedValues assignment) const; + void printAssignment(const Values& assignment) const; /** Special print for single-student case */ - void printSpecial(sharedValues assignment) const; + void printSpecial(const Values& assignment) const; /** Accumulate faculty stats */ - void accumulateStats(sharedValues assignment, + void accumulateStats(const Values& assignment, std::vector& stats) const; /** Eliminate, return a Bayes net */ DiscreteBayesNet::shared_ptr eliminate() const; /** Find the best total assignment - can be expensive */ - sharedValues optimalAssignment() const; + Values optimalAssignment() const; /** find the assignment of students to slots with most possible committees */ - sharedValues bestSchedule() const; + Values bestSchedule() const; /** find the corresponding most desirable committee assignment */ - sharedValues bestAssignment(sharedValues bestSchedule) const; + Values bestAssignment(const Values& bestSchedule) const; }; // Scheduler diff --git a/gtsam_unstable/discrete/examples/schedulingExample.cpp b/gtsam_unstable/discrete/examples/schedulingExample.cpp index e9f63b2d8..cebd9e350 100644 --- a/gtsam_unstable/discrete/examples/schedulingExample.cpp +++ b/gtsam_unstable/discrete/examples/schedulingExample.cpp @@ -122,7 +122,7 @@ void runLargeExample() { // SETDEBUG("timing-verbose", true); SETDEBUG("DiscreteConditional::DiscreteConditional", true); gttic(large); - DiscreteFactor::sharedValues MPE = scheduler.optimalAssignment(); + autoMPE = scheduler.optimalAssignment(); gttoc(large); tictoc_finishedIteration(); tictoc_print(); @@ -331,7 +331,7 @@ void accomodateStudent() { // sample schedules for (size_t n = 0; n < 10; n++) { - Scheduler::sharedValues sample0 = chordal->sample(); + auto sample0 = chordal->sample(); scheduler.printAssignment(sample0); } } diff --git a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp index 1fc4a1459..51eb565af 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp @@ -129,7 +129,7 @@ void runLargeExample() { tictoc_finishedIteration(); tictoc_print(); for (size_t i=0;i<100;i++) { - DiscreteFactor::sharedValues assignment = chordal->sample(); + autoassignment = chordal->sample(); vector stats(scheduler.nrFaculty()); scheduler.accumulateStats(assignment, stats); size_t max = *max_element(stats.begin(), stats.end()); @@ -143,7 +143,7 @@ void runLargeExample() { } #else gttic(large); - DiscreteFactor::sharedValues MPE = scheduler.optimalAssignment(); + autoMPE = scheduler.optimalAssignment(); gttoc(large); tictoc_finishedIteration(); tictoc_print(); diff --git a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp index 95b64f289..9cc21be4b 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp @@ -153,7 +153,7 @@ void runLargeExample() { tictoc_finishedIteration(); tictoc_print(); for (size_t i=0;i<100;i++) { - DiscreteFactor::sharedValues assignment = sample(*chordal); + autoassignment = sample(*chordal); vector stats(scheduler.nrFaculty()); scheduler.accumulateStats(assignment, stats); size_t max = *max_element(stats.begin(), stats.end()); @@ -167,7 +167,7 @@ void runLargeExample() { } #else gttic(large); - DiscreteFactor::sharedValues MPE = scheduler.optimalAssignment(); + autoMPE = scheduler.optimalAssignment(); gttoc(large); tictoc_finishedIteration(); tictoc_print(); diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index 1552fcbf1..e27cd3486 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -73,11 +73,11 @@ TEST_UNSAFE(CSP, allInOne) { EXPECT(assert_equal(expectedProduct, product)); // Solve - CSP::sharedValues mpe = csp.optimalAssignment(); + auto mpe = csp.optimalAssignment(); CSP::Values expected; insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1); - EXPECT(assert_equal(expected, *mpe)); - EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); + EXPECT(assert_equal(expected, mpe)); + EXPECT_DOUBLES_EQUAL(1, csp(mpe), 1e-9); } /* ************************************************************************* */ @@ -120,8 +120,8 @@ TEST_UNSAFE(CSP, WesternUS) { Ordering ordering; ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7), Key(8), Key(9), Key(10); - CSP::sharedValues mpe = csp.optimalAssignment(ordering); - // GTSAM_PRINT(*mpe); + auto mpe = csp.optimalAssignment(ordering); + // GTSAM_PRINT(mpe); CSP::Values expected; insert(expected)(WA.first, 1)(CA.first, 1)(NV.first, 3)(OR.first, 0)( MT.first, 1)(WY.first, 0)(NM.first, 3)(CO.first, 2)(ID.first, 2)( @@ -130,8 +130,8 @@ TEST_UNSAFE(CSP, WesternUS) { // TODO: Fix me! mpe result seems to be right. (See the printing) // It has the same prob as the expected solution. // Is mpe another solution, or the expected solution is unique??? - EXPECT(assert_equal(expected, *mpe)); - EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); + EXPECT(assert_equal(expected, mpe)); + EXPECT_DOUBLES_EQUAL(1, csp(mpe), 1e-9); // Write out the dual graph for hmetis #ifdef DUAL @@ -186,11 +186,11 @@ TEST_UNSAFE(CSP, AllDiff) { EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9); // Solve - CSP::sharedValues mpe = csp.optimalAssignment(); + auto mpe = csp.optimalAssignment(); CSP::Values expected; insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2); - EXPECT(assert_equal(expected, *mpe)); - EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); + EXPECT(assert_equal(expected, mpe)); + EXPECT_DOUBLES_EQUAL(1, csp(mpe), 1e-9); // Arc-consistency vector domains; diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index 4eb86fe1f..7822cbd38 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -122,7 +122,7 @@ TEST(schedulingExample, test) { // Do exact inference gttic(small); - DiscreteFactor::sharedValues MPE = s.optimalAssignment(); + auto MPE = s.optimalAssignment(); gttoc(small); // print MPE, commented out as unit tests don't print @@ -133,13 +133,13 @@ TEST(schedulingExample, test) { // find the assignment of students to slots with most possible committees // Commented out as not implemented yet - // sharedValues bestSchedule = s.bestSchedule(); - // GTSAM_PRINT(*bestSchedule); + // auto bestSchedule = s.bestSchedule(); + // GTSAM_PRINT(bestSchedule); // find the corresponding most desirable committee assignment // Commented out as not implemented yet - // sharedValues bestAssignment = s.bestAssignment(bestSchedule); - // GTSAM_PRINT(*bestAssignment); + // auto bestAssignment = s.bestAssignment(bestSchedule); + // GTSAM_PRINT(bestAssignment); } /* ************************************************************************* */ diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index 4843ae269..cedf900a2 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -88,11 +88,11 @@ class Sudoku : public CSP { } /// Print readable form of assignment - void printAssignment(DiscreteFactor::sharedValues assignment) const { + void printAssignment(const DiscreteFactor::Values& assignment) const { for (size_t i = 0; i < n_; i++) { for (size_t j = 0; j < n_; j++) { Key k = key(i, j); - cout << 1 + assignment->at(k) << " "; + cout << 1 + assignment.at(k) << " "; } cout << endl; } @@ -100,7 +100,7 @@ class Sudoku : public CSP { /// solve and print solution void printSolution() { - DiscreteFactor::sharedValues MPE = optimalAssignment(); + auto MPE = optimalAssignment(); printAssignment(MPE); } }; @@ -117,14 +117,14 @@ TEST_UNSAFE(Sudoku, small) { csp.runArcConsistency(4, 10, PRINT); // optimize and check - CSP::sharedValues solution = csp.optimalAssignment(); + auto solution = csp.optimalAssignment(); CSP::Values expected; insert(expected)(csp.key(0, 0), 0)(csp.key(0, 1), 1)(csp.key(0, 2), 2)( csp.key(0, 3), 3)(csp.key(1, 0), 2)(csp.key(1, 1), 3)(csp.key(1, 2), 0)( csp.key(1, 3), 1)(csp.key(2, 0), 3)(csp.key(2, 1), 2)(csp.key(2, 2), 1)( csp.key(2, 3), 0)(csp.key(3, 0), 1)(csp.key(3, 1), 0)(csp.key(3, 2), 3)( csp.key(3, 3), 2); - EXPECT(assert_equal(expected, *solution)); + EXPECT(assert_equal(expected, solution)); // csp.printAssignment(solution); } From 8206d8d09d3ecd0e7fd5e53ff807c30048536023 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Nov 2021 16:34:44 -0500 Subject: [PATCH 43/71] Got rid of straggling typedefs --- gtsam/discrete/DiscreteConditional.h | 1 - gtsam/discrete/DiscreteFactorGraph.h | 1 - gtsam_unstable/discrete/CSP.h | 1 - 3 files changed, 3 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 40413a9e7..191a80fb0 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -45,7 +45,6 @@ public: /** A map from keys to values.. * TODO: Again, do we need this??? */ typedef Assignment Values; - typedef boost::shared_ptr sharedValues; /// @name Standard Constructors /// @{ diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 87820c9d6..329d015e9 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -74,7 +74,6 @@ public: /** A map from keys to values */ typedef KeyVector Indices; typedef Assignment Values; - typedef boost::shared_ptr sharedValues; /** Default constructor */ DiscreteFactorGraph() {} diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index 8b0af617f..098ef56c9 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -22,7 +22,6 @@ class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { public: /** A map from keys to values */ typedef Assignment Values; - typedef boost::shared_ptr sharedValues; public: /// Add a unary constraint, allowing only a single value From 371fe3e86573abbf2da541aeca4a9abe85ac1a1a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Nov 2021 16:34:53 -0500 Subject: [PATCH 44/71] Fixed all examples --- examples/DiscreteBayesNetExample.cpp | 12 ++++++------ examples/DiscreteBayesNet_FG.cpp | 14 +++++++------- examples/HMMExample.cpp | 8 ++++---- examples/UGM_chain.cpp | 4 ++-- examples/UGM_small.cpp | 4 ++-- .../discrete/examples/schedulingExample.cpp | 4 ++-- .../discrete/examples/schedulingQuals12.cpp | 6 +++--- .../discrete/examples/schedulingQuals13.cpp | 6 +++--- 8 files changed, 29 insertions(+), 29 deletions(-) diff --git a/examples/DiscreteBayesNetExample.cpp b/examples/DiscreteBayesNetExample.cpp index 25259e24e..febc1e128 100644 --- a/examples/DiscreteBayesNetExample.cpp +++ b/examples/DiscreteBayesNetExample.cpp @@ -56,8 +56,8 @@ int main(int argc, char **argv) { DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering); // solve - autompe = chordal->optimize(); - GTSAM_PRINT(*mpe); + auto mpe = chordal->optimize(); + GTSAM_PRINT(mpe); // We can also build a Bayes tree (directed junction tree). // The elimination order above will do fine: @@ -70,14 +70,14 @@ int main(int argc, char **argv) { // solve again, now with evidence DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering); - autompe2 = chordal2->optimize(); - GTSAM_PRINT(*mpe2); + auto mpe2 = chordal2->optimize(); + GTSAM_PRINT(mpe2); // We can also sample from it cout << "\n10 samples:" << endl; for (size_t i = 0; i < 10; i++) { - autosample = chordal2->sample(); - GTSAM_PRINT(*sample); + auto sample = chordal2->sample(); + GTSAM_PRINT(sample); } return 0; } diff --git a/examples/DiscreteBayesNet_FG.cpp b/examples/DiscreteBayesNet_FG.cpp index 7669e567c..69283a1be 100644 --- a/examples/DiscreteBayesNet_FG.cpp +++ b/examples/DiscreteBayesNet_FG.cpp @@ -34,10 +34,10 @@ int main(int argc, char **argv) { // Define keys and a print function Key C(1), S(2), R(3), W(4); auto print = [=](const DiscreteFactor::Values& values) { - cout << boolalpha << "Cloudy = " << static_cast(values[C]) - << " Sprinkler = " << static_cast(values[S]) - << " Rain = " << boolalpha << static_cast(values[R]) - << " WetGrass = " << static_cast(values[W]) << endl; + cout << boolalpha << "Cloudy = " << static_cast(values.at(C)) + << " Sprinkler = " << static_cast(values.at(S)) + << " Rain = " << boolalpha << static_cast(values.at(R)) + << " WetGrass = " << static_cast(values.at(W)) << endl; }; // We assume binary state variables @@ -85,7 +85,7 @@ int main(int argc, char **argv) { } // "Most Probable Explanation", i.e., configuration with largest value - autompe = graph.eliminateSequential()->optimize(); + auto mpe = graph.eliminateSequential()->optimize(); cout << "\nMost Probable Explanation (MPE):" << endl; print(mpe); @@ -97,7 +97,7 @@ int main(int argc, char **argv) { // solve again, now with evidence DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); - autompe_with_evidence = chordal->optimize(); + auto mpe_with_evidence = chordal->optimize(); cout << "\nMPE given C=0:" << endl; print(mpe_with_evidence); @@ -113,7 +113,7 @@ int main(int argc, char **argv) { // We can also sample from it cout << "\n10 samples:" << endl; for (size_t i = 0; i < 10; i++) { - autosample = chordal->sample(); + auto sample = chordal->sample(); print(sample); } return 0; diff --git a/examples/HMMExample.cpp b/examples/HMMExample.cpp index e91f751ad..b46baf4e0 100644 --- a/examples/HMMExample.cpp +++ b/examples/HMMExample.cpp @@ -66,14 +66,14 @@ int main(int argc, char **argv) { chordal->print("Eliminated"); // solve - autompe = chordal->optimize(); - GTSAM_PRINT(*mpe); + auto mpe = chordal->optimize(); + GTSAM_PRINT(mpe); // We can also sample from it cout << "\n10 samples:" << endl; for (size_t k = 0; k < 10; k++) { - autosample = chordal->sample(); - GTSAM_PRINT(*sample); + auto sample = chordal->sample(); + GTSAM_PRINT(sample); } // Or compute the marginals. This re-eliminates the FG into a Bayes tree diff --git a/examples/UGM_chain.cpp b/examples/UGM_chain.cpp index 63d58adaf..ababef022 100644 --- a/examples/UGM_chain.cpp +++ b/examples/UGM_chain.cpp @@ -70,8 +70,8 @@ int main(int argc, char** argv) { // "Decoding", i.e., configuration with largest value // We use sequential variable elimination DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); - autooptimalDecoding = chordal->optimize(); - optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n"); + auto optimalDecoding = chordal->optimize(); + optimalDecoding.print("\nMost Probable Explanation (optimalDecoding)\n"); // "Inference" Computing marginals for each node // Here we'll make use of DiscreteMarginals class, which makes use of diff --git a/examples/UGM_small.cpp b/examples/UGM_small.cpp index 9429c2b2e..f4f3f1fd0 100644 --- a/examples/UGM_small.cpp +++ b/examples/UGM_small.cpp @@ -63,8 +63,8 @@ int main(int argc, char** argv) { // "Decoding", i.e., configuration with largest value (MPE) // We use sequential variable elimination DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); - autooptimalDecoding = chordal->optimize(); - optimalDecoding->print("\noptimalDecoding"); + auto optimalDecoding = chordal->optimize(); + GTSAM_PRINT(optimalDecoding); // "Inference" Computing marginals cout << "\nComputing Node Marginals .." << endl; diff --git a/gtsam_unstable/discrete/examples/schedulingExample.cpp b/gtsam_unstable/discrete/examples/schedulingExample.cpp index cebd9e350..3f270e45d 100644 --- a/gtsam_unstable/discrete/examples/schedulingExample.cpp +++ b/gtsam_unstable/discrete/examples/schedulingExample.cpp @@ -122,7 +122,7 @@ void runLargeExample() { // SETDEBUG("timing-verbose", true); SETDEBUG("DiscreteConditional::DiscreteConditional", true); gttic(large); - autoMPE = scheduler.optimalAssignment(); + auto MPE = scheduler.optimalAssignment(); gttoc(large); tictoc_finishedIteration(); tictoc_print(); @@ -225,7 +225,7 @@ void sampleSolutions() { // now, sample schedules for (size_t n = 0; n < 500; n++) { vector stats(19, 0); - vector samples; + vector samples; for (size_t i = 0; i < 7; i++) { samples.push_back(samplers[i]->sample()); schedulers[i].accumulateStats(samples[i], stats); diff --git a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp index 51eb565af..5ed5766d5 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp @@ -129,7 +129,7 @@ void runLargeExample() { tictoc_finishedIteration(); tictoc_print(); for (size_t i=0;i<100;i++) { - autoassignment = chordal->sample(); + auto assignment = chordal->sample(); vector stats(scheduler.nrFaculty()); scheduler.accumulateStats(assignment, stats); size_t max = *max_element(stats.begin(), stats.end()); @@ -143,7 +143,7 @@ void runLargeExample() { } #else gttic(large); - autoMPE = scheduler.optimalAssignment(); + auto MPE = scheduler.optimalAssignment(); gttoc(large); tictoc_finishedIteration(); tictoc_print(); @@ -234,7 +234,7 @@ void sampleSolutions() { // now, sample schedules for (size_t n = 0; n < 500; n++) { vector stats(19, 0); - vector samples; + vector samples; for (size_t i = 0; i < NRSTUDENTS; i++) { samples.push_back(samplers[i]->sample()); schedulers[i].accumulateStats(samples[i], stats); diff --git a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp index 9cc21be4b..2da0eb9b1 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp @@ -153,7 +153,7 @@ void runLargeExample() { tictoc_finishedIteration(); tictoc_print(); for (size_t i=0;i<100;i++) { - autoassignment = sample(*chordal); + auto assignment = sample(*chordal); vector stats(scheduler.nrFaculty()); scheduler.accumulateStats(assignment, stats); size_t max = *max_element(stats.begin(), stats.end()); @@ -167,7 +167,7 @@ void runLargeExample() { } #else gttic(large); - autoMPE = scheduler.optimalAssignment(); + auto MPE = scheduler.optimalAssignment(); gttoc(large); tictoc_finishedIteration(); tictoc_print(); @@ -259,7 +259,7 @@ void sampleSolutions() { // now, sample schedules for (size_t n = 0; n < 10000; n++) { vector stats(nrFaculty, 0); - vector samples; + vector samples; for (size_t i = 0; i < NRSTUDENTS; i++) { samples.push_back(samplers[i]->sample()); schedulers[i].accumulateStats(samples[i], stats); From 58dafd43e9caf63ade6443015f5e7735a97b77e7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Nov 2021 16:44:17 -0500 Subject: [PATCH 45/71] Fixed up sudoku tests after merge --- gtsam_unstable/discrete/tests/testSudoku.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index a8546bc2f..808f98b1c 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -118,7 +118,7 @@ class Sudoku : public CSP { }; /* ************************************************************************* */ -TEST_UNSAFE(Sudoku, small) { +TEST(Sudoku, small) { Sudoku csp(4, // 1, 0, 0, 4, // 0, 0, 0, 0, // @@ -148,13 +148,13 @@ TEST_UNSAFE(Sudoku, small) { EXPECT_LONGS_EQUAL(16, new_csp.size()); // Check that solution - CSP::sharedValues new_solution = new_csp.optimalAssignment(); + auto new_solution = new_csp.optimalAssignment(); // csp.printAssignment(new_solution); - EXPECT(assert_equal(expected, *new_solution)); + EXPECT(assert_equal(expected, new_solution)); } /* ************************************************************************* */ -TEST_UNSAFE(Sudoku, easy) { +TEST(Sudoku, easy) { Sudoku csp(9, // 0, 0, 5, 0, 9, 0, 0, 0, 1, // 0, 0, 0, 0, 0, 2, 0, 7, 3, // @@ -186,7 +186,7 @@ TEST_UNSAFE(Sudoku, easy) { } /* ************************************************************************* */ -TEST_UNSAFE(Sudoku, extreme) { +TEST(Sudoku, extreme) { Sudoku csp(9, // 0, 0, 9, 7, 4, 8, 0, 0, 0, 7, // 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, // @@ -223,7 +223,7 @@ TEST_UNSAFE(Sudoku, extreme) { } /* ************************************************************************* */ -TEST_UNSAFE(Sudoku, AJC_3star_Feb8_2012) { +TEST(Sudoku, AJC_3star_Feb8_2012) { Sudoku csp(9, // 9, 5, 0, 0, 0, 6, 0, 0, 0, // 0, 8, 4, 0, 7, 0, 0, 0, 0, // @@ -250,9 +250,9 @@ TEST_UNSAFE(Sudoku, AJC_3star_Feb8_2012) { EXPECT_LONGS_EQUAL(81, new_csp.size()); // Check that solution - CSP::sharedValues solution = new_csp.optimalAssignment(); + auto solution = new_csp.optimalAssignment(); // csp.printAssignment(solution); - EXPECT_LONGS_EQUAL(6, solution->at(key99)); + EXPECT_LONGS_EQUAL(6, solution.at(key99)); } /* ************************************************************************* */ From 7fbcdc4d2c373ec86e33e33dce0f87e07a58ec79 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 21 Nov 2021 14:14:42 -0500 Subject: [PATCH 46/71] Add deprecations --- gtsam/base/TestableAssertions.h | 2 +- gtsam/base/Vector.h | 4 ++-- gtsam/base/types.h | 8 ++++++++ gtsam/discrete/AlgebraicDecisionTree.h | 3 ++- gtsam/discrete/DecisionTree-inl.h | 4 ++-- gtsam/discrete/DecisionTree.h | 9 +++++---- gtsam/discrete/DiscreteConditional.cpp | 4 ++-- gtsam/discrete/Potentials.cpp | 4 ++-- gtsam/discrete/Potentials.h | 2 +- gtsam/inference/Conditional.h | 3 ++- gtsam/inference/EliminateableFactorGraph.h | 12 ++++++------ gtsam/inference/FactorGraph.h | 2 +- gtsam/nonlinear/ExpressionFactor.h | 2 +- gtsam/nonlinear/Marginals.h | 6 +++--- gtsam/nonlinear/NonlinearFactorGraph.h | 4 ++-- 15 files changed, 40 insertions(+), 29 deletions(-) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 0e6e1c276..c86fbb6d2 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -85,7 +85,7 @@ bool assert_equal(const V& expected, const boost::optional& actual, do * \deprecated: use container equals instead */ template -bool assert_equal(const std::vector& expected, const std::vector& actual, double tol = 1e-9) { +bool GTSAM_DEPRECATED assert_equal(const std::vector& expected, const std::vector& actual, double tol = 1e-9) { bool match = true; if (expected.size() != actual.size()) match = false; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 35c68c4b4..a057da46b 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -207,14 +207,14 @@ inline double inner_prod(const V1 &a, const V2& b) { * BLAS Level 1 scal: x <- alpha*x * \deprecated: use operators instead */ -inline void scal(double alpha, Vector& x) { x *= alpha; } +inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; } /** * BLAS Level 1 axpy: y <- alpha*x + y * \deprecated: use operators instead */ template -inline void axpy(double alpha, const V1& x, V2& y) { +inline void GTSAM_DEPRECATED axpy(double alpha, const V1& x, V2& y) { assert (y.size()==x.size()); y += alpha * x; } diff --git a/gtsam/base/types.h b/gtsam/base/types.h index aaada3cee..a0d24f1a6 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -34,6 +34,14 @@ #include #endif +#if defined(__GNUC__) || defined(__clang__) +#define GTSAM_DEPRECATED __attribute__((deprecated)) +#elif defined(_MSC_VER) +#define GTSAM_DEPRECATED __declspec(deprecated) +#else +#define GTSAM_DEPRECATED +#endif + #ifdef GTSAM_USE_EIGEN_MKL_OPENMP #include #endif diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 9cc55ed6a..b7bf8f291 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -19,6 +19,7 @@ #pragma once #include +#include namespace gtsam { @@ -28,7 +29,7 @@ namespace gtsam { * TODO: consider eliminating this class altogether? */ template - class AlgebraicDecisionTree: public DecisionTree { + class GTSAM_EXPORT AlgebraicDecisionTree: public DecisionTree { public: diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 439889ebf..b8ec53c96 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -49,7 +49,7 @@ namespace gtsam { // Leaf /*********************************************************************************/ template - class DecisionTree::Leaf: public DecisionTree::Node { + class GTSAM_EXPORT DecisionTree::Leaf: public DecisionTree::Node { /** constant stored in this leaf */ Y constant_; @@ -139,7 +139,7 @@ namespace gtsam { // Choice /*********************************************************************************/ template - class DecisionTree::Choice: public DecisionTree::Node { + class GTSAM_EXPORT DecisionTree::Choice: public DecisionTree::Node { /** the label of the variable on which we split */ L label_; diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 0ee0b8be0..06bc43131 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #include @@ -35,7 +36,7 @@ namespace gtsam { * Y = function range (any algebra), e.g., bool, int, double */ template - class DecisionTree { + class GTSAM_EXPORT DecisionTree { public: @@ -47,11 +48,11 @@ namespace gtsam { typedef std::pair LabelC; /** DTs consist of Leaf and Choice nodes, both subclasses of Node */ - class Leaf; - class Choice; + class GTSAM_EXPORT Leaf; + class GTSAM_EXPORT Choice; /** ------------------------ Node base class --------------------------- */ - class Node { + class GTSAM_EXPORT Node { public: typedef boost::shared_ptr Ptr; diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index ac7c58405..9fb57d0a0 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -35,7 +35,7 @@ using namespace std; namespace gtsam { // Instantiate base class -template class Conditional ; +template class GTSAM_EXPORT Conditional ; /* ******************************************************************************** */ DiscreteConditional::DiscreteConditional(const size_t nrFrontals, @@ -91,7 +91,7 @@ bool DiscreteConditional::equals(const DiscreteFactor& other, return false; else { const DecisionTreeFactor& f( - static_cast(other)); + dynamic_cast(other)); return DecisionTreeFactor::equals(f, tol); } } diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index 331a76c13..2d4f28c06 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -27,8 +27,8 @@ using namespace std; namespace gtsam { // explicit instantiation -template class DecisionTree; -template class AlgebraicDecisionTree; +//template class GTSAM_EXPORT DecisionTree; +//template class GTSAM_EXPORT AlgebraicDecisionTree; /* ************************************************************************* */ double Potentials::safe_div(const double& a, const double& b) { diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 1078b4c61..a0d268789 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -29,7 +29,7 @@ namespace gtsam { /** * A base class for both DiscreteFactor and DiscreteConditional */ - class Potentials: public AlgebraicDecisionTree { + class GTSAM_EXPORT Potentials: public AlgebraicDecisionTree { public: diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 295122879..9fb998456 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -21,6 +21,7 @@ #include #include +#include namespace gtsam { @@ -37,7 +38,7 @@ namespace gtsam { * \nosubgrouping */ template - class Conditional + class GTSAM_EXPORT Conditional { protected: /** The first nrFrontal variables are frontal and the rest are parents. */ diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 3c51d8f84..579eed7f9 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -290,7 +290,7 @@ namespace gtsam { public: #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated ordering and orderingType shouldn't both be specified */ - boost::shared_ptr eliminateSequential( + boost::shared_ptr GTSAM_DEPRECATED eliminateSequential( const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex, @@ -299,7 +299,7 @@ namespace gtsam { } /** \deprecated orderingType specified first for consistency */ - boost::shared_ptr eliminateSequential( + boost::shared_ptr GTSAM_DEPRECATED eliminateSequential( const Eliminate& function, OptionalVariableIndex variableIndex = boost::none, OptionalOrderingType orderingType = boost::none) const { @@ -307,7 +307,7 @@ namespace gtsam { } /** \deprecated ordering and orderingType shouldn't both be specified */ - boost::shared_ptr eliminateMultifrontal( + boost::shared_ptr GTSAM_DEPRECATED eliminateMultifrontal( const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex, @@ -316,7 +316,7 @@ namespace gtsam { } /** \deprecated orderingType specified first for consistency */ - boost::shared_ptr eliminateMultifrontal( + boost::shared_ptr GTSAM_DEPRECATED eliminateMultifrontal( const Eliminate& function, OptionalVariableIndex variableIndex = boost::none, OptionalOrderingType orderingType = boost::none) const { @@ -324,7 +324,7 @@ namespace gtsam { } /** \deprecated */ - boost::shared_ptr marginalMultifrontalBayesNet( + boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesNet( boost::variant variables, boost::none_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate, @@ -333,7 +333,7 @@ namespace gtsam { } /** \deprecated */ - boost::shared_ptr marginalMultifrontalBayesTree( + boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesTree( boost::variant variables, boost::none_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate, diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index e337e3249..7298c4784 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -90,7 +90,7 @@ class CRefCallAddCopy { * \nosubgrouping */ template -class FactorGraph { +class GTSAM_EXPORT FactorGraph { public: typedef FACTOR FactorType; ///< factor type typedef boost::shared_ptr diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index b55d643aa..d22690cf3 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -305,7 +305,7 @@ struct traits> * \deprecated Prefer the more general ExpressionFactorN<>. */ template -class ExpressionFactor2 : public ExpressionFactorN { +class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN { public: /// Destructor ~ExpressionFactor2() override {} diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index e73038db0..947274a97 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -133,15 +133,15 @@ protected: public: #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated argument order changed due to removing boost::optional */ - Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, + GTSAM_DEPRECATED Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} /** \deprecated argument order changed due to removing boost::optional */ - Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, + GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} /** \deprecated argument order changed due to removing boost::optional */ - Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, + GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} #endif diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 5b9ed4bb1..61cbbafb9 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -267,12 +267,12 @@ namespace gtsam { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated */ - boost::shared_ptr linearizeToHessianFactor( + boost::shared_ptr GTSAM_DEPRECATED linearizeToHessianFactor( const Values& values, boost::none_t, const Dampen& dampen = nullptr) const {return linearizeToHessianFactor(values, dampen);} /** \deprecated */ - Values updateCholesky(const Values& values, boost::none_t, + Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t, const Dampen& dampen = nullptr) const {return updateCholesky(values, dampen);} #endif From 6bf6ebc2c06d2c3b002dd9de0dc012e9833b8d9b Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 21 Nov 2021 15:26:38 -0500 Subject: [PATCH 47/71] Fix windows build --- gtsam/discrete/Potentials.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index a0d268789..1078b4c61 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -29,7 +29,7 @@ namespace gtsam { /** * A base class for both DiscreteFactor and DiscreteConditional */ - class GTSAM_EXPORT Potentials: public AlgebraicDecisionTree { + class Potentials: public AlgebraicDecisionTree { public: From bdbd9adc60d5ea49d2a4a624c2908e1edc05d446 Mon Sep 17 00:00:00 2001 From: beetleskin Date: Fri, 19 Nov 2021 14:33:49 +0100 Subject: [PATCH 48/71] Use FastDefaultAllocator for Values to adhere allocation compile flags --- gtsam/nonlinear/Values.h | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 33e9e7d82..207f35540 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -24,6 +24,7 @@ #pragma once +#include #include #include #include @@ -62,17 +63,18 @@ namespace gtsam { class GTSAM_EXPORT Values { private: - // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined - // below) to clone and deallocate the Value objects, and a boost - // fast_pool_allocator to allocate map nodes. In this way, all memory is - // allocated in a boost memory pool. + // below) to clone and deallocate the Value objects, and our compile-flag- + // dependent FastDefaultAllocator to allocate map nodes. In this way, the + // user defines the allocation details (i.e. optimize for memory pool/arenas + // concurrency). + typedef internal::FastDefaultAllocator>::type KeyValuePtrPairAllocator; typedef boost::ptr_map< Key, Value, std::less, ValueCloneAllocator, - boost::fast_pool_allocator > > KeyValueMap; + KeyValuePtrPairAllocator > KeyValueMap; // The member to store the values, see just above KeyValueMap values_; From 05d471cd117a07e5bb72334c41c2262ab66cd54e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 22 Nov 2021 12:01:09 -0500 Subject: [PATCH 49/71] Remove Exports --- gtsam/discrete/AlgebraicDecisionTree.h | 3 +-- gtsam/discrete/DecisionTree-inl.h | 4 ++-- gtsam/discrete/DecisionTree.h | 9 ++++----- gtsam/discrete/DiscreteConditional.cpp | 2 +- gtsam/discrete/Potentials.cpp | 4 ++-- gtsam/inference/Conditional.h | 3 +-- gtsam/inference/FactorGraph.h | 2 +- 7 files changed, 12 insertions(+), 15 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index b7bf8f291..9cc55ed6a 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -19,7 +19,6 @@ #pragma once #include -#include namespace gtsam { @@ -29,7 +28,7 @@ namespace gtsam { * TODO: consider eliminating this class altogether? */ template - class GTSAM_EXPORT AlgebraicDecisionTree: public DecisionTree { + class AlgebraicDecisionTree: public DecisionTree { public: diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index b8ec53c96..439889ebf 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -49,7 +49,7 @@ namespace gtsam { // Leaf /*********************************************************************************/ template - class GTSAM_EXPORT DecisionTree::Leaf: public DecisionTree::Node { + class DecisionTree::Leaf: public DecisionTree::Node { /** constant stored in this leaf */ Y constant_; @@ -139,7 +139,7 @@ namespace gtsam { // Choice /*********************************************************************************/ template - class GTSAM_EXPORT DecisionTree::Choice: public DecisionTree::Node { + class DecisionTree::Choice: public DecisionTree::Node { /** the label of the variable on which we split */ L label_; diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 06bc43131..0ee0b8be0 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -20,7 +20,6 @@ #pragma once #include -#include #include #include @@ -36,7 +35,7 @@ namespace gtsam { * Y = function range (any algebra), e.g., bool, int, double */ template - class GTSAM_EXPORT DecisionTree { + class DecisionTree { public: @@ -48,11 +47,11 @@ namespace gtsam { typedef std::pair LabelC; /** DTs consist of Leaf and Choice nodes, both subclasses of Node */ - class GTSAM_EXPORT Leaf; - class GTSAM_EXPORT Choice; + class Leaf; + class Choice; /** ------------------------ Node base class --------------------------- */ - class GTSAM_EXPORT Node { + class Node { public: typedef boost::shared_ptr Ptr; diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 9fb57d0a0..e3d187303 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -91,7 +91,7 @@ bool DiscreteConditional::equals(const DiscreteFactor& other, return false; else { const DecisionTreeFactor& f( - dynamic_cast(other)); + static_cast(other)); return DecisionTreeFactor::equals(f, tol); } } diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index 2d4f28c06..331a76c13 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -27,8 +27,8 @@ using namespace std; namespace gtsam { // explicit instantiation -//template class GTSAM_EXPORT DecisionTree; -//template class GTSAM_EXPORT AlgebraicDecisionTree; +template class DecisionTree; +template class AlgebraicDecisionTree; /* ************************************************************************* */ double Potentials::safe_div(const double& a, const double& b) { diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 9fb998456..295122879 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -21,7 +21,6 @@ #include #include -#include namespace gtsam { @@ -38,7 +37,7 @@ namespace gtsam { * \nosubgrouping */ template - class GTSAM_EXPORT Conditional + class Conditional { protected: /** The first nrFrontal variables are frontal and the rest are parents. */ diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 7298c4784..e337e3249 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -90,7 +90,7 @@ class CRefCallAddCopy { * \nosubgrouping */ template -class GTSAM_EXPORT FactorGraph { +class FactorGraph { public: typedef FACTOR FactorType; ///< factor type typedef boost::shared_ptr From 8a6b2aadca4c05380a94805f66691025dc0377fe Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Mon, 22 Nov 2021 21:02:32 +0100 Subject: [PATCH 50/71] Removed comments --- gtsam/geometry/Cal3Fisheye.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 7bf9ea300..fd2c7ab65 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -87,28 +87,16 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, const double dr_dxi = xi * rinv; const double dr_dyi = yi * rinv; const double dtd_dr = dtd_dt * dt_dr; - // const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; - // const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; const double c2 = dr_dxi * dr_dxi; const double s2 = dr_dyi * dr_dyi; const double cs = dr_dxi * dr_dyi; - // Following refactoring is numerically stable, even for unnormalized radial - // values by avoiding division with the square radius. - // - // const double td = t * K.dot(T); - note: s = td/r - // const double rrinv = 1 / r2; - division by r2 may cause overflow const double dxd_dxi = dtd_dr * c2 + s * (1 - c2); const double dxd_dyi = (dtd_dr - s) * cs; const double dyd_dxi = dxd_dyi; const double dyd_dyi = dtd_dr * s2 + s * (1 - s2); - // Derivatives by depth, for future use to support incident - // angles above 90 deg. - // const double dxd_dzi = -dtd_dt * x / R2 - // const double dyd_dzi = -dtd_dt * y / R2 - Matrix2 DR; DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; From f828f3cb2522e0e0c039a93259ac8a39905dacf1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 27 Nov 2021 18:14:49 -0500 Subject: [PATCH 51/71] wrap additional ISAM2 methods --- gtsam/nonlinear/nonlinear.i | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index ecf63094d..79b63ff6a 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -738,7 +738,14 @@ class ISAM2 { const gtsam::KeyList& extraReelimKeys, bool force_relinearize); + void marginalizeLeaves( + const gtsam::KeyList& leafKeys, + boost::optional marginalFactorIndices = + boost::none, + boost::optional deletedFactorsIndices = + boost::none); gtsam::Values getLinearizationPoint() const; + bool valueExists(gtsam::Key key) const; gtsam::Values calculateEstimate() const; template Date: Sun, 28 Nov 2021 09:48:50 -0500 Subject: [PATCH 52/71] comment out method --- gtsam/nonlinear/nonlinear.i | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 79b63ff6a..928b3029f 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -611,6 +611,16 @@ class ISAM2GaussNewtonParams { void setWildfireThreshold(double wildfireThreshold); }; +class ISAM2LevenbergMarquardtParams { + ISAM2LevenbergMarquardtParams(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); +}; + class ISAM2DoglegParams { ISAM2DoglegParams(); @@ -738,12 +748,13 @@ class ISAM2 { const gtsam::KeyList& extraReelimKeys, bool force_relinearize); - void marginalizeLeaves( - const gtsam::KeyList& leafKeys, - boost::optional marginalFactorIndices = - boost::none, - boost::optional deletedFactorsIndices = - boost::none); + // void marginalizeLeaves( + // const gtsam::KeyList& leafKeys, + // boost::optional marginalFactorIndices = + // boost::none, + // boost::optional deletedFactorsIndices = + // boost::none); + gtsam::Values getLinearizationPoint() const; bool valueExists(gtsam::Key key) const; gtsam::Values calculateEstimate() const; From 007022df6f1601d2e6152fc910dc3c047b31811d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 28 Nov 2021 22:07:13 -0500 Subject: [PATCH 53/71] make noise model default constructors available for serialization --- gtsam/linear/NoiseModel.h | 42 +++++++++++++++++++-------------------- 1 file changed, 20 insertions(+), 22 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 2fb54d329..6ad444491 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -177,17 +177,16 @@ namespace gtsam { return *sqrt_information_; } - protected: - - /** protected constructor takes square root information matrix */ - Gaussian(size_t dim = 1, const boost::optional& sqrt_information = boost::none) : - Base(dim), sqrt_information_(sqrt_information) { - } public: typedef boost::shared_ptr shared_ptr; + /** constructor takes square root information matrix */ + Gaussian(size_t dim = 1, + const boost::optional& sqrt_information = boost::none) + : Base(dim), sqrt_information_(sqrt_information) {} + ~Gaussian() override {} /** @@ -290,13 +289,13 @@ namespace gtsam { Vector sigmas_, invsigmas_, precisions_; protected: - /** protected constructor - no initializations */ - Diagonal(); /** constructor to allow for disabling initialization of invsigmas */ Diagonal(const Vector& sigmas); public: + /** constructor - no initializations, for serialization */ + Diagonal(); typedef boost::shared_ptr shared_ptr; @@ -387,14 +386,6 @@ namespace gtsam { // Sigmas are contained in the base class Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints - /** - * protected constructor takes sigmas. - * prevents any inf values - * from appearing in invsigmas or precisions. - * mu set to large default value (1000.0) - */ - Constrained(const Vector& sigmas = Z_1x1); - /** * Constructor that prevents any inf values * from appearing in invsigmas or precisions. @@ -406,6 +397,14 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; + /** + * protected constructor takes sigmas. + * prevents any inf values + * from appearing in invsigmas or precisions. + * mu set to large default value (1000.0) + */ + Constrained(const Vector& sigmas = Z_1x1); + ~Constrained() override {} /// true if a constrained noise mode, saves slow/clumsy dynamic casting @@ -531,11 +530,11 @@ namespace gtsam { Isotropic(size_t dim, double sigma) : Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {} + public: + /* dummy constructor to allow for serialization */ Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {} - public: - ~Isotropic() override {} typedef boost::shared_ptr shared_ptr; @@ -592,14 +591,13 @@ namespace gtsam { * Unit: i.i.d. unit-variance noise on all m dimensions. */ class GTSAM_EXPORT Unit : public Isotropic { - protected: - - Unit(size_t dim=1): Isotropic(dim,1.0) {} - public: typedef boost::shared_ptr shared_ptr; + /** constructor for serialization */ + Unit(size_t dim=1): Isotropic(dim,1.0) {} + ~Unit() override {} /** From 247d996a79026251c4678a1ecac90f76ba111666 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 28 Nov 2021 22:08:19 -0500 Subject: [PATCH 54/71] enable pickling for a bunch of objects --- gtsam/linear/linear.i | 15 +++++++++++++++ gtsam/navigation/navigation.i | 24 ++++++++++++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index cf65b1a38..d882cb38b 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -34,6 +34,9 @@ virtual class Gaussian : gtsam::noiseModel::Base { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Diagonal : gtsam::noiseModel::Gaussian { @@ -49,6 +52,9 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Constrained : gtsam::noiseModel::Diagonal { @@ -66,6 +72,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Isotropic : gtsam::noiseModel::Diagonal { @@ -78,6 +87,9 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Unit : gtsam::noiseModel::Isotropic { @@ -85,6 +97,9 @@ virtual class Unit : gtsam::noiseModel::Isotropic { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; namespace mEstimator { diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 7a879c3ef..1f9ffcf2e 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -41,6 +41,12 @@ class ConstantBias { Vector gyroscope() const; Vector correctAccelerometer(Vector measurement) const; Vector correctGyroscope(Vector measurement) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; }///\namespace imuBias @@ -64,6 +70,12 @@ class NavState { gtsam::NavState retract(const Vector& x) const; Vector localCoordinates(const gtsam::NavState& g) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -106,6 +118,12 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { Matrix getAccelerometerCovariance() const; Matrix getIntegrationCovariance() const; bool getUse2ndOrderCoriolis() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -135,6 +153,12 @@ class PreintegratedImuMeasurements { Vector biasHatVector() const; gtsam::NavState predict(const gtsam::NavState& state_i, const gtsam::imuBias::ConstantBias& bias) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; virtual class ImuFactor: gtsam::NonlinearFactor { From 5f071585af49ad65eb2376cc80f39553070fdd6b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 28 Nov 2021 22:13:01 -0500 Subject: [PATCH 55/71] comment out incomplete code --- gtsam/nonlinear/nonlinear.i | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 928b3029f..1f8bbc17d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -611,15 +611,15 @@ class ISAM2GaussNewtonParams { void setWildfireThreshold(double wildfireThreshold); }; -class ISAM2LevenbergMarquardtParams { - ISAM2LevenbergMarquardtParams(); +// class ISAM2LevenbergMarquardtParams { +// ISAM2LevenbergMarquardtParams(); - void print(string s = "") const; +// void print(string s = "") const; - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); -}; +// /** Getters and Setters for all properties */ +// double getWildfireThreshold() const; +// void setWildfireThreshold(double wildfireThreshold); +// }; class ISAM2DoglegParams { ISAM2DoglegParams(); From afadea6c1ddf1ff904e229d8dba259e3181115d1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 13:49:56 -0500 Subject: [PATCH 56/71] remove commented out code --- gtsam/nonlinear/nonlinear.i | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 1f8bbc17d..4d81049ea 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -611,16 +611,6 @@ class ISAM2GaussNewtonParams { void setWildfireThreshold(double wildfireThreshold); }; -// class ISAM2LevenbergMarquardtParams { -// ISAM2LevenbergMarquardtParams(); - -// void print(string s = "") const; - -// /** Getters and Setters for all properties */ -// double getWildfireThreshold() const; -// void setWildfireThreshold(double wildfireThreshold); -// }; - class ISAM2DoglegParams { ISAM2DoglegParams(); @@ -748,13 +738,6 @@ class ISAM2 { const gtsam::KeyList& extraReelimKeys, bool force_relinearize); - // void marginalizeLeaves( - // const gtsam::KeyList& leafKeys, - // boost::optional marginalFactorIndices = - // boost::none, - // boost::optional deletedFactorsIndices = - // boost::none); - gtsam::Values getLinearizationPoint() const; bool valueExists(gtsam::Key key) const; gtsam::Values calculateEstimate() const; From 17630c47402db0503992ad0acde9ac2c10206d39 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 14:00:26 -0500 Subject: [PATCH 57/71] fix all axpy deprecations --- gtsam/base/tests/testVector.cpp | 4 ++-- gtsam/linear/Errors.cpp | 4 ++-- gtsam/linear/Errors.h | 2 +- gtsam/linear/SubgraphPreconditioner.cpp | 4 ++-- gtsam/linear/iterative-inl.h | 4 ++-- gtsam/linear/tests/testErrors.cpp | 2 +- gtsam/nonlinear/DoglegOptimizerImpl.cpp | 3 ++- 7 files changed, 12 insertions(+), 11 deletions(-) diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index bd715e3cb..c87732b09 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -220,8 +220,8 @@ TEST(Vector, axpy ) Vector x = Vector3(10., 20., 30.); Vector y0 = Vector3(2.0, 5.0, 6.0); Vector y1 = y0, y2 = y0; - axpy(0.1,x,y1); - axpy(0.1,x,y2.head(3)); + y1 += 0.1 * x; + y2.head(3) += 0.1 * x; Vector expected = Vector3(3.0, 7.0, 9.0); EXPECT(assert_equal(expected,y1)); EXPECT(assert_equal(expected,Vector(y2))); diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index 3fe2f3307..4b30dcc08 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -111,10 +111,10 @@ double dot(const Errors& a, const Errors& b) { /* ************************************************************************* */ template<> -void axpy(double alpha, const Errors& x, Errors& y) { +void axpy(double alpha, const Errors& x, Errors& y) { Errors::const_iterator it = x.begin(); for(Vector& yi: y) - axpy(alpha,*(it++),yi); + yi += alpha * (*(it++)); } /* ************************************************************************* */ diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index eb844e04d..e8ba7344e 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -66,7 +66,7 @@ namespace gtsam { * BLAS level 2 style */ template <> - GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y); + GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y); /** print with optional string */ GTSAM_EXPORT void print(const Errors& a, const std::string& s = "Error"); diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index fdcb4f7ac..215200818 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -173,7 +173,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd Errors::const_iterator it = e.begin(); for(auto& key_value: y) { const Vector& ei = *it; - axpy(alpha, ei, key_value.second); + key_value.second += alpha * ei; ++it; } transposeMultiplyAdd2(alpha, it, e.end(), y); @@ -191,7 +191,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, VectorValues x = VectorValues::Zero(y); // x = 0 Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 - axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x + y += alpha * Rc1_->backSubstituteTranspose(x); // y += alpha*inv(R1')*x } /* ************************************************************************* */ diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index 58ef7d733..906ee80fd 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -72,7 +72,7 @@ namespace gtsam { double takeOptimalStep(V& x) { // TODO: can we use gamma instead of dot(d,g) ????? Answer not trivial double alpha = -dot(d, g) / dot(Ad, Ad); // calculate optimal step-size - axpy(alpha, d, x); // // do step in new search direction, x += alpha*d + x += alpha * d; // do step in new search direction, x += alpha*d return alpha; } @@ -106,7 +106,7 @@ namespace gtsam { double beta = new_gamma / gamma; // d = g + d*beta; d *= beta; - axpy(1.0, g, d); + d += 1.0 * g; } gamma = new_gamma; diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index 74eef9a2c..f11fb90b9 100644 --- a/gtsam/linear/tests/testErrors.cpp +++ b/gtsam/linear/tests/testErrors.cpp @@ -32,7 +32,7 @@ TEST( Errors, arithmetic ) e += Vector2(1.0,2.0), Vector3(3.0,4.0,5.0); DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9); - axpy(2.0,e,e); + axpy(2.0, e, e); Errors expected; expected += Vector2(3.0,6.0), Vector3(9.0,12.0,15.0); CHECK(assert_equal(expected,e)); diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index c319f26e6..7e9db6b64 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -78,7 +78,8 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double delta, const VectorValues& // Compute blended point if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl; - VectorValues blend = (1. - tau) * x_u; axpy(tau, x_n, blend); + VectorValues blend = (1. - tau) * x_u; + blend += tau * x_n; return blend; } From 2f4218d0460325e5e42f92e2bc3244b21c55b6d6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 14:06:01 -0500 Subject: [PATCH 58/71] update the packaged Eigen to 3.3.9 --- .../3rdparty/Eigen/{.hgignore => .gitignore} | 7 +- gtsam/3rdparty/Eigen/.hgtags | 33 - gtsam/3rdparty/Eigen/CMakeLists.txt | 13 +- gtsam/3rdparty/Eigen/CTestConfig.cmake | 6 +- gtsam/3rdparty/Eigen/Eigen/Core | 7 +- gtsam/3rdparty/Eigen/Eigen/Eigenvalues | 4 +- gtsam/3rdparty/Eigen/Eigen/Geometry | 4 +- gtsam/3rdparty/Eigen/Eigen/QR | 4 +- gtsam/3rdparty/Eigen/Eigen/SparseQR | 1 - .../3rdparty/Eigen/Eigen/src/Core/ArrayBase.h | 4 +- .../Eigen/Eigen/src/Core/CwiseUnaryView.h | 2 + .../3rdparty/Eigen/Eigen/src/Core/DenseBase.h | 7 +- .../Eigen/Eigen/src/Core/DenseStorage.h | 6 +- .../Eigen/Eigen/src/Core/GenericPacketMath.h | 9 +- gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h | 5 + .../Eigen/Eigen/src/Core/MathFunctions.h | 16 +- .../Eigen/Eigen/src/Core/MatrixBase.h | 3 +- .../Eigen/Eigen/src/Core/PermutationMatrix.h | 28 - .../Eigen/Eigen/src/Core/PlainObjectBase.h | 12 +- .../Eigen/Eigen/src/Core/ProductEvaluators.h | 28 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h | 5 +- .../Eigen/Eigen/src/Core/SolveTriangular.h | 6 +- .../3rdparty/Eigen/Eigen/src/Core/Transpose.h | 2 + .../Eigen/Eigen/src/Core/Transpositions.h | 39 - .../Eigen/Eigen/src/Core/TriangularMatrix.h | 8 +- .../src/Core/arch/AVX512/MathFunctions.h | 70 +- .../Eigen/src/Core/arch/AVX512/PacketMath.h | 719 +++++++++--------- .../Eigen/Eigen/src/Core/arch/CUDA/Half.h | 1 + .../Eigen/src/Core/arch/CUDA/PacketMathHalf.h | 4 +- .../Eigen/src/Core/functors/UnaryFunctors.h | 2 +- .../Core/products/GeneralBlockPanelKernel.h | 7 +- .../src/Core/products/GeneralMatrixMatrix.h | 37 +- .../products/GeneralMatrixMatrixTriangular.h | 68 +- .../GeneralMatrixMatrixTriangular_BLAS.h | 10 +- .../Core/products/GeneralMatrixMatrix_BLAS.h | 6 +- .../Eigen/src/Core/products/Parallelizer.h | 9 +- .../Core/products/SelfadjointMatrixMatrix.h | 54 +- .../products/SelfadjointMatrixMatrix_BLAS.h | 24 +- .../src/Core/products/SelfadjointProduct.h | 4 +- .../Core/products/TriangularMatrixMatrix.h | 54 +- .../products/TriangularMatrixMatrix_BLAS.h | 26 +- .../Core/products/TriangularSolverMatrix.h | 62 +- .../products/TriangularSolverMatrix_BLAS.h | 12 +- .../Eigen/Eigen/src/Core/util/BlasUtil.h | 115 ++- .../src/Core/util/DisableStupidWarnings.h | 13 +- .../Eigen/src/Core/util/ForwardDeclarations.h | 6 +- .../Eigen/Eigen/src/Core/util/Macros.h | 58 +- .../3rdparty/Eigen/Eigen/src/Core/util/Meta.h | 34 + .../src/Core/util/ReenableStupidWarnings.h | 6 +- .../Eigen/Eigen/src/Core/util/XprHelper.h | 17 + .../Eigen/src/Eigenvalues/ComplexSchur.h | 9 +- .../Eigen/Eigen/src/Eigenvalues/RealSchur.h | 15 +- .../src/Eigenvalues/SelfAdjointEigenSolver.h | 7 +- .../Eigen/Eigen/src/Geometry/Quaternion.h | 22 +- .../Eigen/Eigen/src/Geometry/Scaling.h | 2 +- .../Eigen/Eigen/src/Geometry/Transform.h | 4 +- .../Eigen/Eigen/src/Geometry/Translation.h | 6 - .../Eigen/Eigen/src/Geometry/Umeyama.h | 2 +- .../Eigen/Eigen/src/LU/PartialPivLU.h | 5 +- .../Eigen/Eigen/src/LU/arch/Inverse_SSE.h | 4 +- .../Eigen/src/PardisoSupport/PardisoSupport.h | 3 +- gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h | 67 +- gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h | 2 +- .../src/SparseCholesky/SimplicialCholesky.h | 2 +- .../SparseCholesky/SimplicialCholesky_impl.h | 2 +- .../Eigen/Eigen/src/SparseCore/AmbiVector.h | 5 +- .../Eigen/src/SparseCore/SparseCwiseUnaryOp.h | 2 + .../Eigen/Eigen/src/SparseCore/SparseMatrix.h | 3 +- .../src/SparseCore/SparseSelfAdjointView.h | 4 +- .../Eigen/Eigen/src/SparseCore/SparseView.h | 1 + .../Eigen/Eigen/src/SparseLU/SparseLU.h | 4 +- .../Eigen/Eigen/src/StlSupport/StdDeque.h | 6 +- .../Eigen/src/plugins/ArrayCwiseBinaryOps.h | 2 +- gtsam/3rdparty/Eigen/bench/bench_gemm.cpp | 5 +- gtsam/3rdparty/Eigen/blas/CMakeLists.txt | 6 +- gtsam/3rdparty/Eigen/blas/level3_impl.h | 182 ++--- gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake | 2 + .../Eigen/cmake/FindStandardMathLibrary.cmake | 7 +- gtsam/3rdparty/Eigen/doc/CMakeLists.txt | 9 +- .../doc/CustomizingEigen_CustomScalar.dox | 2 +- gtsam/3rdparty/Eigen/doc/Doxyfile.in | 17 +- gtsam/3rdparty/Eigen/doc/Pitfalls.dox | 84 +- .../Eigen/doc/SparseQuickReference.dox | 2 +- .../Eigen/doc/TopicLazyEvaluation.dox | 76 +- .../Eigen/doc/TopicMultithreading.dox | 1 + gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox | 4 +- .../3rdparty/Eigen/doc/eigen_navtree_hacks.js | 5 +- .../Eigen/doc/eigendoxy_footer.html.in | 13 - .../Eigen/doc/eigendoxy_header.html.in | 3 + ...orial_BlockOperations_block_assignment.cpp | 2 +- gtsam/3rdparty/Eigen/lapack/CMakeLists.txt | 10 +- gtsam/3rdparty/Eigen/test/CMakeLists.txt | 4 +- .../Eigen/test/{array.cpp => array_cwise.cpp} | 8 +- gtsam/3rdparty/Eigen/test/bdcsvd.cpp | 8 +- gtsam/3rdparty/Eigen/test/constructor.cpp | 14 + gtsam/3rdparty/Eigen/test/ctorleak.cpp | 20 +- .../Eigen/test/eigensolver_generic.cpp | 2 +- gtsam/3rdparty/Eigen/test/exceptions.cpp | 4 +- gtsam/3rdparty/Eigen/test/fastmath.cpp | 34 +- gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp | 3 +- gtsam/3rdparty/Eigen/test/geo_quaternion.cpp | 8 + .../Eigen/test/geo_transformations.cpp | 61 +- gtsam/3rdparty/Eigen/test/inverse.cpp | 17 + gtsam/3rdparty/Eigen/test/main.h | 5 + gtsam/3rdparty/Eigen/test/numext.cpp | 5 +- gtsam/3rdparty/Eigen/test/packetmath.cpp | 17 +- gtsam/3rdparty/Eigen/test/product.h | 26 + gtsam/3rdparty/Eigen/test/product_large.cpp | 2 + gtsam/3rdparty/Eigen/test/product_mmtr.cpp | 10 + gtsam/3rdparty/Eigen/test/product_symm.cpp | 20 +- gtsam/3rdparty/Eigen/test/product_syrk.cpp | 11 + gtsam/3rdparty/Eigen/test/product_trmm.cpp | 12 +- gtsam/3rdparty/Eigen/test/product_trsolve.cpp | 26 + gtsam/3rdparty/Eigen/test/ref.cpp | 12 +- gtsam/3rdparty/Eigen/test/rvalue_types.cpp | 74 +- gtsam/3rdparty/Eigen/test/sparse_basic.cpp | 8 + gtsam/3rdparty/Eigen/test/stddeque.cpp | 16 +- .../3rdparty/Eigen/test/stddeque_overload.cpp | 14 +- gtsam/3rdparty/Eigen/test/stdlist.cpp | 16 +- .../3rdparty/Eigen/test/stdlist_overload.cpp | 14 +- gtsam/3rdparty/Eigen/test/stdvector.cpp | 6 +- .../Eigen/test/stdvector_overload.cpp | 6 +- .../Eigen/test/vectorization_logic.cpp | 8 + .../Eigen/unsupported/Eigen/ArpackSupport | 4 +- .../src/Tensor/TensorContractionMapper.h | 2 + .../CXX11/src/Tensor/TensorDeviceThreadPool.h | 2 +- .../Eigen/CXX11/src/ThreadPool/EventCount.h | 2 +- .../Eigen/CXX11/src/ThreadPool/RunQueue.h | 2 +- .../Eigen/unsupported/Eigen/Polynomials | 4 +- .../Eigen/src/AutoDiff/AutoDiffScalar.h | 28 +- .../ArpackSelfAdjointEigenSolver.h | 21 +- .../unsupported/Eigen/src/FFT/ei_fftw_impl.h | 2 + .../src/MatrixFunctions/MatrixExponential.h | 2 +- .../src/MatrixFunctions/MatrixSquareRoot.h | 18 +- .../Eigen/src/Polynomials/Companion.h | 57 +- .../Eigen/src/Polynomials/PolynomialSolver.h | 46 +- .../unsupported/Eigen/src/Splines/Spline.h | 2 +- .../test/NonLinearOptimization.cpp | 119 ++- .../Eigen/unsupported/test/autodiff.cpp | 16 + .../unsupported/test/matrix_function.cpp | 38 + .../unsupported/test/polynomialsolver.cpp | 54 +- 141 files changed, 1974 insertions(+), 1222 deletions(-) rename gtsam/3rdparty/Eigen/{.hgignore => .gitignore} (83%) delete mode 100644 gtsam/3rdparty/Eigen/.hgtags mode change 100755 => 100644 gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h rename gtsam/3rdparty/Eigen/test/{array.cpp => array_cwise.cpp} (98%) diff --git a/gtsam/3rdparty/Eigen/.hgignore b/gtsam/3rdparty/Eigen/.gitignore similarity index 83% rename from gtsam/3rdparty/Eigen/.hgignore rename to gtsam/3rdparty/Eigen/.gitignore index 769a47f1f..fc0e5486c 100644 --- a/gtsam/3rdparty/Eigen/.hgignore +++ b/gtsam/3rdparty/Eigen/.gitignore @@ -1,4 +1,3 @@ -syntax: glob qrc_*cxx *.orig *.pyc @@ -13,7 +12,7 @@ core core.* *.bak *~ -build* +*build* *.moc.* *.moc ui_* @@ -28,7 +27,11 @@ activity.png *.rej log patch +*.patch a a.* lapack/testing lapack/reference +.*project +.settings +Makefile diff --git a/gtsam/3rdparty/Eigen/.hgtags b/gtsam/3rdparty/Eigen/.hgtags deleted file mode 100644 index 32ec946a2..000000000 --- a/gtsam/3rdparty/Eigen/.hgtags +++ /dev/null @@ -1,33 +0,0 @@ -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 -a8e0d153fc5e239ef8b06e3665f1f9e8cb8d49c8 before-evaluators -09a8e21866106b49c5dec1d6d543e5794e82efa0 3.3-alpha1 -ce5a455b34c0a0ac3545a1497cb4a16c38ed90e8 3.3-beta1 -69d418c0699907bcd0bf9e0b3ba0a112ed091d85 3.3-beta2 -bef509908b9da05d0d07ffc0da105e2c8c6d3996 3.3-rc1 -04ab5fa4b241754afcf631117572276444c67239 3.3-rc2 -26667be4f70baf4f0d39e96f330714c87b399090 3.3.0 -f562a193118d4f40514e2f4a0ace6e974926ef06 3.3.1 -da9b4e14c2550e0d11078a3c39e6d56eba9905df 3.3.2 -67e894c6cd8f5f1f604b27d37ed47fdf012674ff 3.3.3 diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt index 2bfb6d560..dbb9bcf22 100644 --- a/gtsam/3rdparty/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/CMakeLists.txt @@ -391,22 +391,27 @@ endif() if(EIGEN_INCLUDE_INSTALL_DIR AND NOT INCLUDE_INSTALL_DIR) set(INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR} - CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed") + CACHE STRING "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed") else() set(INCLUDE_INSTALL_DIR "${CMAKE_INSTALL_INCLUDEDIR}/eigen3" - CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed" + CACHE STRING "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed" ) endif() set(CMAKEPACKAGE_INSTALL_DIR "${CMAKE_INSTALL_DATADIR}/eigen3/cmake" - CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed" + CACHE STRING "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed" ) set(PKGCONFIG_INSTALL_DIR "${CMAKE_INSTALL_DATADIR}/pkgconfig" - CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed" + CACHE STRING "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed" ) +foreach(var INCLUDE_INSTALL_DIR CMAKEPACKAGE_INSTALL_DIR PKGCONFIG_INSTALL_DIR) + if(IS_ABSOLUTE "${${var}}") + message(FATAL_ERROR "${var} must be relative to CMAKE_PREFIX_PATH. Got: ${${var}}") + endif() +endforeach() # similar to set_target_properties but append the property instead of overwriting it macro(ei_add_target_property target prop value) diff --git a/gtsam/3rdparty/Eigen/CTestConfig.cmake b/gtsam/3rdparty/Eigen/CTestConfig.cmake index 0039bf8ac..45de0e6fc 100644 --- a/gtsam/3rdparty/Eigen/CTestConfig.cmake +++ b/gtsam/3rdparty/Eigen/CTestConfig.cmake @@ -4,10 +4,10 @@ ## # The following are required to uses Dart and the Cdash dashboard ## ENABLE_TESTING() ## INCLUDE(CTest) -set(CTEST_PROJECT_NAME "Eigen 3.3") +set(CTEST_PROJECT_NAME "Eigen") set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC") set(CTEST_DROP_METHOD "http") -set(CTEST_DROP_SITE "manao.inria.fr") -set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen+3.3") +set(CTEST_DROP_SITE "my.cdash.org") +set(CTEST_DROP_LOCATION "/submit.php?project=Eigen") set(CTEST_DROP_SITE_CDASH TRUE) diff --git a/gtsam/3rdparty/Eigen/Eigen/Core b/gtsam/3rdparty/Eigen/Eigen/Core index b923b8c00..ac7c5b300 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Core +++ b/gtsam/3rdparty/Eigen/Eigen/Core @@ -279,7 +279,10 @@ #include #include #include -#include +#include +#ifndef EIGEN_NO_IO + #include +#endif #include #include #include @@ -375,7 +378,9 @@ using std::ptrdiff_t; #if defined EIGEN_VECTORIZE_AVX512 #include "src/Core/arch/SSE/PacketMath.h" + #include "src/Core/arch/SSE/MathFunctions.h" #include "src/Core/arch/AVX/PacketMath.h" + #include "src/Core/arch/AVX/MathFunctions.h" #include "src/Core/arch/AVX512/PacketMath.h" #include "src/Core/arch/AVX512/MathFunctions.h" #elif defined EIGEN_VECTORIZE_AVX diff --git a/gtsam/3rdparty/Eigen/Eigen/Eigenvalues b/gtsam/3rdparty/Eigen/Eigen/Eigenvalues index f3f661b07..7d6ac787b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Eigenvalues +++ b/gtsam/3rdparty/Eigen/Eigen/Eigenvalues @@ -10,14 +10,14 @@ #include "Core" -#include "src/Core/util/DisableStupidWarnings.h" - #include "Cholesky" #include "Jacobi" #include "Householder" #include "LU" #include "Geometry" +#include "src/Core/util/DisableStupidWarnings.h" + /** \defgroup Eigenvalues_Module Eigenvalues module * * diff --git a/gtsam/3rdparty/Eigen/Eigen/Geometry b/gtsam/3rdparty/Eigen/Eigen/Geometry index 716d52952..da88c03bb 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Geometry +++ b/gtsam/3rdparty/Eigen/Eigen/Geometry @@ -10,12 +10,12 @@ #include "Core" -#include "src/Core/util/DisableStupidWarnings.h" - #include "SVD" #include "LU" #include +#include "src/Core/util/DisableStupidWarnings.h" + /** \defgroup Geometry_Module Geometry module * * This module provides support for: diff --git a/gtsam/3rdparty/Eigen/Eigen/QR b/gtsam/3rdparty/Eigen/Eigen/QR index c7e914469..1be1863a1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/QR +++ b/gtsam/3rdparty/Eigen/Eigen/QR @@ -10,12 +10,12 @@ #include "Core" -#include "src/Core/util/DisableStupidWarnings.h" - #include "Cholesky" #include "Jacobi" #include "Householder" +#include "src/Core/util/DisableStupidWarnings.h" + /** \defgroup QR_Module QR module * * diff --git a/gtsam/3rdparty/Eigen/Eigen/SparseQR b/gtsam/3rdparty/Eigen/Eigen/SparseQR index a6f3b7f7d..f5fc5fa7f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/SparseQR +++ b/gtsam/3rdparty/Eigen/Eigen/SparseQR @@ -28,7 +28,6 @@ * */ -#include "OrderingMethods" #include "src/SparseCore/SparseColEtree.h" #include "src/SparseQR/SparseQR.h" diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h index 3dbc7084c..33f644e21 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h @@ -153,8 +153,8 @@ template class ArrayBase // inline void evalTo(Dest& dst) const { dst = matrix(); } protected: - EIGEN_DEVICE_FUNC - ArrayBase() : Base() {} + EIGEN_DEFAULT_COPY_CONSTRUCTOR(ArrayBase) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(ArrayBase) private: explicit ArrayBase(Index); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h index 271033056..5a30fa8df 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h @@ -121,6 +121,8 @@ class CwiseUnaryViewImpl { return derived().nestedExpression().outerStride() * sizeof(typename internal::traits::Scalar) / sizeof(Scalar); } + protected: + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(CwiseUnaryViewImpl) }; } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index 90066ae73..c55a68230 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -40,7 +40,7 @@ static inline void check_DenseIndex_is_signed() { */ template class DenseBase #ifndef EIGEN_PARSED_BY_DOXYGEN - : public DenseCoeffsBase + : public DenseCoeffsBase::value> #else : public DenseCoeffsBase #endif // not EIGEN_PARSED_BY_DOXYGEN @@ -71,7 +71,7 @@ template class DenseBase typedef Scalar value_type; typedef typename NumTraits::Real RealScalar; - typedef DenseCoeffsBase Base; + typedef DenseCoeffsBase::value> Base; using Base::derived; using Base::const_cast_derived; @@ -587,11 +587,12 @@ template class DenseBase } protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(DenseBase) /** Default constructor. Do nothing. */ EIGEN_DEVICE_FUNC DenseBase() { /* Just checks for self-consistency of the flags. - * Only do it when debugging Eigen, as this borders on paranoiac and could slow compilation down + * Only do it when debugging Eigen, as this borders on paranoia and could slow compilation down */ #ifdef EIGEN_INTERNAL_DEBUGGING EIGEN_STATIC_ASSERT((EIGEN_IMPLIES(MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1, int(IsRowMajor)) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h index 7958feeb9..7d6d4e66d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h @@ -404,7 +404,7 @@ template class DenseStorage(m_data, m_rows*m_cols); - if (size) + if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative m_data = internal::conditional_aligned_new_auto(size); else m_data = 0; @@ -479,7 +479,7 @@ template class DenseStorage(m_data, _Rows*m_cols); - if (size) + if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative m_data = internal::conditional_aligned_new_auto(size); else m_data = 0; @@ -553,7 +553,7 @@ template class DenseStorage(m_data, _Cols*m_rows); - if (size) + if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative m_data = internal::conditional_aligned_new_auto(size); else m_data = 0; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h index 029f8ac36..e59443779 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h @@ -351,10 +351,7 @@ template EIGEN_DEVICE_FUNC inline Packet preverse(const Packet& /** \internal \returns \a a with real and imaginary part flipped (for complex type only) */ template EIGEN_DEVICE_FUNC inline Packet pcplxflip(const Packet& 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)); + return Packet(a.imag(),a.real()); } /************************** @@ -524,10 +521,10 @@ inline void palign(PacketType& first, const PacketType& second) #ifndef __CUDACC__ template<> inline std::complex pmul(const std::complex& a, const std::complex& b) -{ return std::complex(real(a)*real(b) - imag(a)*imag(b), imag(a)*real(b) + real(a)*imag(b)); } +{ return std::complex(a.real()*b.real() - a.imag()*b.imag(), a.imag()*b.real() + a.real()*b.imag()); } template<> inline std::complex pmul(const std::complex& a, const std::complex& b) -{ return std::complex(real(a)*real(b) - imag(a)*imag(b), imag(a)*real(b) + real(a)*imag(b)); } +{ return std::complex(a.real()*b.real() - a.imag()*b.imag(), a.imag()*b.real() + a.real()*b.imag()); } #endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index 668922ffc..92c3b2818 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -182,6 +182,8 @@ template class MapBase #endif protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase) template EIGEN_DEVICE_FUNC @@ -294,6 +296,9 @@ template class MapBase // In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base, // see bugs 821 and 920. using ReadOnlyMapBase::Base::operator=; + protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase) }; #undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h index b249ce0c8..01736c2a0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h @@ -287,7 +287,7 @@ struct abs2_impl_default // IsComplex EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { - return real(x)*real(x) + imag(x)*imag(x); + return x.real()*x.real() + x.imag()*x.imag(); } }; @@ -313,14 +313,17 @@ struct abs2_retval ****************************************************************************/ template -struct norm1_default_impl +struct norm1_default_impl; + +template +struct norm1_default_impl { typedef typename NumTraits::Real RealScalar; EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { EIGEN_USING_STD_MATH(abs); - return abs(real(x)) + abs(imag(x)); + return abs(x.real()) + abs(x.imag()); } }; @@ -662,8 +665,8 @@ struct random_default_impl { static inline Scalar run(const Scalar& x, const Scalar& y) { - return Scalar(random(real(x), real(y)), - random(imag(x), imag(y))); + return Scalar(random(x.real(), y.real()), + random(x.imag(), y.imag())); } static inline Scalar run() { @@ -916,6 +919,9 @@ inline EIGEN_MATHFUNC_RETVAL(abs2, Scalar) abs2(const Scalar& x) return EIGEN_MATHFUNC_IMPL(abs2, Scalar)::run(x); } +EIGEN_DEVICE_FUNC +inline bool abs2(bool x) { return x; } + template EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(norm1, Scalar) norm1(const Scalar& x) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h index e6c35907c..f8bcc8c6f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h @@ -464,7 +464,8 @@ template class MatrixBase EIGEN_MATRIX_FUNCTION_1(MatrixComplexPowerReturnValue, pow, power to \c p, const std::complex& p) protected: - EIGEN_DEVICE_FUNC MatrixBase() : Base() {} + EIGEN_DEFAULT_COPY_CONSTRUCTOR(MatrixBase) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MatrixBase) private: EIGEN_DEVICE_FUNC explicit MatrixBase(int); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index b1fb455b9..47c06ba77 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -87,17 +87,6 @@ class PermutationBase : public EigenBase return derived(); } - #ifndef EIGEN_PARSED_BY_DOXYGEN - /** This is a special case of the templated operator=. Its purpose is to - * prevent a default operator= from hiding the templated operator=. - */ - Derived& operator=(const PermutationBase& other) - { - indices() = other.indices(); - return derived(); - } - #endif - /** \returns the number of rows */ inline Index rows() const { return Index(indices().size()); } @@ -333,12 +322,6 @@ class PermutationMatrix : public PermutationBase& other) : m_indices(other.indices()) {} - #ifndef EIGEN_PARSED_BY_DOXYGEN - /** Standard copy constructor. Defined only to prevent a default copy constructor - * from hiding the other templated constructor */ - inline PermutationMatrix(const PermutationMatrix& other) : m_indices(other.indices()) {} - #endif - /** Generic constructor from expression of the indices. The indices * array has the meaning that the permutations sends each integer i to indices[i]. * @@ -373,17 +356,6 @@ class PermutationMatrix : public PermutationBase::type EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init2(Index rows, Index cols, typename internal::enable_if::type* = 0) { - EIGEN_STATIC_ASSERT(bool(NumTraits::IsInteger) && - bool(NumTraits::IsInteger), + const bool t0_is_integer_alike = internal::is_valid_index_type::value; + const bool t1_is_integer_alike = internal::is_valid_index_type::value; + EIGEN_STATIC_ASSERT(t0_is_integer_alike && + t1_is_integer_alike, FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED) resize(rows,cols); } @@ -773,9 +775,9 @@ class PlainObjectBase : public internal::dense_xpr_base::type && ((!internal::is_same::XprKind,ArrayXpr>::value || Base::SizeAtCompileTime==Dynamic)),T>::type* = 0) { // NOTE MSVC 2008 complains if we directly put bool(NumTraits::IsInteger) as the EIGEN_STATIC_ASSERT argument. - const bool is_integer = NumTraits::IsInteger; - EIGEN_UNUSED_VARIABLE(is_integer); - EIGEN_STATIC_ASSERT(is_integer, + const bool is_integer_alike = internal::is_valid_index_type::value; + EIGEN_UNUSED_VARIABLE(is_integer_alike); + EIGEN_STATIC_ASSERT(is_integer_alike, FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED) resize(size); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h index 9b99bd769..bce1310c9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h @@ -396,7 +396,7 @@ struct generic_product_impl // but easier on the compiler side call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::assign_op()); } - + template static EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { @@ -410,6 +410,32 @@ struct generic_product_impl // dst.noalias() -= lhs.lazyProduct(rhs); call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::sub_assign_op()); } + + // Catch "dst {,+,-}= (s*A)*B" and evaluate it lazily by moving out the scalar factor: + // dst {,+,-}= s * (A.lazyProduct(B)) + // This is a huge benefit for heap-allocated matrix types as it save one costly allocation. + // For them, this strategy is also faster than simply by-passing the heap allocation through + // stack allocation. + // For fixed sizes matrices, this is less obvious, it is sometimes x2 faster, but sometimes x3 slower, + // and the behavior depends also a lot on the compiler... so let's be conservative and enable them for dynamic-size only, + // that is when coming from generic_product_impl<...,GemmProduct> in file GeneralMatrixMatrix.h + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + void eval_dynamic(Dst& dst, const CwiseBinaryOp, + const CwiseNullaryOp, Plain1>, Xpr2>& lhs, const Rhs& rhs, const Func &func) + { + call_assignment_no_alias(dst, lhs.lhs().functor().m_other * lhs.rhs().lazyProduct(rhs), func); + } + + // Here, we we always have LhsT==Lhs, but we need to make it a template type to make the above + // overload more specialized. + template + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE + void eval_dynamic(Dst& dst, const LhsT& lhs, const Rhs& rhs, const Func &func) + { + call_assignment_no_alias(dst, lhs.lazyProduct(rhs), func); + } + // template // static inline void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h index 9c6e3c5d9..17a1496b8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -28,12 +28,13 @@ struct traits > template struct match { enum { + IsVectorAtCompileTime = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime, HasDirectAccess = internal::has_direct_access::ret, - StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)), + StorageOrderMatch = 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 + OuterStrideMatch = IsVectorAtCompileTime || int(StrideType::OuterStrideAtCompileTime)==int(Dynamic) || int(StrideType::OuterStrideAtCompileTime)==int(Derived::OuterStrideAtCompileTime), // NOTE, this indirection of evaluator::Alignment is needed // to workaround a very strange bug in MSVC related to the instantiation diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h index 4652e2e19..fd0acb1a5 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h @@ -19,7 +19,7 @@ namespace internal { template struct triangular_solve_vector; -template +template struct triangular_solve_matrix; // small helper struct extracting some traits on the underlying solver operation @@ -98,8 +98,8 @@ struct triangular_solver_selector BlockingType blocking(rhs.rows(), rhs.cols(), size, 1, false); triangular_solve_matrix - ::run(size, othersize, &actualLhs.coeffRef(0,0), actualLhs.outerStride(), &rhs.coeffRef(0,0), rhs.outerStride(), blocking); + (Rhs::Flags&RowMajorBit) ? RowMajor : ColMajor, Rhs::InnerStrideAtCompileTime> + ::run(size, othersize, &actualLhs.coeffRef(0,0), actualLhs.outerStride(), &rhs.coeffRef(0,0), rhs.innerStride(), rhs.outerStride(), blocking); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h index 79b767bcc..960dc4510 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h @@ -146,6 +146,8 @@ template class TransposeImpl { return derived().nestedExpression().coeffRef(index); } + protected: + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(TransposeImpl) }; /** \returns an expression of the transpose of *this. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h index 86da5af59..7718625e8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h @@ -33,17 +33,6 @@ class TranspositionsBase indices() = other.indices(); return derived(); } - - #ifndef EIGEN_PARSED_BY_DOXYGEN - /** This is a special case of the templated operator=. Its purpose is to - * prevent a default operator= from hiding the templated operator=. - */ - Derived& operator=(const TranspositionsBase& other) - { - indices() = other.indices(); - return derived(); - } - #endif /** \returns the number of transpositions */ Index size() const { return indices().size(); } @@ -171,12 +160,6 @@ class Transpositions : public TranspositionsBase& other) : m_indices(other.indices()) {} - #ifndef EIGEN_PARSED_BY_DOXYGEN - /** Standard copy constructor. Defined only to prevent a default copy constructor - * from hiding the other templated constructor */ - inline Transpositions(const Transpositions& other) : m_indices(other.indices()) {} - #endif - /** Generic constructor from expression of the transposition indices. */ template explicit inline Transpositions(const MatrixBase& indices) : m_indices(indices) @@ -189,17 +172,6 @@ class Transpositions : public TranspositionsBase class TriangularView explicit inline TriangularView(MatrixType& matrix) : m_matrix(matrix) {} - using Base::operator=; - TriangularView& operator=(const TriangularView &other) - { return Base::operator=(other); } + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TriangularView) /** \copydoc EigenBase::rows() */ EIGEN_DEVICE_FUNC @@ -544,6 +542,10 @@ template class TriangularViewImpl<_Mat template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TriangularViewType& _assignProduct(const ProductType& prod, const Scalar& alpha, bool beta); + protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(TriangularViewImpl) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(TriangularViewImpl) + }; /*************************************************************************** diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h index 9c1717f76..b259c1e1f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h @@ -29,6 +29,7 @@ namespace internal { #define _EIGEN_DECLARE_CONST_Packet8d_FROM_INT64(NAME, X) \ const Packet8d p8d_##NAME = _mm512_castsi512_pd(_mm512_set1_epi64(X)) + // Natural logarithm // Computes log(x) as log(2^e * m) = C*e + log(m), where the constant C =log(2) // and m is in the range [sqrt(1/2),sqrt(2)). In this range, the logarithm can @@ -47,6 +48,7 @@ plog(const Packet16f& _x) { // The smallest non denormalized float number. _EIGEN_DECLARE_CONST_Packet16f_FROM_INT(min_norm_pos, 0x00800000); _EIGEN_DECLARE_CONST_Packet16f_FROM_INT(minus_inf, 0xff800000); + _EIGEN_DECLARE_CONST_Packet16f_FROM_INT(pos_inf, 0x7f800000); _EIGEN_DECLARE_CONST_Packet16f_FROM_INT(nan, 0x7fc00000); // Polynomial coefficients. @@ -64,11 +66,9 @@ plog(const Packet16f& _x) { _EIGEN_DECLARE_CONST_Packet16f(cephes_log_q2, 0.693359375f); // invalid_mask is set to true when x is NaN - __mmask16 invalid_mask = - _mm512_cmp_ps_mask(x, _mm512_setzero_ps(), _CMP_NGE_UQ); - __mmask16 iszero_mask = - _mm512_cmp_ps_mask(x, _mm512_setzero_ps(), _CMP_EQ_UQ); - + __mmask16 invalid_mask = _mm512_cmp_ps_mask(x, _mm512_setzero_ps(), _CMP_NGE_UQ); + __mmask16 iszero_mask = _mm512_cmp_ps_mask(x, _mm512_setzero_ps(), _CMP_EQ_OQ); + // Truncate input values to the minimum positive normal. x = pmax(x, p16f_min_norm_pos); @@ -118,11 +118,18 @@ plog(const Packet16f& _x) { x = padd(x, y); x = padd(x, y2); - // Filter out invalid inputs, i.e. negative arg will be NAN, 0 will be -INF. + __mmask16 pos_inf_mask = _mm512_cmp_ps_mask(_x,p16f_pos_inf,_CMP_EQ_OQ); + // Filter out invalid inputs, i.e.: + // - negative arg will be NAN, + // - 0 will be -INF. + // - +INF will be +INF return _mm512_mask_blend_ps(iszero_mask, - _mm512_mask_blend_ps(invalid_mask, x, p16f_nan), - p16f_minus_inf); + _mm512_mask_blend_ps(invalid_mask, + _mm512_mask_blend_ps(pos_inf_mask,x,p16f_pos_inf), + p16f_nan), + p16f_minus_inf); } + #endif // Exponential function. Works by writing "x = m*log(2) + r" where @@ -258,48 +265,39 @@ pexp(const Packet8d& _x) { template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet16f psqrt(const Packet16f& _x) { - _EIGEN_DECLARE_CONST_Packet16f(one_point_five, 1.5f); - _EIGEN_DECLARE_CONST_Packet16f(minus_half, -0.5f); - _EIGEN_DECLARE_CONST_Packet16f_FROM_INT(flt_min, 0x00800000); + Packet16f neg_half = pmul(_x, pset1(-.5f)); + __mmask16 denormal_mask = _mm512_kand( + _mm512_cmp_ps_mask(_x, pset1((std::numeric_limits::min)()), + _CMP_LT_OQ), + _mm512_cmp_ps_mask(_x, _mm512_setzero_ps(), _CMP_GE_OQ)); - Packet16f neg_half = pmul(_x, p16f_minus_half); - - // select only the inverse sqrt of positive normal inputs (denormals are - // flushed to zero and cause infs as well). - __mmask16 non_zero_mask = _mm512_cmp_ps_mask(_x, p16f_flt_min, _CMP_GE_OQ); - Packet16f x = _mm512_mask_blend_ps(non_zero_mask, _mm512_setzero_ps(), _mm512_rsqrt14_ps(_x)); + Packet16f x = _mm512_rsqrt14_ps(_x); // Do a single step of Newton's iteration. - x = pmul(x, pmadd(neg_half, pmul(x, x), p16f_one_point_five)); + x = pmul(x, pmadd(neg_half, pmul(x, x), pset1(1.5f))); - // Multiply the original _x by it's reciprocal square root to extract the - // square root. - return pmul(_x, x); + // Flush results for denormals to zero. + return _mm512_mask_blend_ps(denormal_mask, pmul(_x,x), _mm512_setzero_ps()); } template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8d psqrt(const Packet8d& _x) { - _EIGEN_DECLARE_CONST_Packet8d(one_point_five, 1.5); - _EIGEN_DECLARE_CONST_Packet8d(minus_half, -0.5); - _EIGEN_DECLARE_CONST_Packet8d_FROM_INT64(dbl_min, 0x0010000000000000LL); + Packet8d neg_half = pmul(_x, pset1(-.5)); + __mmask16 denormal_mask = _mm512_kand( + _mm512_cmp_pd_mask(_x, pset1((std::numeric_limits::min)()), + _CMP_LT_OQ), + _mm512_cmp_pd_mask(_x, _mm512_setzero_pd(), _CMP_GE_OQ)); - Packet8d neg_half = pmul(_x, p8d_minus_half); + Packet8d x = _mm512_rsqrt14_pd(_x); - // select only the inverse sqrt of positive normal inputs (denormals are - // flushed to zero and cause infs as well). - __mmask8 non_zero_mask = _mm512_cmp_pd_mask(_x, p8d_dbl_min, _CMP_GE_OQ); - Packet8d x = _mm512_mask_blend_pd(non_zero_mask, _mm512_setzero_pd(), _mm512_rsqrt14_pd(_x)); - - // Do a first step of Newton's iteration. - x = pmul(x, pmadd(neg_half, pmul(x, x), p8d_one_point_five)); + // Do a single step of Newton's iteration. + x = pmul(x, pmadd(neg_half, pmul(x, x), pset1(1.5))); // Do a second step of Newton's iteration. - x = pmul(x, pmadd(neg_half, pmul(x, x), p8d_one_point_five)); + x = pmul(x, pmadd(neg_half, pmul(x, x), pset1(1.5))); - // Multiply the original _x by it's reciprocal square root to extract the - // square root. - return pmul(_x, x); + return _mm512_mask_blend_pd(denormal_mask, pmul(_x,x), _mm512_setzero_pd()); } #else template <> diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h index 5adddc7ae..000b7762f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h @@ -19,10 +19,10 @@ namespace internal { #endif #ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS -#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS (2*sizeof(void*)) +#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 32 #endif -#ifdef __FMA__ +#ifdef EIGEN_VECTORIZE_FMA #ifndef EIGEN_HAS_SINGLE_INSTRUCTION_MADD #define EIGEN_HAS_SINGLE_INSTRUCTION_MADD #endif @@ -54,13 +54,14 @@ template<> struct packet_traits : default_packet_traits AlignedOnScalar = 1, size = 16, HasHalfPacket = 1, -#if EIGEN_GNUC_AT_LEAST(5, 3) + HasBlend = 0, +#if EIGEN_GNUC_AT_LEAST(5, 3) || (!EIGEN_COMP_GNUC_STRICT) #ifdef EIGEN_VECTORIZE_AVX512DQ HasLog = 1, #endif HasExp = 1, - HasSqrt = 1, - HasRsqrt = 1, + HasSqrt = EIGEN_FAST_MATH, + HasRsqrt = EIGEN_FAST_MATH, #endif HasDiv = 1 }; @@ -74,8 +75,8 @@ template<> struct packet_traits : default_packet_traits AlignedOnScalar = 1, size = 8, HasHalfPacket = 1, -#if EIGEN_GNUC_AT_LEAST(5, 3) - HasSqrt = 1, +#if EIGEN_GNUC_AT_LEAST(5, 3) || (!EIGEN_COMP_GNUC_STRICT) + HasSqrt = EIGEN_FAST_MATH, HasRsqrt = EIGEN_FAST_MATH, #endif HasDiv = 1 @@ -98,6 +99,7 @@ template <> struct unpacket_traits { typedef float type; typedef Packet8f half; + typedef Packet16i integer_packet; enum { size = 16, alignment=Aligned64 }; }; template <> @@ -132,7 +134,7 @@ EIGEN_STRONG_INLINE Packet16f pload1(const float* from) { } template <> EIGEN_STRONG_INLINE Packet8d pload1(const double* from) { - return _mm512_broadcastsd_pd(_mm_load_pd1(from)); + return _mm512_set1_pd(*from); } template <> @@ -158,6 +160,11 @@ EIGEN_STRONG_INLINE Packet8d padd(const Packet8d& a, const Packet8d& b) { return _mm512_add_pd(a, b); } +template <> +EIGEN_STRONG_INLINE Packet16i padd(const Packet16i& a, + const Packet16i& b) { + return _mm512_add_epi32(a, b); +} template <> EIGEN_STRONG_INLINE Packet16f psub(const Packet16f& a, @@ -169,6 +176,11 @@ EIGEN_STRONG_INLINE Packet8d psub(const Packet8d& a, const Packet8d& b) { return _mm512_sub_pd(a, b); } +template <> +EIGEN_STRONG_INLINE Packet16i psub(const Packet16i& a, + const Packet16i& b) { + return _mm512_sub_epi32(a, b); +} template <> EIGEN_STRONG_INLINE Packet16f pnegate(const Packet16f& a) { @@ -202,6 +214,11 @@ EIGEN_STRONG_INLINE Packet8d pmul(const Packet8d& a, const Packet8d& b) { return _mm512_mul_pd(a, b); } +template <> +EIGEN_STRONG_INLINE Packet16i pmul(const Packet16i& a, + const Packet16i& b) { + return _mm512_mul_epi32(a, b); +} template <> EIGEN_STRONG_INLINE Packet16f pdiv(const Packet16f& a, @@ -214,7 +231,7 @@ EIGEN_STRONG_INLINE Packet8d pdiv(const Packet8d& a, return _mm512_div_pd(a, b); } -#ifdef __FMA__ +#ifdef EIGEN_VECTORIZE_FMA template <> EIGEN_STRONG_INLINE Packet16f pmadd(const Packet16f& a, const Packet16f& b, const Packet16f& c) { @@ -230,23 +247,73 @@ EIGEN_STRONG_INLINE Packet8d pmadd(const Packet8d& a, const Packet8d& b, template <> EIGEN_STRONG_INLINE Packet16f pmin(const Packet16f& a, const Packet16f& b) { - return _mm512_min_ps(a, b); + // Arguments are reversed to match NaN propagation behavior of std::min. + return _mm512_min_ps(b, a); } template <> EIGEN_STRONG_INLINE Packet8d pmin(const Packet8d& a, const Packet8d& b) { - return _mm512_min_pd(a, b); + // Arguments are reversed to match NaN propagation behavior of std::min. + return _mm512_min_pd(b, a); } template <> EIGEN_STRONG_INLINE Packet16f pmax(const Packet16f& a, const Packet16f& b) { - return _mm512_max_ps(a, b); + // Arguments are reversed to match NaN propagation behavior of std::max. + return _mm512_max_ps(b, a); } template <> EIGEN_STRONG_INLINE Packet8d pmax(const Packet8d& a, const Packet8d& b) { - return _mm512_max_pd(a, b); + // Arguments are reversed to match NaN propagation behavior of std::max. + return _mm512_max_pd(b, a); +} + +#ifdef EIGEN_VECTORIZE_AVX512DQ +template EIGEN_STRONG_INLINE Packet8f extract256(Packet16f x) { return _mm512_extractf32x8_ps(x,I_); } +template EIGEN_STRONG_INLINE Packet2d extract128(Packet8d x) { return _mm512_extractf64x2_pd(x,I_); } +EIGEN_STRONG_INLINE Packet16f cat256(Packet8f a, Packet8f b) { return _mm512_insertf32x8(_mm512_castps256_ps512(a),b,1); } +#else +// AVX512F does not define _mm512_extractf32x8_ps to extract _m256 from _m512 +template EIGEN_STRONG_INLINE Packet8f extract256(Packet16f x) { + return _mm256_castsi256_ps(_mm512_extracti64x4_epi64( _mm512_castps_si512(x),I_)); +} + +// AVX512F does not define _mm512_extractf64x2_pd to extract _m128 from _m512 +template EIGEN_STRONG_INLINE Packet2d extract128(Packet8d x) { + return _mm_castsi128_pd(_mm512_extracti32x4_epi32( _mm512_castpd_si512(x),I_)); +} + +EIGEN_STRONG_INLINE Packet16f cat256(Packet8f a, Packet8f b) { + return _mm512_castsi512_ps(_mm512_inserti64x4(_mm512_castsi256_si512(_mm256_castps_si256(a)), + _mm256_castps_si256(b),1)); +} +#endif + +// Helper function for bit packing snippet of low precision comparison. +// It packs the flags from 32x16 to 16x16. +EIGEN_STRONG_INLINE __m256i Pack32To16(Packet16f rf) { + // Split data into small pieces and handle with AVX instructions + // to guarantee internal order of vector. + // Operation: + // dst[15:0] := Saturate16(rf[31:0]) + // dst[31:16] := Saturate16(rf[63:32]) + // ... + // dst[255:240] := Saturate16(rf[255:224]) + __m256i lo = _mm256_castps_si256(extract256<0>(rf)); + __m256i hi = _mm256_castps_si256(extract256<1>(rf)); + __m128i result_lo = _mm_packs_epi32(_mm256_extractf128_si256(lo, 0), + _mm256_extractf128_si256(lo, 1)); + __m128i result_hi = _mm_packs_epi32(_mm256_extractf128_si256(hi, 0), + _mm256_extractf128_si256(hi, 1)); + return _mm256_insertf128_si256(_mm256_castsi128_si256(result_lo), result_hi, 1); +} + +template <> +EIGEN_STRONG_INLINE Packet16i pand(const Packet16i& a, + const Packet16i& b) { + return _mm512_and_si512(a,b); } template <> @@ -255,24 +322,7 @@ EIGEN_STRONG_INLINE Packet16f pand(const Packet16f& a, #ifdef EIGEN_VECTORIZE_AVX512DQ return _mm512_and_ps(a, b); #else - Packet16f res = _mm512_undefined_ps(); - Packet4f lane0_a = _mm512_extractf32x4_ps(a, 0); - Packet4f lane0_b = _mm512_extractf32x4_ps(b, 0); - res = _mm512_insertf32x4(res, _mm_and_ps(lane0_a, lane0_b), 0); - - Packet4f lane1_a = _mm512_extractf32x4_ps(a, 1); - Packet4f lane1_b = _mm512_extractf32x4_ps(b, 1); - res = _mm512_insertf32x4(res, _mm_and_ps(lane1_a, lane1_b), 1); - - Packet4f lane2_a = _mm512_extractf32x4_ps(a, 2); - Packet4f lane2_b = _mm512_extractf32x4_ps(b, 2); - res = _mm512_insertf32x4(res, _mm_and_ps(lane2_a, lane2_b), 2); - - Packet4f lane3_a = _mm512_extractf32x4_ps(a, 3); - Packet4f lane3_b = _mm512_extractf32x4_ps(b, 3); - res = _mm512_insertf32x4(res, _mm_and_ps(lane3_a, lane3_b), 3); - - return res; + return _mm512_castsi512_ps(pand(_mm512_castps_si512(a),_mm512_castps_si512(b))); #endif } template <> @@ -288,35 +338,21 @@ EIGEN_STRONG_INLINE Packet8d pand(const Packet8d& a, Packet4d lane1_a = _mm512_extractf64x4_pd(a, 1); Packet4d lane1_b = _mm512_extractf64x4_pd(b, 1); - res = _mm512_insertf64x4(res, _mm256_and_pd(lane1_a, lane1_b), 1); - - return res; + return _mm512_insertf64x4(res, _mm256_and_pd(lane1_a, lane1_b), 1); #endif } + template <> -EIGEN_STRONG_INLINE Packet16f por(const Packet16f& a, - const Packet16f& b) { +EIGEN_STRONG_INLINE Packet16i por(const Packet16i& a, const Packet16i& b) { + return _mm512_or_si512(a, b); +} + +template <> +EIGEN_STRONG_INLINE Packet16f por(const Packet16f& a, const Packet16f& b) { #ifdef EIGEN_VECTORIZE_AVX512DQ return _mm512_or_ps(a, b); #else - Packet16f res = _mm512_undefined_ps(); - Packet4f lane0_a = _mm512_extractf32x4_ps(a, 0); - Packet4f lane0_b = _mm512_extractf32x4_ps(b, 0); - res = _mm512_insertf32x4(res, _mm_or_ps(lane0_a, lane0_b), 0); - - Packet4f lane1_a = _mm512_extractf32x4_ps(a, 1); - Packet4f lane1_b = _mm512_extractf32x4_ps(b, 1); - res = _mm512_insertf32x4(res, _mm_or_ps(lane1_a, lane1_b), 1); - - Packet4f lane2_a = _mm512_extractf32x4_ps(a, 2); - Packet4f lane2_b = _mm512_extractf32x4_ps(b, 2); - res = _mm512_insertf32x4(res, _mm_or_ps(lane2_a, lane2_b), 2); - - Packet4f lane3_a = _mm512_extractf32x4_ps(a, 3); - Packet4f lane3_b = _mm512_extractf32x4_ps(b, 3); - res = _mm512_insertf32x4(res, _mm_or_ps(lane3_a, lane3_b), 3); - - return res; + return _mm512_castsi512_ps(por(_mm512_castps_si512(a),_mm512_castps_si512(b))); #endif } @@ -326,109 +362,67 @@ EIGEN_STRONG_INLINE Packet8d por(const Packet8d& a, #ifdef EIGEN_VECTORIZE_AVX512DQ return _mm512_or_pd(a, b); #else - Packet8d res = _mm512_undefined_pd(); - Packet4d lane0_a = _mm512_extractf64x4_pd(a, 0); - Packet4d lane0_b = _mm512_extractf64x4_pd(b, 0); - res = _mm512_insertf64x4(res, _mm256_or_pd(lane0_a, lane0_b), 0); - - Packet4d lane1_a = _mm512_extractf64x4_pd(a, 1); - Packet4d lane1_b = _mm512_extractf64x4_pd(b, 1); - res = _mm512_insertf64x4(res, _mm256_or_pd(lane1_a, lane1_b), 1); - - return res; + return _mm512_castsi512_pd(por(_mm512_castpd_si512(a),_mm512_castpd_si512(b))); #endif } template <> -EIGEN_STRONG_INLINE Packet16f pxor(const Packet16f& a, - const Packet16f& b) { +EIGEN_STRONG_INLINE Packet16i pxor(const Packet16i& a, const Packet16i& b) { + return _mm512_xor_si512(a, b); +} + +template <> +EIGEN_STRONG_INLINE Packet16f pxor(const Packet16f& a, const Packet16f& b) { #ifdef EIGEN_VECTORIZE_AVX512DQ return _mm512_xor_ps(a, b); #else - Packet16f res = _mm512_undefined_ps(); - Packet4f lane0_a = _mm512_extractf32x4_ps(a, 0); - Packet4f lane0_b = _mm512_extractf32x4_ps(b, 0); - res = _mm512_insertf32x4(res, _mm_xor_ps(lane0_a, lane0_b), 0); - - Packet4f lane1_a = _mm512_extractf32x4_ps(a, 1); - Packet4f lane1_b = _mm512_extractf32x4_ps(b, 1); - res = _mm512_insertf32x4(res, _mm_xor_ps(lane1_a, lane1_b), 1); - - Packet4f lane2_a = _mm512_extractf32x4_ps(a, 2); - Packet4f lane2_b = _mm512_extractf32x4_ps(b, 2); - res = _mm512_insertf32x4(res, _mm_xor_ps(lane2_a, lane2_b), 2); - - Packet4f lane3_a = _mm512_extractf32x4_ps(a, 3); - Packet4f lane3_b = _mm512_extractf32x4_ps(b, 3); - res = _mm512_insertf32x4(res, _mm_xor_ps(lane3_a, lane3_b), 3); - - return res; + return _mm512_castsi512_ps(pxor(_mm512_castps_si512(a),_mm512_castps_si512(b))); #endif } + template <> -EIGEN_STRONG_INLINE Packet8d pxor(const Packet8d& a, - const Packet8d& b) { +EIGEN_STRONG_INLINE Packet8d pxor(const Packet8d& a, const Packet8d& b) { #ifdef EIGEN_VECTORIZE_AVX512DQ return _mm512_xor_pd(a, b); #else - Packet8d res = _mm512_undefined_pd(); - Packet4d lane0_a = _mm512_extractf64x4_pd(a, 0); - Packet4d lane0_b = _mm512_extractf64x4_pd(b, 0); - res = _mm512_insertf64x4(res, _mm256_xor_pd(lane0_a, lane0_b), 0); - - Packet4d lane1_a = _mm512_extractf64x4_pd(a, 1); - Packet4d lane1_b = _mm512_extractf64x4_pd(b, 1); - res = _mm512_insertf64x4(res, _mm256_xor_pd(lane1_a, lane1_b), 1); - - return res; + return _mm512_castsi512_pd(pxor(_mm512_castpd_si512(a),_mm512_castpd_si512(b))); #endif } template <> -EIGEN_STRONG_INLINE Packet16f pandnot(const Packet16f& a, - const Packet16f& b) { +EIGEN_STRONG_INLINE Packet16i pandnot(const Packet16i& a, const Packet16i& b) { + return _mm512_andnot_si512(b, a); +} + +template <> +EIGEN_STRONG_INLINE Packet16f pandnot(const Packet16f& a, const Packet16f& b) { #ifdef EIGEN_VECTORIZE_AVX512DQ - return _mm512_andnot_ps(a, b); + return _mm512_andnot_ps(b, a); #else - Packet16f res = _mm512_undefined_ps(); - Packet4f lane0_a = _mm512_extractf32x4_ps(a, 0); - Packet4f lane0_b = _mm512_extractf32x4_ps(b, 0); - res = _mm512_insertf32x4(res, _mm_andnot_ps(lane0_a, lane0_b), 0); - - Packet4f lane1_a = _mm512_extractf32x4_ps(a, 1); - Packet4f lane1_b = _mm512_extractf32x4_ps(b, 1); - res = _mm512_insertf32x4(res, _mm_andnot_ps(lane1_a, lane1_b), 1); - - Packet4f lane2_a = _mm512_extractf32x4_ps(a, 2); - Packet4f lane2_b = _mm512_extractf32x4_ps(b, 2); - res = _mm512_insertf32x4(res, _mm_andnot_ps(lane2_a, lane2_b), 2); - - Packet4f lane3_a = _mm512_extractf32x4_ps(a, 3); - Packet4f lane3_b = _mm512_extractf32x4_ps(b, 3); - res = _mm512_insertf32x4(res, _mm_andnot_ps(lane3_a, lane3_b), 3); - - return res; + return _mm512_castsi512_ps(pandnot(_mm512_castps_si512(a),_mm512_castps_si512(b))); #endif } template <> -EIGEN_STRONG_INLINE Packet8d pandnot(const Packet8d& a, - const Packet8d& b) { +EIGEN_STRONG_INLINE Packet8d pandnot(const Packet8d& a,const Packet8d& b) { #ifdef EIGEN_VECTORIZE_AVX512DQ - return _mm512_andnot_pd(a, b); + return _mm512_andnot_pd(b, a); #else - Packet8d res = _mm512_undefined_pd(); - Packet4d lane0_a = _mm512_extractf64x4_pd(a, 0); - Packet4d lane0_b = _mm512_extractf64x4_pd(b, 0); - res = _mm512_insertf64x4(res, _mm256_andnot_pd(lane0_a, lane0_b), 0); - - Packet4d lane1_a = _mm512_extractf64x4_pd(a, 1); - Packet4d lane1_b = _mm512_extractf64x4_pd(b, 1); - res = _mm512_insertf64x4(res, _mm256_andnot_pd(lane1_a, lane1_b), 1); - - return res; + return _mm512_castsi512_pd(pandnot(_mm512_castpd_si512(a),_mm512_castpd_si512(b))); #endif } +template EIGEN_STRONG_INLINE Packet16i parithmetic_shift_right(Packet16i a) { + return _mm512_srai_epi32(a, N); +} + +template EIGEN_STRONG_INLINE Packet16i plogical_shift_right(Packet16i a) { + return _mm512_srli_epi32(a, N); +} + +template EIGEN_STRONG_INLINE Packet16i plogical_shift_left(Packet16i a) { + return _mm512_slli_epi32(a, N); +} + template <> EIGEN_STRONG_INLINE Packet16f pload(const float* from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm512_load_ps(from); @@ -461,75 +455,55 @@ EIGEN_STRONG_INLINE Packet16i ploadu(const int* from) { // {a0, a0 a1, a1, a2, a2, a3, a3, a4, a4, a5, a5, a6, a6, a7, a7} template <> EIGEN_STRONG_INLINE Packet16f ploaddup(const float* from) { - Packet8f lane0 = _mm256_broadcast_ps((const __m128*)(const void*)from); - // mimic an "inplace" permutation of the lower 128bits using a blend - lane0 = _mm256_blend_ps( - lane0, _mm256_castps128_ps256(_mm_permute_ps( - _mm256_castps256_ps128(lane0), _MM_SHUFFLE(1, 0, 1, 0))), - 15); - // then we can perform a consistent permutation on the global register to get - // everything in shape: - lane0 = _mm256_permute_ps(lane0, _MM_SHUFFLE(3, 3, 2, 2)); - - Packet8f lane1 = _mm256_broadcast_ps((const __m128*)(const void*)(from + 4)); - // mimic an "inplace" permutation of the lower 128bits using a blend - lane1 = _mm256_blend_ps( - lane1, _mm256_castps128_ps256(_mm_permute_ps( - _mm256_castps256_ps128(lane1), _MM_SHUFFLE(1, 0, 1, 0))), - 15); - // then we can perform a consistent permutation on the global register to get - // everything in shape: - lane1 = _mm256_permute_ps(lane1, _MM_SHUFFLE(3, 3, 2, 2)); + // an unaligned load is required here as there is no requirement + // on the alignment of input pointer 'from' + __m256i low_half = _mm256_loadu_si256(reinterpret_cast(from)); + __m512 even_elements = _mm512_castsi512_ps(_mm512_cvtepu32_epi64(low_half)); + __m512 pairs = _mm512_permute_ps(even_elements, _MM_SHUFFLE(2, 2, 0, 0)); + return pairs; +} #ifdef EIGEN_VECTORIZE_AVX512DQ - Packet16f res = _mm512_undefined_ps(); - return _mm512_insertf32x8(res, lane0, 0); - return _mm512_insertf32x8(res, lane1, 1); - return res; -#else - Packet16f res = _mm512_undefined_ps(); - res = _mm512_insertf32x4(res, _mm256_extractf128_ps(lane0, 0), 0); - res = _mm512_insertf32x4(res, _mm256_extractf128_ps(lane0, 1), 1); - res = _mm512_insertf32x4(res, _mm256_extractf128_ps(lane1, 0), 2); - res = _mm512_insertf32x4(res, _mm256_extractf128_ps(lane1, 1), 3); - return res; -#endif -} +// FIXME: this does not look optimal, better load a Packet4d and shuffle... // Loads 4 doubles from memory a returns the packet {a0, a0 a1, a1, a2, a2, a3, // a3} template <> EIGEN_STRONG_INLINE Packet8d ploaddup(const double* from) { - Packet4d lane0 = _mm256_broadcast_pd((const __m128d*)(const void*)from); - lane0 = _mm256_permute_pd(lane0, 3 << 2); - - Packet4d lane1 = _mm256_broadcast_pd((const __m128d*)(const void*)(from + 2)); - lane1 = _mm256_permute_pd(lane1, 3 << 2); - - Packet8d res = _mm512_undefined_pd(); - res = _mm512_insertf64x4(res, lane0, 0); - return _mm512_insertf64x4(res, lane1, 1); + __m512d x = _mm512_setzero_pd(); + x = _mm512_insertf64x2(x, _mm_loaddup_pd(&from[0]), 0); + x = _mm512_insertf64x2(x, _mm_loaddup_pd(&from[1]), 1); + x = _mm512_insertf64x2(x, _mm_loaddup_pd(&from[2]), 2); + x = _mm512_insertf64x2(x, _mm_loaddup_pd(&from[3]), 3); + return x; } +#else +template <> +EIGEN_STRONG_INLINE Packet8d ploaddup(const double* from) { + __m512d x = _mm512_setzero_pd(); + x = _mm512_mask_broadcastsd_pd(x, 0x3<<0, _mm_load_sd(from+0)); + x = _mm512_mask_broadcastsd_pd(x, 0x3<<2, _mm_load_sd(from+1)); + x = _mm512_mask_broadcastsd_pd(x, 0x3<<4, _mm_load_sd(from+2)); + x = _mm512_mask_broadcastsd_pd(x, 0x3<<6, _mm_load_sd(from+3)); + return x; +} +#endif // Loads 4 floats from memory a returns the packet // {a0, a0 a0, a0, a1, a1, a1, a1, a2, a2, a2, a2, a3, a3, a3, a3} template <> EIGEN_STRONG_INLINE Packet16f ploadquad(const float* from) { - Packet16f tmp = _mm512_undefined_ps(); - tmp = _mm512_insertf32x4(tmp, _mm_load_ps1(from), 0); - tmp = _mm512_insertf32x4(tmp, _mm_load_ps1(from + 1), 1); - tmp = _mm512_insertf32x4(tmp, _mm_load_ps1(from + 2), 2); - tmp = _mm512_insertf32x4(tmp, _mm_load_ps1(from + 3), 3); - return tmp; + Packet16f tmp = _mm512_castps128_ps512(ploadu(from)); + const Packet16i scatter_mask = _mm512_set_epi32(3,3,3,3, 2,2,2,2, 1,1,1,1, 0,0,0,0); + return _mm512_permutexvar_ps(scatter_mask, tmp); } + // Loads 2 doubles from memory a returns the packet // {a0, a0 a0, a0, a1, a1, a1, a1} template <> EIGEN_STRONG_INLINE Packet8d ploadquad(const double* from) { - Packet8d tmp = _mm512_undefined_pd(); - Packet2d tmp0 = _mm_load_pd1(from); - Packet2d tmp1 = _mm_load_pd1(from + 1); - Packet4d lane0 = _mm256_broadcastsd_pd(tmp0); - Packet4d lane1 = _mm256_broadcastsd_pd(tmp1); + __m256d lane0 = _mm256_set1_pd(*from); + __m256d lane1 = _mm256_set1_pd(*(from+1)); + __m512d tmp = _mm512_undefined_pd(); tmp = _mm512_insertf64x4(tmp, lane0, 0); return _mm512_insertf64x4(tmp, lane1, 1); } @@ -565,7 +539,7 @@ EIGEN_STRONG_INLINE void pstoreu(int* to, const Packet16i& from) { template <> EIGEN_DEVICE_FUNC inline Packet16f pgather(const float* from, Index stride) { - Packet16i stride_vector = _mm512_set1_epi32(stride); + Packet16i stride_vector = _mm512_set1_epi32(convert_index(stride)); Packet16i stride_multiplier = _mm512_set_epi32(15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0); Packet16i indices = _mm512_mullo_epi32(stride_vector, stride_multiplier); @@ -575,7 +549,7 @@ EIGEN_DEVICE_FUNC inline Packet16f pgather(const float* from, template <> EIGEN_DEVICE_FUNC inline Packet8d pgather(const double* from, Index stride) { - Packet8i stride_vector = _mm256_set1_epi32(stride); + Packet8i stride_vector = _mm256_set1_epi32(convert_index(stride)); Packet8i stride_multiplier = _mm256_set_epi32(7, 6, 5, 4, 3, 2, 1, 0); Packet8i indices = _mm256_mullo_epi32(stride_vector, stride_multiplier); @@ -586,7 +560,7 @@ template <> EIGEN_DEVICE_FUNC inline void pscatter(float* to, const Packet16f& from, Index stride) { - Packet16i stride_vector = _mm512_set1_epi32(stride); + Packet16i stride_vector = _mm512_set1_epi32(convert_index(stride)); Packet16i stride_multiplier = _mm512_set_epi32(15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0); Packet16i indices = _mm512_mullo_epi32(stride_vector, stride_multiplier); @@ -596,7 +570,7 @@ template <> EIGEN_DEVICE_FUNC inline void pscatter(double* to, const Packet8d& from, Index stride) { - Packet8i stride_vector = _mm256_set1_epi32(stride); + Packet8i stride_vector = _mm256_set1_epi32(convert_index(stride)); Packet8i stride_multiplier = _mm256_set_epi32(7, 6, 5, 4, 3, 2, 1, 0); Packet8i indices = _mm256_mullo_epi32(stride_vector, stride_multiplier); _mm512_i32scatter_pd(to, indices, from, 8); @@ -660,8 +634,8 @@ EIGEN_STRONG_INLINE Packet8d pabs(const Packet8d& a) { #ifdef EIGEN_VECTORIZE_AVX512DQ // AVX512F does not define _mm512_extractf32x8_ps to extract _m256 from _m512 #define EIGEN_EXTRACT_8f_FROM_16f(INPUT, OUTPUT) \ - __m256 OUTPUT##_0 = _mm512_extractf32x8_ps(INPUT, 0) __m256 OUTPUT##_1 = \ - _mm512_extractf32x8_ps(INPUT, 1) + __m256 OUTPUT##_0 = _mm512_extractf32x8_ps(INPUT, 0); \ + __m256 OUTPUT##_1 = _mm512_extractf32x8_ps(INPUT, 1) #else #define EIGEN_EXTRACT_8f_FROM_16f(INPUT, OUTPUT) \ __m256 OUTPUT##_0 = _mm256_insertf128_ps( \ @@ -674,17 +648,136 @@ EIGEN_STRONG_INLINE Packet8d pabs(const Packet8d& a) { #ifdef EIGEN_VECTORIZE_AVX512DQ #define EIGEN_INSERT_8f_INTO_16f(OUTPUT, INPUTA, INPUTB) \ - OUTPUT = _mm512_insertf32x8(OUTPUT, INPUTA, 0); \ - OUTPUT = _mm512_insertf32x8(OUTPUT, INPUTB, 1); + OUTPUT = _mm512_insertf32x8(_mm512_castps256_ps512(INPUTA), INPUTB, 1); #else #define EIGEN_INSERT_8f_INTO_16f(OUTPUT, INPUTA, INPUTB) \ + OUTPUT = _mm512_undefined_ps(); \ OUTPUT = _mm512_insertf32x4(OUTPUT, _mm256_extractf128_ps(INPUTA, 0), 0); \ OUTPUT = _mm512_insertf32x4(OUTPUT, _mm256_extractf128_ps(INPUTA, 1), 1); \ OUTPUT = _mm512_insertf32x4(OUTPUT, _mm256_extractf128_ps(INPUTB, 0), 2); \ OUTPUT = _mm512_insertf32x4(OUTPUT, _mm256_extractf128_ps(INPUTB, 1), 3); #endif -template<> EIGEN_STRONG_INLINE Packet16f preduxp(const Packet16f* -vecs) + +template <> +EIGEN_STRONG_INLINE float predux(const Packet16f& a) { +#ifdef EIGEN_VECTORIZE_AVX512DQ + __m256 lane0 = _mm512_extractf32x8_ps(a, 0); + __m256 lane1 = _mm512_extractf32x8_ps(a, 1); + Packet8f x = _mm256_add_ps(lane0, lane1); + return predux(x); +#else + __m128 lane0 = _mm512_extractf32x4_ps(a, 0); + __m128 lane1 = _mm512_extractf32x4_ps(a, 1); + __m128 lane2 = _mm512_extractf32x4_ps(a, 2); + __m128 lane3 = _mm512_extractf32x4_ps(a, 3); + __m128 sum = _mm_add_ps(_mm_add_ps(lane0, lane1), _mm_add_ps(lane2, lane3)); + sum = _mm_hadd_ps(sum, sum); + sum = _mm_hadd_ps(sum, _mm_permute_ps(sum, 1)); + return _mm_cvtss_f32(sum); +#endif +} +template <> +EIGEN_STRONG_INLINE double predux(const Packet8d& a) { + __m256d lane0 = _mm512_extractf64x4_pd(a, 0); + __m256d lane1 = _mm512_extractf64x4_pd(a, 1); + __m256d sum = _mm256_add_pd(lane0, lane1); + __m256d tmp0 = _mm256_hadd_pd(sum, _mm256_permute2f128_pd(sum, sum, 1)); + return _mm_cvtsd_f64(_mm256_castpd256_pd128(_mm256_hadd_pd(tmp0, tmp0))); +} + +template <> +EIGEN_STRONG_INLINE Packet8f predux_downto4(const Packet16f& a) { +#ifdef EIGEN_VECTORIZE_AVX512DQ + Packet8f lane0 = _mm512_extractf32x8_ps(a, 0); + Packet8f lane1 = _mm512_extractf32x8_ps(a, 1); + return padd(lane0, lane1); +#else + Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); + Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); + Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); + Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); + Packet4f sum0 = padd(lane0, lane2); + Packet4f sum1 = padd(lane1, lane3); + return _mm256_insertf128_ps(_mm256_castps128_ps256(sum0), sum1, 1); +#endif +} +template <> +EIGEN_STRONG_INLINE Packet4d predux_downto4(const Packet8d& a) { + Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); + Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); + Packet4d res = padd(lane0, lane1); + return res; +} + +template <> +EIGEN_STRONG_INLINE float predux_mul(const Packet16f& a) { +//#ifdef EIGEN_VECTORIZE_AVX512DQ +#if 0 + Packet8f lane0 = _mm512_extractf32x8_ps(a, 0); + Packet8f lane1 = _mm512_extractf32x8_ps(a, 1); + Packet8f res = pmul(lane0, lane1); + res = pmul(res, _mm256_permute2f128_ps(res, res, 1)); + res = pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); + return pfirst(pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); +#else + __m128 lane0 = _mm512_extractf32x4_ps(a, 0); + __m128 lane1 = _mm512_extractf32x4_ps(a, 1); + __m128 lane2 = _mm512_extractf32x4_ps(a, 2); + __m128 lane3 = _mm512_extractf32x4_ps(a, 3); + __m128 res = pmul(pmul(lane0, lane1), pmul(lane2, lane3)); + res = pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); + return pfirst(pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); +#endif +} +template <> +EIGEN_STRONG_INLINE double predux_mul(const Packet8d& a) { + __m256d lane0 = _mm512_extractf64x4_pd(a, 0); + __m256d lane1 = _mm512_extractf64x4_pd(a, 1); + __m256d res = pmul(lane0, lane1); + res = pmul(res, _mm256_permute2f128_pd(res, res, 1)); + return pfirst(pmul(res, _mm256_shuffle_pd(res, res, 1))); +} + +template <> +EIGEN_STRONG_INLINE float predux_min(const Packet16f& a) { + __m128 lane0 = _mm512_extractf32x4_ps(a, 0); + __m128 lane1 = _mm512_extractf32x4_ps(a, 1); + __m128 lane2 = _mm512_extractf32x4_ps(a, 2); + __m128 lane3 = _mm512_extractf32x4_ps(a, 3); + __m128 res = _mm_min_ps(_mm_min_ps(lane0, lane1), _mm_min_ps(lane2, lane3)); + res = _mm_min_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); + return pfirst(_mm_min_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); +} +template <> +EIGEN_STRONG_INLINE double predux_min(const Packet8d& a) { + __m256d lane0 = _mm512_extractf64x4_pd(a, 0); + __m256d lane1 = _mm512_extractf64x4_pd(a, 1); + __m256d res = _mm256_min_pd(lane0, lane1); + res = _mm256_min_pd(res, _mm256_permute2f128_pd(res, res, 1)); + return pfirst(_mm256_min_pd(res, _mm256_shuffle_pd(res, res, 1))); +} + +template <> +EIGEN_STRONG_INLINE float predux_max(const Packet16f& a) { + __m128 lane0 = _mm512_extractf32x4_ps(a, 0); + __m128 lane1 = _mm512_extractf32x4_ps(a, 1); + __m128 lane2 = _mm512_extractf32x4_ps(a, 2); + __m128 lane3 = _mm512_extractf32x4_ps(a, 3); + __m128 res = _mm_max_ps(_mm_max_ps(lane0, lane1), _mm_max_ps(lane2, lane3)); + res = _mm_max_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); + return pfirst(_mm_max_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); +} + +template <> +EIGEN_STRONG_INLINE double predux_max(const Packet8d& a) { + __m256d lane0 = _mm512_extractf64x4_pd(a, 0); + __m256d lane1 = _mm512_extractf64x4_pd(a, 1); + __m256d res = _mm256_max_pd(lane0, lane1); + res = _mm256_max_pd(res, _mm256_permute2f128_pd(res, res, 1)); + return pfirst(_mm256_max_pd(res, _mm256_shuffle_pd(res, res, 1))); +} + +template<> EIGEN_STRONG_INLINE Packet16f preduxp(const Packet16f* vecs) { EIGEN_EXTRACT_8f_FROM_16f(vecs[0], vecs0); EIGEN_EXTRACT_8f_FROM_16f(vecs[1], vecs1); @@ -873,174 +966,7 @@ template<> EIGEN_STRONG_INLINE Packet8d preduxp(const Packet8d* vecs) return _mm512_insertf64x4(final_output, final_1, 1); } - -template <> -EIGEN_STRONG_INLINE float predux(const Packet16f& a) { - //#ifdef EIGEN_VECTORIZE_AVX512DQ -#if 0 - Packet8f lane0 = _mm512_extractf32x8_ps(a, 0); - Packet8f lane1 = _mm512_extractf32x8_ps(a, 1); - Packet8f sum = padd(lane0, lane1); - Packet8f tmp0 = _mm256_hadd_ps(sum, _mm256_permute2f128_ps(a, a, 1)); - tmp0 = _mm256_hadd_ps(tmp0, tmp0); - return pfirst(_mm256_hadd_ps(tmp0, tmp0)); -#else - Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); - Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); - Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); - Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); - Packet4f sum = padd(padd(lane0, lane1), padd(lane2, lane3)); - sum = _mm_hadd_ps(sum, sum); - sum = _mm_hadd_ps(sum, _mm_permute_ps(sum, 1)); - return pfirst(sum); -#endif -} -template <> -EIGEN_STRONG_INLINE double predux(const Packet8d& a) { - Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); - Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); - Packet4d sum = padd(lane0, lane1); - Packet4d tmp0 = _mm256_hadd_pd(sum, _mm256_permute2f128_pd(sum, sum, 1)); - return pfirst(_mm256_hadd_pd(tmp0, tmp0)); -} - -template <> -EIGEN_STRONG_INLINE Packet8f predux_downto4(const Packet16f& a) { -#ifdef EIGEN_VECTORIZE_AVX512DQ - Packet8f lane0 = _mm512_extractf32x8_ps(a, 0); - Packet8f lane1 = _mm512_extractf32x8_ps(a, 1); - return padd(lane0, lane1); -#else - Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); - Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); - Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); - Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); - Packet4f sum0 = padd(lane0, lane2); - Packet4f sum1 = padd(lane1, lane3); - return _mm256_insertf128_ps(_mm256_castps128_ps256(sum0), sum1, 1); -#endif -} -template <> -EIGEN_STRONG_INLINE Packet4d predux_downto4(const Packet8d& a) { - Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); - Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); - Packet4d res = padd(lane0, lane1); - return res; -} - -template <> -EIGEN_STRONG_INLINE float predux_mul(const Packet16f& a) { -//#ifdef EIGEN_VECTORIZE_AVX512DQ -#if 0 - Packet8f lane0 = _mm512_extractf32x8_ps(a, 0); - Packet8f lane1 = _mm512_extractf32x8_ps(a, 1); - Packet8f res = pmul(lane0, lane1); - res = pmul(res, _mm256_permute2f128_ps(res, res, 1)); - res = pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); - return pfirst(pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); -#else - Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); - Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); - Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); - Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); - Packet4f res = pmul(pmul(lane0, lane1), pmul(lane2, lane3)); - res = pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); - return pfirst(pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); -#endif -} -template <> -EIGEN_STRONG_INLINE double predux_mul(const Packet8d& a) { - Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); - Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); - Packet4d res = pmul(lane0, lane1); - res = pmul(res, _mm256_permute2f128_pd(res, res, 1)); - return pfirst(pmul(res, _mm256_shuffle_pd(res, res, 1))); -} - -template <> -EIGEN_STRONG_INLINE float predux_min(const Packet16f& a) { - Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); - Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); - Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); - Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); - Packet4f res = _mm_min_ps(_mm_min_ps(lane0, lane1), _mm_min_ps(lane2, lane3)); - res = _mm_min_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); - return pfirst(_mm_min_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); -} -template <> -EIGEN_STRONG_INLINE double predux_min(const Packet8d& a) { - Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); - Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); - Packet4d res = _mm256_min_pd(lane0, lane1); - res = _mm256_min_pd(res, _mm256_permute2f128_pd(res, res, 1)); - return pfirst(_mm256_min_pd(res, _mm256_shuffle_pd(res, res, 1))); -} - -template <> -EIGEN_STRONG_INLINE float predux_max(const Packet16f& a) { - Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); - Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); - Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); - Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); - Packet4f res = _mm_max_ps(_mm_max_ps(lane0, lane1), _mm_max_ps(lane2, lane3)); - res = _mm_max_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); - return pfirst(_mm_max_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); -} -template <> -EIGEN_STRONG_INLINE double predux_max(const Packet8d& a) { - Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); - Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); - Packet4d res = _mm256_max_pd(lane0, lane1); - res = _mm256_max_pd(res, _mm256_permute2f128_pd(res, res, 1)); - return pfirst(_mm256_max_pd(res, _mm256_shuffle_pd(res, res, 1))); -} - -template -struct palign_impl { - static EIGEN_STRONG_INLINE void run(Packet16f& first, - const Packet16f& second) { - if (Offset != 0) { - __m512i first_idx = _mm512_set_epi32( - Offset + 15, Offset + 14, Offset + 13, Offset + 12, Offset + 11, - Offset + 10, Offset + 9, Offset + 8, Offset + 7, Offset + 6, - Offset + 5, Offset + 4, Offset + 3, Offset + 2, Offset + 1, Offset); - - __m512i second_idx = - _mm512_set_epi32(Offset - 1, Offset - 2, Offset - 3, Offset - 4, - Offset - 5, Offset - 6, Offset - 7, Offset - 8, - Offset - 9, Offset - 10, Offset - 11, Offset - 12, - Offset - 13, Offset - 14, Offset - 15, Offset - 16); - - unsigned short mask = 0xFFFF; - mask <<= (16 - Offset); - - first = _mm512_permutexvar_ps(first_idx, first); - Packet16f tmp = _mm512_permutexvar_ps(second_idx, second); - first = _mm512_mask_blend_ps(mask, first, tmp); - } - } -}; -template -struct palign_impl { - static EIGEN_STRONG_INLINE void run(Packet8d& first, const Packet8d& second) { - if (Offset != 0) { - __m512i first_idx = _mm512_set_epi32( - 0, Offset + 7, 0, Offset + 6, 0, Offset + 5, 0, Offset + 4, 0, - Offset + 3, 0, Offset + 2, 0, Offset + 1, 0, Offset); - - __m512i second_idx = _mm512_set_epi32( - 0, Offset - 1, 0, Offset - 2, 0, Offset - 3, 0, Offset - 4, 0, - Offset - 5, 0, Offset - 6, 0, Offset - 7, 0, Offset - 8); - - unsigned char mask = 0xFF; - mask <<= (8 - Offset); - - first = _mm512_permutexvar_pd(first_idx, first); - Packet8d tmp = _mm512_permutexvar_pd(second_idx, second); - first = _mm512_mask_blend_pd(mask, first, tmp); - } - } -}; + #define PACK_OUTPUT(OUTPUT, INPUT, INDEX, STRIDE) \ @@ -1302,13 +1228,76 @@ EIGEN_STRONG_INLINE Packet16f pblend(const Selector<16>& /*ifPacket*/, return Packet16f(); } template <> -EIGEN_STRONG_INLINE Packet8d pblend(const Selector<8>& /*ifPacket*/, - const Packet8d& /*thenPacket*/, - const Packet8d& /*elsePacket*/) { - assert(false && "To be implemented"); - return Packet8d(); +EIGEN_STRONG_INLINE Packet8d pblend(const Selector<8>& ifPacket, + const Packet8d& thenPacket, + const Packet8d& elsePacket) { + __mmask8 m = (ifPacket.select[0] ) + | (ifPacket.select[1]<<1) + | (ifPacket.select[2]<<2) + | (ifPacket.select[3]<<3) + | (ifPacket.select[4]<<4) + | (ifPacket.select[5]<<5) + | (ifPacket.select[6]<<6) + | (ifPacket.select[7]<<7); + return _mm512_mask_blend_pd(m, elsePacket, thenPacket); } +template<> EIGEN_STRONG_INLINE Packet16i pcast(const Packet16f& a) { + return _mm512_cvttps_epi32(a); +} + +template<> EIGEN_STRONG_INLINE Packet16f pcast(const Packet16i& a) { + return _mm512_cvtepi32_ps(a); +} + +template +struct palign_impl { + static EIGEN_STRONG_INLINE void run(Packet16f& first, + const Packet16f& second) { + if (Offset != 0) { + __m512i first_idx = _mm512_set_epi32( + Offset + 15, Offset + 14, Offset + 13, Offset + 12, Offset + 11, + Offset + 10, Offset + 9, Offset + 8, Offset + 7, Offset + 6, + Offset + 5, Offset + 4, Offset + 3, Offset + 2, Offset + 1, Offset); + + __m512i second_idx = + _mm512_set_epi32(Offset - 1, Offset - 2, Offset - 3, Offset - 4, + Offset - 5, Offset - 6, Offset - 7, Offset - 8, + Offset - 9, Offset - 10, Offset - 11, Offset - 12, + Offset - 13, Offset - 14, Offset - 15, Offset - 16); + + unsigned short mask = 0xFFFF; + mask <<= (16 - Offset); + + first = _mm512_permutexvar_ps(first_idx, first); + Packet16f tmp = _mm512_permutexvar_ps(second_idx, second); + first = _mm512_mask_blend_ps(mask, first, tmp); + } + } +}; +template +struct palign_impl { + static EIGEN_STRONG_INLINE void run(Packet8d& first, const Packet8d& second) { + if (Offset != 0) { + __m512i first_idx = _mm512_set_epi32( + 0, Offset + 7, 0, Offset + 6, 0, Offset + 5, 0, Offset + 4, 0, + Offset + 3, 0, Offset + 2, 0, Offset + 1, 0, Offset); + + __m512i second_idx = _mm512_set_epi32( + 0, Offset - 1, 0, Offset - 2, 0, Offset - 3, 0, Offset - 4, 0, + Offset - 5, 0, Offset - 6, 0, Offset - 7, 0, Offset - 8); + + unsigned char mask = 0xFF; + mask <<= (8 - Offset); + + first = _mm512_permutexvar_pd(first_idx, first); + Packet8d tmp = _mm512_permutexvar_pd(second_idx, second); + first = _mm512_mask_blend_pd(mask, first, tmp); + } + } +}; + + } // end namespace internal } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h index 755e6209d..59717b4fe 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h @@ -42,6 +42,7 @@ #define EIGEN_EXPLICIT_CAST(tgt_type) operator tgt_type() #endif +#include namespace Eigen { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h index c66d38469..f749c573f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h @@ -230,7 +230,7 @@ template<> __device__ EIGEN_STRONG_INLINE Eigen::half predux(const half2& #else float a1 = __low2float(a); float a2 = __high2float(a); - return Eigen::half(half_impl::raw_uint16_to_half(__float2half_rn(a1 + a2))); + return Eigen::half(__float2half_rn(a1 + a2)); #endif } @@ -264,7 +264,7 @@ template<> __device__ EIGEN_STRONG_INLINE Eigen::half predux_mul(const ha #else float a1 = __low2float(a); float a2 = __high2float(a); - return Eigen::half(half_impl::raw_uint16_to_half(__float2half_rn(a1 * a2))); + return Eigen::half(__float2half_rn(a1 * a2)); #endif } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/UnaryFunctors.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/UnaryFunctors.h index 2e6a00ffd..b56e7afd2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/UnaryFunctors.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/UnaryFunctors.h @@ -768,7 +768,7 @@ struct scalar_sign_op { if (aa==real_type(0)) return Scalar(0); aa = real_type(1)/aa; - return Scalar(real(a)*aa, imag(a)*aa ); + return Scalar(a.real()*aa, a.imag()*aa ); } //TODO //template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h index e3980f6ff..681451cc3 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h @@ -115,7 +115,8 @@ void evaluateProductBlockingSizesHeuristic(Index& k, Index& m, Index& n, Index n // registers. However once the latency is hidden there is no point in // increasing the value of k, so we'll cap it at 320 (value determined // experimentally). - const Index k_cache = (numext::mini)((l1-ksub)/kdiv, 320); + // To avoid that k vanishes, we make k_cache at least as big as kr + const Index k_cache = numext::maxi(kr, (numext::mini)((l1-ksub)/kdiv, 320)); if (k_cache < k) { k = k_cache - (k_cache % kr); eigen_internal_assert(k > 0); @@ -648,8 +649,8 @@ public: // Vectorized path EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, DoublePacketType& dest) const { - dest.first = pset1(real(*b)); - dest.second = pset1(imag(*b)); + dest.first = pset1(numext::real(*b)); + dest.second = pset1(numext::imag(*b)); } EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, ResPacket& dest) const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h index 6440e1d09..ed6234c37 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h @@ -20,8 +20,9 @@ template class level3_blocking; template< typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs, - typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs> -struct general_matrix_matrix_product + typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride> +struct general_matrix_matrix_product { typedef gebp_traits Traits; @@ -30,7 +31,7 @@ struct general_matrix_matrix_product& blocking, GemmParallelInfo* info = 0) @@ -39,8 +40,8 @@ struct general_matrix_matrix_product - ::run(cols,rows,depth,rhs,rhsStride,lhs,lhsStride,res,resStride,alpha,blocking,info); + ColMajor,ResInnerStride> + ::run(cols,rows,depth,rhs,rhsStride,lhs,lhsStride,res,resIncr,resStride,alpha,blocking,info); } }; @@ -49,8 +50,9 @@ struct general_matrix_matrix_product -struct general_matrix_matrix_product + typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride> +struct general_matrix_matrix_product { typedef gebp_traits Traits; @@ -59,17 +61,17 @@ typedef typename ScalarBinaryOpTraits::ReturnType ResScala static void run(Index rows, Index cols, Index depth, const LhsScalar* _lhs, Index lhsStride, const RhsScalar* _rhs, Index rhsStride, - ResScalar* _res, Index resStride, + ResScalar* _res, Index resIncr, Index resStride, ResScalar alpha, level3_blocking& blocking, GemmParallelInfo* info = 0) { typedef const_blas_data_mapper LhsMapper; typedef const_blas_data_mapper RhsMapper; - typedef blas_data_mapper ResMapper; - LhsMapper lhs(_lhs,lhsStride); - RhsMapper rhs(_rhs,rhsStride); - ResMapper res(_res, resStride); + typedef blas_data_mapper ResMapper; + LhsMapper lhs(_lhs, lhsStride); + RhsMapper rhs(_rhs, rhsStride); + ResMapper res(_res, resStride, resIncr); Index kc = blocking.kc(); // cache block size along the K direction Index mc = (std::min)(rows,blocking.mc()); // cache block size along the M direction @@ -226,7 +228,7 @@ struct gemm_functor Gemm::run(rows, cols, m_lhs.cols(), &m_lhs.coeffRef(row,0), m_lhs.outerStride(), &m_rhs.coeffRef(0,col), m_rhs.outerStride(), - (Scalar*)&(m_dest.coeffRef(row,col)), m_dest.outerStride(), + (Scalar*)&(m_dest.coeffRef(row,col)), m_dest.innerStride(), m_dest.outerStride(), m_actualAlpha, m_blocking, info); } @@ -428,7 +430,7 @@ struct generic_product_impl static void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { if((rhs.rows()+dst.rows()+dst.cols())<20 && rhs.rows()>0) - lazyproduct::evalTo(dst, lhs, rhs); + lazyproduct::eval_dynamic(dst, lhs, rhs, internal::assign_op()); else { dst.setZero(); @@ -440,7 +442,7 @@ struct generic_product_impl static void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { if((rhs.rows()+dst.rows()+dst.cols())<20 && rhs.rows()>0) - lazyproduct::addTo(dst, lhs, rhs); + lazyproduct::eval_dynamic(dst, lhs, rhs, internal::add_assign_op()); else scaleAndAddTo(dst,lhs, rhs, Scalar(1)); } @@ -449,7 +451,7 @@ struct generic_product_impl static void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { if((rhs.rows()+dst.rows()+dst.cols())<20 && rhs.rows()>0) - lazyproduct::subTo(dst, lhs, rhs); + lazyproduct::eval_dynamic(dst, lhs, rhs, internal::sub_assign_op()); else scaleAndAddTo(dst, lhs, rhs, Scalar(-1)); } @@ -476,7 +478,8 @@ struct generic_product_impl Index, LhsScalar, (ActualLhsTypeCleaned::Flags&RowMajorBit) ? RowMajor : ColMajor, bool(LhsBlasTraits::NeedToConjugate), RhsScalar, (ActualRhsTypeCleaned::Flags&RowMajorBit) ? RowMajor : ColMajor, bool(RhsBlasTraits::NeedToConjugate), - (Dest::Flags&RowMajorBit) ? RowMajor : ColMajor>, + (Dest::Flags&RowMajorBit) ? RowMajor : ColMajor, + Dest::InnerStrideAtCompileTime>, ActualLhsTypeCleaned, ActualRhsTypeCleaned, Dest, BlockingType> GemmFunctor; BlockingType blocking(dst.rows(), dst.cols(), lhs.cols(), 1, true); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h index e844e37d1..d68d2f965 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h @@ -25,51 +25,54 @@ namespace internal { **********************************************************************/ // forward declarations (defined at the end of this file) -template +template struct tribb_kernel; /* Optimized matrix-matrix product evaluating only one triangular half */ template + int ResStorageOrder, int ResInnerStride, int UpLo, int Version = Specialized> struct general_matrix_matrix_triangular_product; // as usual if the result is row major => we transpose the product template -struct general_matrix_matrix_triangular_product + typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride, int UpLo, int Version> +struct general_matrix_matrix_triangular_product { typedef typename ScalarBinaryOpTraits::ReturnType ResScalar; static EIGEN_STRONG_INLINE void run(Index size, Index depth,const LhsScalar* lhs, Index lhsStride, - const RhsScalar* rhs, Index rhsStride, ResScalar* res, Index resStride, + const RhsScalar* rhs, Index rhsStride, ResScalar* res, Index resIncr, Index resStride, const ResScalar& alpha, level3_blocking& blocking) { general_matrix_matrix_triangular_product - ::run(size,depth,rhs,rhsStride,lhs,lhsStride,res,resStride,alpha,blocking); + ColMajor, ResInnerStride, UpLo==Lower?Upper:Lower> + ::run(size,depth,rhs,rhsStride,lhs,lhsStride,res,resIncr,resStride,alpha,blocking); } }; template -struct general_matrix_matrix_triangular_product + typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride, int UpLo, int Version> +struct general_matrix_matrix_triangular_product { typedef typename ScalarBinaryOpTraits::ReturnType ResScalar; static EIGEN_STRONG_INLINE void run(Index size, Index depth,const LhsScalar* _lhs, Index lhsStride, - const RhsScalar* _rhs, Index rhsStride, ResScalar* _res, Index resStride, + const RhsScalar* _rhs, Index rhsStride, + ResScalar* _res, Index resIncr, Index resStride, const ResScalar& alpha, level3_blocking& blocking) { typedef gebp_traits Traits; typedef const_blas_data_mapper LhsMapper; typedef const_blas_data_mapper RhsMapper; - typedef blas_data_mapper ResMapper; + typedef blas_data_mapper ResMapper; LhsMapper lhs(_lhs,lhsStride); RhsMapper rhs(_rhs,rhsStride); - ResMapper res(_res, resStride); + ResMapper res(_res, resStride, resIncr); Index kc = blocking.kc(); Index mc = (std::min)(size,blocking.mc()); @@ -87,7 +90,7 @@ struct general_matrix_matrix_triangular_product pack_lhs; gemm_pack_rhs pack_rhs; gebp_kernel gebp; - tribb_kernel sybb; + tribb_kernel sybb; for(Index k2=0; k2 +template struct tribb_kernel { typedef gebp_traits Traits; @@ -142,11 +144,13 @@ struct tribb_kernel enum { BlockSize = meta_least_common_multiple::ret }; - void operator()(ResScalar* _res, Index resStride, const LhsScalar* blockA, const RhsScalar* blockB, Index size, Index depth, const ResScalar& alpha) + void operator()(ResScalar* _res, Index resIncr, Index resStride, const LhsScalar* blockA, const RhsScalar* blockB, Index size, Index depth, const ResScalar& alpha) { - typedef blas_data_mapper ResMapper; - ResMapper res(_res, resStride); - gebp_kernel gebp_kernel; + typedef blas_data_mapper ResMapper; + typedef blas_data_mapper BufferMapper; + ResMapper res(_res, resStride, resIncr); + gebp_kernel gebp_kernel1; + gebp_kernel gebp_kernel2; Matrix buffer((internal::constructor_without_unaligned_array_assert())); @@ -158,31 +162,32 @@ struct tribb_kernel const RhsScalar* actual_b = blockB+j*depth; if(UpLo==Upper) - gebp_kernel(res.getSubMapper(0, j), blockA, actual_b, j, depth, actualBlockSize, alpha, - -1, -1, 0, 0); - + gebp_kernel1(res.getSubMapper(0, j), blockA, actual_b, j, depth, actualBlockSize, alpha, + -1, -1, 0, 0); + // selfadjoint micro block { Index i = j; buffer.setZero(); // 1 - apply the kernel on the temporary buffer - gebp_kernel(ResMapper(buffer.data(), BlockSize), blockA+depth*i, actual_b, actualBlockSize, depth, actualBlockSize, alpha, - -1, -1, 0, 0); + gebp_kernel2(BufferMapper(buffer.data(), BlockSize), blockA+depth*i, actual_b, actualBlockSize, depth, actualBlockSize, alpha, + -1, -1, 0, 0); + // 2 - triangular accumulation for(Index j1=0; j1 internal::general_matrix_matrix_triangular_product + IsRowMajor ? RowMajor : ColMajor, MatrixType::InnerStrideAtCompileTime, UpLo&(Lower|Upper)> ::run(size, depth, &actualLhs.coeffRef(SkipDiag&&(UpLo&Lower)==Lower ? 1 : 0,0), actualLhs.outerStride(), &actualRhs.coeffRef(0,SkipDiag&&(UpLo&Upper)==Upper ? 1 : 0), actualRhs.outerStride(), - mat.data() + (SkipDiag ? (bool(IsRowMajor) != ((UpLo&Lower)==Lower) ? 1 : mat.outerStride() ) : 0), mat.outerStride(), actualAlpha, blocking); + mat.data() + (SkipDiag ? (bool(IsRowMajor) != ((UpLo&Lower)==Lower) ? mat.innerStride() : mat.outerStride() ) : 0), + mat.innerStride(), mat.outerStride(), actualAlpha, blocking); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h index f6f9ebeca..691f95d69 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h @@ -40,7 +40,7 @@ namespace internal { template struct general_matrix_matrix_rankupdate : general_matrix_matrix_triangular_product< - Index,Scalar,AStorageOrder,ConjugateA,Scalar,AStorageOrder,ConjugateA,ResStorageOrder,UpLo,BuiltIn> {}; + Index,Scalar,AStorageOrder,ConjugateA,Scalar,AStorageOrder,ConjugateA,ResStorageOrder,1,UpLo,BuiltIn> {}; // try to go to BLAS specialization @@ -48,9 +48,9 @@ struct general_matrix_matrix_rankupdate : template \ struct general_matrix_matrix_triangular_product { \ + Scalar,RhsStorageOrder,ConjugateRhs,ColMajor,1,UpLo,Specialized> { \ static EIGEN_STRONG_INLINE void run(Index size, Index depth,const Scalar* lhs, Index lhsStride, \ - const Scalar* rhs, Index rhsStride, Scalar* res, Index resStride, Scalar alpha, level3_blocking& blocking) \ + const Scalar* rhs, Index rhsStride, Scalar* res, Index resIncr, Index resStride, Scalar alpha, level3_blocking& blocking) \ { \ if ( lhs==rhs && ((UpLo&(Lower|Upper))==UpLo) ) { \ general_matrix_matrix_rankupdate \ @@ -59,8 +59,8 @@ struct general_matrix_matrix_triangular_product \ - ::run(size,depth,lhs,lhsStride,rhs,rhsStride,res,resStride,alpha,blocking); \ + ColMajor, 1, UpLo, BuiltIn> \ + ::run(size,depth,lhs,lhsStride,rhs,rhsStride,res,resIncr,resStride,alpha,blocking); \ } \ } \ }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h index b0f6b0d5b..71abf4013 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h @@ -51,20 +51,22 @@ template< \ typename Index, \ int LhsStorageOrder, bool ConjugateLhs, \ int RhsStorageOrder, bool ConjugateRhs> \ -struct general_matrix_matrix_product \ +struct general_matrix_matrix_product \ { \ typedef gebp_traits Traits; \ \ static void run(Index rows, Index cols, Index depth, \ const EIGTYPE* _lhs, Index lhsStride, \ const EIGTYPE* _rhs, Index rhsStride, \ - EIGTYPE* res, Index resStride, \ + EIGTYPE* res, Index resIncr, Index resStride, \ EIGTYPE alpha, \ level3_blocking& /*blocking*/, \ GemmParallelInfo* /*info = 0*/) \ { \ using std::conj; \ \ + EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ + eigen_assert(resIncr == 1); \ char transa, transb; \ BlasIndex m, n, k, lda, ldb, ldc; \ const EIGTYPE *a, *b; \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h index c2f084c82..a3cc05b77 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h @@ -17,7 +17,8 @@ namespace internal { /** \internal */ inline void manage_multi_threading(Action action, int* v) { - static EIGEN_UNUSED int m_maxThreads = -1; + static int m_maxThreads = -1; + EIGEN_UNUSED_VARIABLE(m_maxThreads); if(action==SetAction) { @@ -150,8 +151,10 @@ void parallelize_gemm(const Functor& func, Index rows, Index cols, Index depth, info[i].lhs_start = r0; info[i].lhs_length = actualBlockRows; - if(transpose) func(c0, actualBlockCols, 0, rows, info); - else func(0, rows, c0, actualBlockCols, info); + if(transpose) + func(c0, actualBlockCols, 0, rows, info); + else + func(0, rows, c0, actualBlockCols, info); } #endif } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h index da6f82abc..04c933480 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h @@ -277,20 +277,21 @@ struct symm_pack_rhs template + int ResStorageOrder, int ResInnerStride> struct product_selfadjoint_matrix; template -struct product_selfadjoint_matrix + int RhsStorageOrder, bool RhsSelfAdjoint, bool ConjugateRhs, + int ResInnerStride> +struct product_selfadjoint_matrix { static EIGEN_STRONG_INLINE void run( Index rows, Index cols, const Scalar* lhs, Index lhsStride, const Scalar* rhs, Index rhsStride, - Scalar* res, Index resStride, + Scalar* res, Index resIncr, Index resStride, const Scalar& alpha, level3_blocking& blocking) { product_selfadjoint_matrix::IsComplex && EIGEN_LOGICAL_XOR(RhsSelfAdjoint,ConjugateRhs), EIGEN_LOGICAL_XOR(LhsSelfAdjoint,LhsStorageOrder==RowMajor) ? ColMajor : RowMajor, LhsSelfAdjoint, NumTraits::IsComplex && EIGEN_LOGICAL_XOR(LhsSelfAdjoint,ConjugateLhs), - ColMajor> - ::run(cols, rows, rhs, rhsStride, lhs, lhsStride, res, resStride, alpha, blocking); + ColMajor,ResInnerStride> + ::run(cols, rows, rhs, rhsStride, lhs, lhsStride, res, resIncr, resStride, alpha, blocking); } }; template -struct product_selfadjoint_matrix + int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride> +struct product_selfadjoint_matrix { static EIGEN_DONT_INLINE void run( Index rows, Index cols, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* res, Index resStride, + Scalar* res, Index resIncr, Index resStride, const Scalar& alpha, level3_blocking& blocking); }; template -EIGEN_DONT_INLINE void product_selfadjoint_matrix::run( + int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride> +EIGEN_DONT_INLINE void product_selfadjoint_matrix::run( Index rows, Index cols, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* _res, Index resStride, + Scalar* _res, Index resIncr, Index resStride, const Scalar& alpha, level3_blocking& blocking) { Index size = rows; @@ -334,11 +337,11 @@ EIGEN_DONT_INLINE void product_selfadjoint_matrix LhsMapper; typedef const_blas_data_mapper LhsTransposeMapper; typedef const_blas_data_mapper RhsMapper; - typedef blas_data_mapper ResMapper; + typedef blas_data_mapper ResMapper; LhsMapper lhs(_lhs,lhsStride); LhsTransposeMapper lhs_transpose(_lhs,lhsStride); RhsMapper rhs(_rhs,rhsStride); - ResMapper res(_res, resStride); + ResMapper res(_res, resStride, resIncr); Index kc = blocking.kc(); // cache block size along the K direction Index mc = (std::min)(rows,blocking.mc()); // cache block size along the M direction @@ -398,26 +401,28 @@ EIGEN_DONT_INLINE void product_selfadjoint_matrix -struct product_selfadjoint_matrix + int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride> +struct product_selfadjoint_matrix { static EIGEN_DONT_INLINE void run( Index rows, Index cols, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* res, Index resStride, + Scalar* res, Index resIncr, Index resStride, const Scalar& alpha, level3_blocking& blocking); }; template -EIGEN_DONT_INLINE void product_selfadjoint_matrix::run( + int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride> +EIGEN_DONT_INLINE void product_selfadjoint_matrix::run( Index rows, Index cols, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* _res, Index resStride, + Scalar* _res, Index resIncr, Index resStride, const Scalar& alpha, level3_blocking& blocking) { Index size = cols; @@ -425,9 +430,9 @@ EIGEN_DONT_INLINE void product_selfadjoint_matrix Traits; typedef const_blas_data_mapper LhsMapper; - typedef blas_data_mapper ResMapper; + typedef blas_data_mapper ResMapper; LhsMapper lhs(_lhs,lhsStride); - ResMapper res(_res,resStride); + ResMapper res(_res,resStride, resIncr); Index kc = blocking.kc(); // cache block size along the K direction Index mc = (std::min)(rows,blocking.mc()); // cache block size along the M direction @@ -503,12 +508,13 @@ struct selfadjoint_product_impl NumTraits::IsComplex && EIGEN_LOGICAL_XOR(LhsIsUpper,bool(LhsBlasTraits::NeedToConjugate)), EIGEN_LOGICAL_XOR(RhsIsUpper,internal::traits::Flags &RowMajorBit) ? RowMajor : ColMajor, RhsIsSelfAdjoint, NumTraits::IsComplex && EIGEN_LOGICAL_XOR(RhsIsUpper,bool(RhsBlasTraits::NeedToConjugate)), - internal::traits::Flags&RowMajorBit ? RowMajor : ColMajor> + internal::traits::Flags&RowMajorBit ? RowMajor : ColMajor, + Dest::InnerStrideAtCompileTime> ::run( lhs.rows(), rhs.cols(), // sizes &lhs.coeffRef(0,0), lhs.outerStride(), // lhs info &rhs.coeffRef(0,0), rhs.outerStride(), // rhs info - &dst.coeffRef(0,0), dst.outerStride(), // result info + &dst.coeffRef(0,0), dst.innerStride(), dst.outerStride(), // result info actualAlpha, blocking // alpha ); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h index 9a5318507..61396dbdf 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h @@ -44,16 +44,18 @@ namespace internal { template \ -struct product_selfadjoint_matrix \ +struct product_selfadjoint_matrix \ {\ \ static void run( \ Index rows, Index cols, \ const EIGTYPE* _lhs, Index lhsStride, \ const EIGTYPE* _rhs, Index rhsStride, \ - EIGTYPE* res, Index resStride, \ + EIGTYPE* res, Index resIncr, Index resStride, \ EIGTYPE alpha, level3_blocking& /*blocking*/) \ { \ + EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ + eigen_assert(resIncr == 1); \ char side='L', uplo='L'; \ BlasIndex m, n, lda, ldb, ldc; \ const EIGTYPE *a, *b; \ @@ -91,15 +93,17 @@ struct product_selfadjoint_matrix \ -struct product_selfadjoint_matrix \ +struct product_selfadjoint_matrix \ {\ static void run( \ Index rows, Index cols, \ const EIGTYPE* _lhs, Index lhsStride, \ const EIGTYPE* _rhs, Index rhsStride, \ - EIGTYPE* res, Index resStride, \ + EIGTYPE* res, Index resIncr, Index resStride, \ EIGTYPE alpha, level3_blocking& /*blocking*/) \ { \ + EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ + eigen_assert(resIncr == 1); \ char side='L', uplo='L'; \ BlasIndex m, n, lda, ldb, ldc; \ const EIGTYPE *a, *b; \ @@ -167,16 +171,18 @@ EIGEN_BLAS_HEMM_L(scomplex, float, cf, chemm_) template \ -struct product_selfadjoint_matrix \ +struct product_selfadjoint_matrix \ {\ \ static void run( \ Index rows, Index cols, \ const EIGTYPE* _lhs, Index lhsStride, \ const EIGTYPE* _rhs, Index rhsStride, \ - EIGTYPE* res, Index resStride, \ + EIGTYPE* res, Index resIncr, Index resStride, \ EIGTYPE alpha, level3_blocking& /*blocking*/) \ { \ + EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ + eigen_assert(resIncr == 1); \ char side='R', uplo='L'; \ BlasIndex m, n, lda, ldb, ldc; \ const EIGTYPE *a, *b; \ @@ -213,15 +219,17 @@ struct product_selfadjoint_matrix \ -struct product_selfadjoint_matrix \ +struct product_selfadjoint_matrix \ {\ static void run( \ Index rows, Index cols, \ const EIGTYPE* _lhs, Index lhsStride, \ const EIGTYPE* _rhs, Index rhsStride, \ - EIGTYPE* res, Index resStride, \ + EIGTYPE* res, Index resIncr, Index resStride, \ EIGTYPE alpha, level3_blocking& /*blocking*/) \ { \ + EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ + eigen_assert(resIncr == 1); \ char side='R', uplo='L'; \ BlasIndex m, n, lda, ldb, ldc; \ const EIGTYPE *a, *b; \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h index f038d686f..ef12c98f6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h @@ -109,10 +109,10 @@ struct selfadjoint_product_selector internal::general_matrix_matrix_triangular_product::IsComplex, Scalar, OtherIsRowMajor ? ColMajor : RowMajor, (!OtherBlasTraits::NeedToConjugate) && NumTraits::IsComplex, - IsRowMajor ? RowMajor : ColMajor, UpLo> + IsRowMajor ? RowMajor : ColMajor, MatrixType::InnerStrideAtCompileTime, UpLo> ::run(size, depth, &actualOther.coeffRef(0,0), actualOther.outerStride(), &actualOther.coeffRef(0,0), actualOther.outerStride(), - mat.data(), mat.outerStride(), actualAlpha, blocking); + mat.data(), mat.innerStride(), mat.outerStride(), actualAlpha, blocking); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h index f784507e7..2fb408d1d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h @@ -45,22 +45,24 @@ template + int ResStorageOrder, int ResInnerStride, + int Version = Specialized> struct product_triangular_matrix_matrix; template + int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride, int Version> struct product_triangular_matrix_matrix + RhsStorageOrder,ConjugateRhs,RowMajor,ResInnerStride,Version> { static EIGEN_STRONG_INLINE void run( Index rows, Index cols, Index depth, const Scalar* lhs, Index lhsStride, const Scalar* rhs, Index rhsStride, - Scalar* res, Index resStride, + Scalar* res, Index resIncr, Index resStride, const Scalar& alpha, level3_blocking& blocking) { product_triangular_matrix_matrix - ::run(cols, rows, depth, rhs, rhsStride, lhs, lhsStride, res, resStride, alpha, blocking); + ColMajor, ResInnerStride> + ::run(cols, rows, depth, rhs, rhsStride, lhs, lhsStride, res, resIncr, resStride, alpha, blocking); } }; // implements col-major += alpha * op(triangular) * op(general) template + int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride, int Version> struct product_triangular_matrix_matrix + RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version> { typedef gebp_traits Traits; @@ -95,20 +98,21 @@ struct product_triangular_matrix_matrix& blocking); }; template + int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride, int Version> EIGEN_DONT_INLINE void product_triangular_matrix_matrix::run( + RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version>::run( Index _rows, Index _cols, Index _depth, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* _res, Index resStride, + Scalar* _res, Index resIncr, Index resStride, const Scalar& alpha, level3_blocking& blocking) { // strip zeros @@ -119,10 +123,10 @@ EIGEN_DONT_INLINE void product_triangular_matrix_matrix LhsMapper; typedef const_blas_data_mapper RhsMapper; - typedef blas_data_mapper ResMapper; + typedef blas_data_mapper ResMapper; LhsMapper lhs(_lhs,lhsStride); RhsMapper rhs(_rhs,rhsStride); - ResMapper res(_res, resStride); + ResMapper res(_res, resStride, resIncr); Index kc = blocking.kc(); // cache block size along the K direction Index mc = (std::min)(rows,blocking.mc()); // cache block size along the M direction @@ -235,10 +239,11 @@ EIGEN_DONT_INLINE void product_triangular_matrix_matrix + int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride, int Version> struct product_triangular_matrix_matrix + RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version> { typedef gebp_traits Traits; enum { @@ -251,20 +256,21 @@ struct product_triangular_matrix_matrix& blocking); }; template + int RhsStorageOrder, bool ConjugateRhs, + int ResInnerStride, int Version> EIGEN_DONT_INLINE void product_triangular_matrix_matrix::run( + RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version>::run( Index _rows, Index _cols, Index _depth, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* _res, Index resStride, + Scalar* _res, Index resIncr, Index resStride, const Scalar& alpha, level3_blocking& blocking) { const Index PacketBytes = packet_traits::size*sizeof(Scalar); @@ -276,10 +282,10 @@ EIGEN_DONT_INLINE void product_triangular_matrix_matrix LhsMapper; typedef const_blas_data_mapper RhsMapper; - typedef blas_data_mapper ResMapper; + typedef blas_data_mapper ResMapper; LhsMapper lhs(_lhs,lhsStride); RhsMapper rhs(_rhs,rhsStride); - ResMapper res(_res, resStride); + ResMapper res(_res, resStride, resIncr); Index kc = blocking.kc(); // cache block size along the K direction Index mc = (std::min)(rows,blocking.mc()); // cache block size along the M direction @@ -433,12 +439,12 @@ struct triangular_product_impl Mode, LhsIsTriangular, (internal::traits::Flags&RowMajorBit) ? RowMajor : ColMajor, LhsBlasTraits::NeedToConjugate, (internal::traits::Flags&RowMajorBit) ? RowMajor : ColMajor, RhsBlasTraits::NeedToConjugate, - (internal::traits::Flags&RowMajorBit) ? RowMajor : ColMajor> + (internal::traits::Flags&RowMajorBit) ? RowMajor : ColMajor, Dest::InnerStrideAtCompileTime> ::run( stripedRows, stripedCols, stripedDepth, // sizes &lhs.coeffRef(0,0), lhs.outerStride(), // lhs info &rhs.coeffRef(0,0), rhs.outerStride(), // rhs info - &dst.coeffRef(0,0), dst.outerStride(), // result info + &dst.coeffRef(0,0), dst.innerStride(), dst.outerStride(), // result info actualAlpha, blocking ); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h index a25197ab0..a98d12e4a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h @@ -46,7 +46,7 @@ template {}; + RhsStorageOrder, ConjugateRhs, ResStorageOrder, 1, BuiltIn> {}; // try to go to BLAS specialization @@ -55,13 +55,15 @@ template \ struct product_triangular_matrix_matrix { \ + LhsStorageOrder,ConjugateLhs, RhsStorageOrder,ConjugateRhs,ColMajor,1,Specialized> { \ static inline void run(Index _rows, Index _cols, Index _depth, const Scalar* _lhs, Index lhsStride,\ - const Scalar* _rhs, Index rhsStride, Scalar* res, Index resStride, Scalar alpha, level3_blocking& blocking) { \ + const Scalar* _rhs, Index rhsStride, Scalar* res, Index resIncr, Index resStride, Scalar alpha, level3_blocking& blocking) { \ + EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ + eigen_assert(resIncr == 1); \ product_triangular_matrix_matrix_trmm::run( \ - _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha, blocking); \ + _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha, blocking); \ } \ }; @@ -115,8 +117,8 @@ struct product_triangular_matrix_matrix_trmm::run( \ - _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha, blocking); \ + LhsStorageOrder,ConjugateLhs, RhsStorageOrder, ConjugateRhs, ColMajor, 1, BuiltIn>::run( \ + _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, 1, resStride, alpha, blocking); \ /*std::cout << "TRMM_L: A is not square! Go to Eigen TRMM implementation!\n";*/ \ } else { \ /* Make sense to call GEMM */ \ @@ -124,8 +126,8 @@ struct product_triangular_matrix_matrix_trmm(); \ BlasIndex aStride = convert_index(aa_tmp.outerStride()); \ gemm_blocking_space gemm_blocking(_rows,_cols,_depth, 1, true); \ - general_matrix_matrix_product::run( \ - rows, cols, depth, aa_tmp.data(), aStride, _rhs, rhsStride, res, resStride, alpha, gemm_blocking, 0); \ + general_matrix_matrix_product::run( \ + rows, cols, depth, aa_tmp.data(), aStride, _rhs, rhsStride, res, 1, resStride, alpha, gemm_blocking, 0); \ \ /*std::cout << "TRMM_L: A is not square! Go to BLAS GEMM implementation! " << nthr<<" \n";*/ \ } \ @@ -232,8 +234,8 @@ struct product_triangular_matrix_matrix_trmm::run( \ - _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha, blocking); \ + LhsStorageOrder,ConjugateLhs, RhsStorageOrder, ConjugateRhs, ColMajor, 1, BuiltIn>::run( \ + _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, 1, resStride, alpha, blocking); \ /*std::cout << "TRMM_R: A is not square! Go to Eigen TRMM implementation!\n";*/ \ } else { \ /* Make sense to call GEMM */ \ @@ -241,8 +243,8 @@ struct product_triangular_matrix_matrix_trmm(); \ BlasIndex aStride = convert_index(aa_tmp.outerStride()); \ gemm_blocking_space gemm_blocking(_rows,_cols,_depth, 1, true); \ - general_matrix_matrix_product::run( \ - rows, cols, depth, _lhs, lhsStride, aa_tmp.data(), aStride, res, resStride, alpha, gemm_blocking, 0); \ + general_matrix_matrix_product::run( \ + rows, cols, depth, _lhs, lhsStride, aa_tmp.data(), aStride, res, 1, resStride, alpha, gemm_blocking, 0); \ \ /*std::cout << "TRMM_R: A is not square! Go to BLAS GEMM implementation! " << nthr<<" \n";*/ \ } \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h index 223c38b86..e3ed2cd19 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h @@ -15,48 +15,48 @@ namespace Eigen { namespace internal { // if the rhs is row major, let's transpose the product -template -struct triangular_solve_matrix +template +struct triangular_solve_matrix { static void run( Index size, Index cols, const Scalar* tri, Index triStride, - Scalar* _other, Index otherStride, + Scalar* _other, Index otherIncr, Index otherStride, level3_blocking& blocking) { triangular_solve_matrix< Scalar, Index, Side==OnTheLeft?OnTheRight:OnTheLeft, (Mode&UnitDiag) | ((Mode&Upper) ? Lower : Upper), NumTraits::IsComplex && Conjugate, - TriStorageOrder==RowMajor ? ColMajor : RowMajor, ColMajor> - ::run(size, cols, tri, triStride, _other, otherStride, blocking); + TriStorageOrder==RowMajor ? ColMajor : RowMajor, ColMajor, OtherInnerStride> + ::run(size, cols, tri, triStride, _other, otherIncr, otherStride, blocking); } }; /* Optimized triangular solver with multiple right hand side and the triangular matrix on the left */ -template -struct triangular_solve_matrix +template +struct triangular_solve_matrix { static EIGEN_DONT_INLINE void run( Index size, Index otherSize, const Scalar* _tri, Index triStride, - Scalar* _other, Index otherStride, + Scalar* _other, Index otherIncr, Index otherStride, level3_blocking& blocking); }; -template -EIGEN_DONT_INLINE void triangular_solve_matrix::run( +template +EIGEN_DONT_INLINE void triangular_solve_matrix::run( Index size, Index otherSize, const Scalar* _tri, Index triStride, - Scalar* _other, Index otherStride, + Scalar* _other, Index otherIncr, Index otherStride, level3_blocking& blocking) { Index cols = otherSize; typedef const_blas_data_mapper TriMapper; - typedef blas_data_mapper OtherMapper; + typedef blas_data_mapper OtherMapper; TriMapper tri(_tri, triStride); - OtherMapper other(_other, otherStride); + OtherMapper other(_other, otherStride, otherIncr); typedef gebp_traits Traits; @@ -128,19 +128,19 @@ EIGEN_DONT_INLINE void triangular_solve_matrix -struct triangular_solve_matrix +template +struct triangular_solve_matrix { static EIGEN_DONT_INLINE void run( Index size, Index otherSize, const Scalar* _tri, Index triStride, - Scalar* _other, Index otherStride, + Scalar* _other, Index otherIncr, Index otherStride, level3_blocking& blocking); }; -template -EIGEN_DONT_INLINE void triangular_solve_matrix::run( +template +EIGEN_DONT_INLINE void triangular_solve_matrix::run( Index size, Index otherSize, const Scalar* _tri, Index triStride, - Scalar* _other, Index otherStride, + Scalar* _other, Index otherIncr, Index otherStride, level3_blocking& blocking) { Index rows = otherSize; typedef typename NumTraits::Real RealScalar; - typedef blas_data_mapper LhsMapper; + typedef blas_data_mapper LhsMapper; typedef const_blas_data_mapper RhsMapper; - LhsMapper lhs(_other, otherStride); + LhsMapper lhs(_other, otherStride, otherIncr); RhsMapper rhs(_tri, triStride); typedef gebp_traits Traits; @@ -297,24 +297,24 @@ EIGEN_DONT_INLINE void triangular_solve_matrix \ -struct triangular_solve_matrix \ +struct triangular_solve_matrix \ { \ enum { \ IsLower = (Mode&Lower) == Lower, \ @@ -51,8 +51,10 @@ struct triangular_solve_matrix& /*blocking*/) \ + EIGTYPE* _other, Index otherIncr, Index otherStride, level3_blocking& /*blocking*/) \ { \ + EIGEN_ONLY_USED_FOR_DEBUG(otherIncr); \ + eigen_assert(otherIncr == 1); \ BlasIndex m = convert_index(size), n = convert_index(otherSize), lda, ldb; \ char side = 'L', uplo, diag='N', transa; \ /* Set alpha_ */ \ @@ -99,7 +101,7 @@ EIGEN_BLAS_TRSM_L(scomplex, float, ctrsm_) // implements RightSide general * op(triangular)^-1 #define EIGEN_BLAS_TRSM_R(EIGTYPE, BLASTYPE, BLASFUNC) \ template \ -struct triangular_solve_matrix \ +struct triangular_solve_matrix \ { \ enum { \ IsLower = (Mode&Lower) == Lower, \ @@ -110,8 +112,10 @@ struct triangular_solve_matrix& /*blocking*/) \ + EIGTYPE* _other, Index otherIncr, Index otherStride, level3_blocking& /*blocking*/) \ { \ + EIGEN_ONLY_USED_FOR_DEBUG(otherIncr); \ + eigen_assert(otherIncr == 1); \ BlasIndex m = convert_index(otherSize), n = convert_index(size), lda, ldb; \ char side = 'R', uplo, diag='N', transa; \ /* Set alpha_ */ \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h index 6e6ee119b..3dff9bc9b 100755 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h @@ -31,7 +31,7 @@ template< typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs, typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs, - int ResStorageOrder> + int ResStorageOrder, int ResInnerStride> struct general_matrix_matrix_product; template +class BlasLinearMapper; + template -class BlasLinearMapper { +class BlasLinearMapper { public: typedef typename packet_traits::type Packet; typedef typename packet_traits::half HalfPacket; - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar *data) : m_data(data) {} + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar *data, Index incr=1) + : m_data(data) + { + EIGEN_ONLY_USED_FOR_DEBUG(incr); + eigen_assert(incr==1); + } EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(int i) const { internal::prefetch(&operator()(i)); @@ -188,16 +196,25 @@ class BlasLinearMapper { }; // Lightweight helper class to access matrix coefficients. -template -class blas_data_mapper { - public: +template +class blas_data_mapper; + +template +class blas_data_mapper +{ +public: typedef typename packet_traits::type Packet; typedef typename packet_traits::half HalfPacket; typedef BlasLinearMapper LinearMapper; typedef BlasVectorMapper VectorMapper; - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride) : m_data(data), m_stride(stride) {} + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride, Index incr=1) + : m_data(data), m_stride(stride) + { + EIGEN_ONLY_USED_FOR_DEBUG(incr); + eigen_assert(incr==1); + } EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper getSubMapper(Index i, Index j) const { @@ -251,6 +268,90 @@ class blas_data_mapper { const Index m_stride; }; +// Implementation of non-natural increment (i.e. inner-stride != 1) +// The exposed API is not complete yet compared to the Incr==1 case +// because some features makes less sense in this case. +template +class BlasLinearMapper +{ +public: + typedef typename packet_traits::type Packet; + typedef typename packet_traits::half HalfPacket; + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar *data,Index incr) : m_data(data), m_incr(incr) {} + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(int i) const { + internal::prefetch(&operator()(i)); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar& operator()(Index i) const { + return m_data[i*m_incr.value()]; + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet loadPacket(Index i) const { + return pgather(m_data + i*m_incr.value(), m_incr.value()); + } + + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, const PacketType &p) const { + pscatter(m_data + i*m_incr.value(), p, m_incr.value()); + } + +protected: + Scalar *m_data; + const internal::variable_if_dynamic m_incr; +}; + +template +class blas_data_mapper +{ +public: + typedef typename packet_traits::type Packet; + typedef typename packet_traits::half HalfPacket; + + typedef BlasLinearMapper LinearMapper; + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride, Index incr) : m_data(data), m_stride(stride), m_incr(incr) {} + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper + getSubMapper(Index i, Index j) const { + return blas_data_mapper(&operator()(i, j), m_stride, m_incr.value()); + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE LinearMapper getLinearMapper(Index i, Index j) const { + return LinearMapper(&operator()(i, j), m_incr.value()); + } + + EIGEN_DEVICE_FUNC + EIGEN_ALWAYS_INLINE Scalar& operator()(Index i, Index j) const { + return m_data[StorageOrder==RowMajor ? j*m_incr.value() + i*m_stride : i*m_incr.value() + j*m_stride]; + } + + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet loadPacket(Index i, Index j) const { + return pgather(&operator()(i, j),m_incr.value()); + } + + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketT load(Index i, Index j) const { + return pgather(&operator()(i, j),m_incr.value()); + } + + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void scatterPacket(Index i, Index j, const SubPacket &p) const { + pscatter(&operator()(i, j), p, m_stride); + } + + template + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE SubPacket gatherPacket(Index i, Index j) const { + return pgather(&operator()(i, j), m_stride); + } + +protected: + Scalar* EIGEN_RESTRICT m_data; + const Index m_stride; + const internal::variable_if_dynamic m_incr; +}; + // lightweight helper class to access matrix coefficients (const version) template class const_blas_data_mapper : public blas_data_mapper { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h index 351bd6c60..74f74cc42 100755 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h @@ -57,7 +57,10 @@ #if __GNUC__>=6 #pragma GCC diagnostic ignored "-Wignored-attributes" #endif - + #if __GNUC__==7 + // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325 + #pragma GCC diagnostic ignored "-Wattributes" + #endif #endif #if defined __NVCC__ @@ -80,4 +83,12 @@ #pragma diag_suppress 2737 #endif +#else +// warnings already disabled: +# ifndef EIGEN_WARNINGS_DISABLED_2 +# define EIGEN_WARNINGS_DISABLED_2 +# elif defined(EIGEN_INTERNAL_DEBUGGING) +# error "Do not include \"DisableStupidWarnings.h\" recursively more than twice!" +# endif + #endif // not EIGEN_WARNINGS_DISABLED diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ForwardDeclarations.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ForwardDeclarations.h index ea107393a..134544f96 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ForwardDeclarations.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ForwardDeclarations.h @@ -47,11 +47,7 @@ template struct NumTraits; template struct EigenBase; template class DenseBase; template class PlainObjectBase; - - -template::value > -class DenseCoeffsBase; +template class DenseCoeffsBase; templatex || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -380,7 +380,8 @@ #if EIGEN_MAX_CPP_VER>=11 && \ ((defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901)) \ || (defined(__GNUC__) && defined(_GLIBCXX_USE_C99)) \ - || (defined(_LIBCPP_VERSION) && !defined(_MSC_VER))) + || (defined(_LIBCPP_VERSION) && !defined(_MSC_VER)) \ + || (EIGEN_COMP_MSVC >= 1900) ) #define EIGEN_HAS_C99_MATH 1 #else #define EIGEN_HAS_C99_MATH 0 @@ -396,6 +397,20 @@ #endif #endif +// Does the compiler support type_traits? +// - full support of type traits was added only to GCC 5.1.0. +// - 20150626 corresponds to the last release of 4.x libstdc++ +#ifndef EIGEN_HAS_TYPE_TRAITS +#if EIGEN_MAX_CPP_VER>=11 && (EIGEN_HAS_CXX11 || EIGEN_COMP_MSVC >= 1700) \ + && ((!EIGEN_COMP_GNUC_STRICT) || EIGEN_GNUC_AT_LEAST(5, 1)) \ + && ((!defined(__GLIBCXX__)) || __GLIBCXX__ > 20150626) +#define EIGEN_HAS_TYPE_TRAITS 1 +#define EIGEN_INCLUDE_TYPE_TRAITS +#else +#define EIGEN_HAS_TYPE_TRAITS 0 +#endif +#endif + // Does the compiler support variadic templates? #ifndef EIGEN_HAS_VARIADIC_TEMPLATES #if EIGEN_MAX_CPP_VER>=11 && (__cplusplus > 199711L || EIGEN_COMP_MSVC >= 1900) \ @@ -835,11 +850,48 @@ namespace Eigen { #endif +/** + * \internal + * \brief Macro to explicitly define the default copy constructor. + * This is necessary, because the implicit definition is deprecated if the copy-assignment is overridden. + */ +#if EIGEN_HAS_CXX11 +#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) EIGEN_DEVICE_FUNC CLASS(const CLASS&) = default; +#else +#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) +#endif + + + /** \internal * \brief Macro to manually inherit assignment operators. * This is necessary, because the implicitly defined assignment operator gets deleted when a custom operator= is defined. + * With C++11 or later this also default-implements the copy-constructor */ -#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) +#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ + EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \ + EIGEN_DEFAULT_COPY_CONSTRUCTOR(Derived) + +/** \internal + * \brief Macro to manually define default constructors and destructors. + * This is necessary when the copy constructor is re-defined. + * For empty helper classes this should usually be protected, to avoid accidentally creating empty objects. + * + * Hiding the default destructor lead to problems in C++03 mode together with boost::multiprecision + */ +#if EIGEN_HAS_CXX11 +#define EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(Derived) \ + EIGEN_DEVICE_FUNC Derived() = default; \ + EIGEN_DEVICE_FUNC ~Derived() = default; +#else +#define EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(Derived) \ + EIGEN_DEVICE_FUNC Derived() {}; \ + /* EIGEN_DEVICE_FUNC ~Derived() {}; */ +#endif + + + + /** * Just a side note. Commenting within defines works only by documenting diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h index d31e95411..9b61ff037 100755 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h @@ -97,6 +97,9 @@ template<> struct is_arithmetic { enum { value = true }; }; template<> struct is_arithmetic { enum { value = true }; }; template<> struct is_arithmetic { enum { value = true }; }; +#if EIGEN_HAS_CXX11 +using std::is_integral; +#else template struct is_integral { enum { value = false }; }; template<> struct is_integral { enum { value = true }; }; template<> struct is_integral { enum { value = true }; }; @@ -108,6 +111,11 @@ template<> struct is_integral { enum { value = true }; }; template<> struct is_integral { enum { value = true }; }; template<> struct is_integral { enum { value = true }; }; template<> struct is_integral { enum { value = true }; }; +#if EIGEN_COMP_MSVC +template<> struct is_integral { enum { value = true }; }; +template<> struct is_integral{ enum { value = true }; }; +#endif +#endif #if EIGEN_HAS_CXX11 using std::make_unsigned; @@ -531,4 +539,30 @@ bool not_equal_strict(const double& x,const double& y) { return std::not_equal_t } // end namespace Eigen +// Define portable (u)int{32,64} types +#if EIGEN_HAS_CXX11 +#include +namespace Eigen { +namespace numext { +typedef std::uint32_t uint32_t; +typedef std::int32_t int32_t; +typedef std::uint64_t uint64_t; +typedef std::int64_t int64_t; +} +} +#else +// Without c++11, all compilers able to compile Eigen also +// provides the C99 stdint.h header file. +#include +namespace Eigen { +namespace numext { +typedef ::uint32_t uint32_t; +typedef ::int32_t int32_t; +typedef ::uint64_t uint64_t; +typedef ::int64_t int64_t; +} +} +#endif + + #endif // EIGEN_META_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h index ecc82b7c8..1ce6fd1b0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h @@ -1,4 +1,8 @@ -#ifdef EIGEN_WARNINGS_DISABLED +#ifdef EIGEN_WARNINGS_DISABLED_2 +// "DisableStupidWarnings.h" was included twice recursively: Do not reenable warnings yet! +# undef EIGEN_WARNINGS_DISABLED_2 + +#elif defined(EIGEN_WARNINGS_DISABLED) #undef EIGEN_WARNINGS_DISABLED #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h index ba5bd186d..6bb497082 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h @@ -34,6 +34,20 @@ inline IndexDest convert_index(const IndexSrc& idx) { return IndexDest(idx); } +// true if T can be considered as an integral index (i.e., and integral type or enum) +template struct is_valid_index_type +{ + enum { value = +#if EIGEN_HAS_TYPE_TRAITS + internal::is_integral::value || std::is_enum::value +#elif EIGEN_COMP_MSVC + internal::is_integral::value || __is_enum(T) +#else + // without C++11, we use is_convertible to Index instead of is_integral in order to treat enums as Index. + internal::is_convertible::value && !internal::is_same::value && !is_same::value +#endif + }; +}; // promote_scalar_arg is an helper used in operation between an expression and a scalar, like: // expression * scalar @@ -90,6 +104,9 @@ class no_assignment_operator { private: no_assignment_operator& operator=(const no_assignment_operator&); + protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(no_assignment_operator) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(no_assignment_operator) }; /** \internal return the index type with the largest number of bits */ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur.h index 7f38919f7..4354e4018 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur.h @@ -300,10 +300,13 @@ typename ComplexSchur::ComplexScalar ComplexSchur::compu ComplexScalar trace = t.coeff(0,0) + t.coeff(1,1); ComplexScalar eival1 = (trace + disc) / RealScalar(2); ComplexScalar eival2 = (trace - disc) / RealScalar(2); - - if(numext::norm1(eival1) > numext::norm1(eival2)) + RealScalar eival1_norm = numext::norm1(eival1); + RealScalar eival2_norm = numext::norm1(eival2); + // A division by zero can only occur if eival1==eival2==0. + // In this case, det==0, and all we have to do is checking that eival2_norm!=0 + if(eival1_norm > eival2_norm) eival2 = det / eival1; - else + else if(eival2_norm!=RealScalar(0)) eival1 = det / eival2; // choose the eigenvalue closest to the bottom entry of the diagonal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h index 17ea903f5..9191519ab 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h @@ -236,7 +236,7 @@ template class RealSchur typedef Matrix Vector3s; Scalar computeNormOfT(); - Index findSmallSubdiagEntry(Index iu); + Index findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero); void splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift); void computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo); void initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im, Vector3s& firstHouseholderVector); @@ -302,12 +302,16 @@ RealSchur& RealSchur::computeFromHessenberg(const HessMa Index totalIter = 0; // iteration count for whole matrix Scalar exshift(0); // sum of exceptional shifts Scalar norm = computeNormOfT(); + // sub-diagonal entries smaller than considerAsZero will be treated as zero. + // We use eps^2 to enable more precision in small eigenvalues. + Scalar considerAsZero = numext::maxi( norm * numext::abs2(NumTraits::epsilon()), + (std::numeric_limits::min)() ); if(norm!=Scalar(0)) { while (iu >= 0) { - Index il = findSmallSubdiagEntry(iu); + Index il = findSmallSubdiagEntry(iu,considerAsZero); // Check for convergence if (il == iu) // One root found @@ -364,14 +368,17 @@ inline typename MatrixType::Scalar RealSchur::computeNormOfT() /** \internal Look for single small sub-diagonal element and returns its index */ template -inline Index RealSchur::findSmallSubdiagEntry(Index iu) +inline Index RealSchur::findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero) { using std::abs; Index res = iu; while (res > 0) { Scalar s = abs(m_matT.coeff(res-1,res-1)) + abs(m_matT.coeff(res,res)); - if (abs(m_matT.coeff(res,res-1)) <= NumTraits::epsilon() * s) + + s = numext::maxi(s * NumTraits::epsilon(), considerAsZero); + + if (abs(m_matT.coeff(res,res-1)) <= s) break; res--; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h index 9ddd553f2..d37656fa2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h @@ -605,7 +605,8 @@ template struct direct_selfadjoint_eigenvalues res, Ref representative) { - using std::abs; + EIGEN_USING_STD_MATH(sqrt) + EIGEN_USING_STD_MATH(abs) Index i0; // Find non-zero column i0 (by construction, there must exist a non zero coefficient on the diagonal): mat.diagonal().cwiseAbs().maxCoeff(&i0); @@ -616,8 +617,8 @@ template struct direct_selfadjoint_eigenvaluesn1) res = c0/std::sqrt(n0); - else res = c1/std::sqrt(n1); + if(n0>n1) res = c0/sqrt(n0); + else res = c1/sqrt(n1); return true; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h index c3fd8c3e0..b81820656 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h @@ -169,20 +169,38 @@ class QuaternionBase : public RotationBase /** return the result vector of \a v through the rotation*/ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; + #ifdef EIGEN_PARSED_BY_DOXYGEN /** \returns \c *this with scalar type casted to \a NewScalarType * * Note that if \a NewScalarType is equal to the current scalar type of \c *this * then this function smartly returns a const reference to \c *this. */ template - EIGEN_DEVICE_FUNC inline typename internal::cast_return_type >::type cast() const + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type >::type cast() const; + + #else + + template + EIGEN_DEVICE_FUNC inline + typename internal::enable_if::value,const Derived&>::type cast() const { - return typename internal::cast_return_type >::type(derived()); + return derived(); } + template + EIGEN_DEVICE_FUNC inline + typename internal::enable_if::value,Quaternion >::type cast() const + { + return Quaternion(coeffs().template cast()); + } + #endif + #ifdef EIGEN_QUATERNIONBASE_PLUGIN # include EIGEN_QUATERNIONBASE_PLUGIN #endif +protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(QuaternionBase) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(QuaternionBase) }; /*************************************************************************** diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h old mode 100755 new mode 100644 index f58ca03d9..33eabd81a --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h @@ -14,7 +14,7 @@ namespace Eigen { /** \geometry_module \ingroup Geometry_Module * - * \class Scaling + * \class UniformScaling * * \brief Represents a generic uniform scaling transformation * diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index 3f31ee45d..c21d9e550 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -252,11 +252,11 @@ protected: public: /** Default constructor without initialization of the meaningful coefficients. - * If Mode==Affine, then the last row is set to [0 ... 0 1] */ + * If Mode==Affine or Mode==Isometry, then the last row is set to [0 ... 0 1] */ EIGEN_DEVICE_FUNC inline Transform() { check_template_params(); - internal::transform_make_affine<(int(Mode)==Affine) ? Affine : AffineCompact>::run(m_matrix); + internal::transform_make_affine<(int(Mode)==Affine || int(Mode)==Isometry) ? Affine : AffineCompact>::run(m_matrix); } EIGEN_DEVICE_FUNC inline Transform(const Transform& other) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h index 51d9a82eb..0e99ce68e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h @@ -138,12 +138,6 @@ public: /** \returns the inverse translation (opposite) */ Translation inverse() const { return Translation(-m_coeffs); } - Translation& operator=(const Translation& other) - { - m_coeffs = other.m_coeffs; - return *this; - } - static const Translation Identity() { return Translation(VectorType::Zero()); } /** \returns \c *this with scalar type casted to \a NewScalarType diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h index 7e933fca1..6b755008f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h @@ -87,7 +87,7 @@ struct umeyama_transform_matrix_type * \f{align*} * T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix} * \f} -* minimizing the resudiual above. This transformation is always returned as an +* minimizing the residual above. This transformation is always returned as an * Eigen::Matrix. */ template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h index d43961887..6b10f39fa 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h @@ -519,7 +519,10 @@ void PartialPivLU::compute() // the row permutation is stored as int indices, so just to be sure: eigen_assert(m_lu.rows()::highest()); - m_l1_norm = m_lu.cwiseAbs().colwise().sum().maxCoeff(); + if(m_lu.cols()>0) + m_l1_norm = m_lu.cwiseAbs().colwise().sum().maxCoeff(); + else + m_l1_norm = RealScalar(0); eigen_assert(m_lu.rows() == m_lu.cols() && "PartialPivLU is only for square (and moreover invertible) matrices"); const Index size = m_lu.rows(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h index ebb64a62b..4dce2ef20 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h @@ -44,7 +44,7 @@ struct compute_inverse_size4 static void run(const MatrixType& mat, ResultType& result) { ActualMatrixType matrix(mat); - EIGEN_ALIGN16 const unsigned int _Sign_PNNP[4] = { 0x00000000, 0x80000000, 0x80000000, 0x00000000 }; + const Packet4f p4f_sign_PNNP = _mm_castsi128_ps(_mm_set_epi32(0x00000000, 0x80000000, 0x80000000, 0x00000000)); // Load the full matrix into registers __m128 _L1 = matrix.template packet( 0); @@ -139,7 +139,7 @@ struct compute_inverse_size4 iC = _mm_sub_ps(iC, _mm_mul_ps(_mm_shuffle_ps(A,A,0xB1), _mm_shuffle_ps(DC,DC,0x66))); rd = _mm_shuffle_ps(rd,rd,0); - rd = _mm_xor_ps(rd, _mm_load_ps((float*)_Sign_PNNP)); + rd = _mm_xor_ps(rd, p4f_sign_PNNP); // iB = C*|B| - D*B#*A iB = _mm_sub_ps(_mm_mul_ps(C,_mm_shuffle_ps(dB,dB,0)), iB); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h index 091c3970e..98d0e3f21 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h @@ -192,7 +192,8 @@ class PardisoImpl : public SparseSolverBase void pardisoInit(int type) { m_type = type; - bool symmetric = std::abs(m_type) < 10; + EIGEN_USING_STD_MATH(abs); + bool symmetric = abs(m_type) < 10; m_iparm[0] = 1; // No solver default m_iparm[1] = 2; // use Metis for the ordering m_iparm[2] = 0; // Reserved. Set to zero. (??Numbers of processors, value of OMP_NUM_THREADS??) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h index 1134d66e7..a5b73f8f2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h @@ -768,6 +768,21 @@ void BDCSVD::computeSingVals(const ArrayRef& col0, const ArrayRef& d // measure everything relative to shift Map diagShifted(m_workspace.data()+4*n, n); diagShifted = diag - shift; + + if(k!=actual_n-1) + { + // check that after the shift, f(mid) is still negative: + RealScalar midShifted = (right - left) / RealScalar(2); + if(shift==right) + midShifted = -midShifted; + RealScalar fMidShifted = secularEq(midShifted, col0, diag, perm, diagShifted, shift); + if(fMidShifted>0) + { + // fMid was erroneous, fix it: + shift = fMidShifted > Literal(0) ? left : right; + diagShifted = diag - shift; + } + } // initial guess RealScalar muPrev, muCur; @@ -845,11 +860,13 @@ void BDCSVD::computeSingVals(const ArrayRef& col0, const ArrayRef& d } RealScalar fLeft = secularEq(leftShifted, col0, diag, perm, diagShifted, shift); + eigen_internal_assert(fLeft::computeSingVals(const ArrayRef& col0, const ArrayRef& d } #endif eigen_internal_assert(fLeft * fRight < Literal(0)); - - while (rightShifted - leftShifted > Literal(2) * NumTraits::epsilon() * numext::maxi(abs(leftShifted), abs(rightShifted))) - { - RealScalar midShifted = (leftShifted + rightShifted) / Literal(2); - fMid = secularEq(midShifted, col0, diag, perm, diagShifted, shift); - if (fLeft * fMid < Literal(0)) - { - rightShifted = midShifted; - } - else - { - leftShifted = midShifted; - fLeft = fMid; - } - } - muCur = (leftShifted + rightShifted) / Literal(2); + if(fLeft Literal(2) * NumTraits::epsilon() * numext::maxi(abs(leftShifted), abs(rightShifted))) + { + RealScalar midShifted = (leftShifted + rightShifted) / Literal(2); + fMid = secularEq(midShifted, col0, diag, perm, diagShifted, shift); + eigen_internal_assert((numext::isfinite)(fMid)); + + if (fLeft * fMid < Literal(0)) + { + rightShifted = midShifted; + } + else + { + leftShifted = midShifted; + fLeft = fMid; + } + } + muCur = (leftShifted + rightShifted) / Literal(2); + } + else + { + // We have a problem as shifting on the left or right give either a positive or negative value + // at the middle of [left,right]... + // Instead fo abbording or entering an infinite loop, + // let's just use the middle as the estimated zero-crossing: + muCur = (right - left) * RealScalar(0.5); + if(shift == right) + muCur = -muCur; + } } singVals[k] = shift + muCur; @@ -924,7 +955,7 @@ void BDCSVD::perturbCol0 Index j = i 0.9 ) + if(i!=k && numext::abs(((singVals(j)+dk)*(mus(j)+(shifts(j)-dk)))/((diag(i)+dk)*(diag(i)-dk)) - 1) > 0.9 ) std::cout << " " << ((singVals(j)+dk)*(mus(j)+(shifts(j)-dk)))/((diag(i)+dk)*(diag(i)-dk)) << " == (" << (singVals(j)+dk) << " * " << (mus(j)+(shifts(j)-dk)) << ") / (" << (diag(i)+dk) << " * " << (diag(i)-dk) << ")\n"; #endif @@ -934,7 +965,7 @@ void BDCSVD::perturbCol0 std::cout << "zhat(" << k << ") = sqrt( " << prod << ") ; " << (singVals(last) + dk) << " * " << mus(last) + shifts(last) << " - " << dk << "\n"; #endif RealScalar tmp = sqrt(prod); - zhat(k) = col0(k) > Literal(0) ? tmp : -tmp; + zhat(k) = col0(k) > Literal(0) ? RealScalar(tmp) : RealScalar(-tmp); } } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h index 3d1ef373e..53da28488 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h @@ -183,7 +183,7 @@ public: // this temporary is needed to workaround a MSVC issue Index diagSize = (std::max)(1,m_diagSize); return m_usePrescribedThreshold ? m_prescribedThreshold - : diagSize*NumTraits::epsilon(); + : RealScalar(diagSize)*NumTraits::epsilon(); } /** \returns true if \a U (full or thin) is asked for in this SVD decomposition */ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h index 2907f6529..369e6804a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h @@ -608,7 +608,7 @@ public: } if(Base::m_diag.size()>0) - dest = Base::m_diag.asDiagonal().inverse() * dest; + dest = Base::m_diag.real().asDiagonal().inverse() * dest; if (Base::m_matrix.nonZeros()>0) // otherwise I==I { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h index 31e06995b..7b6183d08 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h @@ -156,7 +156,7 @@ void SimplicialCholeskyBase::factorize_preordered(const CholMatrixType& /* the nonzero entry L(k,i) */ Scalar l_ki; if(DoLDLT) - l_ki = yi / m_diag[i]; + l_ki = yi / numext::real(m_diag[i]); else yi = l_ki = yi / Lx[Lp[i]]; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h index e0295f2af..2cb7747cc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h @@ -28,7 +28,7 @@ class AmbiVector typedef typename NumTraits::Real RealScalar; explicit AmbiVector(Index size) - : m_buffer(0), m_zero(0), m_size(0), m_allocatedSize(0), m_allocatedElements(0), m_mode(-1) + : m_buffer(0), m_zero(0), m_size(0), m_end(0), m_allocatedSize(0), m_allocatedElements(0), m_mode(-1) { resize(size); } @@ -147,7 +147,8 @@ template void AmbiVector<_Scalar,_StorageIndex>::init(int mode) { m_mode = mode; - if (m_mode==IsSparse) + // This is only necessary in sparse mode, but we set these unconditionally to avoid some maybe-uninitialized warnings + // if (m_mode==IsSparse) { m_llSize = 0; m_llStart = -1; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseUnaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseUnaryOp.h index ea7973790..df6c28d2b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseUnaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseUnaryOp.h @@ -49,6 +49,7 @@ template class unary_evaluator, IteratorBased>::InnerIterator : public unary_evaluator, IteratorBased>::EvalIterator { + protected: typedef typename XprType::Scalar Scalar; typedef typename unary_evaluator, IteratorBased>::EvalIterator Base; public: @@ -99,6 +100,7 @@ template class unary_evaluator, IteratorBased>::InnerIterator : public unary_evaluator, IteratorBased>::EvalIterator { + protected: typedef typename XprType::Scalar Scalar; typedef typename unary_evaluator, IteratorBased>::EvalIterator Base; public: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h index 0a2490bcc..a5396538b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h @@ -327,7 +327,8 @@ class SparseMatrix m_outerIndex[j] = newOuterIndex[j]; m_innerNonZeros[j] = innerNNZ; } - m_outerIndex[m_outerSize] = m_outerIndex[m_outerSize-1] + m_innerNonZeros[m_outerSize-1] + reserveSizes[m_outerSize-1]; + if(m_outerSize>0) + m_outerIndex[m_outerSize] = m_outerIndex[m_outerSize-1] + m_innerNonZeros[m_outerSize-1] + reserveSizes[m_outerSize-1]; m_data.resize(m_outerIndex[m_outerSize]); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h index 65611b3d4..76117a010 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h @@ -453,7 +453,7 @@ void permute_symm_to_fullsymm(const MatrixType& mat, SparseMatrix, IteratorBased> class InnerIterator : public EvalIterator { + protected: typedef typename XprType::Scalar Scalar; public: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h index 7104831c0..87f0efe37 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h @@ -43,8 +43,8 @@ template struct SparseLUMatrixURetu * Simple example with key steps * \code * VectorXd x(n), b(n); - * SparseMatrix A; - * SparseLU, COLAMDOrdering > solver; + * SparseMatrix A; + * SparseLU, COLAMDOrdering > solver; * // fill A and b; * // Compute the ordering permutation vector from the structural pattern of A * solver.analyzePattern(A); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h index cf1fedf92..af158f425 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h @@ -98,8 +98,10 @@ namespace std { { return deque_base::insert(position,x); } void insert(const_iterator position, size_type new_size, const value_type& x) { deque_base::insert(position, new_size, x); } -#elif defined(_GLIBCXX_DEQUE) && EIGEN_GNUC_AT_LEAST(4,2) +#elif defined(_GLIBCXX_DEQUE) && EIGEN_GNUC_AT_LEAST(4,2) && !EIGEN_GNUC_AT_LEAST(10, 1) // workaround GCC std::deque implementation + // GCC 10.1 doesn't let us access _Deque_impl _M_impl anymore and we have to + // fall-back to the default case void resize(size_type new_size, const value_type& x) { if (new_size < deque_base::size()) @@ -108,7 +110,7 @@ namespace std { deque_base::insert(deque_base::end(), new_size - deque_base::size(), x); } #else - // either GCC 4.1 or non-GCC + // either non-GCC or GCC between 4.1 and 10.1 // default implementation which should always work. void resize(size_type new_size, const value_type& x) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h index 1f8a531af..05a7449bc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h @@ -119,7 +119,7 @@ OP(const Scalar& s) const { \ return this->OP(Derived::PlainObject::Constant(rows(), cols(), s)); \ } \ EIGEN_DEVICE_FUNC friend EIGEN_STRONG_INLINE const RCmp ## COMPARATOR ## ReturnType \ -OP(const Scalar& s, const Derived& d) { \ +OP(const Scalar& s, const EIGEN_CURRENT_STORAGE_BASE_CLASS& d) { \ return Derived::PlainObject::Constant(d.rows(), d.cols(), s).OP(d); \ } diff --git a/gtsam/3rdparty/Eigen/bench/bench_gemm.cpp b/gtsam/3rdparty/Eigen/bench/bench_gemm.cpp index 8528c5587..dccab96a8 100644 --- a/gtsam/3rdparty/Eigen/bench/bench_gemm.cpp +++ b/gtsam/3rdparty/Eigen/bench/bench_gemm.cpp @@ -112,6 +112,7 @@ void matlab_cplx_cplx(const M& ar, const M& ai, const M& br, const M& bi, M& cr, cr.noalias() -= ai * bi; ci.noalias() += ar * bi; ci.noalias() += ai * br; + // [cr ci] += [ar ai] * br + [-ai ar] * bi } void matlab_real_cplx(const M& a, const M& br, const M& bi, M& cr, M& ci) @@ -240,7 +241,7 @@ int main(int argc, char ** argv) blas_gemm(a,b,r); c.noalias() += a * b; if(!r.isApprox(c)) { - std::cout << r - c << "\n"; + std::cout << (r - c).norm() << "\n"; std::cerr << "Warning, your product is crap!\n\n"; } #else @@ -249,7 +250,7 @@ int main(int argc, char ** argv) gemm(a,b,c); r.noalias() += a.cast() .lazyProduct( b.cast() ); if(!r.isApprox(c)) { - std::cout << r - c << "\n"; + std::cout << (r - c).norm() << "\n"; std::cerr << "Warning, your product is crap!\n\n"; } } diff --git a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt index e2f1dd6b4..9887d5804 100644 --- a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt @@ -39,9 +39,9 @@ endif() add_dependencies(blas eigen_blas eigen_blas_static) install(TARGETS eigen_blas eigen_blas_static - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) if(EIGEN_Fortran_COMPILER_WORKS) diff --git a/gtsam/3rdparty/Eigen/blas/level3_impl.h b/gtsam/3rdparty/Eigen/blas/level3_impl.h index 6c802cd5f..6dd6338b4 100644 --- a/gtsam/3rdparty/Eigen/blas/level3_impl.h +++ b/gtsam/3rdparty/Eigen/blas/level3_impl.h @@ -13,28 +13,28 @@ int EIGEN_BLAS_FUNC(gemm)(const char *opa, const char *opb, const int *m, const const RealScalar *pa, const int *lda, const RealScalar *pb, const int *ldb, const RealScalar *pbeta, RealScalar *pc, const int *ldc) { // std::cerr << "in gemm " << *opa << " " << *opb << " " << *m << " " << *n << " " << *k << " " << *lda << " " << *ldb << " " << *ldc << " " << *palpha << " " << *pbeta << "\n"; - typedef void (*functype)(DenseIndex, DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, Scalar, internal::level3_blocking&, Eigen::internal::GemmParallelInfo*); + typedef void (*functype)(DenseIndex, DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, DenseIndex, Scalar, internal::level3_blocking&, Eigen::internal::GemmParallelInfo*); static const functype func[12] = { // array index: NOTR | (NOTR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: TR | (NOTR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: ADJ | (NOTR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), 0, // array index: NOTR | (TR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: TR | (TR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: ADJ | (TR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), 0, // array index: NOTR | (ADJ << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: TR | (ADJ << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: ADJ | (ADJ << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), 0 }; @@ -71,7 +71,7 @@ int EIGEN_BLAS_FUNC(gemm)(const char *opa, const char *opb, const int *m, const internal::gemm_blocking_space blocking(*m,*n,*k,1,true); int code = OP(*opa) | (OP(*opb) << 2); - func[code](*m, *n, *k, a, *lda, b, *ldb, c, *ldc, alpha, blocking, 0); + func[code](*m, *n, *k, a, *lda, b, *ldb, c, 1, *ldc, alpha, blocking, 0); return 0; } @@ -79,63 +79,63 @@ int EIGEN_BLAS_FUNC(trsm)(const char *side, const char *uplo, const char *opa, c const RealScalar *palpha, const RealScalar *pa, const int *lda, RealScalar *pb, const int *ldb) { // std::cerr << "in trsm " << *side << " " << *uplo << " " << *opa << " " << *diag << " " << *m << "," << *n << " " << *palpha << " " << *lda << " " << *ldb<< "\n"; - typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, internal::level3_blocking&); + typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, DenseIndex, internal::level3_blocking&); static const functype func[32] = { // array index: NOTR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run),\ + (internal::triangular_solve_matrix::run),\ 0, // array index: NOTR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0 }; @@ -163,12 +163,12 @@ int EIGEN_BLAS_FUNC(trsm)(const char *side, const char *uplo, const char *opa, c if(SIDE(*side)==LEFT) { internal::gemm_blocking_space blocking(*m,*n,*m,1,false); - func[code](*m, *n, a, *lda, b, *ldb, blocking); + func[code](*m, *n, a, *lda, b, 1, *ldb, blocking); } else { internal::gemm_blocking_space blocking(*m,*n,*n,1,false); - func[code](*n, *m, a, *lda, b, *ldb, blocking); + func[code](*n, *m, a, *lda, b, 1, *ldb, blocking); } if(alpha!=Scalar(1)) @@ -184,63 +184,63 @@ int EIGEN_BLAS_FUNC(trmm)(const char *side, const char *uplo, const char *opa, c const RealScalar *palpha, const RealScalar *pa, const int *lda, RealScalar *pb, const int *ldb) { // std::cerr << "in trmm " << *side << " " << *uplo << " " << *opa << " " << *diag << " " << *m << " " << *n << " " << *lda << " " << *ldb << " " << *palpha << "\n"; - typedef void (*functype)(DenseIndex, DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, const Scalar&, internal::level3_blocking&); + typedef void (*functype)(DenseIndex, DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, DenseIndex, const Scalar&, internal::level3_blocking&); static const functype func[32] = { // array index: NOTR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0 }; @@ -272,12 +272,12 @@ int EIGEN_BLAS_FUNC(trmm)(const char *side, const char *uplo, const char *opa, c if(SIDE(*side)==LEFT) { internal::gemm_blocking_space blocking(*m,*n,*m,1,false); - func[code](*m, *n, *m, a, *lda, tmp.data(), tmp.outerStride(), b, *ldb, alpha, blocking); + func[code](*m, *n, *m, a, *lda, tmp.data(), tmp.outerStride(), b, 1, *ldb, alpha, blocking); } else { internal::gemm_blocking_space blocking(*m,*n,*n,1,false); - func[code](*m, *n, *n, tmp.data(), tmp.outerStride(), a, *lda, b, *ldb, alpha, blocking); + func[code](*m, *n, *n, tmp.data(), tmp.outerStride(), a, *lda, b, 1, *ldb, alpha, blocking); } return 1; } @@ -338,12 +338,12 @@ int EIGEN_BLAS_FUNC(symm)(const char *side, const char *uplo, const int *m, cons internal::gemm_blocking_space blocking(*m,*n,size,1,false); if(SIDE(*side)==LEFT) - if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); - else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); + if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, 1, *ldc, alpha, blocking); + else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, 1, *ldc, alpha, blocking); else return 0; else if(SIDE(*side)==RIGHT) - if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking); - else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking); + if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, 1, *ldc, alpha, blocking); + else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, 1, *ldc, alpha, blocking); else return 0; else return 0; @@ -359,21 +359,21 @@ int EIGEN_BLAS_FUNC(syrk)(const char *uplo, const char *op, const int *n, const { // std::cerr << "in syrk " << *uplo << " " << *op << " " << *n << " " << *k << " " << *palpha << " " << *lda << " " << *pbeta << " " << *ldc << "\n"; #if !ISCOMPLEX - typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, const Scalar&, internal::level3_blocking&); + typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, DenseIndex, const Scalar&, internal::level3_blocking&); static const functype func[8] = { // array index: NOTR | (UP << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), // array index: TR | (UP << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), // array index: ADJ | (UP << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0, // array index: NOTR | (LO << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), // array index: TR | (LO << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), // array index: ADJ | (LO << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0 }; #endif @@ -426,7 +426,7 @@ int EIGEN_BLAS_FUNC(syrk)(const char *uplo, const char *op, const int *n, const internal::gemm_blocking_space blocking(*n,*n,*k,1,false); int code = OP(*op) | (UPLO(*uplo) << 2); - func[code](*n, *k, a, *lda, a, *lda, c, *ldc, alpha, blocking); + func[code](*n, *k, a, *lda, a, *lda, c, 1, *ldc, alpha, blocking); #endif return 0; @@ -537,18 +537,18 @@ int EIGEN_BLAS_FUNC(hemm)(const char *side, const char *uplo, const int *m, cons if(SIDE(*side)==LEFT) { - if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix - ::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); - else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix - ::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); + if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix + ::run(*m, *n, a, *lda, b, *ldb, c, 1, *ldc, alpha, blocking); + else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix + ::run(*m, *n, a, *lda, b, *ldb, c, 1, *ldc, alpha, blocking); else return 0; } else if(SIDE(*side)==RIGHT) { - if(UPLO(*uplo)==UP) matrix(c,*m,*n,*ldc) += alpha * matrix(b,*m,*n,*ldb) * matrix(a,*n,*n,*lda).selfadjointView();/*internal::product_selfadjoint_matrix - ::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking);*/ - else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix - ::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking); + if(UPLO(*uplo)==UP) matrix(c,*m,*n,*ldc) += alpha * matrix(b,*m,*n,*ldb) * matrix(a,*n,*n,*lda).selfadjointView();/*internal::product_selfadjoint_matrix + ::run(*m, *n, b, *ldb, a, *lda, c, 1, *ldc, alpha, blocking);*/ + else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix + ::run(*m, *n, b, *ldb, a, *lda, c, 1, *ldc, alpha, blocking); else return 0; } else @@ -566,19 +566,19 @@ int EIGEN_BLAS_FUNC(herk)(const char *uplo, const char *op, const int *n, const { // std::cerr << "in herk " << *uplo << " " << *op << " " << *n << " " << *k << " " << *palpha << " " << *lda << " " << *pbeta << " " << *ldc << "\n"; - typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, const Scalar&, internal::level3_blocking&); + typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, DenseIndex, const Scalar&, internal::level3_blocking&); static const functype func[8] = { // array index: NOTR | (UP << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0, // array index: ADJ | (UP << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0, // array index: NOTR | (LO << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0, // array index: ADJ | (LO << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0 }; @@ -620,7 +620,7 @@ int EIGEN_BLAS_FUNC(herk)(const char *uplo, const char *op, const int *n, const if(*k>0 && alpha!=RealScalar(0)) { internal::gemm_blocking_space blocking(*n,*n,*k,1,false); - func[code](*n, *k, a, *lda, a, *lda, c, *ldc, alpha, blocking); + func[code](*n, *k, a, *lda, a, *lda, c, 1, *ldc, alpha, blocking); matrix(c, *n, *n, *ldc).diagonal().imag().setZero(); } return 0; diff --git a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake index a92a2978b..3d0074c71 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake @@ -677,6 +677,8 @@ macro(ei_set_build_string) set(TMP_BUILD_STRING ${TMP_BUILD_STRING}-cxx11) endif() + set(TMP_BUILD_STRING ${TMP_BUILD_STRING}-v3.3) + if(EIGEN_BUILD_STRING_SUFFIX) set(TMP_BUILD_STRING ${TMP_BUILD_STRING}-${EIGEN_BUILD_STRING_SUFFIX}) endif() diff --git a/gtsam/3rdparty/Eigen/cmake/FindStandardMathLibrary.cmake b/gtsam/3rdparty/Eigen/cmake/FindStandardMathLibrary.cmake index 711b0e4b4..337f1b304 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindStandardMathLibrary.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindStandardMathLibrary.cmake @@ -19,8 +19,11 @@ include(CheckCXXSourceCompiles) # notice the std:: is required on some platforms such as QNX set(find_standard_math_library_test_program -"#include -int main() { std::sin(0.0); std::log(0.0f); }") +" +#include +int main(int argc, char **){ + return int(std::sin(double(argc)) + std::log(double(argc))); +}") # first try compiling/linking the test program without any linker flags diff --git a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt index 8ff755988..179824dd1 100644 --- a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt @@ -11,7 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX) endif(CMAKE_COMPILER_IS_GNUCXX) option(EIGEN_INTERNAL_DOCUMENTATION "Build internal documentation" OFF) - +option(EIGEN_DOC_USE_MATHJAX "Use MathJax for rendering math in HTML docs" ON) # Set some Doxygen flags set(EIGEN_DOXY_PROJECT_NAME "Eigen") @@ -19,12 +19,19 @@ set(EIGEN_DOXY_OUTPUT_DIRECTORY_SUFFIX "") set(EIGEN_DOXY_INPUT "\"${Eigen_SOURCE_DIR}/Eigen\" \"${Eigen_SOURCE_DIR}/doc\"") set(EIGEN_DOXY_HTML_COLORSTYLE_HUE "220") set(EIGEN_DOXY_TAGFILES "") + if(EIGEN_INTERNAL_DOCUMENTATION) set(EIGEN_DOXY_INTERNAL "YES") else(EIGEN_INTERNAL_DOCUMENTATION) set(EIGEN_DOXY_INTERNAL "NO") endif(EIGEN_INTERNAL_DOCUMENTATION) +if (EIGEN_DOC_USE_MATHJAX) + set(EIGEN_DOXY_USE_MATHJAX "YES") +else () + set(EIGEN_DOXY_USE_MATHJAX "NO") +endif() + configure_file( ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile diff --git a/gtsam/3rdparty/Eigen/doc/CustomizingEigen_CustomScalar.dox b/gtsam/3rdparty/Eigen/doc/CustomizingEigen_CustomScalar.dox index 1ee78cbe5..24e5f563b 100644 --- a/gtsam/3rdparty/Eigen/doc/CustomizingEigen_CustomScalar.dox +++ b/gtsam/3rdparty/Eigen/doc/CustomizingEigen_CustomScalar.dox @@ -75,7 +75,7 @@ namespace Eigen { static inline Real epsilon() { return 0; } static inline Real dummy_precision() { return 0; } - static inline Real digits10() { return 0; } + static inline int digits10() { return 0; } enum { IsInteger = 0, diff --git a/gtsam/3rdparty/Eigen/doc/Doxyfile.in b/gtsam/3rdparty/Eigen/doc/Doxyfile.in index 37948a612..ac6eafcf9 100644 --- a/gtsam/3rdparty/Eigen/doc/Doxyfile.in +++ b/gtsam/3rdparty/Eigen/doc/Doxyfile.in @@ -736,6 +736,14 @@ EXCLUDE = "${Eigen_SOURCE_DIR}/Eigen/src/Core/products" \ "${Eigen_SOURCE_DIR}/unsupported/doc/examples" \ "${Eigen_SOURCE_DIR}/unsupported/doc/snippets" +# Forward declarations of class templates cause the title of the main page for +# the class template to not contain the template signature. This only happens +# when the \class command is used to document the class. Possibly caused +# by https://github.com/doxygen/doxygen/issues/7698. Confirmed fixed by +# doxygen release 1.8.19. + +EXCLUDE += "${Eigen_SOURCE_DIR}/Eigen/src/Core/util/ForwardDeclarations.h" + # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded # from the input. @@ -1245,7 +1253,7 @@ FORMULA_TRANSPARENT = YES # output. When enabled you may also need to install MathJax separately and # configure the path to it using the MATHJAX_RELPATH option. -USE_MATHJAX = NO +USE_MATHJAX = @EIGEN_DOXY_USE_MATHJAX@ # When MathJax is enabled you need to specify the location relative to the # HTML output directory using the MATHJAX_RELPATH option. The destination @@ -1257,12 +1265,12 @@ USE_MATHJAX = NO # However, it is strongly recommended to install a local # copy of MathJax from http://www.mathjax.org before deployment. -MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest +MATHJAX_RELPATH = https://cdn.mathjax.org/mathjax/latest # The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension # names that should be enabled during MathJax rendering. -MATHJAX_EXTENSIONS = +MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols # When the SEARCHENGINE tag is enabled doxygen will generate a search box # for the HTML output. The underlying search engine uses javascript @@ -1609,6 +1617,9 @@ PREDEFINED = EIGEN_EMPTY_STRUCT \ EXPAND_AS_DEFINED = EIGEN_MAKE_TYPEDEFS \ EIGEN_MAKE_FIXED_TYPEDEFS \ EIGEN_MAKE_TYPEDEFS_ALL_SIZES \ + EIGEN_MAKE_ARRAY_TYPEDEFS \ + EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS \ + EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES \ EIGEN_CWISE_UNOP_RETURN_TYPE \ EIGEN_CWISE_BINOP_RETURN_TYPE \ EIGEN_CURRENT_STORAGE_BASE_CLASS \ diff --git a/gtsam/3rdparty/Eigen/doc/Pitfalls.dox b/gtsam/3rdparty/Eigen/doc/Pitfalls.dox index 3f395053d..fda402572 100644 --- a/gtsam/3rdparty/Eigen/doc/Pitfalls.dox +++ b/gtsam/3rdparty/Eigen/doc/Pitfalls.dox @@ -7,14 +7,30 @@ namespace Eigen { See this \link TopicTemplateKeyword page \endlink. + \section TopicPitfalls_aliasing Aliasing Don't miss this \link TopicAliasing page \endlink on aliasing, especially if you got wrong results in statements where the destination appears on the right hand side of the expression. + +\section TopicPitfalls_alignment_issue Alignment Issues (runtime assertion) + +%Eigen does explicit vectorization, and while that is appreciated by many users, that also leads to some issues in special situations where data alignment is compromised. +Indeed, since C++17, C++ does not have quite good enough support for explicit data alignment. +In that case your program hits an assertion failure (that is, a "controlled crash") with a message that tells you to consult this page: +\code +http://eigen.tuxfamily.org/dox/group__TopicUnalignedArrayAssert.html +\endcode +Have a look at \link TopicUnalignedArrayAssert it \endlink and see for yourself if that's something that you can cope with. +It contains detailed information about how to deal with each known cause for that issue. + +Now what if you don't care about vectorization and so don't want to be annoyed with these alignment issues? Then read \link getrid how to get rid of them \endlink. + + \section TopicPitfalls_auto_keyword C++11 and the auto keyword -In short: do not use the auto keywords with Eigen's expressions, unless you are 100% sure about what you are doing. In particular, do not use the auto keyword as a replacement for a Matrix<> type. Here is an example: +In short: do not use the auto keywords with %Eigen's expressions, unless you are 100% sure about what you are doing. In particular, do not use the auto keyword as a replacement for a \c Matrix<> type. Here is an example: \code MatrixXd A, B; @@ -22,23 +38,81 @@ auto C = A*B; for(...) { ... w = C * v; ...} \endcode -In this example, the type of C is not a MatrixXd but an abstract expression representing a matrix product and storing references to A and B. Therefore, the product of A*B will be carried out multiple times, once per iteration of the for loop. Moreover, if the coefficients of A or B change during the iteration, then C will evaluate to different values. +In this example, the type of C is not a \c MatrixXd but an abstract expression representing a matrix product and storing references to \c A and \c B. +Therefore, the product of \c A*B will be carried out multiple times, once per iteration of the for loop. +Moreover, if the coefficients of A or B change during the iteration, then C will evaluate to different values. Here is another example leading to a segfault: \code auto C = ((A+B).eval()).transpose(); // do something with C \endcode -The problem is that eval() returns a temporary object (in this case a MatrixXd) which is then referenced by the Transpose<> expression. However, this temporary is deleted right after the first line, and there the C expression reference a dead object. The same issue might occur when sub expressions are automatically evaluated by Eigen as in the following example: +The problem is that \c eval() returns a temporary object (in this case a \c MatrixXd) which is then referenced by the \c Transpose<> expression. +However, this temporary is deleted right after the first line, and then the \c C expression references a dead object. +One possible fix consists in applying \c eval() on the whole expression: +\code +auto C = (A+B).transpose().eval(); +\endcode + +The same issue might occur when sub expressions are automatically evaluated by %Eigen as in the following example: \code VectorXd u, v; auto C = u + (A*v).normalized(); // do something with C \endcode -where the normalized() method has to evaluate the expensive product A*v to avoid evaluating it twice. On the other hand, the following example is perfectly fine: +Here the \c normalized() method has to evaluate the expensive product \c A*v to avoid evaluating it twice. +Again, one possible fix is to call \c .eval() on the whole expression: \code auto C = (u + (A*v).normalized()).eval(); \endcode -In this case, C will be a regular VectorXd object. +In this case, \c C will be a regular \c VectorXd object. +Note that DenseBase::eval() is smart enough to avoid copies when the underlying expression is already a plain \c Matrix<>. + + +\section TopicPitfalls_header_issues Header Issues (failure to compile) + +With all libraries, one must check the documentation for which header to include. +The same is true with %Eigen, but slightly worse: with %Eigen, a method in a class may require an additional #include over what the class itself requires! +For example, if you want to use the \c cross() method on a vector (it computes a cross-product) then you need to: +\code +#include +\endcode +We try to always document this, but do tell us if we forgot an occurrence. + + +\section TopicPitfalls_ternary_operator Ternary operator + +In short: avoid the use of the ternary operator (COND ? THEN : ELSE) with %Eigen's expressions for the \c THEN and \c ELSE statements. +To see why, let's consider the following example: +\code +Vector3f A; +A << 1, 2, 3; +Vector3f B = ((1 < 0) ? (A.reverse()) : A); +\endcode +This example will return B = 3, 2, 1. Do you see why? +The reason is that in c++ the type of the \c ELSE statement is inferred from the type of the \c THEN expression such that both match. +Since \c THEN is a Reverse, the \c ELSE statement A is converted to a Reverse, and the compiler thus generates: +\code +Vector3f B = ((1 < 0) ? (A.reverse()) : Reverse(A)); +\endcode +In this very particular case, a workaround would be to call A.reverse().eval() for the \c THEN statement, but the safest and fastest is really to avoid this ternary operator with %Eigen's expressions and use a if/else construct. + + +\section TopicPitfalls_pass_by_value Pass-by-value + +If you don't know why passing-by-value is wrong with %Eigen, read this \link TopicPassingByValue page \endlink first. + +While you may be extremely careful and use care to make sure that all of your code that explicitly uses %Eigen types is pass-by-reference you have to watch out for templates which define the argument types at compile time. + +If a template has a function that takes arguments pass-by-value, and the relevant template parameter ends up being an %Eigen type, then you will of course have the same alignment problems that you would in an explicitly defined function passing %Eigen types by reference. + +Using %Eigen types with other third party libraries or even the STL can present the same problem. +boost::bind for example uses pass-by-value to store arguments in the returned functor. +This will of course be a problem. + +There are at least two ways around this: + - If the value you are passing is guaranteed to be around for the life of the functor, you can use boost::ref() to wrap the value as you pass it to boost::bind. Generally this is not a solution for values on the stack as if the functor ever gets passed to a lower or independent scope, the object may be gone by the time it's attempted to be used. + - The other option is to make your functions take a reference counted pointer like boost::shared_ptr as the argument. This avoids needing to worry about managing the lifetime of the object being passed. + */ } diff --git a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox index a25622e80..653bf33ef 100644 --- a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox +++ b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox @@ -244,7 +244,7 @@ As stated earlier, for a read-write sub-matrix (RW), the evaluation can be done \code sm1.valuePtr(); // Pointer to the values -sm1.innerIndextr(); // Pointer to the indices. +sm1.innerIndexPtr(); // Pointer to the indices. sm1.outerIndexPtr(); // Pointer to the beginning of each inner vector \endcode diff --git a/gtsam/3rdparty/Eigen/doc/TopicLazyEvaluation.dox b/gtsam/3rdparty/Eigen/doc/TopicLazyEvaluation.dox index 101ef8c72..d2a704f13 100644 --- a/gtsam/3rdparty/Eigen/doc/TopicLazyEvaluation.dox +++ b/gtsam/3rdparty/Eigen/doc/TopicLazyEvaluation.dox @@ -2,63 +2,95 @@ namespace Eigen { /** \page TopicLazyEvaluation Lazy Evaluation and Aliasing -Executive summary: Eigen has intelligent compile-time mechanisms to enable lazy evaluation and removing temporaries where appropriate. +Executive summary: %Eigen has intelligent compile-time mechanisms to enable lazy evaluation and removing temporaries where appropriate. It will handle aliasing automatically in most cases, for example with matrix products. The automatic behavior can be overridden manually by using the MatrixBase::eval() and MatrixBase::noalias() methods. When you write a line of code involving a complex expression such as -\code mat1 = mat2 + mat3 * (mat4 + mat5); \endcode +\code mat1 = mat2 + mat3 * (mat4 + mat5); +\endcode -Eigen determines automatically, for each sub-expression, whether to evaluate it into a temporary variable. Indeed, in certain cases it is better to evaluate immediately a sub-expression into a temporary variable, while in other cases it is better to avoid that. +%Eigen determines automatically, for each sub-expression, whether to evaluate it into a temporary variable. Indeed, in certain cases it is better to evaluate a sub-expression into a temporary variable, while in other cases it is better to avoid that. A traditional math library without expression templates always evaluates all sub-expressions into temporaries. So with this code, -\code vec1 = vec2 + vec3; \endcode +\code vec1 = vec2 + vec3; +\endcode a traditional library would evaluate \c vec2 + vec3 into a temporary \c vec4 and then copy \c vec4 into \c vec1. This is of course inefficient: the arrays are traversed twice, so there are a lot of useless load/store operations. -Expression-templates-based libraries can avoid evaluating sub-expressions into temporaries, which in many cases results in large speed improvements. This is called lazy evaluation as an expression is getting evaluated as late as possible, instead of immediately. However, most other expression-templates-based libraries always choose lazy evaluation. There are two problems with that: first, lazy evaluation is not always a good choice for performance; second, lazy evaluation can be very dangerous, for example with matrix products: doing matrix = matrix*matrix gives a wrong result if the matrix product is lazy-evaluated, because of the way matrix product works. +Expression-templates-based libraries can avoid evaluating sub-expressions into temporaries, which in many cases results in large speed improvements. +This is called lazy evaluation as an expression is getting evaluated as late as possible. +In %Eigen all expressions are lazy-evaluated. +More precisely, an expression starts to be evaluated once it is assigned to a matrix. +Until then nothing happens beyond constructing the abstract expression tree. +In contrast to most other expression-templates-based libraries, however, %Eigen might choose to evaluate some sub-expressions into temporaries. +There are two reasons for that: first, pure lazy evaluation is not always a good choice for performance; second, pure lazy evaluation can be very dangerous, for example with matrix products: doing mat = mat*mat gives a wrong result if the matrix product is directly evaluated within the destination matrix, because of the way matrix product works. -For these reasons, Eigen has intelligent compile-time mechanisms to determine automatically when to use lazy evaluation, and when on the contrary it should evaluate immediately into a temporary variable. +For these reasons, %Eigen has intelligent compile-time mechanisms to determine automatically which sub-expression should be evaluated into a temporary variable. So in the basic example, -\code matrix1 = matrix2 + matrix3; \endcode +\code mat1 = mat2 + mat3; +\endcode -Eigen chooses lazy evaluation. Thus the arrays are traversed only once, producing optimized code. If you really want to force immediate evaluation, use \link MatrixBase::eval() eval()\endlink: +%Eigen chooses not to introduce any temporary. Thus the arrays are traversed only once, producing optimized code. +If you really want to force immediate evaluation, use \link MatrixBase::eval() eval()\endlink: -\code matrix1 = (matrix2 + matrix3).eval(); \endcode +\code mat1 = (mat2 + mat3).eval(); +\endcode Here is now a more involved example: -\code matrix1 = -matrix2 + matrix3 + 5 * matrix4; \endcode +\code mat1 = -mat2 + mat3 + 5 * mat4; +\endcode -Eigen chooses lazy evaluation at every stage in that example, which is clearly the correct choice. In fact, lazy evaluation is the "default choice" and Eigen will choose it except in a few circumstances. +Here again %Eigen won't introduce any temporary, thus producing a single fused evaluation loop, which is clearly the correct choice. -The first circumstance in which Eigen chooses immediate evaluation, is when it sees an assignment a = b; and the expression \c b has the evaluate-before-assigning \link flags flag\endlink. The most important example of such an expression is the \link Product matrix product expression\endlink. For example, when you do +\section TopicLazyEvaluationWhichExpr Which sub-expressions are evaluated into temporaries? -\code matrix = matrix * matrix; \endcode +The default evaluation strategy is to fuse the operations in a single loop, and %Eigen will choose it except in a few circumstances. -Eigen first evaluates matrix * matrix into a temporary matrix, and then copies it into the original \c matrix. This guarantees a correct result as we saw above that lazy evaluation gives wrong results with matrix products. It also doesn't cost much, as the cost of the matrix product itself is much higher. +The first circumstance in which %Eigen chooses to evaluate a sub-expression is when it sees an assignment a = b; and the expression \c b has the evaluate-before-assigning \link flags flag\endlink. +The most important example of such an expression is the \link Product matrix product expression\endlink. For example, when you do + +\code mat = mat * mat; +\endcode + +%Eigen will evaluate mat * mat into a temporary matrix, and then copies it into the original \c mat. +This guarantees a correct result as we saw above that lazy evaluation gives wrong results with matrix products. +It also doesn't cost much, as the cost of the matrix product itself is much higher. +Note that this temporary is introduced at evaluation time only, that is, within operator= in this example. +The expression mat * mat still return a abstract product type. What if you know that the result does no alias the operand of the product and want to force lazy evaluation? Then use \link MatrixBase::noalias() .noalias()\endlink instead. Here is an example: -\code matrix1.noalias() = matrix2 * matrix2; \endcode +\code mat1.noalias() = mat2 * mat2; +\endcode -Here, since we know that matrix2 is not the same matrix as matrix1, we know that lazy evaluation is not dangerous, so we may force lazy evaluation. Concretely, the effect of noalias() here is to bypass the evaluate-before-assigning \link flags flag\endlink. +Here, since we know that mat2 is not the same matrix as mat1, we know that lazy evaluation is not dangerous, so we may force lazy evaluation. Concretely, the effect of noalias() here is to bypass the evaluate-before-assigning \link flags flag\endlink. -The second circumstance in which Eigen chooses immediate evaluation, is when it sees a nested expression such as a + b where \c b is already an expression having the evaluate-before-nesting \link flags flag\endlink. Again, the most important example of such an expression is the \link Product matrix product expression\endlink. For example, when you do +The second circumstance in which %Eigen chooses to evaluate a sub-expression, is when it sees a nested expression such as a + b where \c b is already an expression having the evaluate-before-nesting \link flags flag\endlink. +Again, the most important example of such an expression is the \link Product matrix product expression\endlink. +For example, when you do -\code matrix1 = matrix2 + matrix3 * matrix4; \endcode +\code mat1 = mat2 * mat3 + mat4 * mat5; +\endcode -the product matrix3 * matrix4 gets evaluated immediately into a temporary matrix. Indeed, experiments showed that it is often beneficial for performance to evaluate immediately matrix products when they are nested into bigger expressions. +the products mat2 * mat3 and mat4 * mat5 gets evaluated separately into temporary matrices before being summed up in mat1. +Indeed, to be efficient matrix products need to be evaluated within a destination matrix at hand, and not as simple "dot products". +For small matrices, however, you might want to enforce a "dot-product" based lazy evaluation with lazyProduct(). +Again, it is important to understand that those temporaries are created at evaluation time only, that is in operator =. +See TopicPitfalls_auto_keyword for common pitfalls regarding this remark. -The third circumstance in which Eigen chooses immediate evaluation, is when its cost model shows that the total cost of an operation is reduced if a sub-expression gets evaluated into a temporary. Indeed, in certain cases, an intermediate result is sufficiently costly to compute and is reused sufficiently many times, that is worth "caching". Here is an example: +The third circumstance in which %Eigen chooses to evaluate a sub-expression, is when its cost model shows that the total cost of an operation is reduced if a sub-expression gets evaluated into a temporary. +Indeed, in certain cases, an intermediate result is sufficiently costly to compute and is reused sufficiently many times, that is worth "caching". Here is an example: -\code matrix1 = matrix2 * (matrix3 + matrix4); \endcode +\code mat1 = mat2 * (mat3 + mat4); +\endcode -Here, provided the matrices have at least 2 rows and 2 columns, each coefficienct of the expression matrix3 + matrix4 is going to be used several times in the matrix product. Instead of computing the sum everytime, it is much better to compute it once and store it in a temporary variable. Eigen understands this and evaluates matrix3 + matrix4 into a temporary variable before evaluating the product. +Here, provided the matrices have at least 2 rows and 2 columns, each coefficient of the expression mat3 + mat4 is going to be used several times in the matrix product. Instead of computing the sum every time, it is much better to compute it once and store it in a temporary variable. %Eigen understands this and evaluates mat3 + mat4 into a temporary variable before evaluating the product. */ diff --git a/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox b/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox index 47c9b261f..a2855745b 100644 --- a/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox +++ b/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox @@ -49,6 +49,7 @@ int main(int argc, char** argv) In the case your application is parallelized with OpenMP, you might want to disable Eigen's own parallization as detailed in the previous section. +\warning Using OpenMP with custom scalar types that might throw exceptions can lead to unexpected behaviour in the event of throwing. */ } diff --git a/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox b/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox index 2e1420f98..723f4dbce 100644 --- a/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox +++ b/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox @@ -232,8 +232,8 @@ On the other hand, since there exist 24 different conventions, they are pretty c to create a rotation matrix according to the 2-1-2 convention.\code Matrix3f m; m = AngleAxisf(angle1, Vector3f::UnitZ()) - * AngleAxisf(angle2, Vector3f::UnitY()) - * AngleAxisf(angle3, Vector3f::UnitZ()); + * * AngleAxisf(angle2, Vector3f::UnitY()) + * * AngleAxisf(angle3, Vector3f::UnitZ()); \endcode diff --git a/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js b/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js index a6f8c3428..afb97edf5 100644 --- a/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js +++ b/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js @@ -5,6 +5,7 @@ function generate_autotoc() { if(headers.length > 1) { var toc = $("#side-nav").append(''); toc = $("#nav-toc"); + var footer = $("#nav-path"); var footerHeight = footer.height(); toc = toc.append('
    '); toc = toc.find('ul'); @@ -137,7 +138,7 @@ function initNavTree(toroot,relpath) } }) - $(window).load(showRoot); + $(window).on("load", showRoot); } // return false if the the node has no children at all, or has only section/subsection children @@ -241,6 +242,6 @@ $(document).ready(function() { } })(); - $(window).load(resizeHeight); + $(window).on("load", resizeHeight); }); diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in index 9ac0596cb..126653589 100644 --- a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in +++ b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in @@ -17,19 +17,6 @@ $generatedby   - - - - diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in b/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in index bb149f8f0..a6b1c1d08 100644 --- a/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in +++ b/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in @@ -20,6 +20,9 @@ $mathjax + + +
    diff --git a/gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_block_assignment.cpp b/gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_block_assignment.cpp index 76f49f2fb..0b87313a1 100644 --- a/gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_block_assignment.cpp +++ b/gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_block_assignment.cpp @@ -14,5 +14,5 @@ int main() a.block<2,2>(1,1) = m; cout << "Here is now a with m copied into its central 2x2 block:" << endl << a << endl << endl; a.block(0,0,2,3) = a.block(2,1,2,3); - cout << "Here is now a with bottom-right 2x3 block copied into top-left 2x2 block:" << endl << a << endl << endl; + cout << "Here is now a with bottom-right 2x3 block copied into top-left 2x3 block:" << endl << a << endl << endl; } diff --git a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt index 9aa209faa..fbecd6624 100644 --- a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt @@ -103,9 +103,9 @@ endif() add_dependencies(lapack eigen_lapack eigen_lapack_static) install(TARGETS eigen_lapack eigen_lapack_static - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) @@ -133,12 +133,14 @@ if(EXISTS ${eigen_full_path_to_testing_lapack}) string(REGEX REPLACE "(.*)/STACK:(.*) (.*)" "\\1/STACK:900000000000000000 \\3" CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS}") endif() + file(MAKE_DIRECTORY "${LAPACK_BINARY_DIR}/TESTING") add_subdirectory(testing/MATGEN) add_subdirectory(testing/LIN) add_subdirectory(testing/EIG) + cmake_policy(SET CMP0026 OLD) macro(add_lapack_test output input target) set(TEST_INPUT "${LAPACK_SOURCE_DIR}/testing/${input}") - set(TEST_OUTPUT "${LAPACK_BINARY_DIR}/testing/${output}") + set(TEST_OUTPUT "${LAPACK_BINARY_DIR}/TESTING/${output}") get_target_property(TEST_LOC ${target} LOCATION) string(REPLACE "." "_" input_name ${input}) set(testName "${target}_${input_name}") diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index 0747aa6cb..47e6fee4b 100644 --- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt @@ -163,7 +163,7 @@ ei_add_test(constructor) ei_add_test(linearstructure) ei_add_test(integer_types) ei_add_test(unalignedcount) -if(NOT EIGEN_TEST_NO_EXCEPTIONS) +if(NOT EIGEN_TEST_NO_EXCEPTIONS AND NOT EIGEN_TEST_OPENMP) ei_add_test(exceptions) endif() ei_add_test(redux) @@ -185,7 +185,7 @@ ei_add_test(smallvectors) ei_add_test(mapped_matrix) ei_add_test(mapstride) ei_add_test(mapstaticmethods) -ei_add_test(array) +ei_add_test(array_cwise) ei_add_test(array_for_matrix) ei_add_test(array_replicate) ei_add_test(array_reverse) diff --git a/gtsam/3rdparty/Eigen/test/array.cpp b/gtsam/3rdparty/Eigen/test/array_cwise.cpp similarity index 98% rename from gtsam/3rdparty/Eigen/test/array.cpp rename to gtsam/3rdparty/Eigen/test/array_cwise.cpp index 7afd3ed3f..7c5709dbe 100644 --- a/gtsam/3rdparty/Eigen/test/array.cpp +++ b/gtsam/3rdparty/Eigen/test/array_cwise.cpp @@ -279,7 +279,7 @@ template void array_real(const ArrayType& m) VERIFY_IS_APPROX(m1.sign() * m1.abs(), m1); VERIFY_IS_APPROX(numext::abs2(numext::real(m1)) + numext::abs2(numext::imag(m1)), numext::abs2(m1)); - VERIFY_IS_APPROX(numext::abs2(real(m1)) + numext::abs2(imag(m1)), numext::abs2(m1)); + VERIFY_IS_APPROX(numext::abs2(Eigen::real(m1)) + numext::abs2(Eigen::imag(m1)), numext::abs2(m1)); if(!NumTraits::IsComplex) VERIFY_IS_APPROX(numext::real(m1), m1); @@ -368,7 +368,7 @@ template void array_complex(const ArrayType& m) for (Index i = 0; i < m.rows(); ++i) for (Index j = 0; j < m.cols(); ++j) - m3(i,j) = std::atan2(imag(m1(i,j)), real(m1(i,j))); + m3(i,j) = std::atan2(m1(i,j).imag(), m1(i,j).real()); VERIFY_IS_APPROX(arg(m1), m3); std::complex zero(0.0,0.0); @@ -395,7 +395,7 @@ template void array_complex(const ArrayType& m) VERIFY_IS_APPROX(inverse(inverse(m1)),m1); VERIFY_IS_APPROX(conj(m1.conjugate()), m1); - VERIFY_IS_APPROX(abs(m1), sqrt(square(real(m1))+square(imag(m1)))); + VERIFY_IS_APPROX(abs(m1), sqrt(square(m1.real())+square(m1.imag()))); VERIFY_IS_APPROX(abs(m1), sqrt(abs2(m1))); VERIFY_IS_APPROX(log10(m1), log(m1)/log(10)); @@ -446,7 +446,7 @@ template void min_max(const ArrayType& m) } -void test_array() +void test_array_cwise() { for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST_1( array(Array()) ); diff --git a/gtsam/3rdparty/Eigen/test/bdcsvd.cpp b/gtsam/3rdparty/Eigen/test/bdcsvd.cpp index 6c7b09696..3ca273635 100644 --- a/gtsam/3rdparty/Eigen/test/bdcsvd.cpp +++ b/gtsam/3rdparty/Eigen/test/bdcsvd.cpp @@ -28,9 +28,13 @@ template void bdcsvd(const MatrixType& a = MatrixType(), bool pickrandom = true) { - MatrixType m = a; - if(pickrandom) + MatrixType m; + if(pickrandom) { + m.resizeLike(a); svd_fill_random(m); + } + else + m = a; CALL_SUBTEST(( svd_test_all_computation_options >(m, false) )); } diff --git a/gtsam/3rdparty/Eigen/test/constructor.cpp b/gtsam/3rdparty/Eigen/test/constructor.cpp index eec9e2192..988539951 100644 --- a/gtsam/3rdparty/Eigen/test/constructor.cpp +++ b/gtsam/3rdparty/Eigen/test/constructor.cpp @@ -20,6 +20,8 @@ template struct Wrapper inline operator MatrixType& () { return m_mat; } }; +enum my_sizes { M = 12, N = 7}; + template void ctor_init1(const MatrixType& m) { // Check logic in PlainObjectBase::_init1 @@ -81,4 +83,16 @@ void test_constructor() Array a(123); VERIFY_IS_EQUAL(a(4), 123.f); } + { + MatrixXi m1(M,N); + VERIFY_IS_EQUAL(m1.rows(),M); + VERIFY_IS_EQUAL(m1.cols(),N); + ArrayXXi a1(M,N); + VERIFY_IS_EQUAL(a1.rows(),M); + VERIFY_IS_EQUAL(a1.cols(),N); + VectorXi v1(M); + VERIFY_IS_EQUAL(v1.size(),M); + ArrayXi a2(M); + VERIFY_IS_EQUAL(a2.size(),M); + } } diff --git a/gtsam/3rdparty/Eigen/test/ctorleak.cpp b/gtsam/3rdparty/Eigen/test/ctorleak.cpp index c158f5e4e..d73fecfe2 100644 --- a/gtsam/3rdparty/Eigen/test/ctorleak.cpp +++ b/gtsam/3rdparty/Eigen/test/ctorleak.cpp @@ -8,7 +8,7 @@ struct Foo static Index object_limit; int dummy; - Foo() + Foo() : dummy(0) { #ifdef EIGEN_EXCEPTIONS // TODO: Is this the correct way to handle this? @@ -37,22 +37,33 @@ void test_ctorleak() { typedef Matrix MatrixX; typedef Matrix VectorX; + Foo::object_count = 0; for(int i = 0; i < g_repeat; i++) { Index rows = internal::random(2,EIGEN_TEST_MAX_SIZE), cols = internal::random(2,EIGEN_TEST_MAX_SIZE); - Foo::object_limit = internal::random(0, rows*cols - 2); + Foo::object_limit = rows*cols; + { + MatrixX r(rows, cols); + Foo::object_limit = r.size()+internal::random(0, rows*cols - 2); std::cout << "object_limit =" << Foo::object_limit << std::endl; #ifdef EIGEN_EXCEPTIONS try { #endif - std::cout << "\nMatrixX m(" << rows << ", " << cols << ");\n"; - MatrixX m(rows, cols); + if(internal::random()) { + std::cout << "\nMatrixX m(" << rows << ", " << cols << ");\n"; + MatrixX m(rows, cols); + } + else { + std::cout << "\nMatrixX m(r);\n"; + MatrixX m(r); + } #ifdef EIGEN_EXCEPTIONS VERIFY(false); // not reached if exceptions are enabled } catch (const Foo::Fail&) { /* ignore */ } #endif + } VERIFY_IS_EQUAL(Index(0), Foo::object_count); { @@ -66,4 +77,5 @@ void test_ctorleak() } VERIFY_IS_EQUAL(Index(0), Foo::object_count); } + std::cout << "\n"; } diff --git a/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp b/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp index 07bf65e03..5c1317569 100644 --- a/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp +++ b/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp @@ -67,7 +67,7 @@ template void eigensolver(const MatrixType& m) // Test matrix with NaN a(0,0) = std::numeric_limits::quiet_NaN(); EigenSolver eiNaN(a); - VERIFY_IS_EQUAL(eiNaN.info(), NoConvergence); + VERIFY_IS_NOT_EQUAL(eiNaN.info(), Success); } // regression test for bug 1098 diff --git a/gtsam/3rdparty/Eigen/test/exceptions.cpp b/gtsam/3rdparty/Eigen/test/exceptions.cpp index b83fb82ba..015b9fd33 100644 --- a/gtsam/3rdparty/Eigen/test/exceptions.cpp +++ b/gtsam/3rdparty/Eigen/test/exceptions.cpp @@ -109,5 +109,7 @@ void memoryleak() void test_exceptions() { - CALL_SUBTEST( memoryleak() ); + EIGEN_TRY { + CALL_SUBTEST( memoryleak() ); + } EIGEN_CATCH(...) {} } diff --git a/gtsam/3rdparty/Eigen/test/fastmath.cpp b/gtsam/3rdparty/Eigen/test/fastmath.cpp index cc5db0746..e84bdc972 100644 --- a/gtsam/3rdparty/Eigen/test/fastmath.cpp +++ b/gtsam/3rdparty/Eigen/test/fastmath.cpp @@ -43,11 +43,11 @@ void check_inf_nan(bool dryrun) { } else { - VERIFY( !(numext::isfinite)(m(3)) ); - VERIFY( !(numext::isinf)(m(3)) ); - VERIFY( (numext::isnan)(m(3)) ); - VERIFY( !m.allFinite() ); - VERIFY( m.hasNaN() ); + if( (std::isfinite)(m(3))) g_test_level=1; VERIFY( !(numext::isfinite)(m(3)) ); g_test_level=0; + if( (std::isinf) (m(3))) g_test_level=1; VERIFY( !(numext::isinf)(m(3)) ); g_test_level=0; + if(!(std::isnan) (m(3))) g_test_level=1; VERIFY( (numext::isnan)(m(3)) ); g_test_level=0; + if( (std::isfinite)(m(3))) g_test_level=1; VERIFY( !m.allFinite() ); g_test_level=0; + if(!(std::isnan) (m(3))) g_test_level=1; VERIFY( m.hasNaN() ); g_test_level=0; } T hidden_zero = (std::numeric_limits::min)()*(std::numeric_limits::min)(); m(4) /= hidden_zero; @@ -62,29 +62,29 @@ void check_inf_nan(bool dryrun) { } else { - VERIFY( !(numext::isfinite)(m(4)) ); - VERIFY( (numext::isinf)(m(4)) ); - VERIFY( !(numext::isnan)(m(4)) ); - VERIFY( !m.allFinite() ); - VERIFY( m.hasNaN() ); + if( (std::isfinite)(m(3))) g_test_level=1; VERIFY( !(numext::isfinite)(m(4)) ); g_test_level=0; + if(!(std::isinf) (m(3))) g_test_level=1; VERIFY( (numext::isinf)(m(4)) ); g_test_level=0; + if( (std::isnan) (m(3))) g_test_level=1; VERIFY( !(numext::isnan)(m(4)) ); g_test_level=0; + if( (std::isfinite)(m(3))) g_test_level=1; VERIFY( !m.allFinite() ); g_test_level=0; + if(!(std::isnan) (m(3))) g_test_level=1; VERIFY( m.hasNaN() ); g_test_level=0; } m(3) = 0; if(dryrun) { std::cout << "std::isfinite(" << m(3) << ") = "; check((std::isfinite)(m(3)),true); std::cout << " ; numext::isfinite = "; check((numext::isfinite)(m(3)), true); std::cout << "\n"; - std::cout << "std::isinf(" << m(3) << ") = "; check((std::isinf)(m(3)),false); std::cout << " ; numext::isinf = "; check((numext::isinf)(m(3)), false); std::cout << "\n"; - std::cout << "std::isnan(" << m(3) << ") = "; check((std::isnan)(m(3)),false); std::cout << " ; numext::isnan = "; check((numext::isnan)(m(3)), false); std::cout << "\n"; + std::cout << "std::isinf(" << m(3) << ") = "; check((std::isinf)(m(3)),false); std::cout << " ; numext::isinf = "; check((numext::isinf)(m(3)), false); std::cout << "\n"; + std::cout << "std::isnan(" << m(3) << ") = "; check((std::isnan)(m(3)),false); std::cout << " ; numext::isnan = "; check((numext::isnan)(m(3)), false); std::cout << "\n"; std::cout << "allFinite: "; check(m.allFinite(), 0); std::cout << "\n"; std::cout << "hasNaN: "; check(m.hasNaN(), 0); std::cout << "\n"; std::cout << "\n\n"; } else { - VERIFY( (numext::isfinite)(m(3)) ); - VERIFY( !(numext::isinf)(m(3)) ); - VERIFY( !(numext::isnan)(m(3)) ); - VERIFY( !m.allFinite() ); - VERIFY( !m.hasNaN() ); + if(!(std::isfinite)(m(3))) g_test_level=1; VERIFY( (numext::isfinite)(m(3)) ); g_test_level=0; + if( (std::isinf) (m(3))) g_test_level=1; VERIFY( !(numext::isinf)(m(3)) ); g_test_level=0; + if( (std::isnan) (m(3))) g_test_level=1; VERIFY( !(numext::isnan)(m(3)) ); g_test_level=0; + if( (std::isfinite)(m(3))) g_test_level=1; VERIFY( !m.allFinite() ); g_test_level=0; + if( (std::isnan) (m(3))) g_test_level=1; VERIFY( !m.hasNaN() ); g_test_level=0; } } diff --git a/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp b/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp index b64ea3bdc..4cf51aafb 100644 --- a/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp @@ -15,8 +15,9 @@ #include using namespace std; +// TODO not sure if this is actually still necessary anywhere ... template EIGEN_DONT_INLINE -void kill_extra_precision(T& x) { eigen_assert((void*)(&x) != (void*)0); } +void kill_extra_precision(T& ) { } template void alignedbox(const BoxType& _box) diff --git a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp index 8ee8fdb27..87680f1cc 100644 --- a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp @@ -244,6 +244,14 @@ template void mapQuaternion(void){ // is used to determine wether we can return a coeff by reference or not, which is not enough for Map. //const MCQuaternionUA& cmcq3(mcq3); //VERIFY( &cmcq3.x() == &mcq3.x() ); + + // test cast + { + Quaternion q1f = mq1.template cast(); + VERIFY_IS_APPROX(q1f.template cast(),mq1); + Quaternion q1d = mq1.template cast(); + VERIFY_IS_APPROX(q1d.template cast(),mq1); + } } template void quaternionAlignment(void){ diff --git a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp index 278e527c2..8d064ddc3 100755 --- a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp @@ -612,6 +612,62 @@ template void transform_products() VERIFY_IS_APPROX((ac*p).matrix(), a_m*p_m); } +template void transformations_no_scale() +{ + /* this test covers the following files: + Cross.h Quaternion.h, Transform.h + */ + typedef Matrix Vector3; + typedef Matrix Vector4; + typedef Quaternion Quaternionx; + typedef AngleAxis AngleAxisx; + typedef Transform Transform3; + typedef Translation Translation3; + typedef Matrix Matrix4; + + Vector3 v0 = Vector3::Random(), + v1 = Vector3::Random(); + + Transform3 t0, t1, t2; + + Scalar a = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); + + Quaternionx q1, q2; + + q1 = AngleAxisx(a, v0.normalized()); + + t0 = Transform3::Identity(); + VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); + + t0.setIdentity(); + t1.setIdentity(); + v1 = Vector3::Ones(); + t0.linear() = q1.toRotationMatrix(); + t0.pretranslate(v0); + t1.linear() = q1.conjugate().toRotationMatrix(); + t1.translate(-v0); + + VERIFY((t0 * t1).matrix().isIdentity(test_precision())); + + t1.fromPositionOrientationScale(v0, q1, v1); + VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); + VERIFY_IS_APPROX(t1*v1, t0*v1); + + // translation * vector + t0.setIdentity(); + t0.translate(v0); + VERIFY_IS_APPROX((t0 * v1).template head<3>(), Translation3(v0) * v1); + + // Conversion to matrix. + Transform3 t3; + t3.linear() = q1.toRotationMatrix(); + t3.translation() = v1; + Matrix4 m3 = t3.matrix(); + VERIFY((m3 * m3.inverse()).isIdentity(test_precision())); + // Verify implicit last row is initialized. + VERIFY_IS_APPROX(Vector4(m3.row(3)), Vector4(0.0, 0.0, 0.0, 1.0)); +} + void test_geo_transformations() { for(int i = 0; i < g_repeat; i++) { @@ -625,7 +681,7 @@ void test_geo_transformations() CALL_SUBTEST_3(( transformations() )); CALL_SUBTEST_3(( transformations() )); CALL_SUBTEST_3(( transform_alignment() )); - + CALL_SUBTEST_4(( transformations() )); CALL_SUBTEST_4(( non_projective_only() )); @@ -641,5 +697,8 @@ void test_geo_transformations() CALL_SUBTEST_8(( transform_associativity(Rotation2D(internal::random()*double(EIGEN_PI))) )); CALL_SUBTEST_8(( transform_associativity(Quaterniond::UnitRandom()) )); + + CALL_SUBTEST_9(( transformations_no_scale() )); + CALL_SUBTEST_9(( transformations_no_scale() )); } } diff --git a/gtsam/3rdparty/Eigen/test/inverse.cpp b/gtsam/3rdparty/Eigen/test/inverse.cpp index be607cc8b..d81af26c1 100644 --- a/gtsam/3rdparty/Eigen/test/inverse.cpp +++ b/gtsam/3rdparty/Eigen/test/inverse.cpp @@ -92,6 +92,22 @@ template void inverse(const MatrixType& m) } } +template +void inverse_zerosized() +{ + Matrix A(0,0); + { + Matrix b, x; + x = A.inverse() * b; + } + { + Matrix b(0,1), x; + x = A.inverse() * b; + VERIFY_IS_EQUAL(x.rows(), 0); + VERIFY_IS_EQUAL(x.cols(), 1); + } +} + void test_inverse() { int s = 0; @@ -105,6 +121,7 @@ void test_inverse() s = internal::random(50,320); CALL_SUBTEST_5( inverse(MatrixXf(s,s)) ); TEST_SET_BUT_UNUSED_VARIABLE(s) + CALL_SUBTEST_5( inverse_zerosized() ); s = internal::random(25,100); CALL_SUBTEST_6( inverse(MatrixXcd(s,s)) ); diff --git a/gtsam/3rdparty/Eigen/test/main.h b/gtsam/3rdparty/Eigen/test/main.h index 8c868ee79..18bb5c825 100644 --- a/gtsam/3rdparty/Eigen/test/main.h +++ b/gtsam/3rdparty/Eigen/test/main.h @@ -72,6 +72,11 @@ #define isnan(X) please_protect_your_isnan_with_parentheses #define isinf(X) please_protect_your_isinf_with_parentheses #define isfinite(X) please_protect_your_isfinite_with_parentheses + +// test possible conflicts +struct real {}; +struct imag {}; + #ifdef M_PI #undef M_PI #endif diff --git a/gtsam/3rdparty/Eigen/test/numext.cpp b/gtsam/3rdparty/Eigen/test/numext.cpp index 3de33e2f9..beba9e911 100644 --- a/gtsam/3rdparty/Eigen/test/numext.cpp +++ b/gtsam/3rdparty/Eigen/test/numext.cpp @@ -12,6 +12,7 @@ template void check_abs() { typedef typename NumTraits::Real Real; + Real zero(0); if(NumTraits::IsSigned) VERIFY_IS_EQUAL(numext::abs(-T(1)), T(1)); @@ -26,9 +27,9 @@ void check_abs() { if(NumTraits::IsSigned) { VERIFY_IS_EQUAL(numext::abs(x), numext::abs(-x)); - VERIFY( numext::abs(-x) >= Real(0)); + VERIFY( numext::abs(-x) >= zero ); } - VERIFY( numext::abs(x) >= Real(0)); + VERIFY( numext::abs(x) >= zero ); VERIFY_IS_APPROX( numext::abs2(x), numext::abs2(numext::abs(x)) ); } } diff --git a/gtsam/3rdparty/Eigen/test/packetmath.cpp b/gtsam/3rdparty/Eigen/test/packetmath.cpp index 7821a1738..74ac435cf 100644 --- a/gtsam/3rdparty/Eigen/test/packetmath.cpp +++ b/gtsam/3rdparty/Eigen/test/packetmath.cpp @@ -16,12 +16,6 @@ #endif // using namespace Eigen; -#ifdef EIGEN_VECTORIZE_SSE -const bool g_vectorize_sse = true; -#else -const bool g_vectorize_sse = false; -#endif - namespace Eigen { namespace internal { template T negate(const T& x) { return -x; } @@ -248,12 +242,13 @@ template void packetmath() VERIFY(isApproxAbs(ref[0], internal::predux(internal::pload(data1)), refvalue) && "internal::predux"); { - for (int i=0; i<4; ++i) + int newsize = PacketSize>4?PacketSize/2:PacketSize; + for (int i=0; i(data1))); - VERIFY(areApprox(ref, data2, PacketSize>4?PacketSize/2:PacketSize) && "internal::predux_downto4"); + VERIFY(areApprox(ref, data2, newsize) && "internal::predux_downto4"); } ref[0] = 1; @@ -304,7 +299,7 @@ template void packetmath() } } - if (PacketTraits::HasBlend || g_vectorize_sse) { + if (PacketTraits::HasBlend) { // pinsertfirst for (int i=0; i void packetmath() VERIFY(areApprox(ref, data2, PacketSize) && "internal::pinsertfirst"); } - if (PacketTraits::HasBlend || g_vectorize_sse) { + if (PacketTraits::HasBlend) { // pinsertlast for (int i=0; i void product(const MatrixType& m) vcres.noalias() -= m1.transpose() * v1; VERIFY_IS_APPROX(vcres, vc2 - m1.transpose() * v1); + // test scaled products + res = square; + res.noalias() = s1 * m1 * m2.transpose(); + VERIFY_IS_APPROX(res, ((s1*m1).eval() * m2.transpose())); + res = square; + res.noalias() += s1 * m1 * m2.transpose(); + VERIFY_IS_APPROX(res, square + ((s1*m1).eval() * m2.transpose())); + res = square; + res.noalias() -= s1 * m1 * m2.transpose(); + VERIFY_IS_APPROX(res, square - ((s1*m1).eval() * m2.transpose())); + // test d ?= a+b*c rules res.noalias() = square + m1 * m2.transpose(); VERIFY_IS_APPROX(res, square + m1 * m2.transpose()); @@ -228,4 +239,19 @@ template void product(const MatrixType& m) VERIFY_IS_APPROX(square * (square*square).conjugate(), square * square.conjugate() * square.conjugate()); } + // destination with a non-default inner-stride + // see bug 1741 + if(!MatrixType::IsRowMajor) + { + typedef Matrix MatrixX; + MatrixX buffer(2*rows,2*rows); + Map > map1(buffer.data(),rows,rows,Stride(2*rows,2)); + buffer.setZero(); + VERIFY_IS_APPROX(map1 = m1 * m2.transpose(), (m1 * m2.transpose()).eval()); + buffer.setZero(); + VERIFY_IS_APPROX(map1.noalias() = m1 * m2.transpose(), (m1 * m2.transpose()).eval()); + buffer.setZero(); + VERIFY_IS_APPROX(map1.noalias() += m1 * m2.transpose(), (m1 * m2.transpose()).eval()); + } + } diff --git a/gtsam/3rdparty/Eigen/test/product_large.cpp b/gtsam/3rdparty/Eigen/test/product_large.cpp index 845cd40ca..14a4f739d 100644 --- a/gtsam/3rdparty/Eigen/test/product_large.cpp +++ b/gtsam/3rdparty/Eigen/test/product_large.cpp @@ -35,6 +35,8 @@ void test_product_large() for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST_1( product(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_2( product(MatrixXd(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + CALL_SUBTEST_2( product(MatrixXd(internal::random(1,10), internal::random(1,10))) ); + CALL_SUBTEST_3( product(MatrixXi(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_4( product(MatrixXcf(internal::random(1,EIGEN_TEST_MAX_SIZE/2), internal::random(1,EIGEN_TEST_MAX_SIZE/2))) ); CALL_SUBTEST_5( product(Matrix(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); diff --git a/gtsam/3rdparty/Eigen/test/product_mmtr.cpp b/gtsam/3rdparty/Eigen/test/product_mmtr.cpp index d3e24b012..35686460c 100644 --- a/gtsam/3rdparty/Eigen/test/product_mmtr.cpp +++ b/gtsam/3rdparty/Eigen/test/product_mmtr.cpp @@ -82,6 +82,16 @@ template void mmtr(int size) ref2.template triangularView() = ref1.template triangularView(); matc.template triangularView() = sqc * matc * sqc.adjoint(); VERIFY_IS_APPROX(matc, ref2); + + // destination with a non-default inner-stride + // see bug 1741 + { + typedef Matrix MatrixX; + MatrixX buffer(2*size,2*size); + Map > map1(buffer.data(),size,size,Stride(2*size,2)); + buffer.setZero(); + CHECK_MMTR(map1, Lower, = s*soc*sor.adjoint()); + } } void test_product_mmtr() diff --git a/gtsam/3rdparty/Eigen/test/product_symm.cpp b/gtsam/3rdparty/Eigen/test/product_symm.cpp index 7d1042a4f..0ed027dff 100644 --- a/gtsam/3rdparty/Eigen/test/product_symm.cpp +++ b/gtsam/3rdparty/Eigen/test/product_symm.cpp @@ -75,12 +75,12 @@ template void symm(int size = Size, in rhs13 = (s1*m1.adjoint()) * (s2*rhs2.adjoint())); // test row major = <...> - m2 = m1.template triangularView(); rhs12.setRandom(); rhs13 = rhs12; - VERIFY_IS_APPROX(rhs12 -= (s1*m2).template selfadjointView() * (s2*rhs3), + m2 = m1.template triangularView(); rhs32.setRandom(); rhs13 = rhs32; + VERIFY_IS_APPROX(rhs32.noalias() -= (s1*m2).template selfadjointView() * (s2*rhs3), rhs13 -= (s1*m1) * (s2 * rhs3)); m2 = m1.template triangularView(); - VERIFY_IS_APPROX(rhs12 = (s1*m2.adjoint()).template selfadjointView() * (s2*rhs3).conjugate(), + VERIFY_IS_APPROX(rhs32.noalias() = (s1*m2.adjoint()).template selfadjointView() * (s2*rhs3).conjugate(), rhs13 = (s1*m1.adjoint()) * (s2*rhs3).conjugate()); @@ -92,6 +92,20 @@ template void symm(int size = Size, in VERIFY_IS_APPROX(rhs22 = (rhs2) * (m2).template selfadjointView(), rhs23 = (rhs2) * (m1)); VERIFY_IS_APPROX(rhs22 = (s2*rhs2) * (s1*m2).template selfadjointView(), rhs23 = (s2*rhs2) * (s1*m1)); + // destination with a non-default inner-stride + // see bug 1741 + { + typedef Matrix MatrixX; + MatrixX buffer(2*cols,2*othersize); + Map > map1(buffer.data(),cols,othersize,Stride(2*rows,2)); + buffer.setZero(); + VERIFY_IS_APPROX( map1.noalias() = (s1*m2).template selfadjointView() * (s2*rhs1), + rhs13 = (s1*m1) * (s2*rhs1)); + + Map > map2(buffer.data(),rhs22.rows(),rhs22.cols(),Stride(2*rhs22.outerStride(),2)); + buffer.setZero(); + VERIFY_IS_APPROX(map2 = (rhs2) * (m2).template selfadjointView(), rhs23 = (rhs2) * (m1)); + } } void test_product_symm() diff --git a/gtsam/3rdparty/Eigen/test/product_syrk.cpp b/gtsam/3rdparty/Eigen/test/product_syrk.cpp index 3ebbe14ca..b8578215f 100644 --- a/gtsam/3rdparty/Eigen/test/product_syrk.cpp +++ b/gtsam/3rdparty/Eigen/test/product_syrk.cpp @@ -115,6 +115,17 @@ template void syrk(const MatrixType& m) m2.setZero(); VERIFY_IS_APPROX((m2.template selfadjointView().rankUpdate(m1.row(c).adjoint(),s1)._expression()), ((s1 * m1.row(c).adjoint() * m1.row(c).adjoint().adjoint()).eval().template triangularView().toDenseMatrix())); + + // destination with a non-default inner-stride + // see bug 1741 + { + typedef Matrix MatrixX; + MatrixX buffer(2*rows,2*cols); + Map > map1(buffer.data(),rows,cols,Stride(2*rows,2)); + buffer.setZero(); + VERIFY_IS_APPROX((map1.template selfadjointView().rankUpdate(rhs2,s1)._expression()), + ((s1 * rhs2 * rhs2.adjoint()).eval().template triangularView().toDenseMatrix())); + } } void test_product_syrk() diff --git a/gtsam/3rdparty/Eigen/test/product_trmm.cpp b/gtsam/3rdparty/Eigen/test/product_trmm.cpp index e08d9f39f..ddcde9622 100644 --- a/gtsam/3rdparty/Eigen/test/product_trmm.cpp +++ b/gtsam/3rdparty/Eigen/test/product_trmm.cpp @@ -76,8 +76,18 @@ void trmm(int rows=get_random_size(), VERIFY_IS_APPROX( ge_xs = (s1*mat).adjoint().template triangularView() * ge_left.adjoint(), numext::conj(s1) * triTr.conjugate() * ge_left.adjoint()); VERIFY_IS_APPROX( ge_xs = (s1*mat).transpose().template triangularView() * ge_left.adjoint(), s1triTr * ge_left.adjoint()); - // TODO check with sub-matrix expressions ? + + // destination with a non-default inner-stride + // see bug 1741 + { + VERIFY_IS_APPROX( ge_xs.noalias() = mat.template triangularView() * ge_right, tri * ge_right); + typedef Matrix MatrixX; + MatrixX buffer(2*ge_xs.rows(),2*ge_xs.cols()); + Map > map1(buffer.data(),ge_xs.rows(),ge_xs.cols(),Stride(2*ge_xs.outerStride(),2)); + buffer.setZero(); + VERIFY_IS_APPROX( map1.noalias() = mat.template triangularView() * ge_right, tri * ge_right); + } } template diff --git a/gtsam/3rdparty/Eigen/test/product_trsolve.cpp b/gtsam/3rdparty/Eigen/test/product_trsolve.cpp index 4b97fa9d6..eaf62cb11 100644 --- a/gtsam/3rdparty/Eigen/test/product_trsolve.cpp +++ b/gtsam/3rdparty/Eigen/test/product_trsolve.cpp @@ -71,6 +71,32 @@ template void trsolve(int size=Size,int cols int c = internal::random(0,cols-1); VERIFY_TRSM(rmLhs.template triangularView(), rmRhs.col(c)); VERIFY_TRSM(cmLhs.template triangularView(), rmRhs.col(c)); + + // destination with a non-default inner-stride + // see bug 1741 + { + typedef Matrix MatrixX; + MatrixX buffer(2*cmRhs.rows(),2*cmRhs.cols()); + Map,0,Stride > map1(buffer.data(),cmRhs.rows(),cmRhs.cols(),Stride(2*cmRhs.outerStride(),2)); + Map,0,Stride > map2(buffer.data(),rmRhs.rows(),rmRhs.cols(),Stride(2*rmRhs.outerStride(),2)); + buffer.setZero(); + VERIFY_TRSM(cmLhs.conjugate().template triangularView(), map1); + buffer.setZero(); + VERIFY_TRSM(cmLhs .template triangularView(), map2); + } + + if(Size==Dynamic) + { + cmLhs.resize(0,0); + cmRhs.resize(0,cmRhs.cols()); + Matrix res = cmLhs.template triangularView().solve(cmRhs); + VERIFY_IS_EQUAL(res.rows(),0); + VERIFY_IS_EQUAL(res.cols(),cmRhs.cols()); + res = cmRhs; + cmLhs.template triangularView().solveInPlace(res); + VERIFY_IS_EQUAL(res.rows(),0); + VERIFY_IS_EQUAL(res.cols(),cmRhs.cols()); + } } void test_product_trsolve() diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index 704495aff..da399e287 100644 --- a/gtsam/3rdparty/Eigen/test/ref.cpp +++ b/gtsam/3rdparty/Eigen/test/ref.cpp @@ -102,10 +102,14 @@ template void ref_vector(const VectorType& m) Index i = internal::random(0,size-1); Index bsize = internal::random(1,size-i); - RefMat rm0 = v1; - VERIFY_IS_EQUAL(rm0, v1); - RefDynMat rv1 = v1; - VERIFY_IS_EQUAL(rv1, v1); + { RefMat rm0 = v1; VERIFY_IS_EQUAL(rm0, v1); } + { RefMat rm0 = v1.block(0,0,size,1); VERIFY_IS_EQUAL(rm0, v1); } + { RefDynMat rv1 = v1; VERIFY_IS_EQUAL(rv1, v1); } + { RefDynMat rv1 = v1.block(0,0,size,1); VERIFY_IS_EQUAL(rv1, v1); } + { VERIFY_RAISES_ASSERT( RefMat rm0 = v1.block(0, 0, size, 0); EIGEN_UNUSED_VARIABLE(rm0); ); } + if(VectorType::SizeAtCompileTime!=1) + { VERIFY_RAISES_ASSERT( RefDynMat rv1 = v1.block(0, 0, size, 0); EIGEN_UNUSED_VARIABLE(rv1); ); } + RefDynMat rv2 = v1.segment(i,bsize); VERIFY_IS_EQUAL(rv2, v1.segment(i,bsize)); rv2.setOnes(); diff --git a/gtsam/3rdparty/Eigen/test/rvalue_types.cpp b/gtsam/3rdparty/Eigen/test/rvalue_types.cpp index 8887f1b1b..6a97dae34 100644 --- a/gtsam/3rdparty/Eigen/test/rvalue_types.cpp +++ b/gtsam/3rdparty/Eigen/test/rvalue_types.cpp @@ -7,6 +7,8 @@ // 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/. +#define EIGEN_RUNTIME_NO_MALLOC + #include "main.h" #include @@ -24,41 +26,85 @@ void rvalue_copyassign(const MatrixType& m) MatrixType tmp = m; UIntPtr src_address = reinterpret_cast(tmp.data()); + Eigen::internal::set_is_malloc_allowed(false); // moving from an rvalue reference shall never allocate // move the temporary to n MatrixType n = std::move(tmp); UIntPtr dst_address = reinterpret_cast(n.data()); - if (MatrixType::RowsAtCompileTime==Dynamic|| MatrixType::ColsAtCompileTime==Dynamic) { // verify that we actually moved the guts VERIFY_IS_EQUAL(src_address, dst_address); + VERIFY_IS_EQUAL(tmp.size(), 0); + VERIFY_IS_EQUAL(reinterpret_cast(tmp.data()), UIntPtr(0)); } // verify that the content did not change Scalar abs_diff = (m-n).array().abs().sum(); VERIFY_IS_EQUAL(abs_diff, Scalar(0)); + Eigen::internal::set_is_malloc_allowed(true); +} +template +void rvalue_transpositions(Index rows) +{ + typedef typename TranspositionsType::IndicesType PermutationVectorType; + + PermutationVectorType vec; + randomPermutationVector(vec, rows); + TranspositionsType t0(vec); + + Eigen::internal::set_is_malloc_allowed(false); // moving from an rvalue reference shall never allocate + + UIntPtr t0_address = reinterpret_cast(t0.indices().data()); + + // Move constructors: + TranspositionsType t1 = std::move(t0); + UIntPtr t1_address = reinterpret_cast(t1.indices().data()); + VERIFY_IS_EQUAL(t0_address, t1_address); + // t0 must be de-allocated: + VERIFY_IS_EQUAL(t0.size(), 0); + VERIFY_IS_EQUAL(reinterpret_cast(t0.indices().data()), UIntPtr(0)); + + + // Move assignment: + t0 = std::move(t1); + t0_address = reinterpret_cast(t0.indices().data()); + VERIFY_IS_EQUAL(t0_address, t1_address); + // t1 must be de-allocated: + VERIFY_IS_EQUAL(t1.size(), 0); + VERIFY_IS_EQUAL(reinterpret_cast(t1.indices().data()), UIntPtr(0)); + + Eigen::internal::set_is_malloc_allowed(true); } #else template void rvalue_copyassign(const MatrixType&) {} +template +void rvalue_transpositions(Index) {} #endif void test_rvalue_types() { - CALL_SUBTEST_1(rvalue_copyassign( MatrixXf::Random(50,50).eval() )); - CALL_SUBTEST_1(rvalue_copyassign( ArrayXXf::Random(50,50).eval() )); + for(int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_1(rvalue_copyassign( MatrixXf::Random(50,50).eval() )); + CALL_SUBTEST_1(rvalue_copyassign( ArrayXXf::Random(50,50).eval() )); - CALL_SUBTEST_1(rvalue_copyassign( Matrix::Random(50).eval() )); - CALL_SUBTEST_1(rvalue_copyassign( Array::Random(50).eval() )); + CALL_SUBTEST_1(rvalue_copyassign( Matrix::Random(50).eval() )); + CALL_SUBTEST_1(rvalue_copyassign( Array::Random(50).eval() )); - CALL_SUBTEST_1(rvalue_copyassign( Matrix::Random(50).eval() )); - CALL_SUBTEST_1(rvalue_copyassign( Array::Random(50).eval() )); + CALL_SUBTEST_1(rvalue_copyassign( Matrix::Random(50).eval() )); + CALL_SUBTEST_1(rvalue_copyassign( Array::Random(50).eval() )); + + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); - - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + CALL_SUBTEST_3((rvalue_transpositions >(internal::random(1,EIGEN_TEST_MAX_SIZE)))); + CALL_SUBTEST_3((rvalue_transpositions >(internal::random(1,EIGEN_TEST_MAX_SIZE)))); + CALL_SUBTEST_4((rvalue_transpositions >(internal::random(1,EIGEN_TEST_MAX_SIZE)))); + CALL_SUBTEST_4((rvalue_transpositions >(internal::random(1,EIGEN_TEST_MAX_SIZE)))); + } } diff --git a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp index d0ef722b6..43318da79 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp @@ -612,6 +612,14 @@ template void sparse_basic(const SparseMatrixType& re iters[0] = IteratorType(m2,0); iters[1] = IteratorType(m2,m2.outerSize()-1); } + + // test reserve with empty rows/columns + { + SparseMatrixType m1(0,cols); + m1.reserve(ArrayXi::Constant(m1.outerSize(),1)); + SparseMatrixType m2(rows,0); + m2.reserve(ArrayXi::Constant(m2.outerSize(),1)); + } } diff --git a/gtsam/3rdparty/Eigen/test/stddeque.cpp b/gtsam/3rdparty/Eigen/test/stddeque.cpp index b511c4e61..b6955f747 100644 --- a/gtsam/3rdparty/Eigen/test/stddeque.cpp +++ b/gtsam/3rdparty/Eigen/test/stddeque.cpp @@ -18,7 +18,7 @@ void check_stddeque_matrix(const MatrixType& m) Index rows = m.rows(); Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::deque > v(10, MatrixType(rows,cols)), w(20, y); + std::deque > v(10, MatrixType::Zero(rows,cols)), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -33,7 +33,7 @@ void check_stddeque_matrix(const MatrixType& m) ++wi; } - v.resize(21); + v.resize(21,MatrixType::Zero(rows,cols)); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); @@ -46,8 +46,8 @@ template void check_stddeque_transform(const TransformType&) { typedef typename TransformType::MatrixType MatrixType; - TransformType x(MatrixType::Random()), y(MatrixType::Random()); - std::deque > v(10), w(20, y); + TransformType x(MatrixType::Random()), y(MatrixType::Random()), ti=TransformType::Identity(); + std::deque > v(10,ti), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -62,7 +62,7 @@ void check_stddeque_transform(const TransformType&) ++wi; } - v.resize(21); + v.resize(21,ti); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); @@ -75,8 +75,8 @@ template void check_stddeque_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); - std::deque > v(10), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); + std::deque > v(10,qi), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -91,7 +91,7 @@ void check_stddeque_quaternion(const QuaternionType&) ++wi; } - v.resize(21); + v.resize(21,qi); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); diff --git a/gtsam/3rdparty/Eigen/test/stddeque_overload.cpp b/gtsam/3rdparty/Eigen/test/stddeque_overload.cpp index 4da618bbf..f495b5a04 100644 --- a/gtsam/3rdparty/Eigen/test/stddeque_overload.cpp +++ b/gtsam/3rdparty/Eigen/test/stddeque_overload.cpp @@ -31,7 +31,7 @@ void check_stddeque_matrix(const MatrixType& m) typename MatrixType::Index rows = m.rows(); typename MatrixType::Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::deque v(10, MatrixType(rows,cols)), w(20, y); + std::deque v(10, MatrixType::Zero(rows,cols)), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); @@ -64,8 +64,8 @@ template void check_stddeque_transform(const TransformType&) { typedef typename TransformType::MatrixType MatrixType; - TransformType x(MatrixType::Random()), y(MatrixType::Random()); - std::deque v(10), w(20, y); + TransformType x(MatrixType::Random()), y(MatrixType::Random()), ti=TransformType::Identity(); + std::deque v(10,ti), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); @@ -75,7 +75,7 @@ void check_stddeque_transform(const TransformType&) VERIFY_IS_APPROX(w[i], v[i]); } - v.resize(21); + v.resize(21,ti); v[20] = x; VERIFY_IS_APPROX(v[20], x); v.resize(22,y); @@ -98,8 +98,8 @@ template void check_stddeque_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); - std::deque v(10), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); + std::deque v(10,qi), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); @@ -109,7 +109,7 @@ void check_stddeque_quaternion(const QuaternionType&) VERIFY_IS_APPROX(w[i], v[i]); } - v.resize(21); + v.resize(21,qi); v[20] = x; VERIFY_IS_APPROX(v[20], x); v.resize(22,y); diff --git a/gtsam/3rdparty/Eigen/test/stdlist.cpp b/gtsam/3rdparty/Eigen/test/stdlist.cpp index 23cbe9039..23b30ccaf 100644 --- a/gtsam/3rdparty/Eigen/test/stdlist.cpp +++ b/gtsam/3rdparty/Eigen/test/stdlist.cpp @@ -18,7 +18,7 @@ void check_stdlist_matrix(const MatrixType& m) Index rows = m.rows(); Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::list > v(10, MatrixType(rows,cols)), w(20, y); + std::list > v(10, MatrixType::Zero(rows,cols)), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -33,7 +33,7 @@ void check_stdlist_matrix(const MatrixType& m) ++wi; } - v.resize(21); + v.resize(21, MatrixType::Zero(rows,cols)); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); @@ -46,8 +46,8 @@ template void check_stdlist_transform(const TransformType&) { typedef typename TransformType::MatrixType MatrixType; - TransformType x(MatrixType::Random()), y(MatrixType::Random()); - std::list > v(10), w(20, y); + TransformType x(MatrixType::Random()), y(MatrixType::Random()), ti=TransformType::Identity(); + std::list > v(10,ti), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -62,7 +62,7 @@ void check_stdlist_transform(const TransformType&) ++wi; } - v.resize(21); + v.resize(21, ti); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); @@ -75,8 +75,8 @@ template void check_stdlist_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); - std::list > v(10), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); + std::list > v(10,qi), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -91,7 +91,7 @@ void check_stdlist_quaternion(const QuaternionType&) ++wi; } - v.resize(21); + v.resize(21,qi); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); diff --git a/gtsam/3rdparty/Eigen/test/stdlist_overload.cpp b/gtsam/3rdparty/Eigen/test/stdlist_overload.cpp index bb910bd43..aea7a2846 100644 --- a/gtsam/3rdparty/Eigen/test/stdlist_overload.cpp +++ b/gtsam/3rdparty/Eigen/test/stdlist_overload.cpp @@ -47,7 +47,7 @@ void check_stdlist_matrix(const MatrixType& m) typename MatrixType::Index rows = m.rows(); typename MatrixType::Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::list v(10, MatrixType(rows,cols)), w(20, y); + std::list v(10, MatrixType::Zero(rows,cols)), w(20, y); typename std::list::iterator itv = get(v, 5); typename std::list::iterator itw = get(w, 6); *itv = x; @@ -86,8 +86,8 @@ template void check_stdlist_transform(const TransformType&) { typedef typename TransformType::MatrixType MatrixType; - TransformType x(MatrixType::Random()), y(MatrixType::Random()); - std::list v(10), w(20, y); + TransformType x(MatrixType::Random()), y(MatrixType::Random()), ti=TransformType::Identity(); + std::list v(10,ti), w(20, y); typename std::list::iterator itv = get(v, 5); typename std::list::iterator itw = get(w, 6); *itv = x; @@ -103,7 +103,7 @@ void check_stdlist_transform(const TransformType&) ++itw; } - v.resize(21); + v.resize(21, ti); set(v, 20, x); VERIFY_IS_APPROX(*get(v, 20), x); v.resize(22,y); @@ -126,8 +126,8 @@ template void check_stdlist_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); - std::list v(10), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); + std::list v(10,qi), w(20, y); typename std::list::iterator itv = get(v, 5); typename std::list::iterator itw = get(w, 6); *itv = x; @@ -143,7 +143,7 @@ void check_stdlist_quaternion(const QuaternionType&) ++itw; } - v.resize(21); + v.resize(21,qi); set(v, 20, x); VERIFY_IS_APPROX(*get(v, 20), x); v.resize(22,y); diff --git a/gtsam/3rdparty/Eigen/test/stdvector.cpp b/gtsam/3rdparty/Eigen/test/stdvector.cpp index fa928ea4f..383d9a509 100644 --- a/gtsam/3rdparty/Eigen/test/stdvector.cpp +++ b/gtsam/3rdparty/Eigen/test/stdvector.cpp @@ -17,7 +17,7 @@ void check_stdvector_matrix(const MatrixType& m) typename MatrixType::Index rows = m.rows(); typename MatrixType::Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::vector > v(10, MatrixType(rows,cols)), w(20, y); + std::vector > v(10, MatrixType::Zero(rows,cols)), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); @@ -86,8 +86,8 @@ template void check_stdvector_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); - std::vector > v(10), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); + std::vector > v(10,qi), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); diff --git a/gtsam/3rdparty/Eigen/test/stdvector_overload.cpp b/gtsam/3rdparty/Eigen/test/stdvector_overload.cpp index 959665954..637e3ef52 100644 --- a/gtsam/3rdparty/Eigen/test/stdvector_overload.cpp +++ b/gtsam/3rdparty/Eigen/test/stdvector_overload.cpp @@ -31,7 +31,7 @@ void check_stdvector_matrix(const MatrixType& m) typename MatrixType::Index rows = m.rows(); typename MatrixType::Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::vector v(10, MatrixType(rows,cols)), w(20, y); + std::vector v(10, MatrixType::Zero(rows,cols)), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); @@ -100,8 +100,8 @@ template void check_stdvector_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); - std::vector v(10), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); + std::vector v(10,qi), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); diff --git a/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp b/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp index 37e7495f5..c2f77bfec 100644 --- a/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp +++ b/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp @@ -22,6 +22,14 @@ #include "main.h" #include +// Disable "ignoring attributes on template argument" +// for packet_traits +// => The only workaround would be to wrap _m128 and the likes +// within wrappers. +#if EIGEN_GNUC_AT_LEAST(6,0) + #pragma GCC diagnostic ignored "-Wignored-attributes" +#endif + using internal::demangle_flags; using internal::demangle_traversal; using internal::demangle_unrolling; diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/ArpackSupport b/gtsam/3rdparty/Eigen/unsupported/Eigen/ArpackSupport index 37a2799ef..a0d4820e1 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/ArpackSupport +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/ArpackSupport @@ -11,8 +11,6 @@ #include -#include - /** \defgroup ArpackSupport_Module Arpack support module * * This module provides a wrapper to Arpack, a library for sparse eigenvalue decomposition. @@ -23,6 +21,8 @@ */ #include + +#include #include "src/Eigenvalues/ArpackSelfAdjointEigenSolver.h" #include diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h index 9b2cb3ff6..c28a10dd4 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h @@ -113,6 +113,7 @@ class SimpleTensorContractionMapper { EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index computeIndex(Index row, Index col) const { const bool left = (side == Lhs); + EIGEN_UNUSED_VARIABLE(left); // annoying bug in g++8.1: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=85963 Index nocontract_val = left ? row : col; Index linidx = 0; for (int i = static_cast(array_size::value) - 1; i > 0; i--) { @@ -151,6 +152,7 @@ class SimpleTensorContractionMapper { EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE IndexPair computeIndexPair(Index row, Index col, const Index distance) const { const bool left = (side == Lhs); + EIGEN_UNUSED_VARIABLE(left); // annoying bug in g++8.1: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=85963 Index nocontract_val[2] = {left ? row : col, left ? row + distance : col}; Index linidx[2] = {0, 0}; if (array_size::value > array_size::value) { diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h index 17f04665a..a5e084a24 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h @@ -31,7 +31,7 @@ class Barrier { eigen_assert(((count << 1) >> 1) == count); } ~Barrier() { - eigen_assert((state_>>1) == 0); + eigen_plain_assert((state_>>1) == 0); } void Notify() { diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/EventCount.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/EventCount.h index 71d55552d..4749d6240 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/EventCount.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/EventCount.h @@ -58,7 +58,7 @@ class EventCount { ~EventCount() { // Ensure there are no waiters. - eigen_assert((state_.load() & (kStackMask | kWaiterMask)) == kStackMask); + eigen_plain_assert((state_.load() & (kStackMask | kWaiterMask)) == kStackMask); } // Prewait prepares for waiting. diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/RunQueue.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/RunQueue.h index 05ed76cbe..6e505fc14 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/RunQueue.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/RunQueue.h @@ -47,7 +47,7 @@ class RunQueue { array_[i].state.store(kEmpty, std::memory_order_relaxed); } - ~RunQueue() { eigen_assert(Size() == 0); } + ~RunQueue() { eigen_plain_assert(Size() == 0); } // PushFront inserts w at the beginning of the queue. // If queue is full returns w, otherwise returns default-constructed Work. diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/Polynomials b/gtsam/3rdparty/Eigen/unsupported/Eigen/Polynomials index cece56337..334b03142 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/Polynomials +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/Polynomials @@ -11,10 +11,10 @@ #include -#include - #include +#include + // Note that EIGEN_HIDE_HEAVY_CODE has to be defined per module #if (defined EIGEN_EXTERN_INSTANTIATIONS) && (EIGEN_EXTERN_INSTANTIATIONS>=2) #ifndef EIGEN_HIDE_HEAVY_CODE diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h index 2f50e9968..58f3f3319 100755 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h @@ -453,6 +453,24 @@ struct auto_diff_special_op<_DerType, false> void operator+() const; }; +template +void make_coherent_expression(CwiseBinaryOp xpr, const RefType &ref) +{ + make_coherent(xpr.const_cast_derived().lhs(), ref); + make_coherent(xpr.const_cast_derived().rhs(), ref); +} + +template +void make_coherent_expression(const CwiseUnaryOp &xpr, const RefType &ref) +{ + make_coherent(xpr.nestedExpression().const_cast_derived(), ref); +} + +// needed for compilation only +template +void make_coherent_expression(const CwiseNullaryOp &, const RefType &) +{} + template struct make_coherent_impl, B> { typedef Matrix A; @@ -462,6 +480,10 @@ struct make_coherent_impl struct make_coherent_impl, - Matrix > { + Matrix > { typedef Matrix A; typedef Matrix B; static void run(A& a, B& b) { diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h index 866a8a460..4170d26b6 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h @@ -3,24 +3,9 @@ // // Copyright (C) 2012 David Harmon // -// Eigen is free software; you can redistribute it and/or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 3 of the License, or (at your option) any later version. -// -// Alternatively, you can redistribute it and/or -// modify it under the terms of the GNU General Public License as -// published by the Free Software Foundation; either version 2 of -// the License, or (at your option) any later version. -// -// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY -// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS -// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU Lesser General Public -// License and a copy of the GNU General Public License along with -// Eigen. If not, see . +// 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_ARPACKGENERALIZEDSELFADJOINTEIGENSOLVER_H #define EIGEN_ARPACKGENERALIZEDSELFADJOINTEIGENSOLVER_H diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_fftw_impl.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_fftw_impl.h index d49aa17f5..7c1f716e2 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_fftw_impl.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_fftw_impl.h @@ -231,6 +231,8 @@ namespace internal { protected: typedef fftw_plan PlanData; + typedef Eigen::numext::int64_t int64_t; + typedef std::map PlanMap; PlanMap m_plans; diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h index e5ebbcf23..0b0ee6546 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h @@ -412,7 +412,7 @@ template struct MatrixExponentialReturnValue inline void evalTo(ResultType& result) const { const typename internal::nested_eval::type tmp(m_src); - internal::matrix_exp_compute(tmp, result, internal::is_exp_known_type()); + internal::matrix_exp_compute(tmp, result, internal::is_exp_known_type()); } Index rows() const { return m_src.rows(); } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h index 2e5abda38..9de0c3574 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h @@ -253,18 +253,19 @@ struct matrix_sqrt_compute template struct matrix_sqrt_compute { + typedef typename MatrixType::PlainObject PlainType; template static void run(const MatrixType &arg, ResultType &result) { eigen_assert(arg.rows() == arg.cols()); // Compute Schur decomposition of arg - const RealSchur schurOfA(arg); - const MatrixType& T = schurOfA.matrixT(); - const MatrixType& U = schurOfA.matrixU(); + const RealSchur schurOfA(arg); + const PlainType& T = schurOfA.matrixT(); + const PlainType& U = schurOfA.matrixU(); // Compute square root of T - MatrixType sqrtT = MatrixType::Zero(arg.rows(), arg.cols()); + PlainType sqrtT = PlainType::Zero(arg.rows(), arg.cols()); matrix_sqrt_quasi_triangular(T, sqrtT); // Compute square root of arg @@ -278,18 +279,19 @@ struct matrix_sqrt_compute template struct matrix_sqrt_compute { + typedef typename MatrixType::PlainObject PlainType; template static void run(const MatrixType &arg, ResultType &result) { eigen_assert(arg.rows() == arg.cols()); // Compute Schur decomposition of arg - const ComplexSchur schurOfA(arg); - const MatrixType& T = schurOfA.matrixT(); - const MatrixType& U = schurOfA.matrixU(); + const ComplexSchur schurOfA(arg); + const PlainType& T = schurOfA.matrixT(); + const PlainType& U = schurOfA.matrixU(); // Compute square root of T - MatrixType sqrtT; + PlainType sqrtT; matrix_sqrt_triangular(T, sqrtT); // Compute square root of arg diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/Companion.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/Companion.h index b515c2920..359836cac 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/Companion.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/Companion.h @@ -75,8 +75,7 @@ class companion void setPolynomial( const VectorType& poly ) { const Index deg = poly.size()-1; - m_monic = -1/poly[deg] * poly.head(deg); - //m_bl_diag.setIdentity( deg-1 ); + m_monic = -poly.head(deg)/poly[deg]; m_bl_diag.setOnes(deg-1); } @@ -89,13 +88,13 @@ class companion { const Index deg = m_monic.size(); const Index deg_1 = deg-1; - DenseCompanionMatrixType companion(deg,deg); - companion << + DenseCompanionMatrixType companMat(deg,deg); + companMat << ( LeftBlock(deg,deg_1) << LeftBlockFirstRow::Zero(1,deg_1), BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished() , m_monic; - return companion; + return companMat; } @@ -107,8 +106,8 @@ class companion * colB and rowB are repectively the multipliers for * the column and the row in order to balance them. * */ - bool balanced( Scalar colNorm, Scalar rowNorm, - bool& isBalanced, Scalar& colB, Scalar& rowB ); + bool balanced( RealScalar colNorm, RealScalar rowNorm, + bool& isBalanced, RealScalar& colB, RealScalar& rowB ); /** Helper function for the balancing algorithm. * \returns true if the row and the column, having colNorm and rowNorm @@ -116,8 +115,8 @@ class companion * colB and rowB are repectively the multipliers for * the column and the row in order to balance them. * */ - bool balancedR( Scalar colNorm, Scalar rowNorm, - bool& isBalanced, Scalar& colB, Scalar& rowB ); + bool balancedR( RealScalar colNorm, RealScalar rowNorm, + bool& isBalanced, RealScalar& colB, RealScalar& rowB ); public: /** @@ -139,10 +138,10 @@ class companion template< typename _Scalar, int _Deg > inline -bool companion<_Scalar,_Deg>::balanced( Scalar colNorm, Scalar rowNorm, - bool& isBalanced, Scalar& colB, Scalar& rowB ) +bool companion<_Scalar,_Deg>::balanced( RealScalar colNorm, RealScalar rowNorm, + bool& isBalanced, RealScalar& colB, RealScalar& rowB ) { - if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; } + if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){ return true; } else { //To find the balancing coefficients, if the radix is 2, @@ -150,29 +149,29 @@ bool companion<_Scalar,_Deg>::balanced( Scalar colNorm, Scalar rowNorm, // \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$ // then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$ // and the balancing coefficient for the column is \f$ 2^{\sigma} \f$ - rowB = rowNorm / radix(); - colB = Scalar(1); - const Scalar s = colNorm + rowNorm; + rowB = rowNorm / radix(); + colB = RealScalar(1); + const RealScalar s = colNorm + rowNorm; while (colNorm < rowB) { - colB *= radix(); - colNorm *= radix2(); + colB *= radix(); + colNorm *= radix2(); } - rowB = rowNorm * radix(); + rowB = rowNorm * radix(); while (colNorm >= rowB) { - colB /= radix(); - colNorm /= radix2(); + colB /= radix(); + colNorm /= radix2(); } //This line is used to avoid insubstantial balancing - if ((rowNorm + colNorm) < Scalar(0.95) * s * colB) + if ((rowNorm + colNorm) < RealScalar(0.95) * s * colB) { isBalanced = false; - rowB = Scalar(1) / colB; + rowB = RealScalar(1) / colB; return false; } else{ @@ -182,21 +181,21 @@ bool companion<_Scalar,_Deg>::balanced( Scalar colNorm, Scalar rowNorm, template< typename _Scalar, int _Deg > inline -bool companion<_Scalar,_Deg>::balancedR( Scalar colNorm, Scalar rowNorm, - bool& isBalanced, Scalar& colB, Scalar& rowB ) +bool companion<_Scalar,_Deg>::balancedR( RealScalar colNorm, RealScalar rowNorm, + bool& isBalanced, RealScalar& colB, RealScalar& rowB ) { - if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; } + if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){ return true; } else { /** * Set the norm of the column and the row to the geometric mean * of the row and column norm */ - const _Scalar q = colNorm/rowNorm; + const RealScalar q = colNorm/rowNorm; if( !isApprox( q, _Scalar(1) ) ) { rowB = sqrt( colNorm/rowNorm ); - colB = Scalar(1)/rowB; + colB = RealScalar(1)/rowB; isBalanced = false; return false; @@ -219,8 +218,8 @@ void companion<_Scalar,_Deg>::balance() while( !hasConverged ) { hasConverged = true; - Scalar colNorm,rowNorm; - Scalar colB,rowB; + RealScalar colNorm,rowNorm; + RealScalar colB,rowB; //First row, first column excluding the diagonal //============================================== diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialSolver.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialSolver.h index 03198ec8e..5e0ecbb43 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialSolver.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialSolver.h @@ -99,7 +99,7 @@ class PolynomialSolverBase */ inline const RootType& greatestRoot() const { - std::greater greater; + std::greater greater; return selectComplexRoot_withRespectToNorm( greater ); } @@ -108,7 +108,7 @@ class PolynomialSolverBase */ inline const RootType& smallestRoot() const { - std::less less; + std::less less; return selectComplexRoot_withRespectToNorm( less ); } @@ -126,7 +126,7 @@ class PolynomialSolverBase for( Index i=0; i::dummy_precision() ) const { - std::greater greater; + std::greater greater; return selectRealRoot_withRespectToAbsRealPart( greater, hasArealRoot, absImaginaryThreshold ); } @@ -236,7 +236,7 @@ class PolynomialSolverBase bool& hasArealRoot, const RealScalar& absImaginaryThreshold = NumTraits::dummy_precision() ) const { - std::less less; + std::less less; return selectRealRoot_withRespectToAbsRealPart( less, hasArealRoot, absImaginaryThreshold ); } @@ -259,7 +259,7 @@ class PolynomialSolverBase bool& hasArealRoot, const RealScalar& absImaginaryThreshold = NumTraits::dummy_precision() ) const { - std::greater greater; + std::greater greater; return selectRealRoot_withRespectToRealPart( greater, hasArealRoot, absImaginaryThreshold ); } @@ -282,7 +282,7 @@ class PolynomialSolverBase bool& hasArealRoot, const RealScalar& absImaginaryThreshold = NumTraits::dummy_precision() ) const { - std::less less; + std::less less; return selectRealRoot_withRespectToRealPart( less, hasArealRoot, absImaginaryThreshold ); } @@ -327,7 +327,7 @@ class PolynomialSolverBase * However, almost always, correct accuracy is reached even in these cases for 64bit * (double) floating types and small polynomial degree (<20). */ -template< typename _Scalar, int _Deg > +template class PolynomialSolver : public PolynomialSolverBase<_Scalar,_Deg> { public: @@ -337,7 +337,10 @@ class PolynomialSolver : public PolynomialSolverBase<_Scalar,_Deg> EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES( PS_Base ) typedef Matrix CompanionMatrixType; - typedef EigenSolver EigenSolverType; + typedef typename internal::conditional::IsComplex, + ComplexEigenSolver, + EigenSolver >::type EigenSolverType; + typedef typename internal::conditional::IsComplex, Scalar, std::complex >::type ComplexScalar; public: /** Computes the complex roots of a new polynomial. */ @@ -352,6 +355,25 @@ class PolynomialSolver : public PolynomialSolverBase<_Scalar,_Deg> companion.balance(); m_eigenSolver.compute( companion.denseMatrix() ); m_roots = m_eigenSolver.eigenvalues(); + // cleanup noise in imaginary part of real roots: + // if the imaginary part is rather small compared to the real part + // and that cancelling the imaginary part yield a smaller evaluation, + // then it's safe to keep the real part only. + RealScalar coarse_prec = RealScalar(std::pow(4,poly.size()+1))*NumTraits::epsilon(); + for(Index i = 0; i::Scalar u, DenseIndex degree, const typename SplineTraits::KnotVectorType& knots); diff --git a/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp b/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp index f0c336c15..dd93c21e9 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp @@ -15,6 +15,15 @@ // tolerance for chekcing number of iterations #define LM_EVAL_COUNT_TOL 4/3 +#define LM_CHECK_N_ITERS(SOLVER,NFEV,NJEV) { \ + ++g_test_level; \ + VERIFY_IS_EQUAL(SOLVER.nfev, NFEV); \ + VERIFY_IS_EQUAL(SOLVER.njev, NJEV); \ + --g_test_level; \ + VERIFY(SOLVER.nfev <= NFEV * LM_EVAL_COUNT_TOL); \ + VERIFY(SOLVER.njev <= NJEV * LM_EVAL_COUNT_TOL); \ + } + int fcn_chkder(const VectorXd &x, VectorXd &fvec, MatrixXd &fjac, int iflag) { /* subroutine fcn for chkder example. */ @@ -180,8 +189,7 @@ void testLmder1() // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 6); - VERIFY_IS_EQUAL(lm.njev, 5); + LM_CHECK_N_ITERS(lm, 6, 5); // check norm VERIFY_IS_APPROX(lm.fvec.blueNorm(), 0.09063596); @@ -209,8 +217,7 @@ void testLmder() // check return values VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 6); - VERIFY_IS_EQUAL(lm.njev, 5); + LM_CHECK_N_ITERS(lm, 6, 5); // check norm fnorm = lm.fvec.blueNorm(); @@ -294,8 +301,7 @@ void testHybrj1() // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(solver.nfev, 11); - VERIFY_IS_EQUAL(solver.njev, 1); + LM_CHECK_N_ITERS(solver, 11, 1); // check norm VERIFY_IS_APPROX(solver.fvec.blueNorm(), 1.192636e-08); @@ -329,8 +335,7 @@ void testHybrj() // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(solver.nfev, 11); - VERIFY_IS_EQUAL(solver.njev, 1); + LM_CHECK_N_ITERS(solver, 11, 1); // check norm VERIFY_IS_APPROX(solver.fvec.blueNorm(), 1.192636e-08); @@ -485,8 +490,7 @@ void testLmstr1() // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 6); - VERIFY_IS_EQUAL(lm.njev, 5); + LM_CHECK_N_ITERS(lm, 6, 5); // check norm VERIFY_IS_APPROX(lm.fvec.blueNorm(), 0.09063596); @@ -514,8 +518,7 @@ void testLmstr() // check return values VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 6); - VERIFY_IS_EQUAL(lm.njev, 5); + LM_CHECK_N_ITERS(lm, 6, 5); // check norm fnorm = lm.fvec.blueNorm(); @@ -686,8 +689,7 @@ void testNistChwirut2(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 10); - VERIFY_IS_EQUAL(lm.njev, 8); + LM_CHECK_N_ITERS(lm, 10, 8); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.1304802941E+02); // check x @@ -707,8 +709,7 @@ void testNistChwirut2(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 7); - VERIFY_IS_EQUAL(lm.njev, 6); + LM_CHECK_N_ITERS(lm, 7, 6); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.1304802941E+02); // check x @@ -766,8 +767,7 @@ void testNistMisra1a(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 19); - VERIFY_IS_EQUAL(lm.njev, 15); + LM_CHECK_N_ITERS(lm, 19, 15); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.2455138894E-01); // check x @@ -783,8 +783,7 @@ void testNistMisra1a(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 5); - VERIFY_IS_EQUAL(lm.njev, 4); + LM_CHECK_N_ITERS(lm, 5, 4); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.2455138894E-01); // check x @@ -856,8 +855,7 @@ void testNistHahn1(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 11); - VERIFY_IS_EQUAL(lm.njev, 10); + LM_CHECK_N_ITERS(lm, 11, 10); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.5324382854E+00); // check x @@ -878,8 +876,7 @@ void testNistHahn1(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 11); - VERIFY_IS_EQUAL(lm.njev, 10); + LM_CHECK_N_ITERS(lm, 11, 10); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.5324382854E+00); // check x @@ -942,8 +939,7 @@ void testNistMisra1d(void) // check return value VERIFY_IS_EQUAL(info, 3); - VERIFY_IS_EQUAL(lm.nfev, 9); - VERIFY_IS_EQUAL(lm.njev, 7); + LM_CHECK_N_ITERS(lm, 9, 7); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.6419295283E-02); // check x @@ -959,8 +955,7 @@ void testNistMisra1d(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 4); - VERIFY_IS_EQUAL(lm.njev, 3); + LM_CHECK_N_ITERS(lm, 4, 3); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.6419295283E-02); // check x @@ -1020,8 +1015,7 @@ void testNistLanczos1(void) // check return value VERIFY_IS_EQUAL(info, 2); - VERIFY_IS_EQUAL(lm.nfev, 79); - VERIFY_IS_EQUAL(lm.njev, 72); + LM_CHECK_N_ITERS(lm, 79, 72); // check norm^2 std::cout.precision(30); std::cout << lm.fvec.squaredNorm() << "\n"; @@ -1043,8 +1037,7 @@ void testNistLanczos1(void) // check return value VERIFY_IS_EQUAL(info, 2); - VERIFY_IS_EQUAL(lm.nfev, 9); - VERIFY_IS_EQUAL(lm.njev, 8); + LM_CHECK_N_ITERS(lm, 9, 8); // check norm^2 VERIFY(lm.fvec.squaredNorm() <= 1.4307867721E-25); // check x @@ -1108,8 +1101,7 @@ void testNistRat42(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 10); - VERIFY_IS_EQUAL(lm.njev, 8); + LM_CHECK_N_ITERS(lm, 10, 8); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.0565229338E+00); // check x @@ -1126,8 +1118,7 @@ void testNistRat42(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 6); - VERIFY_IS_EQUAL(lm.njev, 5); + LM_CHECK_N_ITERS(lm, 6, 5); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.0565229338E+00); // check x @@ -1186,8 +1177,7 @@ void testNistMGH10(void) // check return value VERIFY_IS_EQUAL(info, 2); - VERIFY_IS_EQUAL(lm.nfev, 284 ); - VERIFY_IS_EQUAL(lm.njev, 249 ); + LM_CHECK_N_ITERS(lm, 284, 249); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.7945855171E+01); // check x @@ -1204,8 +1194,7 @@ void testNistMGH10(void) // check return value VERIFY_IS_EQUAL(info, 3); - VERIFY_IS_EQUAL(lm.nfev, 126); - VERIFY_IS_EQUAL(lm.njev, 116); + LM_CHECK_N_ITERS(lm, 126, 116); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.7945855171E+01); // check x @@ -1265,8 +1254,7 @@ void testNistBoxBOD(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY(lm.nfev < 31); // 31 - VERIFY(lm.njev < 25); // 25 + LM_CHECK_N_ITERS(lm, 31, 25); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.1680088766E+03); // check x @@ -1284,9 +1272,8 @@ void testNistBoxBOD(void) info = lm.minimize(x); // check return value - VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 15 ); - VERIFY_IS_EQUAL(lm.njev, 14 ); + VERIFY_IS_EQUAL(info, 1); + LM_CHECK_N_ITERS(lm, 15, 14); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.1680088766E+03); // check x @@ -1356,12 +1343,7 @@ void testNistMGH17(void) // check return value VERIFY_IS_EQUAL(info, 2); - ++g_test_level; - VERIFY_IS_EQUAL(lm.nfev, 602); // 602 - VERIFY_IS_EQUAL(lm.njev, 545); // 545 - --g_test_level; - VERIFY(lm.nfev < 602 * LM_EVAL_COUNT_TOL); - VERIFY(lm.njev < 545 * LM_EVAL_COUNT_TOL); + LM_CHECK_N_ITERS(lm, 602, 545); /* * Second try @@ -1373,8 +1355,7 @@ void testNistMGH17(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 18); - VERIFY_IS_EQUAL(lm.njev, 15); + LM_CHECK_N_ITERS(lm, 18, 15); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.4648946975E-05); // check x @@ -1438,9 +1419,8 @@ void testNistMGH09(void) info = lm.minimize(x); // check return value - VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 490 ); - VERIFY_IS_EQUAL(lm.njev, 376 ); + VERIFY_IS_EQUAL(info, 1); + LM_CHECK_N_ITERS(lm, 490, 376); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 3.0750560385E-04); // check x @@ -1459,8 +1439,7 @@ void testNistMGH09(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 18); - VERIFY_IS_EQUAL(lm.njev, 16); + LM_CHECK_N_ITERS(lm, 18, 16); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 3.0750560385E-04); // check x @@ -1525,8 +1504,7 @@ void testNistBennett5(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 758); - VERIFY_IS_EQUAL(lm.njev, 744); + LM_CHECK_N_ITERS(lm, 758, 744); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.2404744073E-04); // check x @@ -1543,8 +1521,7 @@ void testNistBennett5(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 203); - VERIFY_IS_EQUAL(lm.njev, 192); + LM_CHECK_N_ITERS(lm, 203, 192); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.2404744073E-04); // check x @@ -1613,8 +1590,7 @@ void testNistThurber(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 39); - VERIFY_IS_EQUAL(lm.njev, 36); + LM_CHECK_N_ITERS(lm, 39,36); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.6427082397E+03); // check x @@ -1638,8 +1614,7 @@ void testNistThurber(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 29); - VERIFY_IS_EQUAL(lm.njev, 28); + LM_CHECK_N_ITERS(lm, 29, 28); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.6427082397E+03); // check x @@ -1705,8 +1680,7 @@ void testNistRat43(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 27); - VERIFY_IS_EQUAL(lm.njev, 20); + LM_CHECK_N_ITERS(lm, 27, 20); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.7864049080E+03); // check x @@ -1727,8 +1701,7 @@ void testNistRat43(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 9); - VERIFY_IS_EQUAL(lm.njev, 8); + LM_CHECK_N_ITERS(lm, 9, 8); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.7864049080E+03); // check x @@ -1790,8 +1763,7 @@ void testNistEckerle4(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 18); - VERIFY_IS_EQUAL(lm.njev, 15); + LM_CHECK_N_ITERS(lm, 18, 15); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.4635887487E-03); // check x @@ -1808,8 +1780,7 @@ void testNistEckerle4(void) // check return value VERIFY_IS_EQUAL(info, 1); - VERIFY_IS_EQUAL(lm.nfev, 7); - VERIFY_IS_EQUAL(lm.njev, 6); + LM_CHECK_N_ITERS(lm, 7, 6); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.4635887487E-03); // check x diff --git a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp index 1c5e0dc66..1d8c8b5fd 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp @@ -352,6 +352,21 @@ double bug_1264() { return v2(0).value(); } +// check with expressions on constants +double bug_1281() { + int n = 2; + typedef AutoDiffScalar AD; + const AD c = 1.; + AD x0(2,n,0); + AD y1 = (AD(c)+AD(c))*x0; + y1 = x0 * (AD(c)+AD(c)); + AD y2 = (-AD(c))+x0; + y2 = x0+(-AD(c)); + AD y3 = (AD(c)*(-AD(c))+AD(c))*x0; + y3 = x0 * (AD(c)*(-AD(c))+AD(c)); + return (y1+y2+y3).value(); +} + #endif void test_autodiff() @@ -367,5 +382,6 @@ void test_autodiff() CALL_SUBTEST_5( bug_1223() ); CALL_SUBTEST_5( bug_1260() ); CALL_SUBTEST_5( bug_1261() ); + CALL_SUBTEST_5( bug_1281() ); } diff --git a/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp b/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp index 6a2b2194a..005c9c15f 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp @@ -177,6 +177,39 @@ void testMatrixType(const MatrixType& m) } } +template +void testMapRef(const MatrixType& A) +{ + // Test if passing Ref and Map objects is possible + // (Regression test for Bug #1796) + Index size = A.rows(); + MatrixType X; X.setRandom(size, size); + MatrixType Y(size,size); + Ref< MatrixType> R(Y); + Ref Rc(X); + Map< MatrixType> M(Y.data(), size, size); + Map Mc(X.data(), size, size); + + X = X*X; // make sure sqrt is possible + Y = X.sqrt(); + R = Rc.sqrt(); + M = Mc.sqrt(); + Y = X.exp(); + R = Rc.exp(); + M = Mc.exp(); + X = Y; // make sure log is possible + Y = X.log(); + R = Rc.log(); + M = Mc.log(); + + Y = X.cos() + Rc.cos() + Mc.cos(); + Y = X.sin() + Rc.sin() + Mc.sin(); + + Y = X.cosh() + Rc.cosh() + Mc.cosh(); + Y = X.sinh() + Rc.sinh() + Mc.sinh(); +} + + void test_matrix_function() { CALL_SUBTEST_1(testMatrixType(Matrix())); @@ -186,4 +219,9 @@ void test_matrix_function() CALL_SUBTEST_5(testMatrixType(Matrix())); CALL_SUBTEST_6(testMatrixType(Matrix4cd())); CALL_SUBTEST_7(testMatrixType(MatrixXd(13,13))); + + CALL_SUBTEST_1(testMapRef(Matrix())); + CALL_SUBTEST_2(testMapRef(Matrix3cf())); + CALL_SUBTEST_3(testMapRef(MatrixXf(8,8))); + CALL_SUBTEST_7(testMapRef(MatrixXd(13,13))); } diff --git a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp index 4cfc46b41..db8ad7dda 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp @@ -26,14 +26,25 @@ struct increment_if_fixed_size } } +template +PolynomialType polyder(const PolynomialType& p) +{ + typedef typename PolynomialType::Scalar Scalar; + PolynomialType res(p.size()); + for(Index i=1; i bool aux_evalSolver( const POLYNOMIAL& pols, SOLVER& psolve ) { typedef typename POLYNOMIAL::Scalar Scalar; + typedef typename POLYNOMIAL::RealScalar RealScalar; typedef typename SOLVER::RootsType RootsType; - typedef Matrix EvalRootsType; + typedef Matrix EvalRootsType; const Index deg = pols.size()-1; @@ -43,10 +54,17 @@ bool aux_evalSolver( const POLYNOMIAL& pols, SOLVER& psolve ) psolve.compute( pols ); const RootsType& roots( psolve.roots() ); EvalRootsType evr( deg ); + POLYNOMIAL pols_der = polyder(pols); + EvalRootsType der( deg ); for( int i=0; i() ); + // we need to divide by the magnitude of the derivative because + // with a high derivative is very small error in the value of the root + // yiels a very large error in the polynomial evaluation. + bool evalToZero = (evr.cwiseQuotient(der)).isZero( test_precision() ); if( !evalToZero ) { cerr << "WRONG root: " << endl; @@ -56,7 +74,7 @@ bool aux_evalSolver( const POLYNOMIAL& pols, SOLVER& psolve ) cerr << endl; } - std::vector rootModuli( roots.size() ); + std::vector rootModuli( roots.size() ); Map< EvalRootsType > aux( &rootModuli[0], roots.size() ); aux = roots.array().abs(); std::sort( rootModuli.begin(), rootModuli.end() ); @@ -82,7 +100,7 @@ void evalSolver( const POLYNOMIAL& pols ) { typedef typename POLYNOMIAL::Scalar Scalar; - typedef PolynomialSolver PolynomialSolverType; + typedef PolynomialSolver PolynomialSolverType; PolynomialSolverType psolve; aux_evalSolver( pols, psolve ); @@ -96,6 +114,7 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const { using std::sqrt; typedef typename POLYNOMIAL::Scalar Scalar; + typedef typename POLYNOMIAL::RealScalar RealScalar; typedef PolynomialSolver PolynomialSolverType; @@ -106,14 +125,12 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const // 1) the roots found are correct // 2) the roots have distinct moduli - typedef typename REAL_ROOTS::Scalar Real; - //Test realRoots - std::vector< Real > calc_realRoots; - psolve.realRoots( calc_realRoots ); - VERIFY( calc_realRoots.size() == (size_t)real_roots.size() ); + std::vector< RealScalar > calc_realRoots; + psolve.realRoots( calc_realRoots, test_precision()); + VERIFY_IS_EQUAL( calc_realRoots.size() , (size_t)real_roots.size() ); - const Scalar psPrec = sqrt( test_precision() ); + const RealScalar psPrec = sqrt( test_precision() ); for( size_t i=0; i 0 ) ); if( hasRealRoot ){ VERIFY( internal::isApprox( real_roots.array().abs().maxCoeff(), abs(r), psPrec ) ); } @@ -165,9 +182,11 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const template void polynomialsolver(int deg) { - typedef internal::increment_if_fixed_size<_Deg> Dim; + typedef typename NumTraits<_Scalar>::Real RealScalar; + typedef internal::increment_if_fixed_size<_Deg> Dim; typedef Matrix<_Scalar,Dim::ret,1> PolynomialType; typedef Matrix<_Scalar,_Deg,1> EvalRootsType; + typedef Matrix RealRootsType; cout << "Standard cases" << endl; PolynomialType pols = PolynomialType::Random(deg+1); @@ -180,15 +199,11 @@ void polynomialsolver(int deg) evalSolver<_Deg,PolynomialType>( pols ); cout << "Test sugar" << endl; - EvalRootsType realRoots = EvalRootsType::Random(deg); + RealRootsType realRoots = RealRootsType::Random(deg); roots_to_monicPolynomial( realRoots, pols ); evalSolverSugarFunction<_Deg>( pols, - realRoots.template cast < - std::complex< - typename NumTraits<_Scalar>::Real - > - >(), + realRoots.template cast >().eval(), realRoots ); } @@ -212,5 +227,6 @@ void test_polynomialsolver() internal::random(9,13) )) ); CALL_SUBTEST_11((polynomialsolver(1)) ); + CALL_SUBTEST_12((polynomialsolver,Dynamic>(internal::random(2,13))) ); } } From a80308d19fa3f7bb76804da88d051c871b8e76bd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 16:27:17 -0500 Subject: [PATCH 59/71] Revert "update the packaged Eigen to 3.3.9" This reverts commit 2f4218d0460325e5e42f92e2bc3244b21c55b6d6. --- .../3rdparty/Eigen/{.gitignore => .hgignore} | 7 +- gtsam/3rdparty/Eigen/.hgtags | 33 + gtsam/3rdparty/Eigen/CMakeLists.txt | 13 +- gtsam/3rdparty/Eigen/CTestConfig.cmake | 6 +- gtsam/3rdparty/Eigen/Eigen/Core | 7 +- gtsam/3rdparty/Eigen/Eigen/Eigenvalues | 4 +- gtsam/3rdparty/Eigen/Eigen/Geometry | 4 +- gtsam/3rdparty/Eigen/Eigen/QR | 4 +- gtsam/3rdparty/Eigen/Eigen/SparseQR | 1 + .../3rdparty/Eigen/Eigen/src/Core/ArrayBase.h | 4 +- .../Eigen/Eigen/src/Core/CwiseUnaryView.h | 2 - .../3rdparty/Eigen/Eigen/src/Core/DenseBase.h | 7 +- .../Eigen/Eigen/src/Core/DenseStorage.h | 6 +- .../Eigen/Eigen/src/Core/GenericPacketMath.h | 9 +- gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h | 5 - .../Eigen/Eigen/src/Core/MathFunctions.h | 16 +- .../Eigen/Eigen/src/Core/MatrixBase.h | 3 +- .../Eigen/Eigen/src/Core/PermutationMatrix.h | 28 + .../Eigen/Eigen/src/Core/PlainObjectBase.h | 12 +- .../Eigen/Eigen/src/Core/ProductEvaluators.h | 28 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h | 5 +- .../Eigen/Eigen/src/Core/SolveTriangular.h | 6 +- .../3rdparty/Eigen/Eigen/src/Core/Transpose.h | 2 - .../Eigen/Eigen/src/Core/Transpositions.h | 39 + .../Eigen/Eigen/src/Core/TriangularMatrix.h | 8 +- .../src/Core/arch/AVX512/MathFunctions.h | 70 +- .../Eigen/src/Core/arch/AVX512/PacketMath.h | 719 +++++++++--------- .../Eigen/Eigen/src/Core/arch/CUDA/Half.h | 1 - .../Eigen/src/Core/arch/CUDA/PacketMathHalf.h | 4 +- .../Eigen/src/Core/functors/UnaryFunctors.h | 2 +- .../Core/products/GeneralBlockPanelKernel.h | 7 +- .../src/Core/products/GeneralMatrixMatrix.h | 37 +- .../products/GeneralMatrixMatrixTriangular.h | 68 +- .../GeneralMatrixMatrixTriangular_BLAS.h | 10 +- .../Core/products/GeneralMatrixMatrix_BLAS.h | 6 +- .../Eigen/src/Core/products/Parallelizer.h | 9 +- .../Core/products/SelfadjointMatrixMatrix.h | 54 +- .../products/SelfadjointMatrixMatrix_BLAS.h | 24 +- .../src/Core/products/SelfadjointProduct.h | 4 +- .../Core/products/TriangularMatrixMatrix.h | 54 +- .../products/TriangularMatrixMatrix_BLAS.h | 26 +- .../Core/products/TriangularSolverMatrix.h | 62 +- .../products/TriangularSolverMatrix_BLAS.h | 12 +- .../Eigen/Eigen/src/Core/util/BlasUtil.h | 115 +-- .../src/Core/util/DisableStupidWarnings.h | 13 +- .../Eigen/src/Core/util/ForwardDeclarations.h | 6 +- .../Eigen/Eigen/src/Core/util/Macros.h | 58 +- .../3rdparty/Eigen/Eigen/src/Core/util/Meta.h | 34 - .../src/Core/util/ReenableStupidWarnings.h | 6 +- .../Eigen/Eigen/src/Core/util/XprHelper.h | 17 - .../Eigen/src/Eigenvalues/ComplexSchur.h | 9 +- .../Eigen/Eigen/src/Eigenvalues/RealSchur.h | 15 +- .../src/Eigenvalues/SelfAdjointEigenSolver.h | 7 +- .../Eigen/Eigen/src/Geometry/Quaternion.h | 22 +- .../Eigen/Eigen/src/Geometry/Scaling.h | 2 +- .../Eigen/Eigen/src/Geometry/Transform.h | 4 +- .../Eigen/Eigen/src/Geometry/Translation.h | 6 + .../Eigen/Eigen/src/Geometry/Umeyama.h | 2 +- .../Eigen/Eigen/src/LU/PartialPivLU.h | 5 +- .../Eigen/Eigen/src/LU/arch/Inverse_SSE.h | 4 +- .../Eigen/src/PardisoSupport/PardisoSupport.h | 3 +- gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h | 61 +- gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h | 2 +- .../src/SparseCholesky/SimplicialCholesky.h | 2 +- .../SparseCholesky/SimplicialCholesky_impl.h | 2 +- .../Eigen/Eigen/src/SparseCore/AmbiVector.h | 5 +- .../Eigen/src/SparseCore/SparseCwiseUnaryOp.h | 2 - .../Eigen/Eigen/src/SparseCore/SparseMatrix.h | 3 +- .../src/SparseCore/SparseSelfAdjointView.h | 4 +- .../Eigen/Eigen/src/SparseCore/SparseView.h | 1 - .../Eigen/Eigen/src/SparseLU/SparseLU.h | 4 +- .../Eigen/Eigen/src/StlSupport/StdDeque.h | 6 +- .../Eigen/src/plugins/ArrayCwiseBinaryOps.h | 2 +- gtsam/3rdparty/Eigen/bench/bench_gemm.cpp | 5 +- gtsam/3rdparty/Eigen/blas/CMakeLists.txt | 6 +- gtsam/3rdparty/Eigen/blas/level3_impl.h | 182 ++--- gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake | 2 - .../Eigen/cmake/FindStandardMathLibrary.cmake | 7 +- gtsam/3rdparty/Eigen/doc/CMakeLists.txt | 9 +- .../doc/CustomizingEigen_CustomScalar.dox | 2 +- gtsam/3rdparty/Eigen/doc/Doxyfile.in | 17 +- gtsam/3rdparty/Eigen/doc/Pitfalls.dox | 84 +- .../Eigen/doc/SparseQuickReference.dox | 2 +- .../Eigen/doc/TopicLazyEvaluation.dox | 76 +- .../Eigen/doc/TopicMultithreading.dox | 1 - gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox | 4 +- .../3rdparty/Eigen/doc/eigen_navtree_hacks.js | 5 +- .../Eigen/doc/eigendoxy_footer.html.in | 13 + .../Eigen/doc/eigendoxy_header.html.in | 3 - ...orial_BlockOperations_block_assignment.cpp | 2 +- gtsam/3rdparty/Eigen/lapack/CMakeLists.txt | 10 +- gtsam/3rdparty/Eigen/test/CMakeLists.txt | 4 +- .../Eigen/test/{array_cwise.cpp => array.cpp} | 8 +- gtsam/3rdparty/Eigen/test/bdcsvd.cpp | 8 +- gtsam/3rdparty/Eigen/test/constructor.cpp | 14 - gtsam/3rdparty/Eigen/test/ctorleak.cpp | 20 +- .../Eigen/test/eigensolver_generic.cpp | 2 +- gtsam/3rdparty/Eigen/test/exceptions.cpp | 4 +- gtsam/3rdparty/Eigen/test/fastmath.cpp | 34 +- gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp | 3 +- gtsam/3rdparty/Eigen/test/geo_quaternion.cpp | 8 - .../Eigen/test/geo_transformations.cpp | 61 +- gtsam/3rdparty/Eigen/test/inverse.cpp | 17 - gtsam/3rdparty/Eigen/test/main.h | 5 - gtsam/3rdparty/Eigen/test/numext.cpp | 5 +- gtsam/3rdparty/Eigen/test/packetmath.cpp | 17 +- gtsam/3rdparty/Eigen/test/product.h | 26 - gtsam/3rdparty/Eigen/test/product_large.cpp | 2 - gtsam/3rdparty/Eigen/test/product_mmtr.cpp | 10 - gtsam/3rdparty/Eigen/test/product_symm.cpp | 20 +- gtsam/3rdparty/Eigen/test/product_syrk.cpp | 11 - gtsam/3rdparty/Eigen/test/product_trmm.cpp | 12 +- gtsam/3rdparty/Eigen/test/product_trsolve.cpp | 26 - gtsam/3rdparty/Eigen/test/ref.cpp | 12 +- gtsam/3rdparty/Eigen/test/rvalue_types.cpp | 74 +- gtsam/3rdparty/Eigen/test/sparse_basic.cpp | 8 - gtsam/3rdparty/Eigen/test/stddeque.cpp | 16 +- .../3rdparty/Eigen/test/stddeque_overload.cpp | 14 +- gtsam/3rdparty/Eigen/test/stdlist.cpp | 16 +- .../3rdparty/Eigen/test/stdlist_overload.cpp | 14 +- gtsam/3rdparty/Eigen/test/stdvector.cpp | 6 +- .../Eigen/test/stdvector_overload.cpp | 6 +- .../Eigen/test/vectorization_logic.cpp | 8 - .../Eigen/unsupported/Eigen/ArpackSupport | 4 +- .../src/Tensor/TensorContractionMapper.h | 2 - .../CXX11/src/Tensor/TensorDeviceThreadPool.h | 2 +- .../Eigen/CXX11/src/ThreadPool/EventCount.h | 2 +- .../Eigen/CXX11/src/ThreadPool/RunQueue.h | 2 +- .../Eigen/unsupported/Eigen/Polynomials | 4 +- .../Eigen/src/AutoDiff/AutoDiffScalar.h | 28 +- .../ArpackSelfAdjointEigenSolver.h | 21 +- .../unsupported/Eigen/src/FFT/ei_fftw_impl.h | 2 - .../src/MatrixFunctions/MatrixExponential.h | 2 +- .../src/MatrixFunctions/MatrixSquareRoot.h | 18 +- .../Eigen/src/Polynomials/Companion.h | 57 +- .../Eigen/src/Polynomials/PolynomialSolver.h | 46 +- .../unsupported/Eigen/src/Splines/Spline.h | 2 +- .../test/NonLinearOptimization.cpp | 119 +-- .../Eigen/unsupported/test/autodiff.cpp | 16 - .../unsupported/test/matrix_function.cpp | 38 - .../unsupported/test/polynomialsolver.cpp | 56 +- 141 files changed, 1220 insertions(+), 1972 deletions(-) rename gtsam/3rdparty/Eigen/{.gitignore => .hgignore} (83%) create mode 100644 gtsam/3rdparty/Eigen/.hgtags mode change 100644 => 100755 gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h rename gtsam/3rdparty/Eigen/test/{array_cwise.cpp => array.cpp} (98%) diff --git a/gtsam/3rdparty/Eigen/.gitignore b/gtsam/3rdparty/Eigen/.hgignore similarity index 83% rename from gtsam/3rdparty/Eigen/.gitignore rename to gtsam/3rdparty/Eigen/.hgignore index fc0e5486c..769a47f1f 100644 --- a/gtsam/3rdparty/Eigen/.gitignore +++ b/gtsam/3rdparty/Eigen/.hgignore @@ -1,3 +1,4 @@ +syntax: glob qrc_*cxx *.orig *.pyc @@ -12,7 +13,7 @@ core core.* *.bak *~ -*build* +build* *.moc.* *.moc ui_* @@ -27,11 +28,7 @@ activity.png *.rej log patch -*.patch a a.* lapack/testing lapack/reference -.*project -.settings -Makefile diff --git a/gtsam/3rdparty/Eigen/.hgtags b/gtsam/3rdparty/Eigen/.hgtags new file mode 100644 index 000000000..32ec946a2 --- /dev/null +++ b/gtsam/3rdparty/Eigen/.hgtags @@ -0,0 +1,33 @@ +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 +a8e0d153fc5e239ef8b06e3665f1f9e8cb8d49c8 before-evaluators +09a8e21866106b49c5dec1d6d543e5794e82efa0 3.3-alpha1 +ce5a455b34c0a0ac3545a1497cb4a16c38ed90e8 3.3-beta1 +69d418c0699907bcd0bf9e0b3ba0a112ed091d85 3.3-beta2 +bef509908b9da05d0d07ffc0da105e2c8c6d3996 3.3-rc1 +04ab5fa4b241754afcf631117572276444c67239 3.3-rc2 +26667be4f70baf4f0d39e96f330714c87b399090 3.3.0 +f562a193118d4f40514e2f4a0ace6e974926ef06 3.3.1 +da9b4e14c2550e0d11078a3c39e6d56eba9905df 3.3.2 +67e894c6cd8f5f1f604b27d37ed47fdf012674ff 3.3.3 diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt index dbb9bcf22..2bfb6d560 100644 --- a/gtsam/3rdparty/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/CMakeLists.txt @@ -391,27 +391,22 @@ endif() if(EIGEN_INCLUDE_INSTALL_DIR AND NOT INCLUDE_INSTALL_DIR) set(INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR} - CACHE STRING "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed") + CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed") else() set(INCLUDE_INSTALL_DIR "${CMAKE_INSTALL_INCLUDEDIR}/eigen3" - CACHE STRING "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed" + CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed" ) endif() set(CMAKEPACKAGE_INSTALL_DIR "${CMAKE_INSTALL_DATADIR}/eigen3/cmake" - CACHE STRING "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed" + CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed" ) set(PKGCONFIG_INSTALL_DIR "${CMAKE_INSTALL_DATADIR}/pkgconfig" - CACHE STRING "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed" + CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed" ) -foreach(var INCLUDE_INSTALL_DIR CMAKEPACKAGE_INSTALL_DIR PKGCONFIG_INSTALL_DIR) - if(IS_ABSOLUTE "${${var}}") - message(FATAL_ERROR "${var} must be relative to CMAKE_PREFIX_PATH. Got: ${${var}}") - endif() -endforeach() # similar to set_target_properties but append the property instead of overwriting it macro(ei_add_target_property target prop value) diff --git a/gtsam/3rdparty/Eigen/CTestConfig.cmake b/gtsam/3rdparty/Eigen/CTestConfig.cmake index 45de0e6fc..0039bf8ac 100644 --- a/gtsam/3rdparty/Eigen/CTestConfig.cmake +++ b/gtsam/3rdparty/Eigen/CTestConfig.cmake @@ -4,10 +4,10 @@ ## # The following are required to uses Dart and the Cdash dashboard ## ENABLE_TESTING() ## INCLUDE(CTest) -set(CTEST_PROJECT_NAME "Eigen") +set(CTEST_PROJECT_NAME "Eigen 3.3") set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC") set(CTEST_DROP_METHOD "http") -set(CTEST_DROP_SITE "my.cdash.org") -set(CTEST_DROP_LOCATION "/submit.php?project=Eigen") +set(CTEST_DROP_SITE "manao.inria.fr") +set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen+3.3") set(CTEST_DROP_SITE_CDASH TRUE) diff --git a/gtsam/3rdparty/Eigen/Eigen/Core b/gtsam/3rdparty/Eigen/Eigen/Core index ac7c5b300..b923b8c00 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Core +++ b/gtsam/3rdparty/Eigen/Eigen/Core @@ -279,10 +279,7 @@ #include #include #include -#include -#ifndef EIGEN_NO_IO - #include -#endif +#include #include #include #include @@ -378,9 +375,7 @@ using std::ptrdiff_t; #if defined EIGEN_VECTORIZE_AVX512 #include "src/Core/arch/SSE/PacketMath.h" - #include "src/Core/arch/SSE/MathFunctions.h" #include "src/Core/arch/AVX/PacketMath.h" - #include "src/Core/arch/AVX/MathFunctions.h" #include "src/Core/arch/AVX512/PacketMath.h" #include "src/Core/arch/AVX512/MathFunctions.h" #elif defined EIGEN_VECTORIZE_AVX diff --git a/gtsam/3rdparty/Eigen/Eigen/Eigenvalues b/gtsam/3rdparty/Eigen/Eigen/Eigenvalues index 7d6ac787b..f3f661b07 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Eigenvalues +++ b/gtsam/3rdparty/Eigen/Eigen/Eigenvalues @@ -10,14 +10,14 @@ #include "Core" +#include "src/Core/util/DisableStupidWarnings.h" + #include "Cholesky" #include "Jacobi" #include "Householder" #include "LU" #include "Geometry" -#include "src/Core/util/DisableStupidWarnings.h" - /** \defgroup Eigenvalues_Module Eigenvalues module * * diff --git a/gtsam/3rdparty/Eigen/Eigen/Geometry b/gtsam/3rdparty/Eigen/Eigen/Geometry index da88c03bb..716d52952 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Geometry +++ b/gtsam/3rdparty/Eigen/Eigen/Geometry @@ -10,12 +10,12 @@ #include "Core" +#include "src/Core/util/DisableStupidWarnings.h" + #include "SVD" #include "LU" #include -#include "src/Core/util/DisableStupidWarnings.h" - /** \defgroup Geometry_Module Geometry module * * This module provides support for: diff --git a/gtsam/3rdparty/Eigen/Eigen/QR b/gtsam/3rdparty/Eigen/Eigen/QR index 1be1863a1..c7e914469 100644 --- a/gtsam/3rdparty/Eigen/Eigen/QR +++ b/gtsam/3rdparty/Eigen/Eigen/QR @@ -10,12 +10,12 @@ #include "Core" +#include "src/Core/util/DisableStupidWarnings.h" + #include "Cholesky" #include "Jacobi" #include "Householder" -#include "src/Core/util/DisableStupidWarnings.h" - /** \defgroup QR_Module QR module * * diff --git a/gtsam/3rdparty/Eigen/Eigen/SparseQR b/gtsam/3rdparty/Eigen/Eigen/SparseQR index f5fc5fa7f..a6f3b7f7d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/SparseQR +++ b/gtsam/3rdparty/Eigen/Eigen/SparseQR @@ -28,6 +28,7 @@ * */ +#include "OrderingMethods" #include "src/SparseCore/SparseColEtree.h" #include "src/SparseQR/SparseQR.h" diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h index 33f644e21..3dbc7084c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h @@ -153,8 +153,8 @@ template class ArrayBase // inline void evalTo(Dest& dst) const { dst = matrix(); } protected: - EIGEN_DEFAULT_COPY_CONSTRUCTOR(ArrayBase) - EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(ArrayBase) + EIGEN_DEVICE_FUNC + ArrayBase() : Base() {} private: explicit ArrayBase(Index); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h index 5a30fa8df..271033056 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h @@ -121,8 +121,6 @@ class CwiseUnaryViewImpl { return derived().nestedExpression().outerStride() * sizeof(typename internal::traits::Scalar) / sizeof(Scalar); } - protected: - EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(CwiseUnaryViewImpl) }; } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index c55a68230..90066ae73 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -40,7 +40,7 @@ static inline void check_DenseIndex_is_signed() { */ template class DenseBase #ifndef EIGEN_PARSED_BY_DOXYGEN - : public DenseCoeffsBase::value> + : public DenseCoeffsBase #else : public DenseCoeffsBase #endif // not EIGEN_PARSED_BY_DOXYGEN @@ -71,7 +71,7 @@ template class DenseBase typedef Scalar value_type; typedef typename NumTraits::Real RealScalar; - typedef DenseCoeffsBase::value> Base; + typedef DenseCoeffsBase Base; using Base::derived; using Base::const_cast_derived; @@ -587,12 +587,11 @@ template class DenseBase } protected: - EIGEN_DEFAULT_COPY_CONSTRUCTOR(DenseBase) /** Default constructor. Do nothing. */ EIGEN_DEVICE_FUNC DenseBase() { /* Just checks for self-consistency of the flags. - * Only do it when debugging Eigen, as this borders on paranoia and could slow compilation down + * Only do it when debugging Eigen, as this borders on paranoiac and could slow compilation down */ #ifdef EIGEN_INTERNAL_DEBUGGING EIGEN_STATIC_ASSERT((EIGEN_IMPLIES(MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1, int(IsRowMajor)) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h index 7d6d4e66d..7958feeb9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h @@ -404,7 +404,7 @@ template class DenseStorage(m_data, m_rows*m_cols); - if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative + if (size) m_data = internal::conditional_aligned_new_auto(size); else m_data = 0; @@ -479,7 +479,7 @@ template class DenseStorage(m_data, _Rows*m_cols); - if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative + if (size) m_data = internal::conditional_aligned_new_auto(size); else m_data = 0; @@ -553,7 +553,7 @@ template class DenseStorage(m_data, _Cols*m_rows); - if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative + if (size) m_data = internal::conditional_aligned_new_auto(size); else m_data = 0; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h index e59443779..029f8ac36 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h @@ -351,7 +351,10 @@ template EIGEN_DEVICE_FUNC inline Packet preverse(const Packet& /** \internal \returns \a a with real and imaginary part flipped (for complex type only) */ template EIGEN_DEVICE_FUNC inline Packet pcplxflip(const Packet& a) { - return Packet(a.imag(),a.real()); + // 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)); } /************************** @@ -521,10 +524,10 @@ inline void palign(PacketType& first, const PacketType& second) #ifndef __CUDACC__ template<> inline std::complex pmul(const std::complex& a, const std::complex& b) -{ return std::complex(a.real()*b.real() - a.imag()*b.imag(), a.imag()*b.real() + a.real()*b.imag()); } +{ return std::complex(real(a)*real(b) - imag(a)*imag(b), imag(a)*real(b) + real(a)*imag(b)); } template<> inline std::complex pmul(const std::complex& a, const std::complex& b) -{ return std::complex(a.real()*b.real() - a.imag()*b.imag(), a.imag()*b.real() + a.real()*b.imag()); } +{ return std::complex(real(a)*real(b) - imag(a)*imag(b), imag(a)*real(b) + real(a)*imag(b)); } #endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index 92c3b2818..668922ffc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -182,8 +182,6 @@ template class MapBase #endif protected: - EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase) - EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase) template EIGEN_DEVICE_FUNC @@ -296,9 +294,6 @@ template class MapBase // In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base, // see bugs 821 and 920. using ReadOnlyMapBase::Base::operator=; - protected: - EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase) - EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase) }; #undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h index 01736c2a0..b249ce0c8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h @@ -287,7 +287,7 @@ struct abs2_impl_default // IsComplex EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { - return x.real()*x.real() + x.imag()*x.imag(); + return real(x)*real(x) + imag(x)*imag(x); } }; @@ -313,17 +313,14 @@ struct abs2_retval ****************************************************************************/ template -struct norm1_default_impl; - -template -struct norm1_default_impl +struct norm1_default_impl { typedef typename NumTraits::Real RealScalar; EIGEN_DEVICE_FUNC static inline RealScalar run(const Scalar& x) { EIGEN_USING_STD_MATH(abs); - return abs(x.real()) + abs(x.imag()); + return abs(real(x)) + abs(imag(x)); } }; @@ -665,8 +662,8 @@ struct random_default_impl { static inline Scalar run(const Scalar& x, const Scalar& y) { - return Scalar(random(x.real(), y.real()), - random(x.imag(), y.imag())); + return Scalar(random(real(x), real(y)), + random(imag(x), imag(y))); } static inline Scalar run() { @@ -919,9 +916,6 @@ inline EIGEN_MATHFUNC_RETVAL(abs2, Scalar) abs2(const Scalar& x) return EIGEN_MATHFUNC_IMPL(abs2, Scalar)::run(x); } -EIGEN_DEVICE_FUNC -inline bool abs2(bool x) { return x; } - template EIGEN_DEVICE_FUNC inline EIGEN_MATHFUNC_RETVAL(norm1, Scalar) norm1(const Scalar& x) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h index f8bcc8c6f..e6c35907c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h @@ -464,8 +464,7 @@ template class MatrixBase EIGEN_MATRIX_FUNCTION_1(MatrixComplexPowerReturnValue, pow, power to \c p, const std::complex& p) protected: - EIGEN_DEFAULT_COPY_CONSTRUCTOR(MatrixBase) - EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MatrixBase) + EIGEN_DEVICE_FUNC MatrixBase() : Base() {} private: EIGEN_DEVICE_FUNC explicit MatrixBase(int); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index 47c06ba77..b1fb455b9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -87,6 +87,17 @@ class PermutationBase : public EigenBase return derived(); } + #ifndef EIGEN_PARSED_BY_DOXYGEN + /** This is a special case of the templated operator=. Its purpose is to + * prevent a default operator= from hiding the templated operator=. + */ + Derived& operator=(const PermutationBase& other) + { + indices() = other.indices(); + return derived(); + } + #endif + /** \returns the number of rows */ inline Index rows() const { return Index(indices().size()); } @@ -322,6 +333,12 @@ class PermutationMatrix : public PermutationBase& other) : m_indices(other.indices()) {} + #ifndef EIGEN_PARSED_BY_DOXYGEN + /** Standard copy constructor. Defined only to prevent a default copy constructor + * from hiding the other templated constructor */ + inline PermutationMatrix(const PermutationMatrix& other) : m_indices(other.indices()) {} + #endif + /** Generic constructor from expression of the indices. The indices * array has the meaning that the permutations sends each integer i to indices[i]. * @@ -356,6 +373,17 @@ class PermutationMatrix : public PermutationBase::type EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void _init2(Index rows, Index cols, typename internal::enable_if::type* = 0) { - const bool t0_is_integer_alike = internal::is_valid_index_type::value; - const bool t1_is_integer_alike = internal::is_valid_index_type::value; - EIGEN_STATIC_ASSERT(t0_is_integer_alike && - t1_is_integer_alike, + EIGEN_STATIC_ASSERT(bool(NumTraits::IsInteger) && + bool(NumTraits::IsInteger), FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED) resize(rows,cols); } @@ -775,9 +773,9 @@ class PlainObjectBase : public internal::dense_xpr_base::type && ((!internal::is_same::XprKind,ArrayXpr>::value || Base::SizeAtCompileTime==Dynamic)),T>::type* = 0) { // NOTE MSVC 2008 complains if we directly put bool(NumTraits::IsInteger) as the EIGEN_STATIC_ASSERT argument. - const bool is_integer_alike = internal::is_valid_index_type::value; - EIGEN_UNUSED_VARIABLE(is_integer_alike); - EIGEN_STATIC_ASSERT(is_integer_alike, + const bool is_integer = NumTraits::IsInteger; + EIGEN_UNUSED_VARIABLE(is_integer); + EIGEN_STATIC_ASSERT(is_integer, FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED) resize(size); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h index bce1310c9..9b99bd769 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h @@ -396,7 +396,7 @@ struct generic_product_impl // but easier on the compiler side call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::assign_op()); } - + template static EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { @@ -410,32 +410,6 @@ struct generic_product_impl // dst.noalias() -= lhs.lazyProduct(rhs); call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::sub_assign_op()); } - - // Catch "dst {,+,-}= (s*A)*B" and evaluate it lazily by moving out the scalar factor: - // dst {,+,-}= s * (A.lazyProduct(B)) - // This is a huge benefit for heap-allocated matrix types as it save one costly allocation. - // For them, this strategy is also faster than simply by-passing the heap allocation through - // stack allocation. - // For fixed sizes matrices, this is less obvious, it is sometimes x2 faster, but sometimes x3 slower, - // and the behavior depends also a lot on the compiler... so let's be conservative and enable them for dynamic-size only, - // that is when coming from generic_product_impl<...,GemmProduct> in file GeneralMatrixMatrix.h - template - static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE - void eval_dynamic(Dst& dst, const CwiseBinaryOp, - const CwiseNullaryOp, Plain1>, Xpr2>& lhs, const Rhs& rhs, const Func &func) - { - call_assignment_no_alias(dst, lhs.lhs().functor().m_other * lhs.rhs().lazyProduct(rhs), func); - } - - // Here, we we always have LhsT==Lhs, but we need to make it a template type to make the above - // overload more specialized. - template - static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE - void eval_dynamic(Dst& dst, const LhsT& lhs, const Rhs& rhs, const Func &func) - { - call_assignment_no_alias(dst, lhs.lazyProduct(rhs), func); - } - // template // static inline void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h index 17a1496b8..9c6e3c5d9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -28,13 +28,12 @@ struct traits > template struct match { enum { - IsVectorAtCompileTime = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime, HasDirectAccess = internal::has_direct_access::ret, - StorageOrderMatch = IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)), + StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)), InnerStrideMatch = int(StrideType::InnerStrideAtCompileTime)==int(Dynamic) || int(StrideType::InnerStrideAtCompileTime)==int(Derived::InnerStrideAtCompileTime) || (int(StrideType::InnerStrideAtCompileTime)==0 && int(Derived::InnerStrideAtCompileTime)==1), - OuterStrideMatch = IsVectorAtCompileTime + OuterStrideMatch = Derived::IsVectorAtCompileTime || int(StrideType::OuterStrideAtCompileTime)==int(Dynamic) || int(StrideType::OuterStrideAtCompileTime)==int(Derived::OuterStrideAtCompileTime), // NOTE, this indirection of evaluator::Alignment is needed // to workaround a very strange bug in MSVC related to the instantiation diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h index fd0acb1a5..4652e2e19 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h @@ -19,7 +19,7 @@ namespace internal { template struct triangular_solve_vector; -template +template struct triangular_solve_matrix; // small helper struct extracting some traits on the underlying solver operation @@ -98,8 +98,8 @@ struct triangular_solver_selector BlockingType blocking(rhs.rows(), rhs.cols(), size, 1, false); triangular_solve_matrix - ::run(size, othersize, &actualLhs.coeffRef(0,0), actualLhs.outerStride(), &rhs.coeffRef(0,0), rhs.innerStride(), rhs.outerStride(), blocking); + (Rhs::Flags&RowMajorBit) ? RowMajor : ColMajor> + ::run(size, othersize, &actualLhs.coeffRef(0,0), actualLhs.outerStride(), &rhs.coeffRef(0,0), rhs.outerStride(), blocking); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h index 960dc4510..79b767bcc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h @@ -146,8 +146,6 @@ template class TransposeImpl { return derived().nestedExpression().coeffRef(index); } - protected: - EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(TransposeImpl) }; /** \returns an expression of the transpose of *this. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h index 7718625e8..86da5af59 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h @@ -33,6 +33,17 @@ class TranspositionsBase indices() = other.indices(); return derived(); } + + #ifndef EIGEN_PARSED_BY_DOXYGEN + /** This is a special case of the templated operator=. Its purpose is to + * prevent a default operator= from hiding the templated operator=. + */ + Derived& operator=(const TranspositionsBase& other) + { + indices() = other.indices(); + return derived(); + } + #endif /** \returns the number of transpositions */ Index size() const { return indices().size(); } @@ -160,6 +171,12 @@ class Transpositions : public TranspositionsBase& other) : m_indices(other.indices()) {} + #ifndef EIGEN_PARSED_BY_DOXYGEN + /** Standard copy constructor. Defined only to prevent a default copy constructor + * from hiding the other templated constructor */ + inline Transpositions(const Transpositions& other) : m_indices(other.indices()) {} + #endif + /** Generic constructor from expression of the transposition indices. */ template explicit inline Transpositions(const MatrixBase& indices) : m_indices(indices) @@ -172,6 +189,17 @@ class Transpositions : public TranspositionsBase class TriangularView explicit inline TriangularView(MatrixType& matrix) : m_matrix(matrix) {} - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TriangularView) + using Base::operator=; + TriangularView& operator=(const TriangularView &other) + { return Base::operator=(other); } /** \copydoc EigenBase::rows() */ EIGEN_DEVICE_FUNC @@ -542,10 +544,6 @@ template class TriangularViewImpl<_Mat template EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TriangularViewType& _assignProduct(const ProductType& prod, const Scalar& alpha, bool beta); - protected: - EIGEN_DEFAULT_COPY_CONSTRUCTOR(TriangularViewImpl) - EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(TriangularViewImpl) - }; /*************************************************************************** diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h index b259c1e1f..9c1717f76 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h @@ -29,7 +29,6 @@ namespace internal { #define _EIGEN_DECLARE_CONST_Packet8d_FROM_INT64(NAME, X) \ const Packet8d p8d_##NAME = _mm512_castsi512_pd(_mm512_set1_epi64(X)) - // Natural logarithm // Computes log(x) as log(2^e * m) = C*e + log(m), where the constant C =log(2) // and m is in the range [sqrt(1/2),sqrt(2)). In this range, the logarithm can @@ -48,7 +47,6 @@ plog(const Packet16f& _x) { // The smallest non denormalized float number. _EIGEN_DECLARE_CONST_Packet16f_FROM_INT(min_norm_pos, 0x00800000); _EIGEN_DECLARE_CONST_Packet16f_FROM_INT(minus_inf, 0xff800000); - _EIGEN_DECLARE_CONST_Packet16f_FROM_INT(pos_inf, 0x7f800000); _EIGEN_DECLARE_CONST_Packet16f_FROM_INT(nan, 0x7fc00000); // Polynomial coefficients. @@ -66,9 +64,11 @@ plog(const Packet16f& _x) { _EIGEN_DECLARE_CONST_Packet16f(cephes_log_q2, 0.693359375f); // invalid_mask is set to true when x is NaN - __mmask16 invalid_mask = _mm512_cmp_ps_mask(x, _mm512_setzero_ps(), _CMP_NGE_UQ); - __mmask16 iszero_mask = _mm512_cmp_ps_mask(x, _mm512_setzero_ps(), _CMP_EQ_OQ); - + __mmask16 invalid_mask = + _mm512_cmp_ps_mask(x, _mm512_setzero_ps(), _CMP_NGE_UQ); + __mmask16 iszero_mask = + _mm512_cmp_ps_mask(x, _mm512_setzero_ps(), _CMP_EQ_UQ); + // Truncate input values to the minimum positive normal. x = pmax(x, p16f_min_norm_pos); @@ -118,18 +118,11 @@ plog(const Packet16f& _x) { x = padd(x, y); x = padd(x, y2); - __mmask16 pos_inf_mask = _mm512_cmp_ps_mask(_x,p16f_pos_inf,_CMP_EQ_OQ); - // Filter out invalid inputs, i.e.: - // - negative arg will be NAN, - // - 0 will be -INF. - // - +INF will be +INF + // Filter out invalid inputs, i.e. negative arg will be NAN, 0 will be -INF. return _mm512_mask_blend_ps(iszero_mask, - _mm512_mask_blend_ps(invalid_mask, - _mm512_mask_blend_ps(pos_inf_mask,x,p16f_pos_inf), - p16f_nan), - p16f_minus_inf); + _mm512_mask_blend_ps(invalid_mask, x, p16f_nan), + p16f_minus_inf); } - #endif // Exponential function. Works by writing "x = m*log(2) + r" where @@ -265,39 +258,48 @@ pexp(const Packet8d& _x) { template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet16f psqrt(const Packet16f& _x) { - Packet16f neg_half = pmul(_x, pset1(-.5f)); - __mmask16 denormal_mask = _mm512_kand( - _mm512_cmp_ps_mask(_x, pset1((std::numeric_limits::min)()), - _CMP_LT_OQ), - _mm512_cmp_ps_mask(_x, _mm512_setzero_ps(), _CMP_GE_OQ)); + _EIGEN_DECLARE_CONST_Packet16f(one_point_five, 1.5f); + _EIGEN_DECLARE_CONST_Packet16f(minus_half, -0.5f); + _EIGEN_DECLARE_CONST_Packet16f_FROM_INT(flt_min, 0x00800000); - Packet16f x = _mm512_rsqrt14_ps(_x); + Packet16f neg_half = pmul(_x, p16f_minus_half); + + // select only the inverse sqrt of positive normal inputs (denormals are + // flushed to zero and cause infs as well). + __mmask16 non_zero_mask = _mm512_cmp_ps_mask(_x, p16f_flt_min, _CMP_GE_OQ); + Packet16f x = _mm512_mask_blend_ps(non_zero_mask, _mm512_setzero_ps(), _mm512_rsqrt14_ps(_x)); // Do a single step of Newton's iteration. - x = pmul(x, pmadd(neg_half, pmul(x, x), pset1(1.5f))); + x = pmul(x, pmadd(neg_half, pmul(x, x), p16f_one_point_five)); - // Flush results for denormals to zero. - return _mm512_mask_blend_ps(denormal_mask, pmul(_x,x), _mm512_setzero_ps()); + // Multiply the original _x by it's reciprocal square root to extract the + // square root. + return pmul(_x, x); } template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8d psqrt(const Packet8d& _x) { - Packet8d neg_half = pmul(_x, pset1(-.5)); - __mmask16 denormal_mask = _mm512_kand( - _mm512_cmp_pd_mask(_x, pset1((std::numeric_limits::min)()), - _CMP_LT_OQ), - _mm512_cmp_pd_mask(_x, _mm512_setzero_pd(), _CMP_GE_OQ)); + _EIGEN_DECLARE_CONST_Packet8d(one_point_five, 1.5); + _EIGEN_DECLARE_CONST_Packet8d(minus_half, -0.5); + _EIGEN_DECLARE_CONST_Packet8d_FROM_INT64(dbl_min, 0x0010000000000000LL); - Packet8d x = _mm512_rsqrt14_pd(_x); + Packet8d neg_half = pmul(_x, p8d_minus_half); - // Do a single step of Newton's iteration. - x = pmul(x, pmadd(neg_half, pmul(x, x), pset1(1.5))); + // select only the inverse sqrt of positive normal inputs (denormals are + // flushed to zero and cause infs as well). + __mmask8 non_zero_mask = _mm512_cmp_pd_mask(_x, p8d_dbl_min, _CMP_GE_OQ); + Packet8d x = _mm512_mask_blend_pd(non_zero_mask, _mm512_setzero_pd(), _mm512_rsqrt14_pd(_x)); + + // Do a first step of Newton's iteration. + x = pmul(x, pmadd(neg_half, pmul(x, x), p8d_one_point_five)); // Do a second step of Newton's iteration. - x = pmul(x, pmadd(neg_half, pmul(x, x), pset1(1.5))); + x = pmul(x, pmadd(neg_half, pmul(x, x), p8d_one_point_five)); - return _mm512_mask_blend_pd(denormal_mask, pmul(_x,x), _mm512_setzero_pd()); + // Multiply the original _x by it's reciprocal square root to extract the + // square root. + return pmul(_x, x); } #else template <> diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h index 000b7762f..5adddc7ae 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h @@ -19,10 +19,10 @@ namespace internal { #endif #ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS -#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 32 +#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS (2*sizeof(void*)) #endif -#ifdef EIGEN_VECTORIZE_FMA +#ifdef __FMA__ #ifndef EIGEN_HAS_SINGLE_INSTRUCTION_MADD #define EIGEN_HAS_SINGLE_INSTRUCTION_MADD #endif @@ -54,14 +54,13 @@ template<> struct packet_traits : default_packet_traits AlignedOnScalar = 1, size = 16, HasHalfPacket = 1, - HasBlend = 0, -#if EIGEN_GNUC_AT_LEAST(5, 3) || (!EIGEN_COMP_GNUC_STRICT) +#if EIGEN_GNUC_AT_LEAST(5, 3) #ifdef EIGEN_VECTORIZE_AVX512DQ HasLog = 1, #endif HasExp = 1, - HasSqrt = EIGEN_FAST_MATH, - HasRsqrt = EIGEN_FAST_MATH, + HasSqrt = 1, + HasRsqrt = 1, #endif HasDiv = 1 }; @@ -75,8 +74,8 @@ template<> struct packet_traits : default_packet_traits AlignedOnScalar = 1, size = 8, HasHalfPacket = 1, -#if EIGEN_GNUC_AT_LEAST(5, 3) || (!EIGEN_COMP_GNUC_STRICT) - HasSqrt = EIGEN_FAST_MATH, +#if EIGEN_GNUC_AT_LEAST(5, 3) + HasSqrt = 1, HasRsqrt = EIGEN_FAST_MATH, #endif HasDiv = 1 @@ -99,7 +98,6 @@ template <> struct unpacket_traits { typedef float type; typedef Packet8f half; - typedef Packet16i integer_packet; enum { size = 16, alignment=Aligned64 }; }; template <> @@ -134,7 +132,7 @@ EIGEN_STRONG_INLINE Packet16f pload1(const float* from) { } template <> EIGEN_STRONG_INLINE Packet8d pload1(const double* from) { - return _mm512_set1_pd(*from); + return _mm512_broadcastsd_pd(_mm_load_pd1(from)); } template <> @@ -160,11 +158,6 @@ EIGEN_STRONG_INLINE Packet8d padd(const Packet8d& a, const Packet8d& b) { return _mm512_add_pd(a, b); } -template <> -EIGEN_STRONG_INLINE Packet16i padd(const Packet16i& a, - const Packet16i& b) { - return _mm512_add_epi32(a, b); -} template <> EIGEN_STRONG_INLINE Packet16f psub(const Packet16f& a, @@ -176,11 +169,6 @@ EIGEN_STRONG_INLINE Packet8d psub(const Packet8d& a, const Packet8d& b) { return _mm512_sub_pd(a, b); } -template <> -EIGEN_STRONG_INLINE Packet16i psub(const Packet16i& a, - const Packet16i& b) { - return _mm512_sub_epi32(a, b); -} template <> EIGEN_STRONG_INLINE Packet16f pnegate(const Packet16f& a) { @@ -214,11 +202,6 @@ EIGEN_STRONG_INLINE Packet8d pmul(const Packet8d& a, const Packet8d& b) { return _mm512_mul_pd(a, b); } -template <> -EIGEN_STRONG_INLINE Packet16i pmul(const Packet16i& a, - const Packet16i& b) { - return _mm512_mul_epi32(a, b); -} template <> EIGEN_STRONG_INLINE Packet16f pdiv(const Packet16f& a, @@ -231,7 +214,7 @@ EIGEN_STRONG_INLINE Packet8d pdiv(const Packet8d& a, return _mm512_div_pd(a, b); } -#ifdef EIGEN_VECTORIZE_FMA +#ifdef __FMA__ template <> EIGEN_STRONG_INLINE Packet16f pmadd(const Packet16f& a, const Packet16f& b, const Packet16f& c) { @@ -247,73 +230,23 @@ EIGEN_STRONG_INLINE Packet8d pmadd(const Packet8d& a, const Packet8d& b, template <> EIGEN_STRONG_INLINE Packet16f pmin(const Packet16f& a, const Packet16f& b) { - // Arguments are reversed to match NaN propagation behavior of std::min. - return _mm512_min_ps(b, a); + return _mm512_min_ps(a, b); } template <> EIGEN_STRONG_INLINE Packet8d pmin(const Packet8d& a, const Packet8d& b) { - // Arguments are reversed to match NaN propagation behavior of std::min. - return _mm512_min_pd(b, a); + return _mm512_min_pd(a, b); } template <> EIGEN_STRONG_INLINE Packet16f pmax(const Packet16f& a, const Packet16f& b) { - // Arguments are reversed to match NaN propagation behavior of std::max. - return _mm512_max_ps(b, a); + return _mm512_max_ps(a, b); } template <> EIGEN_STRONG_INLINE Packet8d pmax(const Packet8d& a, const Packet8d& b) { - // Arguments are reversed to match NaN propagation behavior of std::max. - return _mm512_max_pd(b, a); -} - -#ifdef EIGEN_VECTORIZE_AVX512DQ -template EIGEN_STRONG_INLINE Packet8f extract256(Packet16f x) { return _mm512_extractf32x8_ps(x,I_); } -template EIGEN_STRONG_INLINE Packet2d extract128(Packet8d x) { return _mm512_extractf64x2_pd(x,I_); } -EIGEN_STRONG_INLINE Packet16f cat256(Packet8f a, Packet8f b) { return _mm512_insertf32x8(_mm512_castps256_ps512(a),b,1); } -#else -// AVX512F does not define _mm512_extractf32x8_ps to extract _m256 from _m512 -template EIGEN_STRONG_INLINE Packet8f extract256(Packet16f x) { - return _mm256_castsi256_ps(_mm512_extracti64x4_epi64( _mm512_castps_si512(x),I_)); -} - -// AVX512F does not define _mm512_extractf64x2_pd to extract _m128 from _m512 -template EIGEN_STRONG_INLINE Packet2d extract128(Packet8d x) { - return _mm_castsi128_pd(_mm512_extracti32x4_epi32( _mm512_castpd_si512(x),I_)); -} - -EIGEN_STRONG_INLINE Packet16f cat256(Packet8f a, Packet8f b) { - return _mm512_castsi512_ps(_mm512_inserti64x4(_mm512_castsi256_si512(_mm256_castps_si256(a)), - _mm256_castps_si256(b),1)); -} -#endif - -// Helper function for bit packing snippet of low precision comparison. -// It packs the flags from 32x16 to 16x16. -EIGEN_STRONG_INLINE __m256i Pack32To16(Packet16f rf) { - // Split data into small pieces and handle with AVX instructions - // to guarantee internal order of vector. - // Operation: - // dst[15:0] := Saturate16(rf[31:0]) - // dst[31:16] := Saturate16(rf[63:32]) - // ... - // dst[255:240] := Saturate16(rf[255:224]) - __m256i lo = _mm256_castps_si256(extract256<0>(rf)); - __m256i hi = _mm256_castps_si256(extract256<1>(rf)); - __m128i result_lo = _mm_packs_epi32(_mm256_extractf128_si256(lo, 0), - _mm256_extractf128_si256(lo, 1)); - __m128i result_hi = _mm_packs_epi32(_mm256_extractf128_si256(hi, 0), - _mm256_extractf128_si256(hi, 1)); - return _mm256_insertf128_si256(_mm256_castsi128_si256(result_lo), result_hi, 1); -} - -template <> -EIGEN_STRONG_INLINE Packet16i pand(const Packet16i& a, - const Packet16i& b) { - return _mm512_and_si512(a,b); + return _mm512_max_pd(a, b); } template <> @@ -322,7 +255,24 @@ EIGEN_STRONG_INLINE Packet16f pand(const Packet16f& a, #ifdef EIGEN_VECTORIZE_AVX512DQ return _mm512_and_ps(a, b); #else - return _mm512_castsi512_ps(pand(_mm512_castps_si512(a),_mm512_castps_si512(b))); + Packet16f res = _mm512_undefined_ps(); + Packet4f lane0_a = _mm512_extractf32x4_ps(a, 0); + Packet4f lane0_b = _mm512_extractf32x4_ps(b, 0); + res = _mm512_insertf32x4(res, _mm_and_ps(lane0_a, lane0_b), 0); + + Packet4f lane1_a = _mm512_extractf32x4_ps(a, 1); + Packet4f lane1_b = _mm512_extractf32x4_ps(b, 1); + res = _mm512_insertf32x4(res, _mm_and_ps(lane1_a, lane1_b), 1); + + Packet4f lane2_a = _mm512_extractf32x4_ps(a, 2); + Packet4f lane2_b = _mm512_extractf32x4_ps(b, 2); + res = _mm512_insertf32x4(res, _mm_and_ps(lane2_a, lane2_b), 2); + + Packet4f lane3_a = _mm512_extractf32x4_ps(a, 3); + Packet4f lane3_b = _mm512_extractf32x4_ps(b, 3); + res = _mm512_insertf32x4(res, _mm_and_ps(lane3_a, lane3_b), 3); + + return res; #endif } template <> @@ -338,21 +288,35 @@ EIGEN_STRONG_INLINE Packet8d pand(const Packet8d& a, Packet4d lane1_a = _mm512_extractf64x4_pd(a, 1); Packet4d lane1_b = _mm512_extractf64x4_pd(b, 1); - return _mm512_insertf64x4(res, _mm256_and_pd(lane1_a, lane1_b), 1); + res = _mm512_insertf64x4(res, _mm256_and_pd(lane1_a, lane1_b), 1); + + return res; #endif } - template <> -EIGEN_STRONG_INLINE Packet16i por(const Packet16i& a, const Packet16i& b) { - return _mm512_or_si512(a, b); -} - -template <> -EIGEN_STRONG_INLINE Packet16f por(const Packet16f& a, const Packet16f& b) { +EIGEN_STRONG_INLINE Packet16f por(const Packet16f& a, + const Packet16f& b) { #ifdef EIGEN_VECTORIZE_AVX512DQ return _mm512_or_ps(a, b); #else - return _mm512_castsi512_ps(por(_mm512_castps_si512(a),_mm512_castps_si512(b))); + Packet16f res = _mm512_undefined_ps(); + Packet4f lane0_a = _mm512_extractf32x4_ps(a, 0); + Packet4f lane0_b = _mm512_extractf32x4_ps(b, 0); + res = _mm512_insertf32x4(res, _mm_or_ps(lane0_a, lane0_b), 0); + + Packet4f lane1_a = _mm512_extractf32x4_ps(a, 1); + Packet4f lane1_b = _mm512_extractf32x4_ps(b, 1); + res = _mm512_insertf32x4(res, _mm_or_ps(lane1_a, lane1_b), 1); + + Packet4f lane2_a = _mm512_extractf32x4_ps(a, 2); + Packet4f lane2_b = _mm512_extractf32x4_ps(b, 2); + res = _mm512_insertf32x4(res, _mm_or_ps(lane2_a, lane2_b), 2); + + Packet4f lane3_a = _mm512_extractf32x4_ps(a, 3); + Packet4f lane3_b = _mm512_extractf32x4_ps(b, 3); + res = _mm512_insertf32x4(res, _mm_or_ps(lane3_a, lane3_b), 3); + + return res; #endif } @@ -362,67 +326,109 @@ EIGEN_STRONG_INLINE Packet8d por(const Packet8d& a, #ifdef EIGEN_VECTORIZE_AVX512DQ return _mm512_or_pd(a, b); #else - return _mm512_castsi512_pd(por(_mm512_castpd_si512(a),_mm512_castpd_si512(b))); + Packet8d res = _mm512_undefined_pd(); + Packet4d lane0_a = _mm512_extractf64x4_pd(a, 0); + Packet4d lane0_b = _mm512_extractf64x4_pd(b, 0); + res = _mm512_insertf64x4(res, _mm256_or_pd(lane0_a, lane0_b), 0); + + Packet4d lane1_a = _mm512_extractf64x4_pd(a, 1); + Packet4d lane1_b = _mm512_extractf64x4_pd(b, 1); + res = _mm512_insertf64x4(res, _mm256_or_pd(lane1_a, lane1_b), 1); + + return res; #endif } template <> -EIGEN_STRONG_INLINE Packet16i pxor(const Packet16i& a, const Packet16i& b) { - return _mm512_xor_si512(a, b); -} - -template <> -EIGEN_STRONG_INLINE Packet16f pxor(const Packet16f& a, const Packet16f& b) { +EIGEN_STRONG_INLINE Packet16f pxor(const Packet16f& a, + const Packet16f& b) { #ifdef EIGEN_VECTORIZE_AVX512DQ return _mm512_xor_ps(a, b); #else - return _mm512_castsi512_ps(pxor(_mm512_castps_si512(a),_mm512_castps_si512(b))); + Packet16f res = _mm512_undefined_ps(); + Packet4f lane0_a = _mm512_extractf32x4_ps(a, 0); + Packet4f lane0_b = _mm512_extractf32x4_ps(b, 0); + res = _mm512_insertf32x4(res, _mm_xor_ps(lane0_a, lane0_b), 0); + + Packet4f lane1_a = _mm512_extractf32x4_ps(a, 1); + Packet4f lane1_b = _mm512_extractf32x4_ps(b, 1); + res = _mm512_insertf32x4(res, _mm_xor_ps(lane1_a, lane1_b), 1); + + Packet4f lane2_a = _mm512_extractf32x4_ps(a, 2); + Packet4f lane2_b = _mm512_extractf32x4_ps(b, 2); + res = _mm512_insertf32x4(res, _mm_xor_ps(lane2_a, lane2_b), 2); + + Packet4f lane3_a = _mm512_extractf32x4_ps(a, 3); + Packet4f lane3_b = _mm512_extractf32x4_ps(b, 3); + res = _mm512_insertf32x4(res, _mm_xor_ps(lane3_a, lane3_b), 3); + + return res; #endif } - template <> -EIGEN_STRONG_INLINE Packet8d pxor(const Packet8d& a, const Packet8d& b) { +EIGEN_STRONG_INLINE Packet8d pxor(const Packet8d& a, + const Packet8d& b) { #ifdef EIGEN_VECTORIZE_AVX512DQ return _mm512_xor_pd(a, b); #else - return _mm512_castsi512_pd(pxor(_mm512_castpd_si512(a),_mm512_castpd_si512(b))); + Packet8d res = _mm512_undefined_pd(); + Packet4d lane0_a = _mm512_extractf64x4_pd(a, 0); + Packet4d lane0_b = _mm512_extractf64x4_pd(b, 0); + res = _mm512_insertf64x4(res, _mm256_xor_pd(lane0_a, lane0_b), 0); + + Packet4d lane1_a = _mm512_extractf64x4_pd(a, 1); + Packet4d lane1_b = _mm512_extractf64x4_pd(b, 1); + res = _mm512_insertf64x4(res, _mm256_xor_pd(lane1_a, lane1_b), 1); + + return res; #endif } template <> -EIGEN_STRONG_INLINE Packet16i pandnot(const Packet16i& a, const Packet16i& b) { - return _mm512_andnot_si512(b, a); -} - -template <> -EIGEN_STRONG_INLINE Packet16f pandnot(const Packet16f& a, const Packet16f& b) { +EIGEN_STRONG_INLINE Packet16f pandnot(const Packet16f& a, + const Packet16f& b) { #ifdef EIGEN_VECTORIZE_AVX512DQ - return _mm512_andnot_ps(b, a); + return _mm512_andnot_ps(a, b); #else - return _mm512_castsi512_ps(pandnot(_mm512_castps_si512(a),_mm512_castps_si512(b))); + Packet16f res = _mm512_undefined_ps(); + Packet4f lane0_a = _mm512_extractf32x4_ps(a, 0); + Packet4f lane0_b = _mm512_extractf32x4_ps(b, 0); + res = _mm512_insertf32x4(res, _mm_andnot_ps(lane0_a, lane0_b), 0); + + Packet4f lane1_a = _mm512_extractf32x4_ps(a, 1); + Packet4f lane1_b = _mm512_extractf32x4_ps(b, 1); + res = _mm512_insertf32x4(res, _mm_andnot_ps(lane1_a, lane1_b), 1); + + Packet4f lane2_a = _mm512_extractf32x4_ps(a, 2); + Packet4f lane2_b = _mm512_extractf32x4_ps(b, 2); + res = _mm512_insertf32x4(res, _mm_andnot_ps(lane2_a, lane2_b), 2); + + Packet4f lane3_a = _mm512_extractf32x4_ps(a, 3); + Packet4f lane3_b = _mm512_extractf32x4_ps(b, 3); + res = _mm512_insertf32x4(res, _mm_andnot_ps(lane3_a, lane3_b), 3); + + return res; #endif } template <> -EIGEN_STRONG_INLINE Packet8d pandnot(const Packet8d& a,const Packet8d& b) { +EIGEN_STRONG_INLINE Packet8d pandnot(const Packet8d& a, + const Packet8d& b) { #ifdef EIGEN_VECTORIZE_AVX512DQ - return _mm512_andnot_pd(b, a); + return _mm512_andnot_pd(a, b); #else - return _mm512_castsi512_pd(pandnot(_mm512_castpd_si512(a),_mm512_castpd_si512(b))); + Packet8d res = _mm512_undefined_pd(); + Packet4d lane0_a = _mm512_extractf64x4_pd(a, 0); + Packet4d lane0_b = _mm512_extractf64x4_pd(b, 0); + res = _mm512_insertf64x4(res, _mm256_andnot_pd(lane0_a, lane0_b), 0); + + Packet4d lane1_a = _mm512_extractf64x4_pd(a, 1); + Packet4d lane1_b = _mm512_extractf64x4_pd(b, 1); + res = _mm512_insertf64x4(res, _mm256_andnot_pd(lane1_a, lane1_b), 1); + + return res; #endif } -template EIGEN_STRONG_INLINE Packet16i parithmetic_shift_right(Packet16i a) { - return _mm512_srai_epi32(a, N); -} - -template EIGEN_STRONG_INLINE Packet16i plogical_shift_right(Packet16i a) { - return _mm512_srli_epi32(a, N); -} - -template EIGEN_STRONG_INLINE Packet16i plogical_shift_left(Packet16i a) { - return _mm512_slli_epi32(a, N); -} - template <> EIGEN_STRONG_INLINE Packet16f pload(const float* from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm512_load_ps(from); @@ -455,55 +461,75 @@ EIGEN_STRONG_INLINE Packet16i ploadu(const int* from) { // {a0, a0 a1, a1, a2, a2, a3, a3, a4, a4, a5, a5, a6, a6, a7, a7} template <> EIGEN_STRONG_INLINE Packet16f ploaddup(const float* from) { - // an unaligned load is required here as there is no requirement - // on the alignment of input pointer 'from' - __m256i low_half = _mm256_loadu_si256(reinterpret_cast(from)); - __m512 even_elements = _mm512_castsi512_ps(_mm512_cvtepu32_epi64(low_half)); - __m512 pairs = _mm512_permute_ps(even_elements, _MM_SHUFFLE(2, 2, 0, 0)); - return pairs; -} + Packet8f lane0 = _mm256_broadcast_ps((const __m128*)(const void*)from); + // mimic an "inplace" permutation of the lower 128bits using a blend + lane0 = _mm256_blend_ps( + lane0, _mm256_castps128_ps256(_mm_permute_ps( + _mm256_castps256_ps128(lane0), _MM_SHUFFLE(1, 0, 1, 0))), + 15); + // then we can perform a consistent permutation on the global register to get + // everything in shape: + lane0 = _mm256_permute_ps(lane0, _MM_SHUFFLE(3, 3, 2, 2)); + + Packet8f lane1 = _mm256_broadcast_ps((const __m128*)(const void*)(from + 4)); + // mimic an "inplace" permutation of the lower 128bits using a blend + lane1 = _mm256_blend_ps( + lane1, _mm256_castps128_ps256(_mm_permute_ps( + _mm256_castps256_ps128(lane1), _MM_SHUFFLE(1, 0, 1, 0))), + 15); + // then we can perform a consistent permutation on the global register to get + // everything in shape: + lane1 = _mm256_permute_ps(lane1, _MM_SHUFFLE(3, 3, 2, 2)); #ifdef EIGEN_VECTORIZE_AVX512DQ -// FIXME: this does not look optimal, better load a Packet4d and shuffle... + Packet16f res = _mm512_undefined_ps(); + return _mm512_insertf32x8(res, lane0, 0); + return _mm512_insertf32x8(res, lane1, 1); + return res; +#else + Packet16f res = _mm512_undefined_ps(); + res = _mm512_insertf32x4(res, _mm256_extractf128_ps(lane0, 0), 0); + res = _mm512_insertf32x4(res, _mm256_extractf128_ps(lane0, 1), 1); + res = _mm512_insertf32x4(res, _mm256_extractf128_ps(lane1, 0), 2); + res = _mm512_insertf32x4(res, _mm256_extractf128_ps(lane1, 1), 3); + return res; +#endif +} // Loads 4 doubles from memory a returns the packet {a0, a0 a1, a1, a2, a2, a3, // a3} template <> EIGEN_STRONG_INLINE Packet8d ploaddup(const double* from) { - __m512d x = _mm512_setzero_pd(); - x = _mm512_insertf64x2(x, _mm_loaddup_pd(&from[0]), 0); - x = _mm512_insertf64x2(x, _mm_loaddup_pd(&from[1]), 1); - x = _mm512_insertf64x2(x, _mm_loaddup_pd(&from[2]), 2); - x = _mm512_insertf64x2(x, _mm_loaddup_pd(&from[3]), 3); - return x; + Packet4d lane0 = _mm256_broadcast_pd((const __m128d*)(const void*)from); + lane0 = _mm256_permute_pd(lane0, 3 << 2); + + Packet4d lane1 = _mm256_broadcast_pd((const __m128d*)(const void*)(from + 2)); + lane1 = _mm256_permute_pd(lane1, 3 << 2); + + Packet8d res = _mm512_undefined_pd(); + res = _mm512_insertf64x4(res, lane0, 0); + return _mm512_insertf64x4(res, lane1, 1); } -#else -template <> -EIGEN_STRONG_INLINE Packet8d ploaddup(const double* from) { - __m512d x = _mm512_setzero_pd(); - x = _mm512_mask_broadcastsd_pd(x, 0x3<<0, _mm_load_sd(from+0)); - x = _mm512_mask_broadcastsd_pd(x, 0x3<<2, _mm_load_sd(from+1)); - x = _mm512_mask_broadcastsd_pd(x, 0x3<<4, _mm_load_sd(from+2)); - x = _mm512_mask_broadcastsd_pd(x, 0x3<<6, _mm_load_sd(from+3)); - return x; -} -#endif // Loads 4 floats from memory a returns the packet // {a0, a0 a0, a0, a1, a1, a1, a1, a2, a2, a2, a2, a3, a3, a3, a3} template <> EIGEN_STRONG_INLINE Packet16f ploadquad(const float* from) { - Packet16f tmp = _mm512_castps128_ps512(ploadu(from)); - const Packet16i scatter_mask = _mm512_set_epi32(3,3,3,3, 2,2,2,2, 1,1,1,1, 0,0,0,0); - return _mm512_permutexvar_ps(scatter_mask, tmp); + Packet16f tmp = _mm512_undefined_ps(); + tmp = _mm512_insertf32x4(tmp, _mm_load_ps1(from), 0); + tmp = _mm512_insertf32x4(tmp, _mm_load_ps1(from + 1), 1); + tmp = _mm512_insertf32x4(tmp, _mm_load_ps1(from + 2), 2); + tmp = _mm512_insertf32x4(tmp, _mm_load_ps1(from + 3), 3); + return tmp; } - // Loads 2 doubles from memory a returns the packet // {a0, a0 a0, a0, a1, a1, a1, a1} template <> EIGEN_STRONG_INLINE Packet8d ploadquad(const double* from) { - __m256d lane0 = _mm256_set1_pd(*from); - __m256d lane1 = _mm256_set1_pd(*(from+1)); - __m512d tmp = _mm512_undefined_pd(); + Packet8d tmp = _mm512_undefined_pd(); + Packet2d tmp0 = _mm_load_pd1(from); + Packet2d tmp1 = _mm_load_pd1(from + 1); + Packet4d lane0 = _mm256_broadcastsd_pd(tmp0); + Packet4d lane1 = _mm256_broadcastsd_pd(tmp1); tmp = _mm512_insertf64x4(tmp, lane0, 0); return _mm512_insertf64x4(tmp, lane1, 1); } @@ -539,7 +565,7 @@ EIGEN_STRONG_INLINE void pstoreu(int* to, const Packet16i& from) { template <> EIGEN_DEVICE_FUNC inline Packet16f pgather(const float* from, Index stride) { - Packet16i stride_vector = _mm512_set1_epi32(convert_index(stride)); + Packet16i stride_vector = _mm512_set1_epi32(stride); Packet16i stride_multiplier = _mm512_set_epi32(15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0); Packet16i indices = _mm512_mullo_epi32(stride_vector, stride_multiplier); @@ -549,7 +575,7 @@ EIGEN_DEVICE_FUNC inline Packet16f pgather(const float* from, template <> EIGEN_DEVICE_FUNC inline Packet8d pgather(const double* from, Index stride) { - Packet8i stride_vector = _mm256_set1_epi32(convert_index(stride)); + Packet8i stride_vector = _mm256_set1_epi32(stride); Packet8i stride_multiplier = _mm256_set_epi32(7, 6, 5, 4, 3, 2, 1, 0); Packet8i indices = _mm256_mullo_epi32(stride_vector, stride_multiplier); @@ -560,7 +586,7 @@ template <> EIGEN_DEVICE_FUNC inline void pscatter(float* to, const Packet16f& from, Index stride) { - Packet16i stride_vector = _mm512_set1_epi32(convert_index(stride)); + Packet16i stride_vector = _mm512_set1_epi32(stride); Packet16i stride_multiplier = _mm512_set_epi32(15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0); Packet16i indices = _mm512_mullo_epi32(stride_vector, stride_multiplier); @@ -570,7 +596,7 @@ template <> EIGEN_DEVICE_FUNC inline void pscatter(double* to, const Packet8d& from, Index stride) { - Packet8i stride_vector = _mm256_set1_epi32(convert_index(stride)); + Packet8i stride_vector = _mm256_set1_epi32(stride); Packet8i stride_multiplier = _mm256_set_epi32(7, 6, 5, 4, 3, 2, 1, 0); Packet8i indices = _mm256_mullo_epi32(stride_vector, stride_multiplier); _mm512_i32scatter_pd(to, indices, from, 8); @@ -634,8 +660,8 @@ EIGEN_STRONG_INLINE Packet8d pabs(const Packet8d& a) { #ifdef EIGEN_VECTORIZE_AVX512DQ // AVX512F does not define _mm512_extractf32x8_ps to extract _m256 from _m512 #define EIGEN_EXTRACT_8f_FROM_16f(INPUT, OUTPUT) \ - __m256 OUTPUT##_0 = _mm512_extractf32x8_ps(INPUT, 0); \ - __m256 OUTPUT##_1 = _mm512_extractf32x8_ps(INPUT, 1) + __m256 OUTPUT##_0 = _mm512_extractf32x8_ps(INPUT, 0) __m256 OUTPUT##_1 = \ + _mm512_extractf32x8_ps(INPUT, 1) #else #define EIGEN_EXTRACT_8f_FROM_16f(INPUT, OUTPUT) \ __m256 OUTPUT##_0 = _mm256_insertf128_ps( \ @@ -648,136 +674,17 @@ EIGEN_STRONG_INLINE Packet8d pabs(const Packet8d& a) { #ifdef EIGEN_VECTORIZE_AVX512DQ #define EIGEN_INSERT_8f_INTO_16f(OUTPUT, INPUTA, INPUTB) \ - OUTPUT = _mm512_insertf32x8(_mm512_castps256_ps512(INPUTA), INPUTB, 1); + OUTPUT = _mm512_insertf32x8(OUTPUT, INPUTA, 0); \ + OUTPUT = _mm512_insertf32x8(OUTPUT, INPUTB, 1); #else #define EIGEN_INSERT_8f_INTO_16f(OUTPUT, INPUTA, INPUTB) \ - OUTPUT = _mm512_undefined_ps(); \ OUTPUT = _mm512_insertf32x4(OUTPUT, _mm256_extractf128_ps(INPUTA, 0), 0); \ OUTPUT = _mm512_insertf32x4(OUTPUT, _mm256_extractf128_ps(INPUTA, 1), 1); \ OUTPUT = _mm512_insertf32x4(OUTPUT, _mm256_extractf128_ps(INPUTB, 0), 2); \ OUTPUT = _mm512_insertf32x4(OUTPUT, _mm256_extractf128_ps(INPUTB, 1), 3); #endif - -template <> -EIGEN_STRONG_INLINE float predux(const Packet16f& a) { -#ifdef EIGEN_VECTORIZE_AVX512DQ - __m256 lane0 = _mm512_extractf32x8_ps(a, 0); - __m256 lane1 = _mm512_extractf32x8_ps(a, 1); - Packet8f x = _mm256_add_ps(lane0, lane1); - return predux(x); -#else - __m128 lane0 = _mm512_extractf32x4_ps(a, 0); - __m128 lane1 = _mm512_extractf32x4_ps(a, 1); - __m128 lane2 = _mm512_extractf32x4_ps(a, 2); - __m128 lane3 = _mm512_extractf32x4_ps(a, 3); - __m128 sum = _mm_add_ps(_mm_add_ps(lane0, lane1), _mm_add_ps(lane2, lane3)); - sum = _mm_hadd_ps(sum, sum); - sum = _mm_hadd_ps(sum, _mm_permute_ps(sum, 1)); - return _mm_cvtss_f32(sum); -#endif -} -template <> -EIGEN_STRONG_INLINE double predux(const Packet8d& a) { - __m256d lane0 = _mm512_extractf64x4_pd(a, 0); - __m256d lane1 = _mm512_extractf64x4_pd(a, 1); - __m256d sum = _mm256_add_pd(lane0, lane1); - __m256d tmp0 = _mm256_hadd_pd(sum, _mm256_permute2f128_pd(sum, sum, 1)); - return _mm_cvtsd_f64(_mm256_castpd256_pd128(_mm256_hadd_pd(tmp0, tmp0))); -} - -template <> -EIGEN_STRONG_INLINE Packet8f predux_downto4(const Packet16f& a) { -#ifdef EIGEN_VECTORIZE_AVX512DQ - Packet8f lane0 = _mm512_extractf32x8_ps(a, 0); - Packet8f lane1 = _mm512_extractf32x8_ps(a, 1); - return padd(lane0, lane1); -#else - Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); - Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); - Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); - Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); - Packet4f sum0 = padd(lane0, lane2); - Packet4f sum1 = padd(lane1, lane3); - return _mm256_insertf128_ps(_mm256_castps128_ps256(sum0), sum1, 1); -#endif -} -template <> -EIGEN_STRONG_INLINE Packet4d predux_downto4(const Packet8d& a) { - Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); - Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); - Packet4d res = padd(lane0, lane1); - return res; -} - -template <> -EIGEN_STRONG_INLINE float predux_mul(const Packet16f& a) { -//#ifdef EIGEN_VECTORIZE_AVX512DQ -#if 0 - Packet8f lane0 = _mm512_extractf32x8_ps(a, 0); - Packet8f lane1 = _mm512_extractf32x8_ps(a, 1); - Packet8f res = pmul(lane0, lane1); - res = pmul(res, _mm256_permute2f128_ps(res, res, 1)); - res = pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); - return pfirst(pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); -#else - __m128 lane0 = _mm512_extractf32x4_ps(a, 0); - __m128 lane1 = _mm512_extractf32x4_ps(a, 1); - __m128 lane2 = _mm512_extractf32x4_ps(a, 2); - __m128 lane3 = _mm512_extractf32x4_ps(a, 3); - __m128 res = pmul(pmul(lane0, lane1), pmul(lane2, lane3)); - res = pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); - return pfirst(pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); -#endif -} -template <> -EIGEN_STRONG_INLINE double predux_mul(const Packet8d& a) { - __m256d lane0 = _mm512_extractf64x4_pd(a, 0); - __m256d lane1 = _mm512_extractf64x4_pd(a, 1); - __m256d res = pmul(lane0, lane1); - res = pmul(res, _mm256_permute2f128_pd(res, res, 1)); - return pfirst(pmul(res, _mm256_shuffle_pd(res, res, 1))); -} - -template <> -EIGEN_STRONG_INLINE float predux_min(const Packet16f& a) { - __m128 lane0 = _mm512_extractf32x4_ps(a, 0); - __m128 lane1 = _mm512_extractf32x4_ps(a, 1); - __m128 lane2 = _mm512_extractf32x4_ps(a, 2); - __m128 lane3 = _mm512_extractf32x4_ps(a, 3); - __m128 res = _mm_min_ps(_mm_min_ps(lane0, lane1), _mm_min_ps(lane2, lane3)); - res = _mm_min_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); - return pfirst(_mm_min_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); -} -template <> -EIGEN_STRONG_INLINE double predux_min(const Packet8d& a) { - __m256d lane0 = _mm512_extractf64x4_pd(a, 0); - __m256d lane1 = _mm512_extractf64x4_pd(a, 1); - __m256d res = _mm256_min_pd(lane0, lane1); - res = _mm256_min_pd(res, _mm256_permute2f128_pd(res, res, 1)); - return pfirst(_mm256_min_pd(res, _mm256_shuffle_pd(res, res, 1))); -} - -template <> -EIGEN_STRONG_INLINE float predux_max(const Packet16f& a) { - __m128 lane0 = _mm512_extractf32x4_ps(a, 0); - __m128 lane1 = _mm512_extractf32x4_ps(a, 1); - __m128 lane2 = _mm512_extractf32x4_ps(a, 2); - __m128 lane3 = _mm512_extractf32x4_ps(a, 3); - __m128 res = _mm_max_ps(_mm_max_ps(lane0, lane1), _mm_max_ps(lane2, lane3)); - res = _mm_max_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); - return pfirst(_mm_max_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); -} - -template <> -EIGEN_STRONG_INLINE double predux_max(const Packet8d& a) { - __m256d lane0 = _mm512_extractf64x4_pd(a, 0); - __m256d lane1 = _mm512_extractf64x4_pd(a, 1); - __m256d res = _mm256_max_pd(lane0, lane1); - res = _mm256_max_pd(res, _mm256_permute2f128_pd(res, res, 1)); - return pfirst(_mm256_max_pd(res, _mm256_shuffle_pd(res, res, 1))); -} - -template<> EIGEN_STRONG_INLINE Packet16f preduxp(const Packet16f* vecs) +template<> EIGEN_STRONG_INLINE Packet16f preduxp(const Packet16f* +vecs) { EIGEN_EXTRACT_8f_FROM_16f(vecs[0], vecs0); EIGEN_EXTRACT_8f_FROM_16f(vecs[1], vecs1); @@ -966,7 +873,174 @@ template<> EIGEN_STRONG_INLINE Packet8d preduxp(const Packet8d* vecs) return _mm512_insertf64x4(final_output, final_1, 1); } - + +template <> +EIGEN_STRONG_INLINE float predux(const Packet16f& a) { + //#ifdef EIGEN_VECTORIZE_AVX512DQ +#if 0 + Packet8f lane0 = _mm512_extractf32x8_ps(a, 0); + Packet8f lane1 = _mm512_extractf32x8_ps(a, 1); + Packet8f sum = padd(lane0, lane1); + Packet8f tmp0 = _mm256_hadd_ps(sum, _mm256_permute2f128_ps(a, a, 1)); + tmp0 = _mm256_hadd_ps(tmp0, tmp0); + return pfirst(_mm256_hadd_ps(tmp0, tmp0)); +#else + Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); + Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); + Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); + Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); + Packet4f sum = padd(padd(lane0, lane1), padd(lane2, lane3)); + sum = _mm_hadd_ps(sum, sum); + sum = _mm_hadd_ps(sum, _mm_permute_ps(sum, 1)); + return pfirst(sum); +#endif +} +template <> +EIGEN_STRONG_INLINE double predux(const Packet8d& a) { + Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); + Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); + Packet4d sum = padd(lane0, lane1); + Packet4d tmp0 = _mm256_hadd_pd(sum, _mm256_permute2f128_pd(sum, sum, 1)); + return pfirst(_mm256_hadd_pd(tmp0, tmp0)); +} + +template <> +EIGEN_STRONG_INLINE Packet8f predux_downto4(const Packet16f& a) { +#ifdef EIGEN_VECTORIZE_AVX512DQ + Packet8f lane0 = _mm512_extractf32x8_ps(a, 0); + Packet8f lane1 = _mm512_extractf32x8_ps(a, 1); + return padd(lane0, lane1); +#else + Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); + Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); + Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); + Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); + Packet4f sum0 = padd(lane0, lane2); + Packet4f sum1 = padd(lane1, lane3); + return _mm256_insertf128_ps(_mm256_castps128_ps256(sum0), sum1, 1); +#endif +} +template <> +EIGEN_STRONG_INLINE Packet4d predux_downto4(const Packet8d& a) { + Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); + Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); + Packet4d res = padd(lane0, lane1); + return res; +} + +template <> +EIGEN_STRONG_INLINE float predux_mul(const Packet16f& a) { +//#ifdef EIGEN_VECTORIZE_AVX512DQ +#if 0 + Packet8f lane0 = _mm512_extractf32x8_ps(a, 0); + Packet8f lane1 = _mm512_extractf32x8_ps(a, 1); + Packet8f res = pmul(lane0, lane1); + res = pmul(res, _mm256_permute2f128_ps(res, res, 1)); + res = pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); + return pfirst(pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); +#else + Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); + Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); + Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); + Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); + Packet4f res = pmul(pmul(lane0, lane1), pmul(lane2, lane3)); + res = pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); + return pfirst(pmul(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); +#endif +} +template <> +EIGEN_STRONG_INLINE double predux_mul(const Packet8d& a) { + Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); + Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); + Packet4d res = pmul(lane0, lane1); + res = pmul(res, _mm256_permute2f128_pd(res, res, 1)); + return pfirst(pmul(res, _mm256_shuffle_pd(res, res, 1))); +} + +template <> +EIGEN_STRONG_INLINE float predux_min(const Packet16f& a) { + Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); + Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); + Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); + Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); + Packet4f res = _mm_min_ps(_mm_min_ps(lane0, lane1), _mm_min_ps(lane2, lane3)); + res = _mm_min_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); + return pfirst(_mm_min_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); +} +template <> +EIGEN_STRONG_INLINE double predux_min(const Packet8d& a) { + Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); + Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); + Packet4d res = _mm256_min_pd(lane0, lane1); + res = _mm256_min_pd(res, _mm256_permute2f128_pd(res, res, 1)); + return pfirst(_mm256_min_pd(res, _mm256_shuffle_pd(res, res, 1))); +} + +template <> +EIGEN_STRONG_INLINE float predux_max(const Packet16f& a) { + Packet4f lane0 = _mm512_extractf32x4_ps(a, 0); + Packet4f lane1 = _mm512_extractf32x4_ps(a, 1); + Packet4f lane2 = _mm512_extractf32x4_ps(a, 2); + Packet4f lane3 = _mm512_extractf32x4_ps(a, 3); + Packet4f res = _mm_max_ps(_mm_max_ps(lane0, lane1), _mm_max_ps(lane2, lane3)); + res = _mm_max_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 3, 2))); + return pfirst(_mm_max_ps(res, _mm_permute_ps(res, _MM_SHUFFLE(0, 0, 0, 1)))); +} +template <> +EIGEN_STRONG_INLINE double predux_max(const Packet8d& a) { + Packet4d lane0 = _mm512_extractf64x4_pd(a, 0); + Packet4d lane1 = _mm512_extractf64x4_pd(a, 1); + Packet4d res = _mm256_max_pd(lane0, lane1); + res = _mm256_max_pd(res, _mm256_permute2f128_pd(res, res, 1)); + return pfirst(_mm256_max_pd(res, _mm256_shuffle_pd(res, res, 1))); +} + +template +struct palign_impl { + static EIGEN_STRONG_INLINE void run(Packet16f& first, + const Packet16f& second) { + if (Offset != 0) { + __m512i first_idx = _mm512_set_epi32( + Offset + 15, Offset + 14, Offset + 13, Offset + 12, Offset + 11, + Offset + 10, Offset + 9, Offset + 8, Offset + 7, Offset + 6, + Offset + 5, Offset + 4, Offset + 3, Offset + 2, Offset + 1, Offset); + + __m512i second_idx = + _mm512_set_epi32(Offset - 1, Offset - 2, Offset - 3, Offset - 4, + Offset - 5, Offset - 6, Offset - 7, Offset - 8, + Offset - 9, Offset - 10, Offset - 11, Offset - 12, + Offset - 13, Offset - 14, Offset - 15, Offset - 16); + + unsigned short mask = 0xFFFF; + mask <<= (16 - Offset); + + first = _mm512_permutexvar_ps(first_idx, first); + Packet16f tmp = _mm512_permutexvar_ps(second_idx, second); + first = _mm512_mask_blend_ps(mask, first, tmp); + } + } +}; +template +struct palign_impl { + static EIGEN_STRONG_INLINE void run(Packet8d& first, const Packet8d& second) { + if (Offset != 0) { + __m512i first_idx = _mm512_set_epi32( + 0, Offset + 7, 0, Offset + 6, 0, Offset + 5, 0, Offset + 4, 0, + Offset + 3, 0, Offset + 2, 0, Offset + 1, 0, Offset); + + __m512i second_idx = _mm512_set_epi32( + 0, Offset - 1, 0, Offset - 2, 0, Offset - 3, 0, Offset - 4, 0, + Offset - 5, 0, Offset - 6, 0, Offset - 7, 0, Offset - 8); + + unsigned char mask = 0xFF; + mask <<= (8 - Offset); + + first = _mm512_permutexvar_pd(first_idx, first); + Packet8d tmp = _mm512_permutexvar_pd(second_idx, second); + first = _mm512_mask_blend_pd(mask, first, tmp); + } + } +}; #define PACK_OUTPUT(OUTPUT, INPUT, INDEX, STRIDE) \ @@ -1228,76 +1302,13 @@ EIGEN_STRONG_INLINE Packet16f pblend(const Selector<16>& /*ifPacket*/, return Packet16f(); } template <> -EIGEN_STRONG_INLINE Packet8d pblend(const Selector<8>& ifPacket, - const Packet8d& thenPacket, - const Packet8d& elsePacket) { - __mmask8 m = (ifPacket.select[0] ) - | (ifPacket.select[1]<<1) - | (ifPacket.select[2]<<2) - | (ifPacket.select[3]<<3) - | (ifPacket.select[4]<<4) - | (ifPacket.select[5]<<5) - | (ifPacket.select[6]<<6) - | (ifPacket.select[7]<<7); - return _mm512_mask_blend_pd(m, elsePacket, thenPacket); +EIGEN_STRONG_INLINE Packet8d pblend(const Selector<8>& /*ifPacket*/, + const Packet8d& /*thenPacket*/, + const Packet8d& /*elsePacket*/) { + assert(false && "To be implemented"); + return Packet8d(); } -template<> EIGEN_STRONG_INLINE Packet16i pcast(const Packet16f& a) { - return _mm512_cvttps_epi32(a); -} - -template<> EIGEN_STRONG_INLINE Packet16f pcast(const Packet16i& a) { - return _mm512_cvtepi32_ps(a); -} - -template -struct palign_impl { - static EIGEN_STRONG_INLINE void run(Packet16f& first, - const Packet16f& second) { - if (Offset != 0) { - __m512i first_idx = _mm512_set_epi32( - Offset + 15, Offset + 14, Offset + 13, Offset + 12, Offset + 11, - Offset + 10, Offset + 9, Offset + 8, Offset + 7, Offset + 6, - Offset + 5, Offset + 4, Offset + 3, Offset + 2, Offset + 1, Offset); - - __m512i second_idx = - _mm512_set_epi32(Offset - 1, Offset - 2, Offset - 3, Offset - 4, - Offset - 5, Offset - 6, Offset - 7, Offset - 8, - Offset - 9, Offset - 10, Offset - 11, Offset - 12, - Offset - 13, Offset - 14, Offset - 15, Offset - 16); - - unsigned short mask = 0xFFFF; - mask <<= (16 - Offset); - - first = _mm512_permutexvar_ps(first_idx, first); - Packet16f tmp = _mm512_permutexvar_ps(second_idx, second); - first = _mm512_mask_blend_ps(mask, first, tmp); - } - } -}; -template -struct palign_impl { - static EIGEN_STRONG_INLINE void run(Packet8d& first, const Packet8d& second) { - if (Offset != 0) { - __m512i first_idx = _mm512_set_epi32( - 0, Offset + 7, 0, Offset + 6, 0, Offset + 5, 0, Offset + 4, 0, - Offset + 3, 0, Offset + 2, 0, Offset + 1, 0, Offset); - - __m512i second_idx = _mm512_set_epi32( - 0, Offset - 1, 0, Offset - 2, 0, Offset - 3, 0, Offset - 4, 0, - Offset - 5, 0, Offset - 6, 0, Offset - 7, 0, Offset - 8); - - unsigned char mask = 0xFF; - mask <<= (8 - Offset); - - first = _mm512_permutexvar_pd(first_idx, first); - Packet8d tmp = _mm512_permutexvar_pd(second_idx, second); - first = _mm512_mask_blend_pd(mask, first, tmp); - } - } -}; - - } // end namespace internal } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h index 59717b4fe..755e6209d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h @@ -42,7 +42,6 @@ #define EIGEN_EXPLICIT_CAST(tgt_type) operator tgt_type() #endif -#include namespace Eigen { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h index f749c573f..c66d38469 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h @@ -230,7 +230,7 @@ template<> __device__ EIGEN_STRONG_INLINE Eigen::half predux(const half2& #else float a1 = __low2float(a); float a2 = __high2float(a); - return Eigen::half(__float2half_rn(a1 + a2)); + return Eigen::half(half_impl::raw_uint16_to_half(__float2half_rn(a1 + a2))); #endif } @@ -264,7 +264,7 @@ template<> __device__ EIGEN_STRONG_INLINE Eigen::half predux_mul(const ha #else float a1 = __low2float(a); float a2 = __high2float(a); - return Eigen::half(__float2half_rn(a1 * a2)); + return Eigen::half(half_impl::raw_uint16_to_half(__float2half_rn(a1 * a2))); #endif } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/UnaryFunctors.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/UnaryFunctors.h index b56e7afd2..2e6a00ffd 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/UnaryFunctors.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/UnaryFunctors.h @@ -768,7 +768,7 @@ struct scalar_sign_op { if (aa==real_type(0)) return Scalar(0); aa = real_type(1)/aa; - return Scalar(a.real()*aa, a.imag()*aa ); + return Scalar(real(a)*aa, imag(a)*aa ); } //TODO //template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h index 681451cc3..e3980f6ff 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h @@ -115,8 +115,7 @@ void evaluateProductBlockingSizesHeuristic(Index& k, Index& m, Index& n, Index n // registers. However once the latency is hidden there is no point in // increasing the value of k, so we'll cap it at 320 (value determined // experimentally). - // To avoid that k vanishes, we make k_cache at least as big as kr - const Index k_cache = numext::maxi(kr, (numext::mini)((l1-ksub)/kdiv, 320)); + const Index k_cache = (numext::mini)((l1-ksub)/kdiv, 320); if (k_cache < k) { k = k_cache - (k_cache % kr); eigen_internal_assert(k > 0); @@ -649,8 +648,8 @@ public: // Vectorized path EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, DoublePacketType& dest) const { - dest.first = pset1(numext::real(*b)); - dest.second = pset1(numext::imag(*b)); + dest.first = pset1(real(*b)); + dest.second = pset1(imag(*b)); } EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, ResPacket& dest) const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h index ed6234c37..6440e1d09 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h @@ -20,9 +20,8 @@ template class level3_blocking; template< typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs, - typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs, - int ResInnerStride> -struct general_matrix_matrix_product + typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs> +struct general_matrix_matrix_product { typedef gebp_traits Traits; @@ -31,7 +30,7 @@ struct general_matrix_matrix_product& blocking, GemmParallelInfo* info = 0) @@ -40,8 +39,8 @@ struct general_matrix_matrix_product - ::run(cols,rows,depth,rhs,rhsStride,lhs,lhsStride,res,resIncr,resStride,alpha,blocking,info); + ColMajor> + ::run(cols,rows,depth,rhs,rhsStride,lhs,lhsStride,res,resStride,alpha,blocking,info); } }; @@ -50,9 +49,8 @@ struct general_matrix_matrix_product -struct general_matrix_matrix_product + typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs> +struct general_matrix_matrix_product { typedef gebp_traits Traits; @@ -61,17 +59,17 @@ typedef typename ScalarBinaryOpTraits::ReturnType ResScala static void run(Index rows, Index cols, Index depth, const LhsScalar* _lhs, Index lhsStride, const RhsScalar* _rhs, Index rhsStride, - ResScalar* _res, Index resIncr, Index resStride, + ResScalar* _res, Index resStride, ResScalar alpha, level3_blocking& blocking, GemmParallelInfo* info = 0) { typedef const_blas_data_mapper LhsMapper; typedef const_blas_data_mapper RhsMapper; - typedef blas_data_mapper ResMapper; - LhsMapper lhs(_lhs, lhsStride); - RhsMapper rhs(_rhs, rhsStride); - ResMapper res(_res, resStride, resIncr); + typedef blas_data_mapper ResMapper; + LhsMapper lhs(_lhs,lhsStride); + RhsMapper rhs(_rhs,rhsStride); + ResMapper res(_res, resStride); Index kc = blocking.kc(); // cache block size along the K direction Index mc = (std::min)(rows,blocking.mc()); // cache block size along the M direction @@ -228,7 +226,7 @@ struct gemm_functor Gemm::run(rows, cols, m_lhs.cols(), &m_lhs.coeffRef(row,0), m_lhs.outerStride(), &m_rhs.coeffRef(0,col), m_rhs.outerStride(), - (Scalar*)&(m_dest.coeffRef(row,col)), m_dest.innerStride(), m_dest.outerStride(), + (Scalar*)&(m_dest.coeffRef(row,col)), m_dest.outerStride(), m_actualAlpha, m_blocking, info); } @@ -430,7 +428,7 @@ struct generic_product_impl static void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { if((rhs.rows()+dst.rows()+dst.cols())<20 && rhs.rows()>0) - lazyproduct::eval_dynamic(dst, lhs, rhs, internal::assign_op()); + lazyproduct::evalTo(dst, lhs, rhs); else { dst.setZero(); @@ -442,7 +440,7 @@ struct generic_product_impl static void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { if((rhs.rows()+dst.rows()+dst.cols())<20 && rhs.rows()>0) - lazyproduct::eval_dynamic(dst, lhs, rhs, internal::add_assign_op()); + lazyproduct::addTo(dst, lhs, rhs); else scaleAndAddTo(dst,lhs, rhs, Scalar(1)); } @@ -451,7 +449,7 @@ struct generic_product_impl static void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs) { if((rhs.rows()+dst.rows()+dst.cols())<20 && rhs.rows()>0) - lazyproduct::eval_dynamic(dst, lhs, rhs, internal::sub_assign_op()); + lazyproduct::subTo(dst, lhs, rhs); else scaleAndAddTo(dst, lhs, rhs, Scalar(-1)); } @@ -478,8 +476,7 @@ struct generic_product_impl Index, LhsScalar, (ActualLhsTypeCleaned::Flags&RowMajorBit) ? RowMajor : ColMajor, bool(LhsBlasTraits::NeedToConjugate), RhsScalar, (ActualRhsTypeCleaned::Flags&RowMajorBit) ? RowMajor : ColMajor, bool(RhsBlasTraits::NeedToConjugate), - (Dest::Flags&RowMajorBit) ? RowMajor : ColMajor, - Dest::InnerStrideAtCompileTime>, + (Dest::Flags&RowMajorBit) ? RowMajor : ColMajor>, ActualLhsTypeCleaned, ActualRhsTypeCleaned, Dest, BlockingType> GemmFunctor; BlockingType blocking(dst.rows(), dst.cols(), lhs.cols(), 1, true); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h index d68d2f965..e844e37d1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h @@ -25,54 +25,51 @@ namespace internal { **********************************************************************/ // forward declarations (defined at the end of this file) -template +template struct tribb_kernel; /* Optimized matrix-matrix product evaluating only one triangular half */ template + int ResStorageOrder, int UpLo, int Version = Specialized> struct general_matrix_matrix_triangular_product; // as usual if the result is row major => we transpose the product template -struct general_matrix_matrix_triangular_product + typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs, int UpLo, int Version> +struct general_matrix_matrix_triangular_product { typedef typename ScalarBinaryOpTraits::ReturnType ResScalar; static EIGEN_STRONG_INLINE void run(Index size, Index depth,const LhsScalar* lhs, Index lhsStride, - const RhsScalar* rhs, Index rhsStride, ResScalar* res, Index resIncr, Index resStride, + const RhsScalar* rhs, Index rhsStride, ResScalar* res, Index resStride, const ResScalar& alpha, level3_blocking& blocking) { general_matrix_matrix_triangular_product - ::run(size,depth,rhs,rhsStride,lhs,lhsStride,res,resIncr,resStride,alpha,blocking); + ColMajor, UpLo==Lower?Upper:Lower> + ::run(size,depth,rhs,rhsStride,lhs,lhsStride,res,resStride,alpha,blocking); } }; template -struct general_matrix_matrix_triangular_product + typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs, int UpLo, int Version> +struct general_matrix_matrix_triangular_product { typedef typename ScalarBinaryOpTraits::ReturnType ResScalar; static EIGEN_STRONG_INLINE void run(Index size, Index depth,const LhsScalar* _lhs, Index lhsStride, - const RhsScalar* _rhs, Index rhsStride, - ResScalar* _res, Index resIncr, Index resStride, + const RhsScalar* _rhs, Index rhsStride, ResScalar* _res, Index resStride, const ResScalar& alpha, level3_blocking& blocking) { typedef gebp_traits Traits; typedef const_blas_data_mapper LhsMapper; typedef const_blas_data_mapper RhsMapper; - typedef blas_data_mapper ResMapper; + typedef blas_data_mapper ResMapper; LhsMapper lhs(_lhs,lhsStride); RhsMapper rhs(_rhs,rhsStride); - ResMapper res(_res, resStride, resIncr); + ResMapper res(_res, resStride); Index kc = blocking.kc(); Index mc = (std::min)(size,blocking.mc()); @@ -90,7 +87,7 @@ struct general_matrix_matrix_triangular_product pack_lhs; gemm_pack_rhs pack_rhs; gebp_kernel gebp; - tribb_kernel sybb; + tribb_kernel sybb; for(Index k2=0; k2 +template struct tribb_kernel { typedef gebp_traits Traits; @@ -144,13 +142,11 @@ struct tribb_kernel enum { BlockSize = meta_least_common_multiple::ret }; - void operator()(ResScalar* _res, Index resIncr, Index resStride, const LhsScalar* blockA, const RhsScalar* blockB, Index size, Index depth, const ResScalar& alpha) + void operator()(ResScalar* _res, Index resStride, const LhsScalar* blockA, const RhsScalar* blockB, Index size, Index depth, const ResScalar& alpha) { - typedef blas_data_mapper ResMapper; - typedef blas_data_mapper BufferMapper; - ResMapper res(_res, resStride, resIncr); - gebp_kernel gebp_kernel1; - gebp_kernel gebp_kernel2; + typedef blas_data_mapper ResMapper; + ResMapper res(_res, resStride); + gebp_kernel gebp_kernel; Matrix buffer((internal::constructor_without_unaligned_array_assert())); @@ -162,32 +158,31 @@ struct tribb_kernel const RhsScalar* actual_b = blockB+j*depth; if(UpLo==Upper) - gebp_kernel1(res.getSubMapper(0, j), blockA, actual_b, j, depth, actualBlockSize, alpha, - -1, -1, 0, 0); - + gebp_kernel(res.getSubMapper(0, j), blockA, actual_b, j, depth, actualBlockSize, alpha, + -1, -1, 0, 0); + // selfadjoint micro block { Index i = j; buffer.setZero(); // 1 - apply the kernel on the temporary buffer - gebp_kernel2(BufferMapper(buffer.data(), BlockSize), blockA+depth*i, actual_b, actualBlockSize, depth, actualBlockSize, alpha, - -1, -1, 0, 0); - + gebp_kernel(ResMapper(buffer.data(), BlockSize), blockA+depth*i, actual_b, actualBlockSize, depth, actualBlockSize, alpha, + -1, -1, 0, 0); // 2 - triangular accumulation for(Index j1=0; j1 internal::general_matrix_matrix_triangular_product + IsRowMajor ? RowMajor : ColMajor, UpLo&(Lower|Upper)> ::run(size, depth, &actualLhs.coeffRef(SkipDiag&&(UpLo&Lower)==Lower ? 1 : 0,0), actualLhs.outerStride(), &actualRhs.coeffRef(0,SkipDiag&&(UpLo&Upper)==Upper ? 1 : 0), actualRhs.outerStride(), - mat.data() + (SkipDiag ? (bool(IsRowMajor) != ((UpLo&Lower)==Lower) ? mat.innerStride() : mat.outerStride() ) : 0), - mat.innerStride(), mat.outerStride(), actualAlpha, blocking); + mat.data() + (SkipDiag ? (bool(IsRowMajor) != ((UpLo&Lower)==Lower) ? 1 : mat.outerStride() ) : 0), mat.outerStride(), actualAlpha, blocking); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h index 691f95d69..f6f9ebeca 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h @@ -40,7 +40,7 @@ namespace internal { template struct general_matrix_matrix_rankupdate : general_matrix_matrix_triangular_product< - Index,Scalar,AStorageOrder,ConjugateA,Scalar,AStorageOrder,ConjugateA,ResStorageOrder,1,UpLo,BuiltIn> {}; + Index,Scalar,AStorageOrder,ConjugateA,Scalar,AStorageOrder,ConjugateA,ResStorageOrder,UpLo,BuiltIn> {}; // try to go to BLAS specialization @@ -48,9 +48,9 @@ struct general_matrix_matrix_rankupdate : template \ struct general_matrix_matrix_triangular_product { \ + Scalar,RhsStorageOrder,ConjugateRhs,ColMajor,UpLo,Specialized> { \ static EIGEN_STRONG_INLINE void run(Index size, Index depth,const Scalar* lhs, Index lhsStride, \ - const Scalar* rhs, Index rhsStride, Scalar* res, Index resIncr, Index resStride, Scalar alpha, level3_blocking& blocking) \ + const Scalar* rhs, Index rhsStride, Scalar* res, Index resStride, Scalar alpha, level3_blocking& blocking) \ { \ if ( lhs==rhs && ((UpLo&(Lower|Upper))==UpLo) ) { \ general_matrix_matrix_rankupdate \ @@ -59,8 +59,8 @@ struct general_matrix_matrix_triangular_product \ - ::run(size,depth,lhs,lhsStride,rhs,rhsStride,res,resIncr,resStride,alpha,blocking); \ + ColMajor, UpLo, BuiltIn> \ + ::run(size,depth,lhs,lhsStride,rhs,rhsStride,res,resStride,alpha,blocking); \ } \ } \ }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h index 71abf4013..b0f6b0d5b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h @@ -51,22 +51,20 @@ template< \ typename Index, \ int LhsStorageOrder, bool ConjugateLhs, \ int RhsStorageOrder, bool ConjugateRhs> \ -struct general_matrix_matrix_product \ +struct general_matrix_matrix_product \ { \ typedef gebp_traits Traits; \ \ static void run(Index rows, Index cols, Index depth, \ const EIGTYPE* _lhs, Index lhsStride, \ const EIGTYPE* _rhs, Index rhsStride, \ - EIGTYPE* res, Index resIncr, Index resStride, \ + EIGTYPE* res, Index resStride, \ EIGTYPE alpha, \ level3_blocking& /*blocking*/, \ GemmParallelInfo* /*info = 0*/) \ { \ using std::conj; \ \ - EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ - eigen_assert(resIncr == 1); \ char transa, transb; \ BlasIndex m, n, k, lda, ldb, ldc; \ const EIGTYPE *a, *b; \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h index a3cc05b77..c2f084c82 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h @@ -17,8 +17,7 @@ namespace internal { /** \internal */ inline void manage_multi_threading(Action action, int* v) { - static int m_maxThreads = -1; - EIGEN_UNUSED_VARIABLE(m_maxThreads); + static EIGEN_UNUSED int m_maxThreads = -1; if(action==SetAction) { @@ -151,10 +150,8 @@ void parallelize_gemm(const Functor& func, Index rows, Index cols, Index depth, info[i].lhs_start = r0; info[i].lhs_length = actualBlockRows; - if(transpose) - func(c0, actualBlockCols, 0, rows, info); - else - func(0, rows, c0, actualBlockCols, info); + if(transpose) func(c0, actualBlockCols, 0, rows, info); + else func(0, rows, c0, actualBlockCols, info); } #endif } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h index 04c933480..da6f82abc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h @@ -277,21 +277,20 @@ struct symm_pack_rhs template + int ResStorageOrder> struct product_selfadjoint_matrix; template -struct product_selfadjoint_matrix + int RhsStorageOrder, bool RhsSelfAdjoint, bool ConjugateRhs> +struct product_selfadjoint_matrix { static EIGEN_STRONG_INLINE void run( Index rows, Index cols, const Scalar* lhs, Index lhsStride, const Scalar* rhs, Index rhsStride, - Scalar* res, Index resIncr, Index resStride, + Scalar* res, Index resStride, const Scalar& alpha, level3_blocking& blocking) { product_selfadjoint_matrix::IsComplex && EIGEN_LOGICAL_XOR(RhsSelfAdjoint,ConjugateRhs), EIGEN_LOGICAL_XOR(LhsSelfAdjoint,LhsStorageOrder==RowMajor) ? ColMajor : RowMajor, LhsSelfAdjoint, NumTraits::IsComplex && EIGEN_LOGICAL_XOR(LhsSelfAdjoint,ConjugateLhs), - ColMajor,ResInnerStride> - ::run(cols, rows, rhs, rhsStride, lhs, lhsStride, res, resIncr, resStride, alpha, blocking); + ColMajor> + ::run(cols, rows, rhs, rhsStride, lhs, lhsStride, res, resStride, alpha, blocking); } }; template -struct product_selfadjoint_matrix + int RhsStorageOrder, bool ConjugateRhs> +struct product_selfadjoint_matrix { static EIGEN_DONT_INLINE void run( Index rows, Index cols, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* res, Index resIncr, Index resStride, + Scalar* res, Index resStride, const Scalar& alpha, level3_blocking& blocking); }; template -EIGEN_DONT_INLINE void product_selfadjoint_matrix::run( + int RhsStorageOrder, bool ConjugateRhs> +EIGEN_DONT_INLINE void product_selfadjoint_matrix::run( Index rows, Index cols, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* _res, Index resIncr, Index resStride, + Scalar* _res, Index resStride, const Scalar& alpha, level3_blocking& blocking) { Index size = rows; @@ -337,11 +334,11 @@ EIGEN_DONT_INLINE void product_selfadjoint_matrix LhsMapper; typedef const_blas_data_mapper LhsTransposeMapper; typedef const_blas_data_mapper RhsMapper; - typedef blas_data_mapper ResMapper; + typedef blas_data_mapper ResMapper; LhsMapper lhs(_lhs,lhsStride); LhsTransposeMapper lhs_transpose(_lhs,lhsStride); RhsMapper rhs(_rhs,rhsStride); - ResMapper res(_res, resStride, resIncr); + ResMapper res(_res, resStride); Index kc = blocking.kc(); // cache block size along the K direction Index mc = (std::min)(rows,blocking.mc()); // cache block size along the M direction @@ -401,28 +398,26 @@ EIGEN_DONT_INLINE void product_selfadjoint_matrix -struct product_selfadjoint_matrix + int RhsStorageOrder, bool ConjugateRhs> +struct product_selfadjoint_matrix { static EIGEN_DONT_INLINE void run( Index rows, Index cols, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* res, Index resIncr, Index resStride, + Scalar* res, Index resStride, const Scalar& alpha, level3_blocking& blocking); }; template -EIGEN_DONT_INLINE void product_selfadjoint_matrix::run( + int RhsStorageOrder, bool ConjugateRhs> +EIGEN_DONT_INLINE void product_selfadjoint_matrix::run( Index rows, Index cols, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* _res, Index resIncr, Index resStride, + Scalar* _res, Index resStride, const Scalar& alpha, level3_blocking& blocking) { Index size = cols; @@ -430,9 +425,9 @@ EIGEN_DONT_INLINE void product_selfadjoint_matrix Traits; typedef const_blas_data_mapper LhsMapper; - typedef blas_data_mapper ResMapper; + typedef blas_data_mapper ResMapper; LhsMapper lhs(_lhs,lhsStride); - ResMapper res(_res,resStride, resIncr); + ResMapper res(_res,resStride); Index kc = blocking.kc(); // cache block size along the K direction Index mc = (std::min)(rows,blocking.mc()); // cache block size along the M direction @@ -508,13 +503,12 @@ struct selfadjoint_product_impl NumTraits::IsComplex && EIGEN_LOGICAL_XOR(LhsIsUpper,bool(LhsBlasTraits::NeedToConjugate)), EIGEN_LOGICAL_XOR(RhsIsUpper,internal::traits::Flags &RowMajorBit) ? RowMajor : ColMajor, RhsIsSelfAdjoint, NumTraits::IsComplex && EIGEN_LOGICAL_XOR(RhsIsUpper,bool(RhsBlasTraits::NeedToConjugate)), - internal::traits::Flags&RowMajorBit ? RowMajor : ColMajor, - Dest::InnerStrideAtCompileTime> + internal::traits::Flags&RowMajorBit ? RowMajor : ColMajor> ::run( lhs.rows(), rhs.cols(), // sizes &lhs.coeffRef(0,0), lhs.outerStride(), // lhs info &rhs.coeffRef(0,0), rhs.outerStride(), // rhs info - &dst.coeffRef(0,0), dst.innerStride(), dst.outerStride(), // result info + &dst.coeffRef(0,0), dst.outerStride(), // result info actualAlpha, blocking // alpha ); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h index 61396dbdf..9a5318507 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h @@ -44,18 +44,16 @@ namespace internal { template \ -struct product_selfadjoint_matrix \ +struct product_selfadjoint_matrix \ {\ \ static void run( \ Index rows, Index cols, \ const EIGTYPE* _lhs, Index lhsStride, \ const EIGTYPE* _rhs, Index rhsStride, \ - EIGTYPE* res, Index resIncr, Index resStride, \ + EIGTYPE* res, Index resStride, \ EIGTYPE alpha, level3_blocking& /*blocking*/) \ { \ - EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ - eigen_assert(resIncr == 1); \ char side='L', uplo='L'; \ BlasIndex m, n, lda, ldb, ldc; \ const EIGTYPE *a, *b; \ @@ -93,17 +91,15 @@ struct product_selfadjoint_matrix \ -struct product_selfadjoint_matrix \ +struct product_selfadjoint_matrix \ {\ static void run( \ Index rows, Index cols, \ const EIGTYPE* _lhs, Index lhsStride, \ const EIGTYPE* _rhs, Index rhsStride, \ - EIGTYPE* res, Index resIncr, Index resStride, \ + EIGTYPE* res, Index resStride, \ EIGTYPE alpha, level3_blocking& /*blocking*/) \ { \ - EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ - eigen_assert(resIncr == 1); \ char side='L', uplo='L'; \ BlasIndex m, n, lda, ldb, ldc; \ const EIGTYPE *a, *b; \ @@ -171,18 +167,16 @@ EIGEN_BLAS_HEMM_L(scomplex, float, cf, chemm_) template \ -struct product_selfadjoint_matrix \ +struct product_selfadjoint_matrix \ {\ \ static void run( \ Index rows, Index cols, \ const EIGTYPE* _lhs, Index lhsStride, \ const EIGTYPE* _rhs, Index rhsStride, \ - EIGTYPE* res, Index resIncr, Index resStride, \ + EIGTYPE* res, Index resStride, \ EIGTYPE alpha, level3_blocking& /*blocking*/) \ { \ - EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ - eigen_assert(resIncr == 1); \ char side='R', uplo='L'; \ BlasIndex m, n, lda, ldb, ldc; \ const EIGTYPE *a, *b; \ @@ -219,17 +213,15 @@ struct product_selfadjoint_matrix \ -struct product_selfadjoint_matrix \ +struct product_selfadjoint_matrix \ {\ static void run( \ Index rows, Index cols, \ const EIGTYPE* _lhs, Index lhsStride, \ const EIGTYPE* _rhs, Index rhsStride, \ - EIGTYPE* res, Index resIncr, Index resStride, \ + EIGTYPE* res, Index resStride, \ EIGTYPE alpha, level3_blocking& /*blocking*/) \ { \ - EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ - eigen_assert(resIncr == 1); \ char side='R', uplo='L'; \ BlasIndex m, n, lda, ldb, ldc; \ const EIGTYPE *a, *b; \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h index ef12c98f6..f038d686f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h @@ -109,10 +109,10 @@ struct selfadjoint_product_selector internal::general_matrix_matrix_triangular_product::IsComplex, Scalar, OtherIsRowMajor ? ColMajor : RowMajor, (!OtherBlasTraits::NeedToConjugate) && NumTraits::IsComplex, - IsRowMajor ? RowMajor : ColMajor, MatrixType::InnerStrideAtCompileTime, UpLo> + IsRowMajor ? RowMajor : ColMajor, UpLo> ::run(size, depth, &actualOther.coeffRef(0,0), actualOther.outerStride(), &actualOther.coeffRef(0,0), actualOther.outerStride(), - mat.data(), mat.innerStride(), mat.outerStride(), actualAlpha, blocking); + mat.data(), mat.outerStride(), actualAlpha, blocking); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h index 2fb408d1d..f784507e7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h @@ -45,24 +45,22 @@ template + int ResStorageOrder, int Version = Specialized> struct product_triangular_matrix_matrix; template + int RhsStorageOrder, bool ConjugateRhs, int Version> struct product_triangular_matrix_matrix + RhsStorageOrder,ConjugateRhs,RowMajor,Version> { static EIGEN_STRONG_INLINE void run( Index rows, Index cols, Index depth, const Scalar* lhs, Index lhsStride, const Scalar* rhs, Index rhsStride, - Scalar* res, Index resIncr, Index resStride, + Scalar* res, Index resStride, const Scalar& alpha, level3_blocking& blocking) { product_triangular_matrix_matrix - ::run(cols, rows, depth, rhs, rhsStride, lhs, lhsStride, res, resIncr, resStride, alpha, blocking); + ColMajor> + ::run(cols, rows, depth, rhs, rhsStride, lhs, lhsStride, res, resStride, alpha, blocking); } }; // implements col-major += alpha * op(triangular) * op(general) template + int RhsStorageOrder, bool ConjugateRhs, int Version> struct product_triangular_matrix_matrix + RhsStorageOrder,ConjugateRhs,ColMajor,Version> { typedef gebp_traits Traits; @@ -98,21 +95,20 @@ struct product_triangular_matrix_matrix& blocking); }; template + int RhsStorageOrder, bool ConjugateRhs, int Version> EIGEN_DONT_INLINE void product_triangular_matrix_matrix::run( + RhsStorageOrder,ConjugateRhs,ColMajor,Version>::run( Index _rows, Index _cols, Index _depth, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* _res, Index resIncr, Index resStride, + Scalar* _res, Index resStride, const Scalar& alpha, level3_blocking& blocking) { // strip zeros @@ -123,10 +119,10 @@ EIGEN_DONT_INLINE void product_triangular_matrix_matrix LhsMapper; typedef const_blas_data_mapper RhsMapper; - typedef blas_data_mapper ResMapper; + typedef blas_data_mapper ResMapper; LhsMapper lhs(_lhs,lhsStride); RhsMapper rhs(_rhs,rhsStride); - ResMapper res(_res, resStride, resIncr); + ResMapper res(_res, resStride); Index kc = blocking.kc(); // cache block size along the K direction Index mc = (std::min)(rows,blocking.mc()); // cache block size along the M direction @@ -239,11 +235,10 @@ EIGEN_DONT_INLINE void product_triangular_matrix_matrix + int RhsStorageOrder, bool ConjugateRhs, int Version> struct product_triangular_matrix_matrix + RhsStorageOrder,ConjugateRhs,ColMajor,Version> { typedef gebp_traits Traits; enum { @@ -256,21 +251,20 @@ struct product_triangular_matrix_matrix& blocking); }; template + int RhsStorageOrder, bool ConjugateRhs, int Version> EIGEN_DONT_INLINE void product_triangular_matrix_matrix::run( + RhsStorageOrder,ConjugateRhs,ColMajor,Version>::run( Index _rows, Index _cols, Index _depth, const Scalar* _lhs, Index lhsStride, const Scalar* _rhs, Index rhsStride, - Scalar* _res, Index resIncr, Index resStride, + Scalar* _res, Index resStride, const Scalar& alpha, level3_blocking& blocking) { const Index PacketBytes = packet_traits::size*sizeof(Scalar); @@ -282,10 +276,10 @@ EIGEN_DONT_INLINE void product_triangular_matrix_matrix LhsMapper; typedef const_blas_data_mapper RhsMapper; - typedef blas_data_mapper ResMapper; + typedef blas_data_mapper ResMapper; LhsMapper lhs(_lhs,lhsStride); RhsMapper rhs(_rhs,rhsStride); - ResMapper res(_res, resStride, resIncr); + ResMapper res(_res, resStride); Index kc = blocking.kc(); // cache block size along the K direction Index mc = (std::min)(rows,blocking.mc()); // cache block size along the M direction @@ -439,12 +433,12 @@ struct triangular_product_impl Mode, LhsIsTriangular, (internal::traits::Flags&RowMajorBit) ? RowMajor : ColMajor, LhsBlasTraits::NeedToConjugate, (internal::traits::Flags&RowMajorBit) ? RowMajor : ColMajor, RhsBlasTraits::NeedToConjugate, - (internal::traits::Flags&RowMajorBit) ? RowMajor : ColMajor, Dest::InnerStrideAtCompileTime> + (internal::traits::Flags&RowMajorBit) ? RowMajor : ColMajor> ::run( stripedRows, stripedCols, stripedDepth, // sizes &lhs.coeffRef(0,0), lhs.outerStride(), // lhs info &rhs.coeffRef(0,0), rhs.outerStride(), // rhs info - &dst.coeffRef(0,0), dst.innerStride(), dst.outerStride(), // result info + &dst.coeffRef(0,0), dst.outerStride(), // result info actualAlpha, blocking ); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h index a98d12e4a..a25197ab0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h @@ -46,7 +46,7 @@ template {}; + RhsStorageOrder, ConjugateRhs, ResStorageOrder, BuiltIn> {}; // try to go to BLAS specialization @@ -55,15 +55,13 @@ template \ struct product_triangular_matrix_matrix { \ + LhsStorageOrder,ConjugateLhs, RhsStorageOrder,ConjugateRhs,ColMajor,Specialized> { \ static inline void run(Index _rows, Index _cols, Index _depth, const Scalar* _lhs, Index lhsStride,\ - const Scalar* _rhs, Index rhsStride, Scalar* res, Index resIncr, Index resStride, Scalar alpha, level3_blocking& blocking) { \ - EIGEN_ONLY_USED_FOR_DEBUG(resIncr); \ - eigen_assert(resIncr == 1); \ + const Scalar* _rhs, Index rhsStride, Scalar* res, Index resStride, Scalar alpha, level3_blocking& blocking) { \ product_triangular_matrix_matrix_trmm::run( \ - _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha, blocking); \ + _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha, blocking); \ } \ }; @@ -117,8 +115,8 @@ struct product_triangular_matrix_matrix_trmm::run( \ - _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, 1, resStride, alpha, blocking); \ + LhsStorageOrder,ConjugateLhs, RhsStorageOrder, ConjugateRhs, ColMajor, BuiltIn>::run( \ + _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha, blocking); \ /*std::cout << "TRMM_L: A is not square! Go to Eigen TRMM implementation!\n";*/ \ } else { \ /* Make sense to call GEMM */ \ @@ -126,8 +124,8 @@ struct product_triangular_matrix_matrix_trmm(); \ BlasIndex aStride = convert_index(aa_tmp.outerStride()); \ gemm_blocking_space gemm_blocking(_rows,_cols,_depth, 1, true); \ - general_matrix_matrix_product::run( \ - rows, cols, depth, aa_tmp.data(), aStride, _rhs, rhsStride, res, 1, resStride, alpha, gemm_blocking, 0); \ + general_matrix_matrix_product::run( \ + rows, cols, depth, aa_tmp.data(), aStride, _rhs, rhsStride, res, resStride, alpha, gemm_blocking, 0); \ \ /*std::cout << "TRMM_L: A is not square! Go to BLAS GEMM implementation! " << nthr<<" \n";*/ \ } \ @@ -234,8 +232,8 @@ struct product_triangular_matrix_matrix_trmm::run( \ - _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, 1, resStride, alpha, blocking); \ + LhsStorageOrder,ConjugateLhs, RhsStorageOrder, ConjugateRhs, ColMajor, BuiltIn>::run( \ + _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha, blocking); \ /*std::cout << "TRMM_R: A is not square! Go to Eigen TRMM implementation!\n";*/ \ } else { \ /* Make sense to call GEMM */ \ @@ -243,8 +241,8 @@ struct product_triangular_matrix_matrix_trmm(); \ BlasIndex aStride = convert_index(aa_tmp.outerStride()); \ gemm_blocking_space gemm_blocking(_rows,_cols,_depth, 1, true); \ - general_matrix_matrix_product::run( \ - rows, cols, depth, _lhs, lhsStride, aa_tmp.data(), aStride, res, 1, resStride, alpha, gemm_blocking, 0); \ + general_matrix_matrix_product::run( \ + rows, cols, depth, _lhs, lhsStride, aa_tmp.data(), aStride, res, resStride, alpha, gemm_blocking, 0); \ \ /*std::cout << "TRMM_R: A is not square! Go to BLAS GEMM implementation! " << nthr<<" \n";*/ \ } \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h index e3ed2cd19..223c38b86 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h @@ -15,48 +15,48 @@ namespace Eigen { namespace internal { // if the rhs is row major, let's transpose the product -template -struct triangular_solve_matrix +template +struct triangular_solve_matrix { static void run( Index size, Index cols, const Scalar* tri, Index triStride, - Scalar* _other, Index otherIncr, Index otherStride, + Scalar* _other, Index otherStride, level3_blocking& blocking) { triangular_solve_matrix< Scalar, Index, Side==OnTheLeft?OnTheRight:OnTheLeft, (Mode&UnitDiag) | ((Mode&Upper) ? Lower : Upper), NumTraits::IsComplex && Conjugate, - TriStorageOrder==RowMajor ? ColMajor : RowMajor, ColMajor, OtherInnerStride> - ::run(size, cols, tri, triStride, _other, otherIncr, otherStride, blocking); + TriStorageOrder==RowMajor ? ColMajor : RowMajor, ColMajor> + ::run(size, cols, tri, triStride, _other, otherStride, blocking); } }; /* Optimized triangular solver with multiple right hand side and the triangular matrix on the left */ -template -struct triangular_solve_matrix +template +struct triangular_solve_matrix { static EIGEN_DONT_INLINE void run( Index size, Index otherSize, const Scalar* _tri, Index triStride, - Scalar* _other, Index otherIncr, Index otherStride, + Scalar* _other, Index otherStride, level3_blocking& blocking); }; -template -EIGEN_DONT_INLINE void triangular_solve_matrix::run( +template +EIGEN_DONT_INLINE void triangular_solve_matrix::run( Index size, Index otherSize, const Scalar* _tri, Index triStride, - Scalar* _other, Index otherIncr, Index otherStride, + Scalar* _other, Index otherStride, level3_blocking& blocking) { Index cols = otherSize; typedef const_blas_data_mapper TriMapper; - typedef blas_data_mapper OtherMapper; + typedef blas_data_mapper OtherMapper; TriMapper tri(_tri, triStride); - OtherMapper other(_other, otherStride, otherIncr); + OtherMapper other(_other, otherStride); typedef gebp_traits Traits; @@ -128,19 +128,19 @@ EIGEN_DONT_INLINE void triangular_solve_matrix -struct triangular_solve_matrix +template +struct triangular_solve_matrix { static EIGEN_DONT_INLINE void run( Index size, Index otherSize, const Scalar* _tri, Index triStride, - Scalar* _other, Index otherIncr, Index otherStride, + Scalar* _other, Index otherStride, level3_blocking& blocking); }; -template -EIGEN_DONT_INLINE void triangular_solve_matrix::run( +template +EIGEN_DONT_INLINE void triangular_solve_matrix::run( Index size, Index otherSize, const Scalar* _tri, Index triStride, - Scalar* _other, Index otherIncr, Index otherStride, + Scalar* _other, Index otherStride, level3_blocking& blocking) { Index rows = otherSize; typedef typename NumTraits::Real RealScalar; - typedef blas_data_mapper LhsMapper; + typedef blas_data_mapper LhsMapper; typedef const_blas_data_mapper RhsMapper; - LhsMapper lhs(_other, otherStride, otherIncr); + LhsMapper lhs(_other, otherStride); RhsMapper rhs(_tri, triStride); typedef gebp_traits Traits; @@ -297,24 +297,24 @@ EIGEN_DONT_INLINE void triangular_solve_matrix \ -struct triangular_solve_matrix \ +struct triangular_solve_matrix \ { \ enum { \ IsLower = (Mode&Lower) == Lower, \ @@ -51,10 +51,8 @@ struct triangular_solve_matrix& /*blocking*/) \ + EIGTYPE* _other, Index otherStride, level3_blocking& /*blocking*/) \ { \ - EIGEN_ONLY_USED_FOR_DEBUG(otherIncr); \ - eigen_assert(otherIncr == 1); \ BlasIndex m = convert_index(size), n = convert_index(otherSize), lda, ldb; \ char side = 'L', uplo, diag='N', transa; \ /* Set alpha_ */ \ @@ -101,7 +99,7 @@ EIGEN_BLAS_TRSM_L(scomplex, float, ctrsm_) // implements RightSide general * op(triangular)^-1 #define EIGEN_BLAS_TRSM_R(EIGTYPE, BLASTYPE, BLASFUNC) \ template \ -struct triangular_solve_matrix \ +struct triangular_solve_matrix \ { \ enum { \ IsLower = (Mode&Lower) == Lower, \ @@ -112,10 +110,8 @@ struct triangular_solve_matrix& /*blocking*/) \ + EIGTYPE* _other, Index otherStride, level3_blocking& /*blocking*/) \ { \ - EIGEN_ONLY_USED_FOR_DEBUG(otherIncr); \ - eigen_assert(otherIncr == 1); \ BlasIndex m = convert_index(otherSize), n = convert_index(size), lda, ldb; \ char side = 'R', uplo, diag='N', transa; \ /* Set alpha_ */ \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h index 3dff9bc9b..6e6ee119b 100755 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h @@ -31,7 +31,7 @@ template< typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs, typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs, - int ResStorageOrder, int ResInnerStride> + int ResStorageOrder> struct general_matrix_matrix_product; template -class BlasLinearMapper; - template -class BlasLinearMapper { +class BlasLinearMapper { public: typedef typename packet_traits::type Packet; typedef typename packet_traits::half HalfPacket; - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar *data, Index incr=1) - : m_data(data) - { - EIGEN_ONLY_USED_FOR_DEBUG(incr); - eigen_assert(incr==1); - } + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar *data) : m_data(data) {} EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(int i) const { internal::prefetch(&operator()(i)); @@ -196,25 +188,16 @@ class BlasLinearMapper { }; // Lightweight helper class to access matrix coefficients. -template -class blas_data_mapper; - -template -class blas_data_mapper -{ -public: +template +class blas_data_mapper { + public: typedef typename packet_traits::type Packet; typedef typename packet_traits::half HalfPacket; typedef BlasLinearMapper LinearMapper; typedef BlasVectorMapper VectorMapper; - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride, Index incr=1) - : m_data(data), m_stride(stride) - { - EIGEN_ONLY_USED_FOR_DEBUG(incr); - eigen_assert(incr==1); - } + EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride) : m_data(data), m_stride(stride) {} EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper getSubMapper(Index i, Index j) const { @@ -268,90 +251,6 @@ public: const Index m_stride; }; -// Implementation of non-natural increment (i.e. inner-stride != 1) -// The exposed API is not complete yet compared to the Incr==1 case -// because some features makes less sense in this case. -template -class BlasLinearMapper -{ -public: - typedef typename packet_traits::type Packet; - typedef typename packet_traits::half HalfPacket; - - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar *data,Index incr) : m_data(data), m_incr(incr) {} - - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(int i) const { - internal::prefetch(&operator()(i)); - } - - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar& operator()(Index i) const { - return m_data[i*m_incr.value()]; - } - - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet loadPacket(Index i) const { - return pgather(m_data + i*m_incr.value(), m_incr.value()); - } - - template - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, const PacketType &p) const { - pscatter(m_data + i*m_incr.value(), p, m_incr.value()); - } - -protected: - Scalar *m_data; - const internal::variable_if_dynamic m_incr; -}; - -template -class blas_data_mapper -{ -public: - typedef typename packet_traits::type Packet; - typedef typename packet_traits::half HalfPacket; - - typedef BlasLinearMapper LinearMapper; - - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride, Index incr) : m_data(data), m_stride(stride), m_incr(incr) {} - - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper - getSubMapper(Index i, Index j) const { - return blas_data_mapper(&operator()(i, j), m_stride, m_incr.value()); - } - - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE LinearMapper getLinearMapper(Index i, Index j) const { - return LinearMapper(&operator()(i, j), m_incr.value()); - } - - EIGEN_DEVICE_FUNC - EIGEN_ALWAYS_INLINE Scalar& operator()(Index i, Index j) const { - return m_data[StorageOrder==RowMajor ? j*m_incr.value() + i*m_stride : i*m_incr.value() + j*m_stride]; - } - - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet loadPacket(Index i, Index j) const { - return pgather(&operator()(i, j),m_incr.value()); - } - - template - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketT load(Index i, Index j) const { - return pgather(&operator()(i, j),m_incr.value()); - } - - template - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void scatterPacket(Index i, Index j, const SubPacket &p) const { - pscatter(&operator()(i, j), p, m_stride); - } - - template - EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE SubPacket gatherPacket(Index i, Index j) const { - return pgather(&operator()(i, j), m_stride); - } - -protected: - Scalar* EIGEN_RESTRICT m_data; - const Index m_stride; - const internal::variable_if_dynamic m_incr; -}; - // lightweight helper class to access matrix coefficients (const version) template class const_blas_data_mapper : public blas_data_mapper { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h index 74f74cc42..351bd6c60 100755 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h @@ -57,10 +57,7 @@ #if __GNUC__>=6 #pragma GCC diagnostic ignored "-Wignored-attributes" #endif - #if __GNUC__==7 - // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325 - #pragma GCC diagnostic ignored "-Wattributes" - #endif + #endif #if defined __NVCC__ @@ -83,12 +80,4 @@ #pragma diag_suppress 2737 #endif -#else -// warnings already disabled: -# ifndef EIGEN_WARNINGS_DISABLED_2 -# define EIGEN_WARNINGS_DISABLED_2 -# elif defined(EIGEN_INTERNAL_DEBUGGING) -# error "Do not include \"DisableStupidWarnings.h\" recursively more than twice!" -# endif - #endif // not EIGEN_WARNINGS_DISABLED diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ForwardDeclarations.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ForwardDeclarations.h index 134544f96..ea107393a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ForwardDeclarations.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ForwardDeclarations.h @@ -47,7 +47,11 @@ template struct NumTraits; template struct EigenBase; template class DenseBase; template class PlainObjectBase; -template class DenseCoeffsBase; + + +template::value > +class DenseCoeffsBase; templatex || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -380,8 +380,7 @@ #if EIGEN_MAX_CPP_VER>=11 && \ ((defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901)) \ || (defined(__GNUC__) && defined(_GLIBCXX_USE_C99)) \ - || (defined(_LIBCPP_VERSION) && !defined(_MSC_VER)) \ - || (EIGEN_COMP_MSVC >= 1900) ) + || (defined(_LIBCPP_VERSION) && !defined(_MSC_VER))) #define EIGEN_HAS_C99_MATH 1 #else #define EIGEN_HAS_C99_MATH 0 @@ -397,20 +396,6 @@ #endif #endif -// Does the compiler support type_traits? -// - full support of type traits was added only to GCC 5.1.0. -// - 20150626 corresponds to the last release of 4.x libstdc++ -#ifndef EIGEN_HAS_TYPE_TRAITS -#if EIGEN_MAX_CPP_VER>=11 && (EIGEN_HAS_CXX11 || EIGEN_COMP_MSVC >= 1700) \ - && ((!EIGEN_COMP_GNUC_STRICT) || EIGEN_GNUC_AT_LEAST(5, 1)) \ - && ((!defined(__GLIBCXX__)) || __GLIBCXX__ > 20150626) -#define EIGEN_HAS_TYPE_TRAITS 1 -#define EIGEN_INCLUDE_TYPE_TRAITS -#else -#define EIGEN_HAS_TYPE_TRAITS 0 -#endif -#endif - // Does the compiler support variadic templates? #ifndef EIGEN_HAS_VARIADIC_TEMPLATES #if EIGEN_MAX_CPP_VER>=11 && (__cplusplus > 199711L || EIGEN_COMP_MSVC >= 1900) \ @@ -850,48 +835,11 @@ namespace Eigen { #endif -/** - * \internal - * \brief Macro to explicitly define the default copy constructor. - * This is necessary, because the implicit definition is deprecated if the copy-assignment is overridden. - */ -#if EIGEN_HAS_CXX11 -#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) EIGEN_DEVICE_FUNC CLASS(const CLASS&) = default; -#else -#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) -#endif - - - /** \internal * \brief Macro to manually inherit assignment operators. * This is necessary, because the implicitly defined assignment operator gets deleted when a custom operator= is defined. - * With C++11 or later this also default-implements the copy-constructor */ -#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \ - EIGEN_DEFAULT_COPY_CONSTRUCTOR(Derived) - -/** \internal - * \brief Macro to manually define default constructors and destructors. - * This is necessary when the copy constructor is re-defined. - * For empty helper classes this should usually be protected, to avoid accidentally creating empty objects. - * - * Hiding the default destructor lead to problems in C++03 mode together with boost::multiprecision - */ -#if EIGEN_HAS_CXX11 -#define EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(Derived) \ - EIGEN_DEVICE_FUNC Derived() = default; \ - EIGEN_DEVICE_FUNC ~Derived() = default; -#else -#define EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(Derived) \ - EIGEN_DEVICE_FUNC Derived() {}; \ - /* EIGEN_DEVICE_FUNC ~Derived() {}; */ -#endif - - - - +#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) /** * Just a side note. Commenting within defines works only by documenting diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h index 9b61ff037..d31e95411 100755 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h @@ -97,9 +97,6 @@ template<> struct is_arithmetic { enum { value = true }; }; template<> struct is_arithmetic { enum { value = true }; }; template<> struct is_arithmetic { enum { value = true }; }; -#if EIGEN_HAS_CXX11 -using std::is_integral; -#else template struct is_integral { enum { value = false }; }; template<> struct is_integral { enum { value = true }; }; template<> struct is_integral { enum { value = true }; }; @@ -111,11 +108,6 @@ template<> struct is_integral { enum { value = true }; }; template<> struct is_integral { enum { value = true }; }; template<> struct is_integral { enum { value = true }; }; template<> struct is_integral { enum { value = true }; }; -#if EIGEN_COMP_MSVC -template<> struct is_integral { enum { value = true }; }; -template<> struct is_integral{ enum { value = true }; }; -#endif -#endif #if EIGEN_HAS_CXX11 using std::make_unsigned; @@ -539,30 +531,4 @@ bool not_equal_strict(const double& x,const double& y) { return std::not_equal_t } // end namespace Eigen -// Define portable (u)int{32,64} types -#if EIGEN_HAS_CXX11 -#include -namespace Eigen { -namespace numext { -typedef std::uint32_t uint32_t; -typedef std::int32_t int32_t; -typedef std::uint64_t uint64_t; -typedef std::int64_t int64_t; -} -} -#else -// Without c++11, all compilers able to compile Eigen also -// provides the C99 stdint.h header file. -#include -namespace Eigen { -namespace numext { -typedef ::uint32_t uint32_t; -typedef ::int32_t int32_t; -typedef ::uint64_t uint64_t; -typedef ::int64_t int64_t; -} -} -#endif - - #endif // EIGEN_META_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h index 1ce6fd1b0..ecc82b7c8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h @@ -1,8 +1,4 @@ -#ifdef EIGEN_WARNINGS_DISABLED_2 -// "DisableStupidWarnings.h" was included twice recursively: Do not reenable warnings yet! -# undef EIGEN_WARNINGS_DISABLED_2 - -#elif defined(EIGEN_WARNINGS_DISABLED) +#ifdef EIGEN_WARNINGS_DISABLED #undef EIGEN_WARNINGS_DISABLED #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h index 6bb497082..ba5bd186d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h @@ -34,20 +34,6 @@ inline IndexDest convert_index(const IndexSrc& idx) { return IndexDest(idx); } -// true if T can be considered as an integral index (i.e., and integral type or enum) -template struct is_valid_index_type -{ - enum { value = -#if EIGEN_HAS_TYPE_TRAITS - internal::is_integral::value || std::is_enum::value -#elif EIGEN_COMP_MSVC - internal::is_integral::value || __is_enum(T) -#else - // without C++11, we use is_convertible to Index instead of is_integral in order to treat enums as Index. - internal::is_convertible::value && !internal::is_same::value && !is_same::value -#endif - }; -}; // promote_scalar_arg is an helper used in operation between an expression and a scalar, like: // expression * scalar @@ -104,9 +90,6 @@ class no_assignment_operator { private: no_assignment_operator& operator=(const no_assignment_operator&); - protected: - EIGEN_DEFAULT_COPY_CONSTRUCTOR(no_assignment_operator) - EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(no_assignment_operator) }; /** \internal return the index type with the largest number of bits */ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur.h index 4354e4018..7f38919f7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur.h @@ -300,13 +300,10 @@ typename ComplexSchur::ComplexScalar ComplexSchur::compu ComplexScalar trace = t.coeff(0,0) + t.coeff(1,1); ComplexScalar eival1 = (trace + disc) / RealScalar(2); ComplexScalar eival2 = (trace - disc) / RealScalar(2); - RealScalar eival1_norm = numext::norm1(eival1); - RealScalar eival2_norm = numext::norm1(eival2); - // A division by zero can only occur if eival1==eival2==0. - // In this case, det==0, and all we have to do is checking that eival2_norm!=0 - if(eival1_norm > eival2_norm) + + if(numext::norm1(eival1) > numext::norm1(eival2)) eival2 = det / eival1; - else if(eival2_norm!=RealScalar(0)) + else eival1 = det / eival2; // choose the eigenvalue closest to the bottom entry of the diagonal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h index 9191519ab..17ea903f5 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h @@ -236,7 +236,7 @@ template class RealSchur typedef Matrix Vector3s; Scalar computeNormOfT(); - Index findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero); + Index findSmallSubdiagEntry(Index iu); void splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift); void computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo); void initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im, Vector3s& firstHouseholderVector); @@ -302,16 +302,12 @@ RealSchur& RealSchur::computeFromHessenberg(const HessMa Index totalIter = 0; // iteration count for whole matrix Scalar exshift(0); // sum of exceptional shifts Scalar norm = computeNormOfT(); - // sub-diagonal entries smaller than considerAsZero will be treated as zero. - // We use eps^2 to enable more precision in small eigenvalues. - Scalar considerAsZero = numext::maxi( norm * numext::abs2(NumTraits::epsilon()), - (std::numeric_limits::min)() ); if(norm!=Scalar(0)) { while (iu >= 0) { - Index il = findSmallSubdiagEntry(iu,considerAsZero); + Index il = findSmallSubdiagEntry(iu); // Check for convergence if (il == iu) // One root found @@ -368,17 +364,14 @@ inline typename MatrixType::Scalar RealSchur::computeNormOfT() /** \internal Look for single small sub-diagonal element and returns its index */ template -inline Index RealSchur::findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero) +inline Index RealSchur::findSmallSubdiagEntry(Index iu) { using std::abs; Index res = iu; while (res > 0) { Scalar s = abs(m_matT.coeff(res-1,res-1)) + abs(m_matT.coeff(res,res)); - - s = numext::maxi(s * NumTraits::epsilon(), considerAsZero); - - if (abs(m_matT.coeff(res,res-1)) <= s) + if (abs(m_matT.coeff(res,res-1)) <= NumTraits::epsilon() * s) break; res--; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h index d37656fa2..9ddd553f2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h @@ -605,8 +605,7 @@ template struct direct_selfadjoint_eigenvalues res, Ref representative) { - EIGEN_USING_STD_MATH(sqrt) - EIGEN_USING_STD_MATH(abs) + using std::abs; Index i0; // Find non-zero column i0 (by construction, there must exist a non zero coefficient on the diagonal): mat.diagonal().cwiseAbs().maxCoeff(&i0); @@ -617,8 +616,8 @@ template struct direct_selfadjoint_eigenvaluesn1) res = c0/sqrt(n0); - else res = c1/sqrt(n1); + if(n0>n1) res = c0/std::sqrt(n0); + else res = c1/std::sqrt(n1); return true; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h index b81820656..c3fd8c3e0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h @@ -169,38 +169,20 @@ class QuaternionBase : public RotationBase /** return the result vector of \a v through the rotation*/ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; - #ifdef EIGEN_PARSED_BY_DOXYGEN /** \returns \c *this with scalar type casted to \a NewScalarType * * Note that if \a NewScalarType is equal to the current scalar type of \c *this * then this function smartly returns a const reference to \c *this. */ template - EIGEN_DEVICE_FUNC inline typename internal::cast_return_type >::type cast() const; - - #else - - template - EIGEN_DEVICE_FUNC inline - typename internal::enable_if::value,const Derived&>::type cast() const + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type >::type cast() const { - return derived(); + return typename internal::cast_return_type >::type(derived()); } - template - EIGEN_DEVICE_FUNC inline - typename internal::enable_if::value,Quaternion >::type cast() const - { - return Quaternion(coeffs().template cast()); - } - #endif - #ifdef EIGEN_QUATERNIONBASE_PLUGIN # include EIGEN_QUATERNIONBASE_PLUGIN #endif -protected: - EIGEN_DEFAULT_COPY_CONSTRUCTOR(QuaternionBase) - EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(QuaternionBase) }; /*************************************************************************** diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h old mode 100644 new mode 100755 index 33eabd81a..f58ca03d9 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h @@ -14,7 +14,7 @@ namespace Eigen { /** \geometry_module \ingroup Geometry_Module * - * \class UniformScaling + * \class Scaling * * \brief Represents a generic uniform scaling transformation * diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index c21d9e550..3f31ee45d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -252,11 +252,11 @@ protected: public: /** Default constructor without initialization of the meaningful coefficients. - * If Mode==Affine or Mode==Isometry, then the last row is set to [0 ... 0 1] */ + * If Mode==Affine, then the last row is set to [0 ... 0 1] */ EIGEN_DEVICE_FUNC inline Transform() { check_template_params(); - internal::transform_make_affine<(int(Mode)==Affine || int(Mode)==Isometry) ? Affine : AffineCompact>::run(m_matrix); + internal::transform_make_affine<(int(Mode)==Affine) ? Affine : AffineCompact>::run(m_matrix); } EIGEN_DEVICE_FUNC inline Transform(const Transform& other) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h index 0e99ce68e..51d9a82eb 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h @@ -138,6 +138,12 @@ public: /** \returns the inverse translation (opposite) */ Translation inverse() const { return Translation(-m_coeffs); } + Translation& operator=(const Translation& other) + { + m_coeffs = other.m_coeffs; + return *this; + } + static const Translation Identity() { return Translation(VectorType::Zero()); } /** \returns \c *this with scalar type casted to \a NewScalarType diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h index 6b755008f..7e933fca1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h @@ -87,7 +87,7 @@ struct umeyama_transform_matrix_type * \f{align*} * T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix} * \f} -* minimizing the residual above. This transformation is always returned as an +* minimizing the resudiual above. This transformation is always returned as an * Eigen::Matrix. */ template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h index 6b10f39fa..d43961887 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h @@ -519,10 +519,7 @@ void PartialPivLU::compute() // the row permutation is stored as int indices, so just to be sure: eigen_assert(m_lu.rows()::highest()); - if(m_lu.cols()>0) - m_l1_norm = m_lu.cwiseAbs().colwise().sum().maxCoeff(); - else - m_l1_norm = RealScalar(0); + m_l1_norm = m_lu.cwiseAbs().colwise().sum().maxCoeff(); eigen_assert(m_lu.rows() == m_lu.cols() && "PartialPivLU is only for square (and moreover invertible) matrices"); const Index size = m_lu.rows(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h index 4dce2ef20..ebb64a62b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h @@ -44,7 +44,7 @@ struct compute_inverse_size4 static void run(const MatrixType& mat, ResultType& result) { ActualMatrixType matrix(mat); - const Packet4f p4f_sign_PNNP = _mm_castsi128_ps(_mm_set_epi32(0x00000000, 0x80000000, 0x80000000, 0x00000000)); + EIGEN_ALIGN16 const unsigned int _Sign_PNNP[4] = { 0x00000000, 0x80000000, 0x80000000, 0x00000000 }; // Load the full matrix into registers __m128 _L1 = matrix.template packet( 0); @@ -139,7 +139,7 @@ struct compute_inverse_size4 iC = _mm_sub_ps(iC, _mm_mul_ps(_mm_shuffle_ps(A,A,0xB1), _mm_shuffle_ps(DC,DC,0x66))); rd = _mm_shuffle_ps(rd,rd,0); - rd = _mm_xor_ps(rd, p4f_sign_PNNP); + rd = _mm_xor_ps(rd, _mm_load_ps((float*)_Sign_PNNP)); // iB = C*|B| - D*B#*A iB = _mm_sub_ps(_mm_mul_ps(C,_mm_shuffle_ps(dB,dB,0)), iB); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h index 98d0e3f21..091c3970e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h @@ -192,8 +192,7 @@ class PardisoImpl : public SparseSolverBase void pardisoInit(int type) { m_type = type; - EIGEN_USING_STD_MATH(abs); - bool symmetric = abs(m_type) < 10; + bool symmetric = std::abs(m_type) < 10; m_iparm[0] = 1; // No solver default m_iparm[1] = 2; // use Metis for the ordering m_iparm[2] = 0; // Reserved. Set to zero. (??Numbers of processors, value of OMP_NUM_THREADS??) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h index a5b73f8f2..1134d66e7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h @@ -768,21 +768,6 @@ void BDCSVD::computeSingVals(const ArrayRef& col0, const ArrayRef& d // measure everything relative to shift Map diagShifted(m_workspace.data()+4*n, n); diagShifted = diag - shift; - - if(k!=actual_n-1) - { - // check that after the shift, f(mid) is still negative: - RealScalar midShifted = (right - left) / RealScalar(2); - if(shift==right) - midShifted = -midShifted; - RealScalar fMidShifted = secularEq(midShifted, col0, diag, perm, diagShifted, shift); - if(fMidShifted>0) - { - // fMid was erroneous, fix it: - shift = fMidShifted > Literal(0) ? left : right; - diagShifted = diag - shift; - } - } // initial guess RealScalar muPrev, muCur; @@ -860,13 +845,11 @@ void BDCSVD::computeSingVals(const ArrayRef& col0, const ArrayRef& d } RealScalar fLeft = secularEq(leftShifted, col0, diag, perm, diagShifted, shift); - eigen_internal_assert(fLeft::computeSingVals(const ArrayRef& col0, const ArrayRef& d } #endif eigen_internal_assert(fLeft * fRight < Literal(0)); - - if(fLeft Literal(2) * NumTraits::epsilon() * numext::maxi(abs(leftShifted), abs(rightShifted))) { - while (rightShifted - leftShifted > Literal(2) * NumTraits::epsilon() * numext::maxi(abs(leftShifted), abs(rightShifted))) + RealScalar midShifted = (leftShifted + rightShifted) / Literal(2); + fMid = secularEq(midShifted, col0, diag, perm, diagShifted, shift); + if (fLeft * fMid < Literal(0)) { - RealScalar midShifted = (leftShifted + rightShifted) / Literal(2); - fMid = secularEq(midShifted, col0, diag, perm, diagShifted, shift); - eigen_internal_assert((numext::isfinite)(fMid)); - - if (fLeft * fMid < Literal(0)) - { - rightShifted = midShifted; - } - else - { - leftShifted = midShifted; - fLeft = fMid; - } + rightShifted = midShifted; + } + else + { + leftShifted = midShifted; + fLeft = fMid; } - muCur = (leftShifted + rightShifted) / Literal(2); - } - else - { - // We have a problem as shifting on the left or right give either a positive or negative value - // at the middle of [left,right]... - // Instead fo abbording or entering an infinite loop, - // let's just use the middle as the estimated zero-crossing: - muCur = (right - left) * RealScalar(0.5); - if(shift == right) - muCur = -muCur; } + + muCur = (leftShifted + rightShifted) / Literal(2); } singVals[k] = shift + muCur; @@ -955,7 +924,7 @@ void BDCSVD::perturbCol0 Index j = i 0.9 ) + if(i!=k && std::abs(((singVals(j)+dk)*(mus(j)+(shifts(j)-dk)))/((diag(i)+dk)*(diag(i)-dk)) - 1) > 0.9 ) std::cout << " " << ((singVals(j)+dk)*(mus(j)+(shifts(j)-dk)))/((diag(i)+dk)*(diag(i)-dk)) << " == (" << (singVals(j)+dk) << " * " << (mus(j)+(shifts(j)-dk)) << ") / (" << (diag(i)+dk) << " * " << (diag(i)-dk) << ")\n"; #endif @@ -965,7 +934,7 @@ void BDCSVD::perturbCol0 std::cout << "zhat(" << k << ") = sqrt( " << prod << ") ; " << (singVals(last) + dk) << " * " << mus(last) + shifts(last) << " - " << dk << "\n"; #endif RealScalar tmp = sqrt(prod); - zhat(k) = col0(k) > Literal(0) ? RealScalar(tmp) : RealScalar(-tmp); + zhat(k) = col0(k) > Literal(0) ? tmp : -tmp; } } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h index 53da28488..3d1ef373e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h @@ -183,7 +183,7 @@ public: // this temporary is needed to workaround a MSVC issue Index diagSize = (std::max)(1,m_diagSize); return m_usePrescribedThreshold ? m_prescribedThreshold - : RealScalar(diagSize)*NumTraits::epsilon(); + : diagSize*NumTraits::epsilon(); } /** \returns true if \a U (full or thin) is asked for in this SVD decomposition */ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h index 369e6804a..2907f6529 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h @@ -608,7 +608,7 @@ public: } if(Base::m_diag.size()>0) - dest = Base::m_diag.real().asDiagonal().inverse() * dest; + dest = Base::m_diag.asDiagonal().inverse() * dest; if (Base::m_matrix.nonZeros()>0) // otherwise I==I { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h index 7b6183d08..31e06995b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h @@ -156,7 +156,7 @@ void SimplicialCholeskyBase::factorize_preordered(const CholMatrixType& /* the nonzero entry L(k,i) */ Scalar l_ki; if(DoLDLT) - l_ki = yi / numext::real(m_diag[i]); + l_ki = yi / m_diag[i]; else yi = l_ki = yi / Lx[Lp[i]]; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h index 2cb7747cc..e0295f2af 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h @@ -28,7 +28,7 @@ class AmbiVector typedef typename NumTraits::Real RealScalar; explicit AmbiVector(Index size) - : m_buffer(0), m_zero(0), m_size(0), m_end(0), m_allocatedSize(0), m_allocatedElements(0), m_mode(-1) + : m_buffer(0), m_zero(0), m_size(0), m_allocatedSize(0), m_allocatedElements(0), m_mode(-1) { resize(size); } @@ -147,8 +147,7 @@ template void AmbiVector<_Scalar,_StorageIndex>::init(int mode) { m_mode = mode; - // This is only necessary in sparse mode, but we set these unconditionally to avoid some maybe-uninitialized warnings - // if (m_mode==IsSparse) + if (m_mode==IsSparse) { m_llSize = 0; m_llStart = -1; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseUnaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseUnaryOp.h index df6c28d2b..ea7973790 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseUnaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseUnaryOp.h @@ -49,7 +49,6 @@ template class unary_evaluator, IteratorBased>::InnerIterator : public unary_evaluator, IteratorBased>::EvalIterator { - protected: typedef typename XprType::Scalar Scalar; typedef typename unary_evaluator, IteratorBased>::EvalIterator Base; public: @@ -100,7 +99,6 @@ template class unary_evaluator, IteratorBased>::InnerIterator : public unary_evaluator, IteratorBased>::EvalIterator { - protected: typedef typename XprType::Scalar Scalar; typedef typename unary_evaluator, IteratorBased>::EvalIterator Base; public: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h index a5396538b..0a2490bcc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h @@ -327,8 +327,7 @@ class SparseMatrix m_outerIndex[j] = newOuterIndex[j]; m_innerNonZeros[j] = innerNNZ; } - if(m_outerSize>0) - m_outerIndex[m_outerSize] = m_outerIndex[m_outerSize-1] + m_innerNonZeros[m_outerSize-1] + reserveSizes[m_outerSize-1]; + m_outerIndex[m_outerSize] = m_outerIndex[m_outerSize-1] + m_innerNonZeros[m_outerSize-1] + reserveSizes[m_outerSize-1]; m_data.resize(m_outerIndex[m_outerSize]); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h index 76117a010..65611b3d4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h @@ -453,7 +453,7 @@ void permute_symm_to_fullsymm(const MatrixType& mat, SparseMatrix, IteratorBased> class InnerIterator : public EvalIterator { - protected: typedef typename XprType::Scalar Scalar; public: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h index 87f0efe37..7104831c0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h @@ -43,8 +43,8 @@ template struct SparseLUMatrixURetu * Simple example with key steps * \code * VectorXd x(n), b(n); - * SparseMatrix A; - * SparseLU, COLAMDOrdering > solver; + * SparseMatrix A; + * SparseLU, COLAMDOrdering > solver; * // fill A and b; * // Compute the ordering permutation vector from the structural pattern of A * solver.analyzePattern(A); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h index af158f425..cf1fedf92 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h @@ -98,10 +98,8 @@ namespace std { { return deque_base::insert(position,x); } void insert(const_iterator position, size_type new_size, const value_type& x) { deque_base::insert(position, new_size, x); } -#elif defined(_GLIBCXX_DEQUE) && EIGEN_GNUC_AT_LEAST(4,2) && !EIGEN_GNUC_AT_LEAST(10, 1) +#elif defined(_GLIBCXX_DEQUE) && EIGEN_GNUC_AT_LEAST(4,2) // workaround GCC std::deque implementation - // GCC 10.1 doesn't let us access _Deque_impl _M_impl anymore and we have to - // fall-back to the default case void resize(size_type new_size, const value_type& x) { if (new_size < deque_base::size()) @@ -110,7 +108,7 @@ namespace std { deque_base::insert(deque_base::end(), new_size - deque_base::size(), x); } #else - // either non-GCC or GCC between 4.1 and 10.1 + // either GCC 4.1 or non-GCC // default implementation which should always work. void resize(size_type new_size, const value_type& x) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h index 05a7449bc..1f8a531af 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h @@ -119,7 +119,7 @@ OP(const Scalar& s) const { \ return this->OP(Derived::PlainObject::Constant(rows(), cols(), s)); \ } \ EIGEN_DEVICE_FUNC friend EIGEN_STRONG_INLINE const RCmp ## COMPARATOR ## ReturnType \ -OP(const Scalar& s, const EIGEN_CURRENT_STORAGE_BASE_CLASS& d) { \ +OP(const Scalar& s, const Derived& d) { \ return Derived::PlainObject::Constant(d.rows(), d.cols(), s).OP(d); \ } diff --git a/gtsam/3rdparty/Eigen/bench/bench_gemm.cpp b/gtsam/3rdparty/Eigen/bench/bench_gemm.cpp index dccab96a8..8528c5587 100644 --- a/gtsam/3rdparty/Eigen/bench/bench_gemm.cpp +++ b/gtsam/3rdparty/Eigen/bench/bench_gemm.cpp @@ -112,7 +112,6 @@ void matlab_cplx_cplx(const M& ar, const M& ai, const M& br, const M& bi, M& cr, cr.noalias() -= ai * bi; ci.noalias() += ar * bi; ci.noalias() += ai * br; - // [cr ci] += [ar ai] * br + [-ai ar] * bi } void matlab_real_cplx(const M& a, const M& br, const M& bi, M& cr, M& ci) @@ -241,7 +240,7 @@ int main(int argc, char ** argv) blas_gemm(a,b,r); c.noalias() += a * b; if(!r.isApprox(c)) { - std::cout << (r - c).norm() << "\n"; + std::cout << r - c << "\n"; std::cerr << "Warning, your product is crap!\n\n"; } #else @@ -250,7 +249,7 @@ int main(int argc, char ** argv) gemm(a,b,c); r.noalias() += a.cast() .lazyProduct( b.cast() ); if(!r.isApprox(c)) { - std::cout << (r - c).norm() << "\n"; + std::cout << r - c << "\n"; std::cerr << "Warning, your product is crap!\n\n"; } } diff --git a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt index 9887d5804..e2f1dd6b4 100644 --- a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt @@ -39,9 +39,9 @@ endif() add_dependencies(blas eigen_blas eigen_blas_static) install(TARGETS eigen_blas eigen_blas_static - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib) + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) if(EIGEN_Fortran_COMPILER_WORKS) diff --git a/gtsam/3rdparty/Eigen/blas/level3_impl.h b/gtsam/3rdparty/Eigen/blas/level3_impl.h index 6dd6338b4..6c802cd5f 100644 --- a/gtsam/3rdparty/Eigen/blas/level3_impl.h +++ b/gtsam/3rdparty/Eigen/blas/level3_impl.h @@ -13,28 +13,28 @@ int EIGEN_BLAS_FUNC(gemm)(const char *opa, const char *opb, const int *m, const const RealScalar *pa, const int *lda, const RealScalar *pb, const int *ldb, const RealScalar *pbeta, RealScalar *pc, const int *ldc) { // std::cerr << "in gemm " << *opa << " " << *opb << " " << *m << " " << *n << " " << *k << " " << *lda << " " << *ldb << " " << *ldc << " " << *palpha << " " << *pbeta << "\n"; - typedef void (*functype)(DenseIndex, DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, DenseIndex, Scalar, internal::level3_blocking&, Eigen::internal::GemmParallelInfo*); + typedef void (*functype)(DenseIndex, DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, Scalar, internal::level3_blocking&, Eigen::internal::GemmParallelInfo*); static const functype func[12] = { // array index: NOTR | (NOTR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: TR | (NOTR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: ADJ | (NOTR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), 0, // array index: NOTR | (TR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: TR | (TR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: ADJ | (TR << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), 0, // array index: NOTR | (ADJ << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: TR | (ADJ << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), // array index: ADJ | (ADJ << 2) - (internal::general_matrix_matrix_product::run), + (internal::general_matrix_matrix_product::run), 0 }; @@ -71,7 +71,7 @@ int EIGEN_BLAS_FUNC(gemm)(const char *opa, const char *opb, const int *m, const internal::gemm_blocking_space blocking(*m,*n,*k,1,true); int code = OP(*opa) | (OP(*opb) << 2); - func[code](*m, *n, *k, a, *lda, b, *ldb, c, 1, *ldc, alpha, blocking, 0); + func[code](*m, *n, *k, a, *lda, b, *ldb, c, *ldc, alpha, blocking, 0); return 0; } @@ -79,63 +79,63 @@ int EIGEN_BLAS_FUNC(trsm)(const char *side, const char *uplo, const char *opa, c const RealScalar *palpha, const RealScalar *pa, const int *lda, RealScalar *pb, const int *ldb) { // std::cerr << "in trsm " << *side << " " << *uplo << " " << *opa << " " << *diag << " " << *m << "," << *n << " " << *palpha << " " << *lda << " " << *ldb<< "\n"; - typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, DenseIndex, internal::level3_blocking&); + typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, internal::level3_blocking&); static const functype func[32] = { // array index: NOTR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run),\ + (internal::triangular_solve_matrix::run),\ 0, // array index: NOTR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: TR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), // array index: ADJ | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::triangular_solve_matrix::run), + (internal::triangular_solve_matrix::run), 0 }; @@ -163,12 +163,12 @@ int EIGEN_BLAS_FUNC(trsm)(const char *side, const char *uplo, const char *opa, c if(SIDE(*side)==LEFT) { internal::gemm_blocking_space blocking(*m,*n,*m,1,false); - func[code](*m, *n, a, *lda, b, 1, *ldb, blocking); + func[code](*m, *n, a, *lda, b, *ldb, blocking); } else { internal::gemm_blocking_space blocking(*m,*n,*n,1,false); - func[code](*n, *m, a, *lda, b, 1, *ldb, blocking); + func[code](*n, *m, a, *lda, b, *ldb, blocking); } if(alpha!=Scalar(1)) @@ -184,63 +184,63 @@ int EIGEN_BLAS_FUNC(trmm)(const char *side, const char *uplo, const char *opa, c const RealScalar *palpha, const RealScalar *pa, const int *lda, RealScalar *pb, const int *ldb) { // std::cerr << "in trmm " << *side << " " << *uplo << " " << *opa << " " << *diag << " " << *m << " " << *n << " " << *lda << " " << *ldb << " " << *palpha << "\n"; - typedef void (*functype)(DenseIndex, DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, DenseIndex, const Scalar&, internal::level3_blocking&); + typedef void (*functype)(DenseIndex, DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, const Scalar&, internal::level3_blocking&); static const functype func[32] = { // array index: NOTR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (LEFT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (RIGHT << 2) | (UP << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (LEFT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (RIGHT << 2) | (LO << 3) | (NUNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (LEFT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (RIGHT << 2) | (UP << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (LEFT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0, // array index: NOTR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: TR | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), // array index: ADJ | (RIGHT << 2) | (LO << 3) | (UNIT << 4) - (internal::product_triangular_matrix_matrix::run), + (internal::product_triangular_matrix_matrix::run), 0 }; @@ -272,12 +272,12 @@ int EIGEN_BLAS_FUNC(trmm)(const char *side, const char *uplo, const char *opa, c if(SIDE(*side)==LEFT) { internal::gemm_blocking_space blocking(*m,*n,*m,1,false); - func[code](*m, *n, *m, a, *lda, tmp.data(), tmp.outerStride(), b, 1, *ldb, alpha, blocking); + func[code](*m, *n, *m, a, *lda, tmp.data(), tmp.outerStride(), b, *ldb, alpha, blocking); } else { internal::gemm_blocking_space blocking(*m,*n,*n,1,false); - func[code](*m, *n, *n, tmp.data(), tmp.outerStride(), a, *lda, b, 1, *ldb, alpha, blocking); + func[code](*m, *n, *n, tmp.data(), tmp.outerStride(), a, *lda, b, *ldb, alpha, blocking); } return 1; } @@ -338,12 +338,12 @@ int EIGEN_BLAS_FUNC(symm)(const char *side, const char *uplo, const int *m, cons internal::gemm_blocking_space blocking(*m,*n,size,1,false); if(SIDE(*side)==LEFT) - if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, 1, *ldc, alpha, blocking); - else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, 1, *ldc, alpha, blocking); + if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); + else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); else return 0; else if(SIDE(*side)==RIGHT) - if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, 1, *ldc, alpha, blocking); - else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, 1, *ldc, alpha, blocking); + if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking); + else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking); else return 0; else return 0; @@ -359,21 +359,21 @@ int EIGEN_BLAS_FUNC(syrk)(const char *uplo, const char *op, const int *n, const { // std::cerr << "in syrk " << *uplo << " " << *op << " " << *n << " " << *k << " " << *palpha << " " << *lda << " " << *pbeta << " " << *ldc << "\n"; #if !ISCOMPLEX - typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, DenseIndex, const Scalar&, internal::level3_blocking&); + typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, const Scalar&, internal::level3_blocking&); static const functype func[8] = { // array index: NOTR | (UP << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), // array index: TR | (UP << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), // array index: ADJ | (UP << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0, // array index: NOTR | (LO << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), // array index: TR | (LO << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), // array index: ADJ | (LO << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0 }; #endif @@ -426,7 +426,7 @@ int EIGEN_BLAS_FUNC(syrk)(const char *uplo, const char *op, const int *n, const internal::gemm_blocking_space blocking(*n,*n,*k,1,false); int code = OP(*op) | (UPLO(*uplo) << 2); - func[code](*n, *k, a, *lda, a, *lda, c, 1, *ldc, alpha, blocking); + func[code](*n, *k, a, *lda, a, *lda, c, *ldc, alpha, blocking); #endif return 0; @@ -537,18 +537,18 @@ int EIGEN_BLAS_FUNC(hemm)(const char *side, const char *uplo, const int *m, cons if(SIDE(*side)==LEFT) { - if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix - ::run(*m, *n, a, *lda, b, *ldb, c, 1, *ldc, alpha, blocking); - else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix - ::run(*m, *n, a, *lda, b, *ldb, c, 1, *ldc, alpha, blocking); + if(UPLO(*uplo)==UP) internal::product_selfadjoint_matrix + ::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); + else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix + ::run(*m, *n, a, *lda, b, *ldb, c, *ldc, alpha, blocking); else return 0; } else if(SIDE(*side)==RIGHT) { - if(UPLO(*uplo)==UP) matrix(c,*m,*n,*ldc) += alpha * matrix(b,*m,*n,*ldb) * matrix(a,*n,*n,*lda).selfadjointView();/*internal::product_selfadjoint_matrix - ::run(*m, *n, b, *ldb, a, *lda, c, 1, *ldc, alpha, blocking);*/ - else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix - ::run(*m, *n, b, *ldb, a, *lda, c, 1, *ldc, alpha, blocking); + if(UPLO(*uplo)==UP) matrix(c,*m,*n,*ldc) += alpha * matrix(b,*m,*n,*ldb) * matrix(a,*n,*n,*lda).selfadjointView();/*internal::product_selfadjoint_matrix + ::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking);*/ + else if(UPLO(*uplo)==LO) internal::product_selfadjoint_matrix + ::run(*m, *n, b, *ldb, a, *lda, c, *ldc, alpha, blocking); else return 0; } else @@ -566,19 +566,19 @@ int EIGEN_BLAS_FUNC(herk)(const char *uplo, const char *op, const int *n, const { // std::cerr << "in herk " << *uplo << " " << *op << " " << *n << " " << *k << " " << *palpha << " " << *lda << " " << *pbeta << " " << *ldc << "\n"; - typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, DenseIndex, const Scalar&, internal::level3_blocking&); + typedef void (*functype)(DenseIndex, DenseIndex, const Scalar *, DenseIndex, const Scalar *, DenseIndex, Scalar *, DenseIndex, const Scalar&, internal::level3_blocking&); static const functype func[8] = { // array index: NOTR | (UP << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0, // array index: ADJ | (UP << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0, // array index: NOTR | (LO << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0, // array index: ADJ | (LO << 2) - (internal::general_matrix_matrix_triangular_product::run), + (internal::general_matrix_matrix_triangular_product::run), 0 }; @@ -620,7 +620,7 @@ int EIGEN_BLAS_FUNC(herk)(const char *uplo, const char *op, const int *n, const if(*k>0 && alpha!=RealScalar(0)) { internal::gemm_blocking_space blocking(*n,*n,*k,1,false); - func[code](*n, *k, a, *lda, a, *lda, c, 1, *ldc, alpha, blocking); + func[code](*n, *k, a, *lda, a, *lda, c, *ldc, alpha, blocking); matrix(c, *n, *n, *ldc).diagonal().imag().setZero(); } return 0; diff --git a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake index 3d0074c71..a92a2978b 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake @@ -677,8 +677,6 @@ macro(ei_set_build_string) set(TMP_BUILD_STRING ${TMP_BUILD_STRING}-cxx11) endif() - set(TMP_BUILD_STRING ${TMP_BUILD_STRING}-v3.3) - if(EIGEN_BUILD_STRING_SUFFIX) set(TMP_BUILD_STRING ${TMP_BUILD_STRING}-${EIGEN_BUILD_STRING_SUFFIX}) endif() diff --git a/gtsam/3rdparty/Eigen/cmake/FindStandardMathLibrary.cmake b/gtsam/3rdparty/Eigen/cmake/FindStandardMathLibrary.cmake index 337f1b304..711b0e4b4 100644 --- a/gtsam/3rdparty/Eigen/cmake/FindStandardMathLibrary.cmake +++ b/gtsam/3rdparty/Eigen/cmake/FindStandardMathLibrary.cmake @@ -19,11 +19,8 @@ include(CheckCXXSourceCompiles) # notice the std:: is required on some platforms such as QNX set(find_standard_math_library_test_program -" -#include -int main(int argc, char **){ - return int(std::sin(double(argc)) + std::log(double(argc))); -}") +"#include +int main() { std::sin(0.0); std::log(0.0f); }") # first try compiling/linking the test program without any linker flags diff --git a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt index 179824dd1..8ff755988 100644 --- a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt @@ -11,7 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX) endif(CMAKE_COMPILER_IS_GNUCXX) option(EIGEN_INTERNAL_DOCUMENTATION "Build internal documentation" OFF) -option(EIGEN_DOC_USE_MATHJAX "Use MathJax for rendering math in HTML docs" ON) + # Set some Doxygen flags set(EIGEN_DOXY_PROJECT_NAME "Eigen") @@ -19,19 +19,12 @@ set(EIGEN_DOXY_OUTPUT_DIRECTORY_SUFFIX "") set(EIGEN_DOXY_INPUT "\"${Eigen_SOURCE_DIR}/Eigen\" \"${Eigen_SOURCE_DIR}/doc\"") set(EIGEN_DOXY_HTML_COLORSTYLE_HUE "220") set(EIGEN_DOXY_TAGFILES "") - if(EIGEN_INTERNAL_DOCUMENTATION) set(EIGEN_DOXY_INTERNAL "YES") else(EIGEN_INTERNAL_DOCUMENTATION) set(EIGEN_DOXY_INTERNAL "NO") endif(EIGEN_INTERNAL_DOCUMENTATION) -if (EIGEN_DOC_USE_MATHJAX) - set(EIGEN_DOXY_USE_MATHJAX "YES") -else () - set(EIGEN_DOXY_USE_MATHJAX "NO") -endif() - configure_file( ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile diff --git a/gtsam/3rdparty/Eigen/doc/CustomizingEigen_CustomScalar.dox b/gtsam/3rdparty/Eigen/doc/CustomizingEigen_CustomScalar.dox index 24e5f563b..1ee78cbe5 100644 --- a/gtsam/3rdparty/Eigen/doc/CustomizingEigen_CustomScalar.dox +++ b/gtsam/3rdparty/Eigen/doc/CustomizingEigen_CustomScalar.dox @@ -75,7 +75,7 @@ namespace Eigen { static inline Real epsilon() { return 0; } static inline Real dummy_precision() { return 0; } - static inline int digits10() { return 0; } + static inline Real digits10() { return 0; } enum { IsInteger = 0, diff --git a/gtsam/3rdparty/Eigen/doc/Doxyfile.in b/gtsam/3rdparty/Eigen/doc/Doxyfile.in index ac6eafcf9..37948a612 100644 --- a/gtsam/3rdparty/Eigen/doc/Doxyfile.in +++ b/gtsam/3rdparty/Eigen/doc/Doxyfile.in @@ -736,14 +736,6 @@ EXCLUDE = "${Eigen_SOURCE_DIR}/Eigen/src/Core/products" \ "${Eigen_SOURCE_DIR}/unsupported/doc/examples" \ "${Eigen_SOURCE_DIR}/unsupported/doc/snippets" -# Forward declarations of class templates cause the title of the main page for -# the class template to not contain the template signature. This only happens -# when the \class command is used to document the class. Possibly caused -# by https://github.com/doxygen/doxygen/issues/7698. Confirmed fixed by -# doxygen release 1.8.19. - -EXCLUDE += "${Eigen_SOURCE_DIR}/Eigen/src/Core/util/ForwardDeclarations.h" - # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded # from the input. @@ -1253,7 +1245,7 @@ FORMULA_TRANSPARENT = YES # output. When enabled you may also need to install MathJax separately and # configure the path to it using the MATHJAX_RELPATH option. -USE_MATHJAX = @EIGEN_DOXY_USE_MATHJAX@ +USE_MATHJAX = NO # When MathJax is enabled you need to specify the location relative to the # HTML output directory using the MATHJAX_RELPATH option. The destination @@ -1265,12 +1257,12 @@ USE_MATHJAX = @EIGEN_DOXY_USE_MATHJAX@ # However, it is strongly recommended to install a local # copy of MathJax from http://www.mathjax.org before deployment. -MATHJAX_RELPATH = https://cdn.mathjax.org/mathjax/latest +MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest # The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension # names that should be enabled during MathJax rendering. -MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +MATHJAX_EXTENSIONS = # When the SEARCHENGINE tag is enabled doxygen will generate a search box # for the HTML output. The underlying search engine uses javascript @@ -1617,9 +1609,6 @@ PREDEFINED = EIGEN_EMPTY_STRUCT \ EXPAND_AS_DEFINED = EIGEN_MAKE_TYPEDEFS \ EIGEN_MAKE_FIXED_TYPEDEFS \ EIGEN_MAKE_TYPEDEFS_ALL_SIZES \ - EIGEN_MAKE_ARRAY_TYPEDEFS \ - EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS \ - EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES \ EIGEN_CWISE_UNOP_RETURN_TYPE \ EIGEN_CWISE_BINOP_RETURN_TYPE \ EIGEN_CURRENT_STORAGE_BASE_CLASS \ diff --git a/gtsam/3rdparty/Eigen/doc/Pitfalls.dox b/gtsam/3rdparty/Eigen/doc/Pitfalls.dox index fda402572..3f395053d 100644 --- a/gtsam/3rdparty/Eigen/doc/Pitfalls.dox +++ b/gtsam/3rdparty/Eigen/doc/Pitfalls.dox @@ -7,30 +7,14 @@ namespace Eigen { See this \link TopicTemplateKeyword page \endlink. - \section TopicPitfalls_aliasing Aliasing Don't miss this \link TopicAliasing page \endlink on aliasing, especially if you got wrong results in statements where the destination appears on the right hand side of the expression. - -\section TopicPitfalls_alignment_issue Alignment Issues (runtime assertion) - -%Eigen does explicit vectorization, and while that is appreciated by many users, that also leads to some issues in special situations where data alignment is compromised. -Indeed, since C++17, C++ does not have quite good enough support for explicit data alignment. -In that case your program hits an assertion failure (that is, a "controlled crash") with a message that tells you to consult this page: -\code -http://eigen.tuxfamily.org/dox/group__TopicUnalignedArrayAssert.html -\endcode -Have a look at \link TopicUnalignedArrayAssert it \endlink and see for yourself if that's something that you can cope with. -It contains detailed information about how to deal with each known cause for that issue. - -Now what if you don't care about vectorization and so don't want to be annoyed with these alignment issues? Then read \link getrid how to get rid of them \endlink. - - \section TopicPitfalls_auto_keyword C++11 and the auto keyword -In short: do not use the auto keywords with %Eigen's expressions, unless you are 100% sure about what you are doing. In particular, do not use the auto keyword as a replacement for a \c Matrix<> type. Here is an example: +In short: do not use the auto keywords with Eigen's expressions, unless you are 100% sure about what you are doing. In particular, do not use the auto keyword as a replacement for a Matrix<> type. Here is an example: \code MatrixXd A, B; @@ -38,81 +22,23 @@ auto C = A*B; for(...) { ... w = C * v; ...} \endcode -In this example, the type of C is not a \c MatrixXd but an abstract expression representing a matrix product and storing references to \c A and \c B. -Therefore, the product of \c A*B will be carried out multiple times, once per iteration of the for loop. -Moreover, if the coefficients of A or B change during the iteration, then C will evaluate to different values. +In this example, the type of C is not a MatrixXd but an abstract expression representing a matrix product and storing references to A and B. Therefore, the product of A*B will be carried out multiple times, once per iteration of the for loop. Moreover, if the coefficients of A or B change during the iteration, then C will evaluate to different values. Here is another example leading to a segfault: \code auto C = ((A+B).eval()).transpose(); // do something with C \endcode -The problem is that \c eval() returns a temporary object (in this case a \c MatrixXd) which is then referenced by the \c Transpose<> expression. -However, this temporary is deleted right after the first line, and then the \c C expression references a dead object. -One possible fix consists in applying \c eval() on the whole expression: -\code -auto C = (A+B).transpose().eval(); -\endcode - -The same issue might occur when sub expressions are automatically evaluated by %Eigen as in the following example: +The problem is that eval() returns a temporary object (in this case a MatrixXd) which is then referenced by the Transpose<> expression. However, this temporary is deleted right after the first line, and there the C expression reference a dead object. The same issue might occur when sub expressions are automatically evaluated by Eigen as in the following example: \code VectorXd u, v; auto C = u + (A*v).normalized(); // do something with C \endcode -Here the \c normalized() method has to evaluate the expensive product \c A*v to avoid evaluating it twice. -Again, one possible fix is to call \c .eval() on the whole expression: +where the normalized() method has to evaluate the expensive product A*v to avoid evaluating it twice. On the other hand, the following example is perfectly fine: \code auto C = (u + (A*v).normalized()).eval(); \endcode -In this case, \c C will be a regular \c VectorXd object. -Note that DenseBase::eval() is smart enough to avoid copies when the underlying expression is already a plain \c Matrix<>. - - -\section TopicPitfalls_header_issues Header Issues (failure to compile) - -With all libraries, one must check the documentation for which header to include. -The same is true with %Eigen, but slightly worse: with %Eigen, a method in a class may require an additional #include over what the class itself requires! -For example, if you want to use the \c cross() method on a vector (it computes a cross-product) then you need to: -\code -#include -\endcode -We try to always document this, but do tell us if we forgot an occurrence. - - -\section TopicPitfalls_ternary_operator Ternary operator - -In short: avoid the use of the ternary operator (COND ? THEN : ELSE) with %Eigen's expressions for the \c THEN and \c ELSE statements. -To see why, let's consider the following example: -\code -Vector3f A; -A << 1, 2, 3; -Vector3f B = ((1 < 0) ? (A.reverse()) : A); -\endcode -This example will return B = 3, 2, 1. Do you see why? -The reason is that in c++ the type of the \c ELSE statement is inferred from the type of the \c THEN expression such that both match. -Since \c THEN is a Reverse, the \c ELSE statement A is converted to a Reverse, and the compiler thus generates: -\code -Vector3f B = ((1 < 0) ? (A.reverse()) : Reverse(A)); -\endcode -In this very particular case, a workaround would be to call A.reverse().eval() for the \c THEN statement, but the safest and fastest is really to avoid this ternary operator with %Eigen's expressions and use a if/else construct. - - -\section TopicPitfalls_pass_by_value Pass-by-value - -If you don't know why passing-by-value is wrong with %Eigen, read this \link TopicPassingByValue page \endlink first. - -While you may be extremely careful and use care to make sure that all of your code that explicitly uses %Eigen types is pass-by-reference you have to watch out for templates which define the argument types at compile time. - -If a template has a function that takes arguments pass-by-value, and the relevant template parameter ends up being an %Eigen type, then you will of course have the same alignment problems that you would in an explicitly defined function passing %Eigen types by reference. - -Using %Eigen types with other third party libraries or even the STL can present the same problem. -boost::bind for example uses pass-by-value to store arguments in the returned functor. -This will of course be a problem. - -There are at least two ways around this: - - If the value you are passing is guaranteed to be around for the life of the functor, you can use boost::ref() to wrap the value as you pass it to boost::bind. Generally this is not a solution for values on the stack as if the functor ever gets passed to a lower or independent scope, the object may be gone by the time it's attempted to be used. - - The other option is to make your functions take a reference counted pointer like boost::shared_ptr as the argument. This avoids needing to worry about managing the lifetime of the object being passed. - +In this case, C will be a regular VectorXd object. */ } diff --git a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox index 653bf33ef..a25622e80 100644 --- a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox +++ b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox @@ -244,7 +244,7 @@ As stated earlier, for a read-write sub-matrix (RW), the evaluation can be done \code sm1.valuePtr(); // Pointer to the values -sm1.innerIndexPtr(); // Pointer to the indices. +sm1.innerIndextr(); // Pointer to the indices. sm1.outerIndexPtr(); // Pointer to the beginning of each inner vector \endcode diff --git a/gtsam/3rdparty/Eigen/doc/TopicLazyEvaluation.dox b/gtsam/3rdparty/Eigen/doc/TopicLazyEvaluation.dox index d2a704f13..101ef8c72 100644 --- a/gtsam/3rdparty/Eigen/doc/TopicLazyEvaluation.dox +++ b/gtsam/3rdparty/Eigen/doc/TopicLazyEvaluation.dox @@ -2,95 +2,63 @@ namespace Eigen { /** \page TopicLazyEvaluation Lazy Evaluation and Aliasing -Executive summary: %Eigen has intelligent compile-time mechanisms to enable lazy evaluation and removing temporaries where appropriate. +Executive summary: Eigen has intelligent compile-time mechanisms to enable lazy evaluation and removing temporaries where appropriate. It will handle aliasing automatically in most cases, for example with matrix products. The automatic behavior can be overridden manually by using the MatrixBase::eval() and MatrixBase::noalias() methods. When you write a line of code involving a complex expression such as -\code mat1 = mat2 + mat3 * (mat4 + mat5); -\endcode +\code mat1 = mat2 + mat3 * (mat4 + mat5); \endcode -%Eigen determines automatically, for each sub-expression, whether to evaluate it into a temporary variable. Indeed, in certain cases it is better to evaluate a sub-expression into a temporary variable, while in other cases it is better to avoid that. +Eigen determines automatically, for each sub-expression, whether to evaluate it into a temporary variable. Indeed, in certain cases it is better to evaluate immediately a sub-expression into a temporary variable, while in other cases it is better to avoid that. A traditional math library without expression templates always evaluates all sub-expressions into temporaries. So with this code, -\code vec1 = vec2 + vec3; -\endcode +\code vec1 = vec2 + vec3; \endcode a traditional library would evaluate \c vec2 + vec3 into a temporary \c vec4 and then copy \c vec4 into \c vec1. This is of course inefficient: the arrays are traversed twice, so there are a lot of useless load/store operations. -Expression-templates-based libraries can avoid evaluating sub-expressions into temporaries, which in many cases results in large speed improvements. -This is called lazy evaluation as an expression is getting evaluated as late as possible. -In %Eigen all expressions are lazy-evaluated. -More precisely, an expression starts to be evaluated once it is assigned to a matrix. -Until then nothing happens beyond constructing the abstract expression tree. -In contrast to most other expression-templates-based libraries, however, %Eigen might choose to evaluate some sub-expressions into temporaries. -There are two reasons for that: first, pure lazy evaluation is not always a good choice for performance; second, pure lazy evaluation can be very dangerous, for example with matrix products: doing mat = mat*mat gives a wrong result if the matrix product is directly evaluated within the destination matrix, because of the way matrix product works. +Expression-templates-based libraries can avoid evaluating sub-expressions into temporaries, which in many cases results in large speed improvements. This is called lazy evaluation as an expression is getting evaluated as late as possible, instead of immediately. However, most other expression-templates-based libraries always choose lazy evaluation. There are two problems with that: first, lazy evaluation is not always a good choice for performance; second, lazy evaluation can be very dangerous, for example with matrix products: doing matrix = matrix*matrix gives a wrong result if the matrix product is lazy-evaluated, because of the way matrix product works. -For these reasons, %Eigen has intelligent compile-time mechanisms to determine automatically which sub-expression should be evaluated into a temporary variable. +For these reasons, Eigen has intelligent compile-time mechanisms to determine automatically when to use lazy evaluation, and when on the contrary it should evaluate immediately into a temporary variable. So in the basic example, -\code mat1 = mat2 + mat3; -\endcode +\code matrix1 = matrix2 + matrix3; \endcode -%Eigen chooses not to introduce any temporary. Thus the arrays are traversed only once, producing optimized code. -If you really want to force immediate evaluation, use \link MatrixBase::eval() eval()\endlink: +Eigen chooses lazy evaluation. Thus the arrays are traversed only once, producing optimized code. If you really want to force immediate evaluation, use \link MatrixBase::eval() eval()\endlink: -\code mat1 = (mat2 + mat3).eval(); -\endcode +\code matrix1 = (matrix2 + matrix3).eval(); \endcode Here is now a more involved example: -\code mat1 = -mat2 + mat3 + 5 * mat4; -\endcode +\code matrix1 = -matrix2 + matrix3 + 5 * matrix4; \endcode -Here again %Eigen won't introduce any temporary, thus producing a single fused evaluation loop, which is clearly the correct choice. +Eigen chooses lazy evaluation at every stage in that example, which is clearly the correct choice. In fact, lazy evaluation is the "default choice" and Eigen will choose it except in a few circumstances. -\section TopicLazyEvaluationWhichExpr Which sub-expressions are evaluated into temporaries? +The first circumstance in which Eigen chooses immediate evaluation, is when it sees an assignment a = b; and the expression \c b has the evaluate-before-assigning \link flags flag\endlink. The most important example of such an expression is the \link Product matrix product expression\endlink. For example, when you do -The default evaluation strategy is to fuse the operations in a single loop, and %Eigen will choose it except in a few circumstances. +\code matrix = matrix * matrix; \endcode -The first circumstance in which %Eigen chooses to evaluate a sub-expression is when it sees an assignment a = b; and the expression \c b has the evaluate-before-assigning \link flags flag\endlink. -The most important example of such an expression is the \link Product matrix product expression\endlink. For example, when you do - -\code mat = mat * mat; -\endcode - -%Eigen will evaluate mat * mat into a temporary matrix, and then copies it into the original \c mat. -This guarantees a correct result as we saw above that lazy evaluation gives wrong results with matrix products. -It also doesn't cost much, as the cost of the matrix product itself is much higher. -Note that this temporary is introduced at evaluation time only, that is, within operator= in this example. -The expression mat * mat still return a abstract product type. +Eigen first evaluates matrix * matrix into a temporary matrix, and then copies it into the original \c matrix. This guarantees a correct result as we saw above that lazy evaluation gives wrong results with matrix products. It also doesn't cost much, as the cost of the matrix product itself is much higher. What if you know that the result does no alias the operand of the product and want to force lazy evaluation? Then use \link MatrixBase::noalias() .noalias()\endlink instead. Here is an example: -\code mat1.noalias() = mat2 * mat2; -\endcode +\code matrix1.noalias() = matrix2 * matrix2; \endcode -Here, since we know that mat2 is not the same matrix as mat1, we know that lazy evaluation is not dangerous, so we may force lazy evaluation. Concretely, the effect of noalias() here is to bypass the evaluate-before-assigning \link flags flag\endlink. +Here, since we know that matrix2 is not the same matrix as matrix1, we know that lazy evaluation is not dangerous, so we may force lazy evaluation. Concretely, the effect of noalias() here is to bypass the evaluate-before-assigning \link flags flag\endlink. -The second circumstance in which %Eigen chooses to evaluate a sub-expression, is when it sees a nested expression such as a + b where \c b is already an expression having the evaluate-before-nesting \link flags flag\endlink. -Again, the most important example of such an expression is the \link Product matrix product expression\endlink. -For example, when you do +The second circumstance in which Eigen chooses immediate evaluation, is when it sees a nested expression such as a + b where \c b is already an expression having the evaluate-before-nesting \link flags flag\endlink. Again, the most important example of such an expression is the \link Product matrix product expression\endlink. For example, when you do -\code mat1 = mat2 * mat3 + mat4 * mat5; -\endcode +\code matrix1 = matrix2 + matrix3 * matrix4; \endcode -the products mat2 * mat3 and mat4 * mat5 gets evaluated separately into temporary matrices before being summed up in mat1. -Indeed, to be efficient matrix products need to be evaluated within a destination matrix at hand, and not as simple "dot products". -For small matrices, however, you might want to enforce a "dot-product" based lazy evaluation with lazyProduct(). -Again, it is important to understand that those temporaries are created at evaluation time only, that is in operator =. -See TopicPitfalls_auto_keyword for common pitfalls regarding this remark. +the product matrix3 * matrix4 gets evaluated immediately into a temporary matrix. Indeed, experiments showed that it is often beneficial for performance to evaluate immediately matrix products when they are nested into bigger expressions. -The third circumstance in which %Eigen chooses to evaluate a sub-expression, is when its cost model shows that the total cost of an operation is reduced if a sub-expression gets evaluated into a temporary. -Indeed, in certain cases, an intermediate result is sufficiently costly to compute and is reused sufficiently many times, that is worth "caching". Here is an example: +The third circumstance in which Eigen chooses immediate evaluation, is when its cost model shows that the total cost of an operation is reduced if a sub-expression gets evaluated into a temporary. Indeed, in certain cases, an intermediate result is sufficiently costly to compute and is reused sufficiently many times, that is worth "caching". Here is an example: -\code mat1 = mat2 * (mat3 + mat4); -\endcode +\code matrix1 = matrix2 * (matrix3 + matrix4); \endcode -Here, provided the matrices have at least 2 rows and 2 columns, each coefficient of the expression mat3 + mat4 is going to be used several times in the matrix product. Instead of computing the sum every time, it is much better to compute it once and store it in a temporary variable. %Eigen understands this and evaluates mat3 + mat4 into a temporary variable before evaluating the product. +Here, provided the matrices have at least 2 rows and 2 columns, each coefficienct of the expression matrix3 + matrix4 is going to be used several times in the matrix product. Instead of computing the sum everytime, it is much better to compute it once and store it in a temporary variable. Eigen understands this and evaluates matrix3 + matrix4 into a temporary variable before evaluating the product. */ diff --git a/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox b/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox index a2855745b..47c9b261f 100644 --- a/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox +++ b/gtsam/3rdparty/Eigen/doc/TopicMultithreading.dox @@ -49,7 +49,6 @@ int main(int argc, char** argv) In the case your application is parallelized with OpenMP, you might want to disable Eigen's own parallization as detailed in the previous section. -\warning Using OpenMP with custom scalar types that might throw exceptions can lead to unexpected behaviour in the event of throwing. */ } diff --git a/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox b/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox index 723f4dbce..2e1420f98 100644 --- a/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox +++ b/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox @@ -232,8 +232,8 @@ On the other hand, since there exist 24 different conventions, they are pretty c to create a rotation matrix according to the 2-1-2 convention.\code Matrix3f m; m = AngleAxisf(angle1, Vector3f::UnitZ()) - * * AngleAxisf(angle2, Vector3f::UnitY()) - * * AngleAxisf(angle3, Vector3f::UnitZ()); + * AngleAxisf(angle2, Vector3f::UnitY()) + * AngleAxisf(angle3, Vector3f::UnitZ()); \endcode diff --git a/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js b/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js index afb97edf5..a6f8c3428 100644 --- a/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js +++ b/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js @@ -5,7 +5,6 @@ function generate_autotoc() { if(headers.length > 1) { var toc = $("#side-nav").append(''); toc = $("#nav-toc"); - var footer = $("#nav-path"); var footerHeight = footer.height(); toc = toc.append('
      '); toc = toc.find('ul'); @@ -138,7 +137,7 @@ function initNavTree(toroot,relpath) } }) - $(window).on("load", showRoot); + $(window).load(showRoot); } // return false if the the node has no children at all, or has only section/subsection children @@ -242,6 +241,6 @@ $(document).ready(function() { } })(); - $(window).on("load", resizeHeight); + $(window).load(resizeHeight); }); diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in index 126653589..9ac0596cb 100644 --- a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in +++ b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in @@ -17,6 +17,19 @@ $generatedby   + + + + diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in b/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in index a6b1c1d08..bb149f8f0 100644 --- a/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in +++ b/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in @@ -20,9 +20,6 @@ $mathjax - - -
      diff --git a/gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_block_assignment.cpp b/gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_block_assignment.cpp index 0b87313a1..76f49f2fb 100644 --- a/gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_block_assignment.cpp +++ b/gtsam/3rdparty/Eigen/doc/examples/Tutorial_BlockOperations_block_assignment.cpp @@ -14,5 +14,5 @@ int main() a.block<2,2>(1,1) = m; cout << "Here is now a with m copied into its central 2x2 block:" << endl << a << endl << endl; a.block(0,0,2,3) = a.block(2,1,2,3); - cout << "Here is now a with bottom-right 2x3 block copied into top-left 2x3 block:" << endl << a << endl << endl; + cout << "Here is now a with bottom-right 2x3 block copied into top-left 2x2 block:" << endl << a << endl << endl; } diff --git a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt index fbecd6624..9aa209faa 100644 --- a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt @@ -103,9 +103,9 @@ endif() add_dependencies(lapack eigen_lapack eigen_lapack_static) install(TARGETS eigen_lapack eigen_lapack_static - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib) + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) @@ -133,14 +133,12 @@ if(EXISTS ${eigen_full_path_to_testing_lapack}) string(REGEX REPLACE "(.*)/STACK:(.*) (.*)" "\\1/STACK:900000000000000000 \\3" CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS}") endif() - file(MAKE_DIRECTORY "${LAPACK_BINARY_DIR}/TESTING") add_subdirectory(testing/MATGEN) add_subdirectory(testing/LIN) add_subdirectory(testing/EIG) - cmake_policy(SET CMP0026 OLD) macro(add_lapack_test output input target) set(TEST_INPUT "${LAPACK_SOURCE_DIR}/testing/${input}") - set(TEST_OUTPUT "${LAPACK_BINARY_DIR}/TESTING/${output}") + set(TEST_OUTPUT "${LAPACK_BINARY_DIR}/testing/${output}") get_target_property(TEST_LOC ${target} LOCATION) string(REPLACE "." "_" input_name ${input}) set(testName "${target}_${input_name}") diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index 47e6fee4b..0747aa6cb 100644 --- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt @@ -163,7 +163,7 @@ ei_add_test(constructor) ei_add_test(linearstructure) ei_add_test(integer_types) ei_add_test(unalignedcount) -if(NOT EIGEN_TEST_NO_EXCEPTIONS AND NOT EIGEN_TEST_OPENMP) +if(NOT EIGEN_TEST_NO_EXCEPTIONS) ei_add_test(exceptions) endif() ei_add_test(redux) @@ -185,7 +185,7 @@ ei_add_test(smallvectors) ei_add_test(mapped_matrix) ei_add_test(mapstride) ei_add_test(mapstaticmethods) -ei_add_test(array_cwise) +ei_add_test(array) ei_add_test(array_for_matrix) ei_add_test(array_replicate) ei_add_test(array_reverse) diff --git a/gtsam/3rdparty/Eigen/test/array_cwise.cpp b/gtsam/3rdparty/Eigen/test/array.cpp similarity index 98% rename from gtsam/3rdparty/Eigen/test/array_cwise.cpp rename to gtsam/3rdparty/Eigen/test/array.cpp index 7c5709dbe..7afd3ed3f 100644 --- a/gtsam/3rdparty/Eigen/test/array_cwise.cpp +++ b/gtsam/3rdparty/Eigen/test/array.cpp @@ -279,7 +279,7 @@ template void array_real(const ArrayType& m) VERIFY_IS_APPROX(m1.sign() * m1.abs(), m1); VERIFY_IS_APPROX(numext::abs2(numext::real(m1)) + numext::abs2(numext::imag(m1)), numext::abs2(m1)); - VERIFY_IS_APPROX(numext::abs2(Eigen::real(m1)) + numext::abs2(Eigen::imag(m1)), numext::abs2(m1)); + VERIFY_IS_APPROX(numext::abs2(real(m1)) + numext::abs2(imag(m1)), numext::abs2(m1)); if(!NumTraits::IsComplex) VERIFY_IS_APPROX(numext::real(m1), m1); @@ -368,7 +368,7 @@ template void array_complex(const ArrayType& m) for (Index i = 0; i < m.rows(); ++i) for (Index j = 0; j < m.cols(); ++j) - m3(i,j) = std::atan2(m1(i,j).imag(), m1(i,j).real()); + m3(i,j) = std::atan2(imag(m1(i,j)), real(m1(i,j))); VERIFY_IS_APPROX(arg(m1), m3); std::complex zero(0.0,0.0); @@ -395,7 +395,7 @@ template void array_complex(const ArrayType& m) VERIFY_IS_APPROX(inverse(inverse(m1)),m1); VERIFY_IS_APPROX(conj(m1.conjugate()), m1); - VERIFY_IS_APPROX(abs(m1), sqrt(square(m1.real())+square(m1.imag()))); + VERIFY_IS_APPROX(abs(m1), sqrt(square(real(m1))+square(imag(m1)))); VERIFY_IS_APPROX(abs(m1), sqrt(abs2(m1))); VERIFY_IS_APPROX(log10(m1), log(m1)/log(10)); @@ -446,7 +446,7 @@ template void min_max(const ArrayType& m) } -void test_array_cwise() +void test_array() { for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST_1( array(Array()) ); diff --git a/gtsam/3rdparty/Eigen/test/bdcsvd.cpp b/gtsam/3rdparty/Eigen/test/bdcsvd.cpp index 3ca273635..6c7b09696 100644 --- a/gtsam/3rdparty/Eigen/test/bdcsvd.cpp +++ b/gtsam/3rdparty/Eigen/test/bdcsvd.cpp @@ -28,13 +28,9 @@ template void bdcsvd(const MatrixType& a = MatrixType(), bool pickrandom = true) { - MatrixType m; - if(pickrandom) { - m.resizeLike(a); + MatrixType m = a; + if(pickrandom) svd_fill_random(m); - } - else - m = a; CALL_SUBTEST(( svd_test_all_computation_options >(m, false) )); } diff --git a/gtsam/3rdparty/Eigen/test/constructor.cpp b/gtsam/3rdparty/Eigen/test/constructor.cpp index 988539951..eec9e2192 100644 --- a/gtsam/3rdparty/Eigen/test/constructor.cpp +++ b/gtsam/3rdparty/Eigen/test/constructor.cpp @@ -20,8 +20,6 @@ template struct Wrapper inline operator MatrixType& () { return m_mat; } }; -enum my_sizes { M = 12, N = 7}; - template void ctor_init1(const MatrixType& m) { // Check logic in PlainObjectBase::_init1 @@ -83,16 +81,4 @@ void test_constructor() Array a(123); VERIFY_IS_EQUAL(a(4), 123.f); } - { - MatrixXi m1(M,N); - VERIFY_IS_EQUAL(m1.rows(),M); - VERIFY_IS_EQUAL(m1.cols(),N); - ArrayXXi a1(M,N); - VERIFY_IS_EQUAL(a1.rows(),M); - VERIFY_IS_EQUAL(a1.cols(),N); - VectorXi v1(M); - VERIFY_IS_EQUAL(v1.size(),M); - ArrayXi a2(M); - VERIFY_IS_EQUAL(a2.size(),M); - } } diff --git a/gtsam/3rdparty/Eigen/test/ctorleak.cpp b/gtsam/3rdparty/Eigen/test/ctorleak.cpp index d73fecfe2..c158f5e4e 100644 --- a/gtsam/3rdparty/Eigen/test/ctorleak.cpp +++ b/gtsam/3rdparty/Eigen/test/ctorleak.cpp @@ -8,7 +8,7 @@ struct Foo static Index object_limit; int dummy; - Foo() : dummy(0) + Foo() { #ifdef EIGEN_EXCEPTIONS // TODO: Is this the correct way to handle this? @@ -37,33 +37,22 @@ void test_ctorleak() { typedef Matrix MatrixX; typedef Matrix VectorX; - Foo::object_count = 0; for(int i = 0; i < g_repeat; i++) { Index rows = internal::random(2,EIGEN_TEST_MAX_SIZE), cols = internal::random(2,EIGEN_TEST_MAX_SIZE); - Foo::object_limit = rows*cols; - { - MatrixX r(rows, cols); - Foo::object_limit = r.size()+internal::random(0, rows*cols - 2); + Foo::object_limit = internal::random(0, rows*cols - 2); std::cout << "object_limit =" << Foo::object_limit << std::endl; #ifdef EIGEN_EXCEPTIONS try { #endif - if(internal::random()) { - std::cout << "\nMatrixX m(" << rows << ", " << cols << ");\n"; - MatrixX m(rows, cols); - } - else { - std::cout << "\nMatrixX m(r);\n"; - MatrixX m(r); - } + std::cout << "\nMatrixX m(" << rows << ", " << cols << ");\n"; + MatrixX m(rows, cols); #ifdef EIGEN_EXCEPTIONS VERIFY(false); // not reached if exceptions are enabled } catch (const Foo::Fail&) { /* ignore */ } #endif - } VERIFY_IS_EQUAL(Index(0), Foo::object_count); { @@ -77,5 +66,4 @@ void test_ctorleak() } VERIFY_IS_EQUAL(Index(0), Foo::object_count); } - std::cout << "\n"; } diff --git a/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp b/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp index 5c1317569..07bf65e03 100644 --- a/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp +++ b/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp @@ -67,7 +67,7 @@ template void eigensolver(const MatrixType& m) // Test matrix with NaN a(0,0) = std::numeric_limits::quiet_NaN(); EigenSolver eiNaN(a); - VERIFY_IS_NOT_EQUAL(eiNaN.info(), Success); + VERIFY_IS_EQUAL(eiNaN.info(), NoConvergence); } // regression test for bug 1098 diff --git a/gtsam/3rdparty/Eigen/test/exceptions.cpp b/gtsam/3rdparty/Eigen/test/exceptions.cpp index 015b9fd33..b83fb82ba 100644 --- a/gtsam/3rdparty/Eigen/test/exceptions.cpp +++ b/gtsam/3rdparty/Eigen/test/exceptions.cpp @@ -109,7 +109,5 @@ void memoryleak() void test_exceptions() { - EIGEN_TRY { - CALL_SUBTEST( memoryleak() ); - } EIGEN_CATCH(...) {} + CALL_SUBTEST( memoryleak() ); } diff --git a/gtsam/3rdparty/Eigen/test/fastmath.cpp b/gtsam/3rdparty/Eigen/test/fastmath.cpp index e84bdc972..cc5db0746 100644 --- a/gtsam/3rdparty/Eigen/test/fastmath.cpp +++ b/gtsam/3rdparty/Eigen/test/fastmath.cpp @@ -43,11 +43,11 @@ void check_inf_nan(bool dryrun) { } else { - if( (std::isfinite)(m(3))) g_test_level=1; VERIFY( !(numext::isfinite)(m(3)) ); g_test_level=0; - if( (std::isinf) (m(3))) g_test_level=1; VERIFY( !(numext::isinf)(m(3)) ); g_test_level=0; - if(!(std::isnan) (m(3))) g_test_level=1; VERIFY( (numext::isnan)(m(3)) ); g_test_level=0; - if( (std::isfinite)(m(3))) g_test_level=1; VERIFY( !m.allFinite() ); g_test_level=0; - if(!(std::isnan) (m(3))) g_test_level=1; VERIFY( m.hasNaN() ); g_test_level=0; + VERIFY( !(numext::isfinite)(m(3)) ); + VERIFY( !(numext::isinf)(m(3)) ); + VERIFY( (numext::isnan)(m(3)) ); + VERIFY( !m.allFinite() ); + VERIFY( m.hasNaN() ); } T hidden_zero = (std::numeric_limits::min)()*(std::numeric_limits::min)(); m(4) /= hidden_zero; @@ -62,29 +62,29 @@ void check_inf_nan(bool dryrun) { } else { - if( (std::isfinite)(m(3))) g_test_level=1; VERIFY( !(numext::isfinite)(m(4)) ); g_test_level=0; - if(!(std::isinf) (m(3))) g_test_level=1; VERIFY( (numext::isinf)(m(4)) ); g_test_level=0; - if( (std::isnan) (m(3))) g_test_level=1; VERIFY( !(numext::isnan)(m(4)) ); g_test_level=0; - if( (std::isfinite)(m(3))) g_test_level=1; VERIFY( !m.allFinite() ); g_test_level=0; - if(!(std::isnan) (m(3))) g_test_level=1; VERIFY( m.hasNaN() ); g_test_level=0; + VERIFY( !(numext::isfinite)(m(4)) ); + VERIFY( (numext::isinf)(m(4)) ); + VERIFY( !(numext::isnan)(m(4)) ); + VERIFY( !m.allFinite() ); + VERIFY( m.hasNaN() ); } m(3) = 0; if(dryrun) { std::cout << "std::isfinite(" << m(3) << ") = "; check((std::isfinite)(m(3)),true); std::cout << " ; numext::isfinite = "; check((numext::isfinite)(m(3)), true); std::cout << "\n"; - std::cout << "std::isinf(" << m(3) << ") = "; check((std::isinf)(m(3)),false); std::cout << " ; numext::isinf = "; check((numext::isinf)(m(3)), false); std::cout << "\n"; - std::cout << "std::isnan(" << m(3) << ") = "; check((std::isnan)(m(3)),false); std::cout << " ; numext::isnan = "; check((numext::isnan)(m(3)), false); std::cout << "\n"; + std::cout << "std::isinf(" << m(3) << ") = "; check((std::isinf)(m(3)),false); std::cout << " ; numext::isinf = "; check((numext::isinf)(m(3)), false); std::cout << "\n"; + std::cout << "std::isnan(" << m(3) << ") = "; check((std::isnan)(m(3)),false); std::cout << " ; numext::isnan = "; check((numext::isnan)(m(3)), false); std::cout << "\n"; std::cout << "allFinite: "; check(m.allFinite(), 0); std::cout << "\n"; std::cout << "hasNaN: "; check(m.hasNaN(), 0); std::cout << "\n"; std::cout << "\n\n"; } else { - if(!(std::isfinite)(m(3))) g_test_level=1; VERIFY( (numext::isfinite)(m(3)) ); g_test_level=0; - if( (std::isinf) (m(3))) g_test_level=1; VERIFY( !(numext::isinf)(m(3)) ); g_test_level=0; - if( (std::isnan) (m(3))) g_test_level=1; VERIFY( !(numext::isnan)(m(3)) ); g_test_level=0; - if( (std::isfinite)(m(3))) g_test_level=1; VERIFY( !m.allFinite() ); g_test_level=0; - if( (std::isnan) (m(3))) g_test_level=1; VERIFY( !m.hasNaN() ); g_test_level=0; + VERIFY( (numext::isfinite)(m(3)) ); + VERIFY( !(numext::isinf)(m(3)) ); + VERIFY( !(numext::isnan)(m(3)) ); + VERIFY( !m.allFinite() ); + VERIFY( !m.hasNaN() ); } } diff --git a/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp b/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp index 4cf51aafb..b64ea3bdc 100644 --- a/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp @@ -15,9 +15,8 @@ #include using namespace std; -// TODO not sure if this is actually still necessary anywhere ... template EIGEN_DONT_INLINE -void kill_extra_precision(T& ) { } +void kill_extra_precision(T& x) { eigen_assert((void*)(&x) != (void*)0); } template void alignedbox(const BoxType& _box) diff --git a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp index 87680f1cc..8ee8fdb27 100644 --- a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp @@ -244,14 +244,6 @@ template void mapQuaternion(void){ // is used to determine wether we can return a coeff by reference or not, which is not enough for Map. //const MCQuaternionUA& cmcq3(mcq3); //VERIFY( &cmcq3.x() == &mcq3.x() ); - - // test cast - { - Quaternion q1f = mq1.template cast(); - VERIFY_IS_APPROX(q1f.template cast(),mq1); - Quaternion q1d = mq1.template cast(); - VERIFY_IS_APPROX(q1d.template cast(),mq1); - } } template void quaternionAlignment(void){ diff --git a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp index 8d064ddc3..278e527c2 100755 --- a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp @@ -612,62 +612,6 @@ template void transform_products() VERIFY_IS_APPROX((ac*p).matrix(), a_m*p_m); } -template void transformations_no_scale() -{ - /* this test covers the following files: - Cross.h Quaternion.h, Transform.h - */ - typedef Matrix Vector3; - typedef Matrix Vector4; - typedef Quaternion Quaternionx; - typedef AngleAxis AngleAxisx; - typedef Transform Transform3; - typedef Translation Translation3; - typedef Matrix Matrix4; - - Vector3 v0 = Vector3::Random(), - v1 = Vector3::Random(); - - Transform3 t0, t1, t2; - - Scalar a = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); - - Quaternionx q1, q2; - - q1 = AngleAxisx(a, v0.normalized()); - - t0 = Transform3::Identity(); - VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); - - t0.setIdentity(); - t1.setIdentity(); - v1 = Vector3::Ones(); - t0.linear() = q1.toRotationMatrix(); - t0.pretranslate(v0); - t1.linear() = q1.conjugate().toRotationMatrix(); - t1.translate(-v0); - - VERIFY((t0 * t1).matrix().isIdentity(test_precision())); - - t1.fromPositionOrientationScale(v0, q1, v1); - VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); - VERIFY_IS_APPROX(t1*v1, t0*v1); - - // translation * vector - t0.setIdentity(); - t0.translate(v0); - VERIFY_IS_APPROX((t0 * v1).template head<3>(), Translation3(v0) * v1); - - // Conversion to matrix. - Transform3 t3; - t3.linear() = q1.toRotationMatrix(); - t3.translation() = v1; - Matrix4 m3 = t3.matrix(); - VERIFY((m3 * m3.inverse()).isIdentity(test_precision())); - // Verify implicit last row is initialized. - VERIFY_IS_APPROX(Vector4(m3.row(3)), Vector4(0.0, 0.0, 0.0, 1.0)); -} - void test_geo_transformations() { for(int i = 0; i < g_repeat; i++) { @@ -681,7 +625,7 @@ void test_geo_transformations() CALL_SUBTEST_3(( transformations() )); CALL_SUBTEST_3(( transformations() )); CALL_SUBTEST_3(( transform_alignment() )); - + CALL_SUBTEST_4(( transformations() )); CALL_SUBTEST_4(( non_projective_only() )); @@ -697,8 +641,5 @@ void test_geo_transformations() CALL_SUBTEST_8(( transform_associativity(Rotation2D(internal::random()*double(EIGEN_PI))) )); CALL_SUBTEST_8(( transform_associativity(Quaterniond::UnitRandom()) )); - - CALL_SUBTEST_9(( transformations_no_scale() )); - CALL_SUBTEST_9(( transformations_no_scale() )); } } diff --git a/gtsam/3rdparty/Eigen/test/inverse.cpp b/gtsam/3rdparty/Eigen/test/inverse.cpp index d81af26c1..be607cc8b 100644 --- a/gtsam/3rdparty/Eigen/test/inverse.cpp +++ b/gtsam/3rdparty/Eigen/test/inverse.cpp @@ -92,22 +92,6 @@ template void inverse(const MatrixType& m) } } -template -void inverse_zerosized() -{ - Matrix A(0,0); - { - Matrix b, x; - x = A.inverse() * b; - } - { - Matrix b(0,1), x; - x = A.inverse() * b; - VERIFY_IS_EQUAL(x.rows(), 0); - VERIFY_IS_EQUAL(x.cols(), 1); - } -} - void test_inverse() { int s = 0; @@ -121,7 +105,6 @@ void test_inverse() s = internal::random(50,320); CALL_SUBTEST_5( inverse(MatrixXf(s,s)) ); TEST_SET_BUT_UNUSED_VARIABLE(s) - CALL_SUBTEST_5( inverse_zerosized() ); s = internal::random(25,100); CALL_SUBTEST_6( inverse(MatrixXcd(s,s)) ); diff --git a/gtsam/3rdparty/Eigen/test/main.h b/gtsam/3rdparty/Eigen/test/main.h index 18bb5c825..8c868ee79 100644 --- a/gtsam/3rdparty/Eigen/test/main.h +++ b/gtsam/3rdparty/Eigen/test/main.h @@ -72,11 +72,6 @@ #define isnan(X) please_protect_your_isnan_with_parentheses #define isinf(X) please_protect_your_isinf_with_parentheses #define isfinite(X) please_protect_your_isfinite_with_parentheses - -// test possible conflicts -struct real {}; -struct imag {}; - #ifdef M_PI #undef M_PI #endif diff --git a/gtsam/3rdparty/Eigen/test/numext.cpp b/gtsam/3rdparty/Eigen/test/numext.cpp index beba9e911..3de33e2f9 100644 --- a/gtsam/3rdparty/Eigen/test/numext.cpp +++ b/gtsam/3rdparty/Eigen/test/numext.cpp @@ -12,7 +12,6 @@ template void check_abs() { typedef typename NumTraits::Real Real; - Real zero(0); if(NumTraits::IsSigned) VERIFY_IS_EQUAL(numext::abs(-T(1)), T(1)); @@ -27,9 +26,9 @@ void check_abs() { if(NumTraits::IsSigned) { VERIFY_IS_EQUAL(numext::abs(x), numext::abs(-x)); - VERIFY( numext::abs(-x) >= zero ); + VERIFY( numext::abs(-x) >= Real(0)); } - VERIFY( numext::abs(x) >= zero ); + VERIFY( numext::abs(x) >= Real(0)); VERIFY_IS_APPROX( numext::abs2(x), numext::abs2(numext::abs(x)) ); } } diff --git a/gtsam/3rdparty/Eigen/test/packetmath.cpp b/gtsam/3rdparty/Eigen/test/packetmath.cpp index 74ac435cf..7821a1738 100644 --- a/gtsam/3rdparty/Eigen/test/packetmath.cpp +++ b/gtsam/3rdparty/Eigen/test/packetmath.cpp @@ -16,6 +16,12 @@ #endif // using namespace Eigen; +#ifdef EIGEN_VECTORIZE_SSE +const bool g_vectorize_sse = true; +#else +const bool g_vectorize_sse = false; +#endif + namespace Eigen { namespace internal { template T negate(const T& x) { return -x; } @@ -242,13 +248,12 @@ template void packetmath() VERIFY(isApproxAbs(ref[0], internal::predux(internal::pload(data1)), refvalue) && "internal::predux"); { - int newsize = PacketSize>4?PacketSize/2:PacketSize; - for (int i=0; i(data1))); - VERIFY(areApprox(ref, data2, newsize) && "internal::predux_downto4"); + VERIFY(areApprox(ref, data2, PacketSize>4?PacketSize/2:PacketSize) && "internal::predux_downto4"); } ref[0] = 1; @@ -299,7 +304,7 @@ template void packetmath() } } - if (PacketTraits::HasBlend) { + if (PacketTraits::HasBlend || g_vectorize_sse) { // pinsertfirst for (int i=0; i void packetmath() VERIFY(areApprox(ref, data2, PacketSize) && "internal::pinsertfirst"); } - if (PacketTraits::HasBlend) { + if (PacketTraits::HasBlend || g_vectorize_sse) { // pinsertlast for (int i=0; i void product(const MatrixType& m) vcres.noalias() -= m1.transpose() * v1; VERIFY_IS_APPROX(vcres, vc2 - m1.transpose() * v1); - // test scaled products - res = square; - res.noalias() = s1 * m1 * m2.transpose(); - VERIFY_IS_APPROX(res, ((s1*m1).eval() * m2.transpose())); - res = square; - res.noalias() += s1 * m1 * m2.transpose(); - VERIFY_IS_APPROX(res, square + ((s1*m1).eval() * m2.transpose())); - res = square; - res.noalias() -= s1 * m1 * m2.transpose(); - VERIFY_IS_APPROX(res, square - ((s1*m1).eval() * m2.transpose())); - // test d ?= a+b*c rules res.noalias() = square + m1 * m2.transpose(); VERIFY_IS_APPROX(res, square + m1 * m2.transpose()); @@ -239,19 +228,4 @@ template void product(const MatrixType& m) VERIFY_IS_APPROX(square * (square*square).conjugate(), square * square.conjugate() * square.conjugate()); } - // destination with a non-default inner-stride - // see bug 1741 - if(!MatrixType::IsRowMajor) - { - typedef Matrix MatrixX; - MatrixX buffer(2*rows,2*rows); - Map > map1(buffer.data(),rows,rows,Stride(2*rows,2)); - buffer.setZero(); - VERIFY_IS_APPROX(map1 = m1 * m2.transpose(), (m1 * m2.transpose()).eval()); - buffer.setZero(); - VERIFY_IS_APPROX(map1.noalias() = m1 * m2.transpose(), (m1 * m2.transpose()).eval()); - buffer.setZero(); - VERIFY_IS_APPROX(map1.noalias() += m1 * m2.transpose(), (m1 * m2.transpose()).eval()); - } - } diff --git a/gtsam/3rdparty/Eigen/test/product_large.cpp b/gtsam/3rdparty/Eigen/test/product_large.cpp index 14a4f739d..845cd40ca 100644 --- a/gtsam/3rdparty/Eigen/test/product_large.cpp +++ b/gtsam/3rdparty/Eigen/test/product_large.cpp @@ -35,8 +35,6 @@ void test_product_large() for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST_1( product(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_2( product(MatrixXd(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); - CALL_SUBTEST_2( product(MatrixXd(internal::random(1,10), internal::random(1,10))) ); - CALL_SUBTEST_3( product(MatrixXi(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_4( product(MatrixXcf(internal::random(1,EIGEN_TEST_MAX_SIZE/2), internal::random(1,EIGEN_TEST_MAX_SIZE/2))) ); CALL_SUBTEST_5( product(Matrix(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); diff --git a/gtsam/3rdparty/Eigen/test/product_mmtr.cpp b/gtsam/3rdparty/Eigen/test/product_mmtr.cpp index 35686460c..d3e24b012 100644 --- a/gtsam/3rdparty/Eigen/test/product_mmtr.cpp +++ b/gtsam/3rdparty/Eigen/test/product_mmtr.cpp @@ -82,16 +82,6 @@ template void mmtr(int size) ref2.template triangularView() = ref1.template triangularView(); matc.template triangularView() = sqc * matc * sqc.adjoint(); VERIFY_IS_APPROX(matc, ref2); - - // destination with a non-default inner-stride - // see bug 1741 - { - typedef Matrix MatrixX; - MatrixX buffer(2*size,2*size); - Map > map1(buffer.data(),size,size,Stride(2*size,2)); - buffer.setZero(); - CHECK_MMTR(map1, Lower, = s*soc*sor.adjoint()); - } } void test_product_mmtr() diff --git a/gtsam/3rdparty/Eigen/test/product_symm.cpp b/gtsam/3rdparty/Eigen/test/product_symm.cpp index 0ed027dff..7d1042a4f 100644 --- a/gtsam/3rdparty/Eigen/test/product_symm.cpp +++ b/gtsam/3rdparty/Eigen/test/product_symm.cpp @@ -75,12 +75,12 @@ template void symm(int size = Size, in rhs13 = (s1*m1.adjoint()) * (s2*rhs2.adjoint())); // test row major = <...> - m2 = m1.template triangularView(); rhs32.setRandom(); rhs13 = rhs32; - VERIFY_IS_APPROX(rhs32.noalias() -= (s1*m2).template selfadjointView() * (s2*rhs3), + m2 = m1.template triangularView(); rhs12.setRandom(); rhs13 = rhs12; + VERIFY_IS_APPROX(rhs12 -= (s1*m2).template selfadjointView() * (s2*rhs3), rhs13 -= (s1*m1) * (s2 * rhs3)); m2 = m1.template triangularView(); - VERIFY_IS_APPROX(rhs32.noalias() = (s1*m2.adjoint()).template selfadjointView() * (s2*rhs3).conjugate(), + VERIFY_IS_APPROX(rhs12 = (s1*m2.adjoint()).template selfadjointView() * (s2*rhs3).conjugate(), rhs13 = (s1*m1.adjoint()) * (s2*rhs3).conjugate()); @@ -92,20 +92,6 @@ template void symm(int size = Size, in VERIFY_IS_APPROX(rhs22 = (rhs2) * (m2).template selfadjointView(), rhs23 = (rhs2) * (m1)); VERIFY_IS_APPROX(rhs22 = (s2*rhs2) * (s1*m2).template selfadjointView(), rhs23 = (s2*rhs2) * (s1*m1)); - // destination with a non-default inner-stride - // see bug 1741 - { - typedef Matrix MatrixX; - MatrixX buffer(2*cols,2*othersize); - Map > map1(buffer.data(),cols,othersize,Stride(2*rows,2)); - buffer.setZero(); - VERIFY_IS_APPROX( map1.noalias() = (s1*m2).template selfadjointView() * (s2*rhs1), - rhs13 = (s1*m1) * (s2*rhs1)); - - Map > map2(buffer.data(),rhs22.rows(),rhs22.cols(),Stride(2*rhs22.outerStride(),2)); - buffer.setZero(); - VERIFY_IS_APPROX(map2 = (rhs2) * (m2).template selfadjointView(), rhs23 = (rhs2) * (m1)); - } } void test_product_symm() diff --git a/gtsam/3rdparty/Eigen/test/product_syrk.cpp b/gtsam/3rdparty/Eigen/test/product_syrk.cpp index b8578215f..3ebbe14ca 100644 --- a/gtsam/3rdparty/Eigen/test/product_syrk.cpp +++ b/gtsam/3rdparty/Eigen/test/product_syrk.cpp @@ -115,17 +115,6 @@ template void syrk(const MatrixType& m) m2.setZero(); VERIFY_IS_APPROX((m2.template selfadjointView().rankUpdate(m1.row(c).adjoint(),s1)._expression()), ((s1 * m1.row(c).adjoint() * m1.row(c).adjoint().adjoint()).eval().template triangularView().toDenseMatrix())); - - // destination with a non-default inner-stride - // see bug 1741 - { - typedef Matrix MatrixX; - MatrixX buffer(2*rows,2*cols); - Map > map1(buffer.data(),rows,cols,Stride(2*rows,2)); - buffer.setZero(); - VERIFY_IS_APPROX((map1.template selfadjointView().rankUpdate(rhs2,s1)._expression()), - ((s1 * rhs2 * rhs2.adjoint()).eval().template triangularView().toDenseMatrix())); - } } void test_product_syrk() diff --git a/gtsam/3rdparty/Eigen/test/product_trmm.cpp b/gtsam/3rdparty/Eigen/test/product_trmm.cpp index ddcde9622..e08d9f39f 100644 --- a/gtsam/3rdparty/Eigen/test/product_trmm.cpp +++ b/gtsam/3rdparty/Eigen/test/product_trmm.cpp @@ -76,18 +76,8 @@ void trmm(int rows=get_random_size(), VERIFY_IS_APPROX( ge_xs = (s1*mat).adjoint().template triangularView() * ge_left.adjoint(), numext::conj(s1) * triTr.conjugate() * ge_left.adjoint()); VERIFY_IS_APPROX( ge_xs = (s1*mat).transpose().template triangularView() * ge_left.adjoint(), s1triTr * ge_left.adjoint()); + // TODO check with sub-matrix expressions ? - - // destination with a non-default inner-stride - // see bug 1741 - { - VERIFY_IS_APPROX( ge_xs.noalias() = mat.template triangularView() * ge_right, tri * ge_right); - typedef Matrix MatrixX; - MatrixX buffer(2*ge_xs.rows(),2*ge_xs.cols()); - Map > map1(buffer.data(),ge_xs.rows(),ge_xs.cols(),Stride(2*ge_xs.outerStride(),2)); - buffer.setZero(); - VERIFY_IS_APPROX( map1.noalias() = mat.template triangularView() * ge_right, tri * ge_right); - } } template diff --git a/gtsam/3rdparty/Eigen/test/product_trsolve.cpp b/gtsam/3rdparty/Eigen/test/product_trsolve.cpp index eaf62cb11..4b97fa9d6 100644 --- a/gtsam/3rdparty/Eigen/test/product_trsolve.cpp +++ b/gtsam/3rdparty/Eigen/test/product_trsolve.cpp @@ -71,32 +71,6 @@ template void trsolve(int size=Size,int cols int c = internal::random(0,cols-1); VERIFY_TRSM(rmLhs.template triangularView(), rmRhs.col(c)); VERIFY_TRSM(cmLhs.template triangularView(), rmRhs.col(c)); - - // destination with a non-default inner-stride - // see bug 1741 - { - typedef Matrix MatrixX; - MatrixX buffer(2*cmRhs.rows(),2*cmRhs.cols()); - Map,0,Stride > map1(buffer.data(),cmRhs.rows(),cmRhs.cols(),Stride(2*cmRhs.outerStride(),2)); - Map,0,Stride > map2(buffer.data(),rmRhs.rows(),rmRhs.cols(),Stride(2*rmRhs.outerStride(),2)); - buffer.setZero(); - VERIFY_TRSM(cmLhs.conjugate().template triangularView(), map1); - buffer.setZero(); - VERIFY_TRSM(cmLhs .template triangularView(), map2); - } - - if(Size==Dynamic) - { - cmLhs.resize(0,0); - cmRhs.resize(0,cmRhs.cols()); - Matrix res = cmLhs.template triangularView().solve(cmRhs); - VERIFY_IS_EQUAL(res.rows(),0); - VERIFY_IS_EQUAL(res.cols(),cmRhs.cols()); - res = cmRhs; - cmLhs.template triangularView().solveInPlace(res); - VERIFY_IS_EQUAL(res.rows(),0); - VERIFY_IS_EQUAL(res.cols(),cmRhs.cols()); - } } void test_product_trsolve() diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index da399e287..704495aff 100644 --- a/gtsam/3rdparty/Eigen/test/ref.cpp +++ b/gtsam/3rdparty/Eigen/test/ref.cpp @@ -102,14 +102,10 @@ template void ref_vector(const VectorType& m) Index i = internal::random(0,size-1); Index bsize = internal::random(1,size-i); - { RefMat rm0 = v1; VERIFY_IS_EQUAL(rm0, v1); } - { RefMat rm0 = v1.block(0,0,size,1); VERIFY_IS_EQUAL(rm0, v1); } - { RefDynMat rv1 = v1; VERIFY_IS_EQUAL(rv1, v1); } - { RefDynMat rv1 = v1.block(0,0,size,1); VERIFY_IS_EQUAL(rv1, v1); } - { VERIFY_RAISES_ASSERT( RefMat rm0 = v1.block(0, 0, size, 0); EIGEN_UNUSED_VARIABLE(rm0); ); } - if(VectorType::SizeAtCompileTime!=1) - { VERIFY_RAISES_ASSERT( RefDynMat rv1 = v1.block(0, 0, size, 0); EIGEN_UNUSED_VARIABLE(rv1); ); } - + RefMat rm0 = v1; + VERIFY_IS_EQUAL(rm0, v1); + RefDynMat rv1 = v1; + VERIFY_IS_EQUAL(rv1, v1); RefDynMat rv2 = v1.segment(i,bsize); VERIFY_IS_EQUAL(rv2, v1.segment(i,bsize)); rv2.setOnes(); diff --git a/gtsam/3rdparty/Eigen/test/rvalue_types.cpp b/gtsam/3rdparty/Eigen/test/rvalue_types.cpp index 6a97dae34..8887f1b1b 100644 --- a/gtsam/3rdparty/Eigen/test/rvalue_types.cpp +++ b/gtsam/3rdparty/Eigen/test/rvalue_types.cpp @@ -7,8 +7,6 @@ // 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/. -#define EIGEN_RUNTIME_NO_MALLOC - #include "main.h" #include @@ -26,85 +24,41 @@ void rvalue_copyassign(const MatrixType& m) MatrixType tmp = m; UIntPtr src_address = reinterpret_cast(tmp.data()); - Eigen::internal::set_is_malloc_allowed(false); // moving from an rvalue reference shall never allocate // move the temporary to n MatrixType n = std::move(tmp); UIntPtr dst_address = reinterpret_cast(n.data()); + if (MatrixType::RowsAtCompileTime==Dynamic|| MatrixType::ColsAtCompileTime==Dynamic) { // verify that we actually moved the guts VERIFY_IS_EQUAL(src_address, dst_address); - VERIFY_IS_EQUAL(tmp.size(), 0); - VERIFY_IS_EQUAL(reinterpret_cast(tmp.data()), UIntPtr(0)); } // verify that the content did not change Scalar abs_diff = (m-n).array().abs().sum(); VERIFY_IS_EQUAL(abs_diff, Scalar(0)); - Eigen::internal::set_is_malloc_allowed(true); -} -template -void rvalue_transpositions(Index rows) -{ - typedef typename TranspositionsType::IndicesType PermutationVectorType; - - PermutationVectorType vec; - randomPermutationVector(vec, rows); - TranspositionsType t0(vec); - - Eigen::internal::set_is_malloc_allowed(false); // moving from an rvalue reference shall never allocate - - UIntPtr t0_address = reinterpret_cast(t0.indices().data()); - - // Move constructors: - TranspositionsType t1 = std::move(t0); - UIntPtr t1_address = reinterpret_cast(t1.indices().data()); - VERIFY_IS_EQUAL(t0_address, t1_address); - // t0 must be de-allocated: - VERIFY_IS_EQUAL(t0.size(), 0); - VERIFY_IS_EQUAL(reinterpret_cast(t0.indices().data()), UIntPtr(0)); - - - // Move assignment: - t0 = std::move(t1); - t0_address = reinterpret_cast(t0.indices().data()); - VERIFY_IS_EQUAL(t0_address, t1_address); - // t1 must be de-allocated: - VERIFY_IS_EQUAL(t1.size(), 0); - VERIFY_IS_EQUAL(reinterpret_cast(t1.indices().data()), UIntPtr(0)); - - Eigen::internal::set_is_malloc_allowed(true); } #else template void rvalue_copyassign(const MatrixType&) {} -template -void rvalue_transpositions(Index) {} #endif void test_rvalue_types() { - for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1(rvalue_copyassign( MatrixXf::Random(50,50).eval() )); - CALL_SUBTEST_1(rvalue_copyassign( ArrayXXf::Random(50,50).eval() )); + CALL_SUBTEST_1(rvalue_copyassign( MatrixXf::Random(50,50).eval() )); + CALL_SUBTEST_1(rvalue_copyassign( ArrayXXf::Random(50,50).eval() )); - CALL_SUBTEST_1(rvalue_copyassign( Matrix::Random(50).eval() )); - CALL_SUBTEST_1(rvalue_copyassign( Array::Random(50).eval() )); + CALL_SUBTEST_1(rvalue_copyassign( Matrix::Random(50).eval() )); + CALL_SUBTEST_1(rvalue_copyassign( Array::Random(50).eval() )); - CALL_SUBTEST_1(rvalue_copyassign( Matrix::Random(50).eval() )); - CALL_SUBTEST_1(rvalue_copyassign( Array::Random(50).eval() )); - - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); - - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); - CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + CALL_SUBTEST_1(rvalue_copyassign( Matrix::Random(50).eval() )); + CALL_SUBTEST_1(rvalue_copyassign( Array::Random(50).eval() )); - CALL_SUBTEST_3((rvalue_transpositions >(internal::random(1,EIGEN_TEST_MAX_SIZE)))); - CALL_SUBTEST_3((rvalue_transpositions >(internal::random(1,EIGEN_TEST_MAX_SIZE)))); - CALL_SUBTEST_4((rvalue_transpositions >(internal::random(1,EIGEN_TEST_MAX_SIZE)))); - CALL_SUBTEST_4((rvalue_transpositions >(internal::random(1,EIGEN_TEST_MAX_SIZE)))); - } + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); + CALL_SUBTEST_2(rvalue_copyassign( Array::Random().eval() )); } diff --git a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp index 43318da79..d0ef722b6 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp @@ -612,14 +612,6 @@ template void sparse_basic(const SparseMatrixType& re iters[0] = IteratorType(m2,0); iters[1] = IteratorType(m2,m2.outerSize()-1); } - - // test reserve with empty rows/columns - { - SparseMatrixType m1(0,cols); - m1.reserve(ArrayXi::Constant(m1.outerSize(),1)); - SparseMatrixType m2(rows,0); - m2.reserve(ArrayXi::Constant(m2.outerSize(),1)); - } } diff --git a/gtsam/3rdparty/Eigen/test/stddeque.cpp b/gtsam/3rdparty/Eigen/test/stddeque.cpp index b6955f747..b511c4e61 100644 --- a/gtsam/3rdparty/Eigen/test/stddeque.cpp +++ b/gtsam/3rdparty/Eigen/test/stddeque.cpp @@ -18,7 +18,7 @@ void check_stddeque_matrix(const MatrixType& m) Index rows = m.rows(); Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::deque > v(10, MatrixType::Zero(rows,cols)), w(20, y); + std::deque > v(10, MatrixType(rows,cols)), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -33,7 +33,7 @@ void check_stddeque_matrix(const MatrixType& m) ++wi; } - v.resize(21,MatrixType::Zero(rows,cols)); + v.resize(21); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); @@ -46,8 +46,8 @@ template void check_stddeque_transform(const TransformType&) { typedef typename TransformType::MatrixType MatrixType; - TransformType x(MatrixType::Random()), y(MatrixType::Random()), ti=TransformType::Identity(); - std::deque > v(10,ti), w(20, y); + TransformType x(MatrixType::Random()), y(MatrixType::Random()); + std::deque > v(10), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -62,7 +62,7 @@ void check_stddeque_transform(const TransformType&) ++wi; } - v.resize(21,ti); + v.resize(21); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); @@ -75,8 +75,8 @@ template void check_stddeque_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); - std::deque > v(10,qi), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); + std::deque > v(10), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -91,7 +91,7 @@ void check_stddeque_quaternion(const QuaternionType&) ++wi; } - v.resize(21,qi); + v.resize(21); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); diff --git a/gtsam/3rdparty/Eigen/test/stddeque_overload.cpp b/gtsam/3rdparty/Eigen/test/stddeque_overload.cpp index f495b5a04..4da618bbf 100644 --- a/gtsam/3rdparty/Eigen/test/stddeque_overload.cpp +++ b/gtsam/3rdparty/Eigen/test/stddeque_overload.cpp @@ -31,7 +31,7 @@ void check_stddeque_matrix(const MatrixType& m) typename MatrixType::Index rows = m.rows(); typename MatrixType::Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::deque v(10, MatrixType::Zero(rows,cols)), w(20, y); + std::deque v(10, MatrixType(rows,cols)), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); @@ -64,8 +64,8 @@ template void check_stddeque_transform(const TransformType&) { typedef typename TransformType::MatrixType MatrixType; - TransformType x(MatrixType::Random()), y(MatrixType::Random()), ti=TransformType::Identity(); - std::deque v(10,ti), w(20, y); + TransformType x(MatrixType::Random()), y(MatrixType::Random()); + std::deque v(10), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); @@ -75,7 +75,7 @@ void check_stddeque_transform(const TransformType&) VERIFY_IS_APPROX(w[i], v[i]); } - v.resize(21,ti); + v.resize(21); v[20] = x; VERIFY_IS_APPROX(v[20], x); v.resize(22,y); @@ -98,8 +98,8 @@ template void check_stddeque_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); - std::deque v(10,qi), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); + std::deque v(10), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); @@ -109,7 +109,7 @@ void check_stddeque_quaternion(const QuaternionType&) VERIFY_IS_APPROX(w[i], v[i]); } - v.resize(21,qi); + v.resize(21); v[20] = x; VERIFY_IS_APPROX(v[20], x); v.resize(22,y); diff --git a/gtsam/3rdparty/Eigen/test/stdlist.cpp b/gtsam/3rdparty/Eigen/test/stdlist.cpp index 23b30ccaf..23cbe9039 100644 --- a/gtsam/3rdparty/Eigen/test/stdlist.cpp +++ b/gtsam/3rdparty/Eigen/test/stdlist.cpp @@ -18,7 +18,7 @@ void check_stdlist_matrix(const MatrixType& m) Index rows = m.rows(); Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::list > v(10, MatrixType::Zero(rows,cols)), w(20, y); + std::list > v(10, MatrixType(rows,cols)), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -33,7 +33,7 @@ void check_stdlist_matrix(const MatrixType& m) ++wi; } - v.resize(21, MatrixType::Zero(rows,cols)); + v.resize(21); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); @@ -46,8 +46,8 @@ template void check_stdlist_transform(const TransformType&) { typedef typename TransformType::MatrixType MatrixType; - TransformType x(MatrixType::Random()), y(MatrixType::Random()), ti=TransformType::Identity(); - std::list > v(10,ti), w(20, y); + TransformType x(MatrixType::Random()), y(MatrixType::Random()); + std::list > v(10), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -62,7 +62,7 @@ void check_stdlist_transform(const TransformType&) ++wi; } - v.resize(21, ti); + v.resize(21); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); @@ -75,8 +75,8 @@ template void check_stdlist_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); - std::list > v(10,qi), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); + std::list > v(10), w(20, y); v.front() = x; w.front() = w.back(); VERIFY_IS_APPROX(w.front(), w.back()); @@ -91,7 +91,7 @@ void check_stdlist_quaternion(const QuaternionType&) ++wi; } - v.resize(21,qi); + v.resize(21); v.back() = x; VERIFY_IS_APPROX(v.back(), x); v.resize(22,y); diff --git a/gtsam/3rdparty/Eigen/test/stdlist_overload.cpp b/gtsam/3rdparty/Eigen/test/stdlist_overload.cpp index aea7a2846..bb910bd43 100644 --- a/gtsam/3rdparty/Eigen/test/stdlist_overload.cpp +++ b/gtsam/3rdparty/Eigen/test/stdlist_overload.cpp @@ -47,7 +47,7 @@ void check_stdlist_matrix(const MatrixType& m) typename MatrixType::Index rows = m.rows(); typename MatrixType::Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::list v(10, MatrixType::Zero(rows,cols)), w(20, y); + std::list v(10, MatrixType(rows,cols)), w(20, y); typename std::list::iterator itv = get(v, 5); typename std::list::iterator itw = get(w, 6); *itv = x; @@ -86,8 +86,8 @@ template void check_stdlist_transform(const TransformType&) { typedef typename TransformType::MatrixType MatrixType; - TransformType x(MatrixType::Random()), y(MatrixType::Random()), ti=TransformType::Identity(); - std::list v(10,ti), w(20, y); + TransformType x(MatrixType::Random()), y(MatrixType::Random()); + std::list v(10), w(20, y); typename std::list::iterator itv = get(v, 5); typename std::list::iterator itw = get(w, 6); *itv = x; @@ -103,7 +103,7 @@ void check_stdlist_transform(const TransformType&) ++itw; } - v.resize(21, ti); + v.resize(21); set(v, 20, x); VERIFY_IS_APPROX(*get(v, 20), x); v.resize(22,y); @@ -126,8 +126,8 @@ template void check_stdlist_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); - std::list v(10,qi), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); + std::list v(10), w(20, y); typename std::list::iterator itv = get(v, 5); typename std::list::iterator itw = get(w, 6); *itv = x; @@ -143,7 +143,7 @@ void check_stdlist_quaternion(const QuaternionType&) ++itw; } - v.resize(21,qi); + v.resize(21); set(v, 20, x); VERIFY_IS_APPROX(*get(v, 20), x); v.resize(22,y); diff --git a/gtsam/3rdparty/Eigen/test/stdvector.cpp b/gtsam/3rdparty/Eigen/test/stdvector.cpp index 383d9a509..fa928ea4f 100644 --- a/gtsam/3rdparty/Eigen/test/stdvector.cpp +++ b/gtsam/3rdparty/Eigen/test/stdvector.cpp @@ -17,7 +17,7 @@ void check_stdvector_matrix(const MatrixType& m) typename MatrixType::Index rows = m.rows(); typename MatrixType::Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::vector > v(10, MatrixType::Zero(rows,cols)), w(20, y); + std::vector > v(10, MatrixType(rows,cols)), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); @@ -86,8 +86,8 @@ template void check_stdvector_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); - std::vector > v(10,qi), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); + std::vector > v(10), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); diff --git a/gtsam/3rdparty/Eigen/test/stdvector_overload.cpp b/gtsam/3rdparty/Eigen/test/stdvector_overload.cpp index 637e3ef52..959665954 100644 --- a/gtsam/3rdparty/Eigen/test/stdvector_overload.cpp +++ b/gtsam/3rdparty/Eigen/test/stdvector_overload.cpp @@ -31,7 +31,7 @@ void check_stdvector_matrix(const MatrixType& m) typename MatrixType::Index rows = m.rows(); typename MatrixType::Index cols = m.cols(); MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols); - std::vector v(10, MatrixType::Zero(rows,cols)), w(20, y); + std::vector v(10, MatrixType(rows,cols)), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); @@ -100,8 +100,8 @@ template void check_stdvector_quaternion(const QuaternionType&) { typedef typename QuaternionType::Coefficients Coefficients; - QuaternionType x(Coefficients::Random()), y(Coefficients::Random()), qi=QuaternionType::Identity(); - std::vector v(10,qi), w(20, y); + QuaternionType x(Coefficients::Random()), y(Coefficients::Random()); + std::vector v(10), w(20, y); v[5] = x; w[6] = v[5]; VERIFY_IS_APPROX(w[6], v[5]); diff --git a/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp b/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp index c2f77bfec..37e7495f5 100644 --- a/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp +++ b/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp @@ -22,14 +22,6 @@ #include "main.h" #include -// Disable "ignoring attributes on template argument" -// for packet_traits -// => The only workaround would be to wrap _m128 and the likes -// within wrappers. -#if EIGEN_GNUC_AT_LEAST(6,0) - #pragma GCC diagnostic ignored "-Wignored-attributes" -#endif - using internal::demangle_flags; using internal::demangle_traversal; using internal::demangle_unrolling; diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/ArpackSupport b/gtsam/3rdparty/Eigen/unsupported/Eigen/ArpackSupport index a0d4820e1..37a2799ef 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/ArpackSupport +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/ArpackSupport @@ -11,6 +11,8 @@ #include +#include + /** \defgroup ArpackSupport_Module Arpack support module * * This module provides a wrapper to Arpack, a library for sparse eigenvalue decomposition. @@ -21,8 +23,6 @@ */ #include - -#include #include "src/Eigenvalues/ArpackSelfAdjointEigenSolver.h" #include diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h index c28a10dd4..9b2cb3ff6 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h @@ -113,7 +113,6 @@ class SimpleTensorContractionMapper { EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index computeIndex(Index row, Index col) const { const bool left = (side == Lhs); - EIGEN_UNUSED_VARIABLE(left); // annoying bug in g++8.1: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=85963 Index nocontract_val = left ? row : col; Index linidx = 0; for (int i = static_cast(array_size::value) - 1; i > 0; i--) { @@ -152,7 +151,6 @@ class SimpleTensorContractionMapper { EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE IndexPair computeIndexPair(Index row, Index col, const Index distance) const { const bool left = (side == Lhs); - EIGEN_UNUSED_VARIABLE(left); // annoying bug in g++8.1: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=85963 Index nocontract_val[2] = {left ? row : col, left ? row + distance : col}; Index linidx[2] = {0, 0}; if (array_size::value > array_size::value) { diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h index a5e084a24..17f04665a 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h @@ -31,7 +31,7 @@ class Barrier { eigen_assert(((count << 1) >> 1) == count); } ~Barrier() { - eigen_plain_assert((state_>>1) == 0); + eigen_assert((state_>>1) == 0); } void Notify() { diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/EventCount.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/EventCount.h index 4749d6240..71d55552d 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/EventCount.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/EventCount.h @@ -58,7 +58,7 @@ class EventCount { ~EventCount() { // Ensure there are no waiters. - eigen_plain_assert((state_.load() & (kStackMask | kWaiterMask)) == kStackMask); + eigen_assert((state_.load() & (kStackMask | kWaiterMask)) == kStackMask); } // Prewait prepares for waiting. diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/RunQueue.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/RunQueue.h index 6e505fc14..05ed76cbe 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/RunQueue.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/ThreadPool/RunQueue.h @@ -47,7 +47,7 @@ class RunQueue { array_[i].state.store(kEmpty, std::memory_order_relaxed); } - ~RunQueue() { eigen_plain_assert(Size() == 0); } + ~RunQueue() { eigen_assert(Size() == 0); } // PushFront inserts w at the beginning of the queue. // If queue is full returns w, otherwise returns default-constructed Work. diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/Polynomials b/gtsam/3rdparty/Eigen/unsupported/Eigen/Polynomials index 334b03142..cece56337 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/Polynomials +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/Polynomials @@ -11,10 +11,10 @@ #include -#include - #include +#include + // Note that EIGEN_HIDE_HEAVY_CODE has to be defined per module #if (defined EIGEN_EXTERN_INSTANTIATIONS) && (EIGEN_EXTERN_INSTANTIATIONS>=2) #ifndef EIGEN_HIDE_HEAVY_CODE diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h index 58f3f3319..2f50e9968 100755 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h @@ -453,24 +453,6 @@ struct auto_diff_special_op<_DerType, false> void operator+() const; }; -template -void make_coherent_expression(CwiseBinaryOp xpr, const RefType &ref) -{ - make_coherent(xpr.const_cast_derived().lhs(), ref); - make_coherent(xpr.const_cast_derived().rhs(), ref); -} - -template -void make_coherent_expression(const CwiseUnaryOp &xpr, const RefType &ref) -{ - make_coherent(xpr.nestedExpression().const_cast_derived(), ref); -} - -// needed for compilation only -template -void make_coherent_expression(const CwiseNullaryOp &, const RefType &) -{} - template struct make_coherent_impl, B> { typedef Matrix A; @@ -480,10 +462,6 @@ struct make_coherent_impl struct make_coherent_impl, - Matrix > { + Matrix > { typedef Matrix A; typedef Matrix B; static void run(A& a, B& b) { diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h index 4170d26b6..866a8a460 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h @@ -3,9 +3,24 @@ // // Copyright (C) 2012 David Harmon // -// 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/. +// Eigen is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 3 of the License, or (at your option) any later version. +// +// Alternatively, you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of +// the License, or (at your option) any later version. +// +// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License and a copy of the GNU General Public License along with +// Eigen. If not, see . #ifndef EIGEN_ARPACKGENERALIZEDSELFADJOINTEIGENSOLVER_H #define EIGEN_ARPACKGENERALIZEDSELFADJOINTEIGENSOLVER_H diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_fftw_impl.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_fftw_impl.h index 7c1f716e2..d49aa17f5 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_fftw_impl.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_fftw_impl.h @@ -231,8 +231,6 @@ namespace internal { protected: typedef fftw_plan PlanData; - typedef Eigen::numext::int64_t int64_t; - typedef std::map PlanMap; PlanMap m_plans; diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h index 0b0ee6546..e5ebbcf23 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h @@ -412,7 +412,7 @@ template struct MatrixExponentialReturnValue inline void evalTo(ResultType& result) const { const typename internal::nested_eval::type tmp(m_src); - internal::matrix_exp_compute(tmp, result, internal::is_exp_known_type()); + internal::matrix_exp_compute(tmp, result, internal::is_exp_known_type()); } Index rows() const { return m_src.rows(); } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h index 9de0c3574..2e5abda38 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h @@ -253,19 +253,18 @@ struct matrix_sqrt_compute template struct matrix_sqrt_compute { - typedef typename MatrixType::PlainObject PlainType; template static void run(const MatrixType &arg, ResultType &result) { eigen_assert(arg.rows() == arg.cols()); // Compute Schur decomposition of arg - const RealSchur schurOfA(arg); - const PlainType& T = schurOfA.matrixT(); - const PlainType& U = schurOfA.matrixU(); + const RealSchur schurOfA(arg); + const MatrixType& T = schurOfA.matrixT(); + const MatrixType& U = schurOfA.matrixU(); // Compute square root of T - PlainType sqrtT = PlainType::Zero(arg.rows(), arg.cols()); + MatrixType sqrtT = MatrixType::Zero(arg.rows(), arg.cols()); matrix_sqrt_quasi_triangular(T, sqrtT); // Compute square root of arg @@ -279,19 +278,18 @@ struct matrix_sqrt_compute template struct matrix_sqrt_compute { - typedef typename MatrixType::PlainObject PlainType; template static void run(const MatrixType &arg, ResultType &result) { eigen_assert(arg.rows() == arg.cols()); // Compute Schur decomposition of arg - const ComplexSchur schurOfA(arg); - const PlainType& T = schurOfA.matrixT(); - const PlainType& U = schurOfA.matrixU(); + const ComplexSchur schurOfA(arg); + const MatrixType& T = schurOfA.matrixT(); + const MatrixType& U = schurOfA.matrixU(); // Compute square root of T - PlainType sqrtT; + MatrixType sqrtT; matrix_sqrt_triangular(T, sqrtT); // Compute square root of arg diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/Companion.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/Companion.h index 359836cac..b515c2920 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/Companion.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/Companion.h @@ -75,7 +75,8 @@ class companion void setPolynomial( const VectorType& poly ) { const Index deg = poly.size()-1; - m_monic = -poly.head(deg)/poly[deg]; + m_monic = -1/poly[deg] * poly.head(deg); + //m_bl_diag.setIdentity( deg-1 ); m_bl_diag.setOnes(deg-1); } @@ -88,13 +89,13 @@ class companion { const Index deg = m_monic.size(); const Index deg_1 = deg-1; - DenseCompanionMatrixType companMat(deg,deg); - companMat << + DenseCompanionMatrixType companion(deg,deg); + companion << ( LeftBlock(deg,deg_1) << LeftBlockFirstRow::Zero(1,deg_1), BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished() , m_monic; - return companMat; + return companion; } @@ -106,8 +107,8 @@ class companion * colB and rowB are repectively the multipliers for * the column and the row in order to balance them. * */ - bool balanced( RealScalar colNorm, RealScalar rowNorm, - bool& isBalanced, RealScalar& colB, RealScalar& rowB ); + bool balanced( Scalar colNorm, Scalar rowNorm, + bool& isBalanced, Scalar& colB, Scalar& rowB ); /** Helper function for the balancing algorithm. * \returns true if the row and the column, having colNorm and rowNorm @@ -115,8 +116,8 @@ class companion * colB and rowB are repectively the multipliers for * the column and the row in order to balance them. * */ - bool balancedR( RealScalar colNorm, RealScalar rowNorm, - bool& isBalanced, RealScalar& colB, RealScalar& rowB ); + bool balancedR( Scalar colNorm, Scalar rowNorm, + bool& isBalanced, Scalar& colB, Scalar& rowB ); public: /** @@ -138,10 +139,10 @@ class companion template< typename _Scalar, int _Deg > inline -bool companion<_Scalar,_Deg>::balanced( RealScalar colNorm, RealScalar rowNorm, - bool& isBalanced, RealScalar& colB, RealScalar& rowB ) +bool companion<_Scalar,_Deg>::balanced( Scalar colNorm, Scalar rowNorm, + bool& isBalanced, Scalar& colB, Scalar& rowB ) { - if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){ return true; } + if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; } else { //To find the balancing coefficients, if the radix is 2, @@ -149,29 +150,29 @@ bool companion<_Scalar,_Deg>::balanced( RealScalar colNorm, RealScalar rowNorm, // \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$ // then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$ // and the balancing coefficient for the column is \f$ 2^{\sigma} \f$ - rowB = rowNorm / radix(); - colB = RealScalar(1); - const RealScalar s = colNorm + rowNorm; + rowB = rowNorm / radix(); + colB = Scalar(1); + const Scalar s = colNorm + rowNorm; while (colNorm < rowB) { - colB *= radix(); - colNorm *= radix2(); + colB *= radix(); + colNorm *= radix2(); } - rowB = rowNorm * radix(); + rowB = rowNorm * radix(); while (colNorm >= rowB) { - colB /= radix(); - colNorm /= radix2(); + colB /= radix(); + colNorm /= radix2(); } //This line is used to avoid insubstantial balancing - if ((rowNorm + colNorm) < RealScalar(0.95) * s * colB) + if ((rowNorm + colNorm) < Scalar(0.95) * s * colB) { isBalanced = false; - rowB = RealScalar(1) / colB; + rowB = Scalar(1) / colB; return false; } else{ @@ -181,21 +182,21 @@ bool companion<_Scalar,_Deg>::balanced( RealScalar colNorm, RealScalar rowNorm, template< typename _Scalar, int _Deg > inline -bool companion<_Scalar,_Deg>::balancedR( RealScalar colNorm, RealScalar rowNorm, - bool& isBalanced, RealScalar& colB, RealScalar& rowB ) +bool companion<_Scalar,_Deg>::balancedR( Scalar colNorm, Scalar rowNorm, + bool& isBalanced, Scalar& colB, Scalar& rowB ) { - if( RealScalar(0) == colNorm || RealScalar(0) == rowNorm ){ return true; } + if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; } else { /** * Set the norm of the column and the row to the geometric mean * of the row and column norm */ - const RealScalar q = colNorm/rowNorm; + const _Scalar q = colNorm/rowNorm; if( !isApprox( q, _Scalar(1) ) ) { rowB = sqrt( colNorm/rowNorm ); - colB = RealScalar(1)/rowB; + colB = Scalar(1)/rowB; isBalanced = false; return false; @@ -218,8 +219,8 @@ void companion<_Scalar,_Deg>::balance() while( !hasConverged ) { hasConverged = true; - RealScalar colNorm,rowNorm; - RealScalar colB,rowB; + Scalar colNorm,rowNorm; + Scalar colB,rowB; //First row, first column excluding the diagonal //============================================== diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialSolver.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialSolver.h index 5e0ecbb43..03198ec8e 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialSolver.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialSolver.h @@ -99,7 +99,7 @@ class PolynomialSolverBase */ inline const RootType& greatestRoot() const { - std::greater greater; + std::greater greater; return selectComplexRoot_withRespectToNorm( greater ); } @@ -108,7 +108,7 @@ class PolynomialSolverBase */ inline const RootType& smallestRoot() const { - std::less less; + std::less less; return selectComplexRoot_withRespectToNorm( less ); } @@ -126,7 +126,7 @@ class PolynomialSolverBase for( Index i=0; i::dummy_precision() ) const { - std::greater greater; + std::greater greater; return selectRealRoot_withRespectToAbsRealPart( greater, hasArealRoot, absImaginaryThreshold ); } @@ -236,7 +236,7 @@ class PolynomialSolverBase bool& hasArealRoot, const RealScalar& absImaginaryThreshold = NumTraits::dummy_precision() ) const { - std::less less; + std::less less; return selectRealRoot_withRespectToAbsRealPart( less, hasArealRoot, absImaginaryThreshold ); } @@ -259,7 +259,7 @@ class PolynomialSolverBase bool& hasArealRoot, const RealScalar& absImaginaryThreshold = NumTraits::dummy_precision() ) const { - std::greater greater; + std::greater greater; return selectRealRoot_withRespectToRealPart( greater, hasArealRoot, absImaginaryThreshold ); } @@ -282,7 +282,7 @@ class PolynomialSolverBase bool& hasArealRoot, const RealScalar& absImaginaryThreshold = NumTraits::dummy_precision() ) const { - std::less less; + std::less less; return selectRealRoot_withRespectToRealPart( less, hasArealRoot, absImaginaryThreshold ); } @@ -327,7 +327,7 @@ class PolynomialSolverBase * However, almost always, correct accuracy is reached even in these cases for 64bit * (double) floating types and small polynomial degree (<20). */ -template +template< typename _Scalar, int _Deg > class PolynomialSolver : public PolynomialSolverBase<_Scalar,_Deg> { public: @@ -337,10 +337,7 @@ class PolynomialSolver : public PolynomialSolverBase<_Scalar,_Deg> EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES( PS_Base ) typedef Matrix CompanionMatrixType; - typedef typename internal::conditional::IsComplex, - ComplexEigenSolver, - EigenSolver >::type EigenSolverType; - typedef typename internal::conditional::IsComplex, Scalar, std::complex >::type ComplexScalar; + typedef EigenSolver EigenSolverType; public: /** Computes the complex roots of a new polynomial. */ @@ -355,25 +352,6 @@ class PolynomialSolver : public PolynomialSolverBase<_Scalar,_Deg> companion.balance(); m_eigenSolver.compute( companion.denseMatrix() ); m_roots = m_eigenSolver.eigenvalues(); - // cleanup noise in imaginary part of real roots: - // if the imaginary part is rather small compared to the real part - // and that cancelling the imaginary part yield a smaller evaluation, - // then it's safe to keep the real part only. - RealScalar coarse_prec = RealScalar(std::pow(4,poly.size()+1))*NumTraits::epsilon(); - for(Index i = 0; i::Scalar u, DenseIndex degree, const typename SplineTraits::KnotVectorType& knots); diff --git a/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp b/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp index dd93c21e9..f0c336c15 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp @@ -15,15 +15,6 @@ // tolerance for chekcing number of iterations #define LM_EVAL_COUNT_TOL 4/3 -#define LM_CHECK_N_ITERS(SOLVER,NFEV,NJEV) { \ - ++g_test_level; \ - VERIFY_IS_EQUAL(SOLVER.nfev, NFEV); \ - VERIFY_IS_EQUAL(SOLVER.njev, NJEV); \ - --g_test_level; \ - VERIFY(SOLVER.nfev <= NFEV * LM_EVAL_COUNT_TOL); \ - VERIFY(SOLVER.njev <= NJEV * LM_EVAL_COUNT_TOL); \ - } - int fcn_chkder(const VectorXd &x, VectorXd &fvec, MatrixXd &fjac, int iflag) { /* subroutine fcn for chkder example. */ @@ -189,7 +180,8 @@ void testLmder1() // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 6, 5); + VERIFY_IS_EQUAL(lm.nfev, 6); + VERIFY_IS_EQUAL(lm.njev, 5); // check norm VERIFY_IS_APPROX(lm.fvec.blueNorm(), 0.09063596); @@ -217,7 +209,8 @@ void testLmder() // check return values VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 6, 5); + VERIFY_IS_EQUAL(lm.nfev, 6); + VERIFY_IS_EQUAL(lm.njev, 5); // check norm fnorm = lm.fvec.blueNorm(); @@ -301,7 +294,8 @@ void testHybrj1() // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(solver, 11, 1); + VERIFY_IS_EQUAL(solver.nfev, 11); + VERIFY_IS_EQUAL(solver.njev, 1); // check norm VERIFY_IS_APPROX(solver.fvec.blueNorm(), 1.192636e-08); @@ -335,7 +329,8 @@ void testHybrj() // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(solver, 11, 1); + VERIFY_IS_EQUAL(solver.nfev, 11); + VERIFY_IS_EQUAL(solver.njev, 1); // check norm VERIFY_IS_APPROX(solver.fvec.blueNorm(), 1.192636e-08); @@ -490,7 +485,8 @@ void testLmstr1() // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 6, 5); + VERIFY_IS_EQUAL(lm.nfev, 6); + VERIFY_IS_EQUAL(lm.njev, 5); // check norm VERIFY_IS_APPROX(lm.fvec.blueNorm(), 0.09063596); @@ -518,7 +514,8 @@ void testLmstr() // check return values VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 6, 5); + VERIFY_IS_EQUAL(lm.nfev, 6); + VERIFY_IS_EQUAL(lm.njev, 5); // check norm fnorm = lm.fvec.blueNorm(); @@ -689,7 +686,8 @@ void testNistChwirut2(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 10, 8); + VERIFY_IS_EQUAL(lm.nfev, 10); + VERIFY_IS_EQUAL(lm.njev, 8); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.1304802941E+02); // check x @@ -709,7 +707,8 @@ void testNistChwirut2(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 7, 6); + VERIFY_IS_EQUAL(lm.nfev, 7); + VERIFY_IS_EQUAL(lm.njev, 6); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.1304802941E+02); // check x @@ -767,7 +766,8 @@ void testNistMisra1a(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 19, 15); + VERIFY_IS_EQUAL(lm.nfev, 19); + VERIFY_IS_EQUAL(lm.njev, 15); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.2455138894E-01); // check x @@ -783,7 +783,8 @@ void testNistMisra1a(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 5, 4); + VERIFY_IS_EQUAL(lm.nfev, 5); + VERIFY_IS_EQUAL(lm.njev, 4); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.2455138894E-01); // check x @@ -855,7 +856,8 @@ void testNistHahn1(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 11, 10); + VERIFY_IS_EQUAL(lm.nfev, 11); + VERIFY_IS_EQUAL(lm.njev, 10); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.5324382854E+00); // check x @@ -876,7 +878,8 @@ void testNistHahn1(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 11, 10); + VERIFY_IS_EQUAL(lm.nfev, 11); + VERIFY_IS_EQUAL(lm.njev, 10); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.5324382854E+00); // check x @@ -939,7 +942,8 @@ void testNistMisra1d(void) // check return value VERIFY_IS_EQUAL(info, 3); - LM_CHECK_N_ITERS(lm, 9, 7); + VERIFY_IS_EQUAL(lm.nfev, 9); + VERIFY_IS_EQUAL(lm.njev, 7); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.6419295283E-02); // check x @@ -955,7 +959,8 @@ void testNistMisra1d(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 4, 3); + VERIFY_IS_EQUAL(lm.nfev, 4); + VERIFY_IS_EQUAL(lm.njev, 3); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.6419295283E-02); // check x @@ -1015,7 +1020,8 @@ void testNistLanczos1(void) // check return value VERIFY_IS_EQUAL(info, 2); - LM_CHECK_N_ITERS(lm, 79, 72); + VERIFY_IS_EQUAL(lm.nfev, 79); + VERIFY_IS_EQUAL(lm.njev, 72); // check norm^2 std::cout.precision(30); std::cout << lm.fvec.squaredNorm() << "\n"; @@ -1037,7 +1043,8 @@ void testNistLanczos1(void) // check return value VERIFY_IS_EQUAL(info, 2); - LM_CHECK_N_ITERS(lm, 9, 8); + VERIFY_IS_EQUAL(lm.nfev, 9); + VERIFY_IS_EQUAL(lm.njev, 8); // check norm^2 VERIFY(lm.fvec.squaredNorm() <= 1.4307867721E-25); // check x @@ -1101,7 +1108,8 @@ void testNistRat42(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 10, 8); + VERIFY_IS_EQUAL(lm.nfev, 10); + VERIFY_IS_EQUAL(lm.njev, 8); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.0565229338E+00); // check x @@ -1118,7 +1126,8 @@ void testNistRat42(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 6, 5); + VERIFY_IS_EQUAL(lm.nfev, 6); + VERIFY_IS_EQUAL(lm.njev, 5); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.0565229338E+00); // check x @@ -1177,7 +1186,8 @@ void testNistMGH10(void) // check return value VERIFY_IS_EQUAL(info, 2); - LM_CHECK_N_ITERS(lm, 284, 249); + VERIFY_IS_EQUAL(lm.nfev, 284 ); + VERIFY_IS_EQUAL(lm.njev, 249 ); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.7945855171E+01); // check x @@ -1194,7 +1204,8 @@ void testNistMGH10(void) // check return value VERIFY_IS_EQUAL(info, 3); - LM_CHECK_N_ITERS(lm, 126, 116); + VERIFY_IS_EQUAL(lm.nfev, 126); + VERIFY_IS_EQUAL(lm.njev, 116); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.7945855171E+01); // check x @@ -1254,7 +1265,8 @@ void testNistBoxBOD(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 31, 25); + VERIFY(lm.nfev < 31); // 31 + VERIFY(lm.njev < 25); // 25 // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.1680088766E+03); // check x @@ -1272,8 +1284,9 @@ void testNistBoxBOD(void) info = lm.minimize(x); // check return value - VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 15, 14); + VERIFY_IS_EQUAL(info, 1); + VERIFY_IS_EQUAL(lm.nfev, 15 ); + VERIFY_IS_EQUAL(lm.njev, 14 ); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.1680088766E+03); // check x @@ -1343,7 +1356,12 @@ void testNistMGH17(void) // check return value VERIFY_IS_EQUAL(info, 2); - LM_CHECK_N_ITERS(lm, 602, 545); + ++g_test_level; + VERIFY_IS_EQUAL(lm.nfev, 602); // 602 + VERIFY_IS_EQUAL(lm.njev, 545); // 545 + --g_test_level; + VERIFY(lm.nfev < 602 * LM_EVAL_COUNT_TOL); + VERIFY(lm.njev < 545 * LM_EVAL_COUNT_TOL); /* * Second try @@ -1355,7 +1373,8 @@ void testNistMGH17(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 18, 15); + VERIFY_IS_EQUAL(lm.nfev, 18); + VERIFY_IS_EQUAL(lm.njev, 15); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.4648946975E-05); // check x @@ -1419,8 +1438,9 @@ void testNistMGH09(void) info = lm.minimize(x); // check return value - VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 490, 376); + VERIFY_IS_EQUAL(info, 1); + VERIFY_IS_EQUAL(lm.nfev, 490 ); + VERIFY_IS_EQUAL(lm.njev, 376 ); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 3.0750560385E-04); // check x @@ -1439,7 +1459,8 @@ void testNistMGH09(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 18, 16); + VERIFY_IS_EQUAL(lm.nfev, 18); + VERIFY_IS_EQUAL(lm.njev, 16); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 3.0750560385E-04); // check x @@ -1504,7 +1525,8 @@ void testNistBennett5(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 758, 744); + VERIFY_IS_EQUAL(lm.nfev, 758); + VERIFY_IS_EQUAL(lm.njev, 744); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.2404744073E-04); // check x @@ -1521,7 +1543,8 @@ void testNistBennett5(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 203, 192); + VERIFY_IS_EQUAL(lm.nfev, 203); + VERIFY_IS_EQUAL(lm.njev, 192); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.2404744073E-04); // check x @@ -1590,7 +1613,8 @@ void testNistThurber(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 39,36); + VERIFY_IS_EQUAL(lm.nfev, 39); + VERIFY_IS_EQUAL(lm.njev, 36); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.6427082397E+03); // check x @@ -1614,7 +1638,8 @@ void testNistThurber(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 29, 28); + VERIFY_IS_EQUAL(lm.nfev, 29); + VERIFY_IS_EQUAL(lm.njev, 28); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.6427082397E+03); // check x @@ -1680,7 +1705,8 @@ void testNistRat43(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 27, 20); + VERIFY_IS_EQUAL(lm.nfev, 27); + VERIFY_IS_EQUAL(lm.njev, 20); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.7864049080E+03); // check x @@ -1701,7 +1727,8 @@ void testNistRat43(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 9, 8); + VERIFY_IS_EQUAL(lm.nfev, 9); + VERIFY_IS_EQUAL(lm.njev, 8); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.7864049080E+03); // check x @@ -1763,7 +1790,8 @@ void testNistEckerle4(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 18, 15); + VERIFY_IS_EQUAL(lm.nfev, 18); + VERIFY_IS_EQUAL(lm.njev, 15); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.4635887487E-03); // check x @@ -1780,7 +1808,8 @@ void testNistEckerle4(void) // check return value VERIFY_IS_EQUAL(info, 1); - LM_CHECK_N_ITERS(lm, 7, 6); + VERIFY_IS_EQUAL(lm.nfev, 7); + VERIFY_IS_EQUAL(lm.njev, 6); // check norm^2 VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.4635887487E-03); // check x diff --git a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp index 1d8c8b5fd..1c5e0dc66 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp @@ -352,21 +352,6 @@ double bug_1264() { return v2(0).value(); } -// check with expressions on constants -double bug_1281() { - int n = 2; - typedef AutoDiffScalar AD; - const AD c = 1.; - AD x0(2,n,0); - AD y1 = (AD(c)+AD(c))*x0; - y1 = x0 * (AD(c)+AD(c)); - AD y2 = (-AD(c))+x0; - y2 = x0+(-AD(c)); - AD y3 = (AD(c)*(-AD(c))+AD(c))*x0; - y3 = x0 * (AD(c)*(-AD(c))+AD(c)); - return (y1+y2+y3).value(); -} - #endif void test_autodiff() @@ -382,6 +367,5 @@ void test_autodiff() CALL_SUBTEST_5( bug_1223() ); CALL_SUBTEST_5( bug_1260() ); CALL_SUBTEST_5( bug_1261() ); - CALL_SUBTEST_5( bug_1281() ); } diff --git a/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp b/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp index 005c9c15f..6a2b2194a 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp @@ -177,39 +177,6 @@ void testMatrixType(const MatrixType& m) } } -template -void testMapRef(const MatrixType& A) -{ - // Test if passing Ref and Map objects is possible - // (Regression test for Bug #1796) - Index size = A.rows(); - MatrixType X; X.setRandom(size, size); - MatrixType Y(size,size); - Ref< MatrixType> R(Y); - Ref Rc(X); - Map< MatrixType> M(Y.data(), size, size); - Map Mc(X.data(), size, size); - - X = X*X; // make sure sqrt is possible - Y = X.sqrt(); - R = Rc.sqrt(); - M = Mc.sqrt(); - Y = X.exp(); - R = Rc.exp(); - M = Mc.exp(); - X = Y; // make sure log is possible - Y = X.log(); - R = Rc.log(); - M = Mc.log(); - - Y = X.cos() + Rc.cos() + Mc.cos(); - Y = X.sin() + Rc.sin() + Mc.sin(); - - Y = X.cosh() + Rc.cosh() + Mc.cosh(); - Y = X.sinh() + Rc.sinh() + Mc.sinh(); -} - - void test_matrix_function() { CALL_SUBTEST_1(testMatrixType(Matrix())); @@ -219,9 +186,4 @@ void test_matrix_function() CALL_SUBTEST_5(testMatrixType(Matrix())); CALL_SUBTEST_6(testMatrixType(Matrix4cd())); CALL_SUBTEST_7(testMatrixType(MatrixXd(13,13))); - - CALL_SUBTEST_1(testMapRef(Matrix())); - CALL_SUBTEST_2(testMapRef(Matrix3cf())); - CALL_SUBTEST_3(testMapRef(MatrixXf(8,8))); - CALL_SUBTEST_7(testMapRef(MatrixXd(13,13))); } diff --git a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp index db8ad7dda..4cfc46b41 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp @@ -26,25 +26,14 @@ struct increment_if_fixed_size } } -template -PolynomialType polyder(const PolynomialType& p) -{ - typedef typename PolynomialType::Scalar Scalar; - PolynomialType res(p.size()); - for(Index i=1; i bool aux_evalSolver( const POLYNOMIAL& pols, SOLVER& psolve ) { typedef typename POLYNOMIAL::Scalar Scalar; - typedef typename POLYNOMIAL::RealScalar RealScalar; typedef typename SOLVER::RootsType RootsType; - typedef Matrix EvalRootsType; + typedef Matrix EvalRootsType; const Index deg = pols.size()-1; @@ -54,17 +43,10 @@ bool aux_evalSolver( const POLYNOMIAL& pols, SOLVER& psolve ) psolve.compute( pols ); const RootsType& roots( psolve.roots() ); EvalRootsType evr( deg ); - POLYNOMIAL pols_der = polyder(pols); - EvalRootsType der( deg ); for( int i=0; i() ); + bool evalToZero = evr.isZero( test_precision() ); if( !evalToZero ) { cerr << "WRONG root: " << endl; @@ -74,7 +56,7 @@ bool aux_evalSolver( const POLYNOMIAL& pols, SOLVER& psolve ) cerr << endl; } - std::vector rootModuli( roots.size() ); + std::vector rootModuli( roots.size() ); Map< EvalRootsType > aux( &rootModuli[0], roots.size() ); aux = roots.array().abs(); std::sort( rootModuli.begin(), rootModuli.end() ); @@ -100,7 +82,7 @@ void evalSolver( const POLYNOMIAL& pols ) { typedef typename POLYNOMIAL::Scalar Scalar; - typedef PolynomialSolver PolynomialSolverType; + typedef PolynomialSolver PolynomialSolverType; PolynomialSolverType psolve; aux_evalSolver( pols, psolve ); @@ -114,7 +96,6 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const { using std::sqrt; typedef typename POLYNOMIAL::Scalar Scalar; - typedef typename POLYNOMIAL::RealScalar RealScalar; typedef PolynomialSolver PolynomialSolverType; @@ -125,12 +106,14 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const // 1) the roots found are correct // 2) the roots have distinct moduli - //Test realRoots - std::vector< RealScalar > calc_realRoots; - psolve.realRoots( calc_realRoots, test_precision()); - VERIFY_IS_EQUAL( calc_realRoots.size() , (size_t)real_roots.size() ); + typedef typename REAL_ROOTS::Scalar Real; - const RealScalar psPrec = sqrt( test_precision() ); + //Test realRoots + std::vector< Real > calc_realRoots; + psolve.realRoots( calc_realRoots ); + VERIFY( calc_realRoots.size() == (size_t)real_roots.size() ); + + const Scalar psPrec = sqrt( test_precision() ); for( size_t i=0; i 0 ) ); if( hasRealRoot ){ VERIFY( internal::isApprox( real_roots.array().abs().maxCoeff(), abs(r), psPrec ) ); } @@ -182,11 +165,9 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const template void polynomialsolver(int deg) { - typedef typename NumTraits<_Scalar>::Real RealScalar; - typedef internal::increment_if_fixed_size<_Deg> Dim; + typedef internal::increment_if_fixed_size<_Deg> Dim; typedef Matrix<_Scalar,Dim::ret,1> PolynomialType; typedef Matrix<_Scalar,_Deg,1> EvalRootsType; - typedef Matrix RealRootsType; cout << "Standard cases" << endl; PolynomialType pols = PolynomialType::Random(deg+1); @@ -199,11 +180,15 @@ void polynomialsolver(int deg) evalSolver<_Deg,PolynomialType>( pols ); cout << "Test sugar" << endl; - RealRootsType realRoots = RealRootsType::Random(deg); + EvalRootsType realRoots = EvalRootsType::Random(deg); roots_to_monicPolynomial( realRoots, pols ); evalSolverSugarFunction<_Deg>( pols, - realRoots.template cast >().eval(), + realRoots.template cast < + std::complex< + typename NumTraits<_Scalar>::Real + > + >(), realRoots ); } @@ -227,6 +212,5 @@ void test_polynomialsolver() internal::random(9,13) )) ); CALL_SUBTEST_11((polynomialsolver(1)) ); - CALL_SUBTEST_12((polynomialsolver,Dynamic>(internal::random(2,13))) ); } } From 120a69d7db2c7ecc04f6a1688df493dc43124a56 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 16:41:13 -0500 Subject: [PATCH 60/71] add workaround for Eigen serialization issue --- gtsam/base/serialization.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index f589ecc5e..98112b256 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -40,6 +40,17 @@ #include #include +// Workaround a bug in GCC >= 7 and C++17 +// ref. https://gitlab.com/libeigen/eigen/-/issues/1676 +#ifdef __GNUC__ +#if __GNUC__ >= 7 && __cplusplus >= 201703L +namespace boost { namespace serialization { struct U; } } +namespace Eigen { namespace internal { +template<> struct traits {enum {Flags=0};}; +} } +#endif +#endif + namespace gtsam { /** @name Standard serialization From c0b65b24887bcdeeebc6629b27e944cae37c2733 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 17:34:08 -0500 Subject: [PATCH 61/71] Add Eigen/Core header --- gtsam/base/serialization.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 98112b256..24355c684 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -19,8 +19,9 @@ #pragma once -#include +#include #include +#include #include // includes for standard serialization types From 1fe7822981c77f759f7d42a5ace271653e46c555 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 18:46:16 -0500 Subject: [PATCH 62/71] make LinearContainerFactor public for serialization --- gtsam/nonlinear/LinearContainerFactor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 8c5b34f01..efc095775 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -29,9 +29,6 @@ protected: GaussianFactor::shared_ptr factor_; boost::optional linearizationPoint_; - /** Default constructor - necessary for serialization */ - LinearContainerFactor() {} - /** direct copy constructor */ GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); @@ -43,6 +40,9 @@ public: typedef boost::shared_ptr shared_ptr; + /** Default constructor - necessary for serialization */ + LinearContainerFactor() {} + /** Primary constructor: store a linear factor with optional linearization point */ GTSAM_EXPORT LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); From bee289880ab684017d0f3104620197e59dc20e81 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 19:39:41 -0500 Subject: [PATCH 63/71] wrap other ISAM2 methods --- gtsam/nonlinear/nonlinear.i | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 4d81049ea..aa595d544 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -749,12 +749,17 @@ class ISAM2 { gtsam::PinholeCamera, gtsam::PinholeCamera, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; - gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; + gtsam::Values calculateBestEstimate() const; gtsam::VectorValues getDelta() const; + double error(const gtsam::VectorValues& x) const; gtsam::NonlinearFactorGraph getFactorsUnsafe() const; gtsam::VariableIndex getVariableIndex() const; + const gtsam::KeySet& getFixedVariables() const; gtsam::ISAM2Params params() const; + + void printStats() const; + gtsam::VectorValues gradientAtZero() const; }; #include From 0b89d2d7abf328214f1bf5478b67a9c260add087 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 19:52:49 -0500 Subject: [PATCH 64/71] wrap alternate ISAM2::update method --- gtsam/nonlinear/nonlinear.i | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index aa595d544..152c4b8e7 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -738,6 +738,10 @@ class ISAM2 { const gtsam::KeyList& extraReelimKeys, bool force_relinearize); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::ISAM2UpdateParams& updateParams); + gtsam::Values getLinearizationPoint() const; bool valueExists(gtsam::Key key) const; gtsam::Values calculateEstimate() const; From be29bc422274c08fb9f22aeff902847dca148a86 Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Tue, 30 Nov 2021 03:28:47 -0800 Subject: [PATCH 65/71] .gitignore: swp files --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index cde059767..e6e38132f 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,7 @@ .idea *.pyc *.DS_Store +*.swp /examples/Data/dubrovnik-3-7-pre-rewritten.txt /examples/Data/pose2example-rewritten.txt /examples/Data/pose3example-rewritten.txt From f3cdd9d8cdd74357b372969e817d77f418ee19d9 Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Tue, 30 Nov 2021 03:29:16 -0800 Subject: [PATCH 66/71] don't return a const --- gtsam/geometry/Similarity3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index fcaf0c874..ea7a6d049 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -40,8 +40,8 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, } /// Form inner products x and y and calculate scale. -static const double calculateScale(const Point3Pairs &d_abPointPairs, - const Rot3 &aRb) { +static double calculateScale(const Point3Pairs &d_abPointPairs, + const Rot3 &aRb) { double x = 0, y = 0; Point3 da, db; for (const Point3Pair& d_abPair : d_abPointPairs) { From e15317ba4c75f513a4264467f071ad8ca1a8300c Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Tue, 30 Nov 2021 03:29:34 -0800 Subject: [PATCH 67/71] missing shared_ptr include --- gtsam/inference/BayesTree.h | 2 ++ gtsam/inference/Factor.h | 1 + 2 files changed, 3 insertions(+) diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 7199da0ad..cc003d8dc 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -19,6 +19,8 @@ #pragma once +#include + #include #include #include diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 6ea81030a..c0ea4ea78 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -22,6 +22,7 @@ #pragma once #include +#include #include #include From 62c22c7fcce668b705c37a38c73a29778ee9d295 Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Tue, 30 Nov 2021 03:30:06 -0800 Subject: [PATCH 68/71] ParseMeasurement initializer list needs an extra argument --- gtsam/slam/dataset.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index c8a8b15c5..d7e925bd9 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -392,7 +392,7 @@ parseMeasurements(const std::string &filename, size_t maxIndex) { ParseMeasurement parse{model ? createSampler(model) : nullptr, maxIndex, true, NoiseFormatAUTO, - KernelFunctionTypeNONE}; + KernelFunctionTypeNONE, nullptr}; return parseToVector>(filename, parse); } From 40c9da525345f824e4c6c8353cd121973c8771bb Mon Sep 17 00:00:00 2001 From: beetleskin Date: Wed, 1 Dec 2021 09:28:34 +0100 Subject: [PATCH 69/71] add missing interface for PoseTranslationPrior --- gtsam/slam/slam.i | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 60000dbab..527e838b2 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -168,6 +168,13 @@ template virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); + POSE measured() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; @@ -178,6 +185,7 @@ template virtual class PoseRotationPrior : gtsam::NoiseModelFactor { PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); + POSE measured() const; }; typedef gtsam::PoseRotationPrior PoseRotationPrior2D; From 27579e4f34fd44d5e87bba8a59e96b4e5e35ee20 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 3 Dec 2021 15:14:19 -0500 Subject: [PATCH 70/71] Fix quaternion on M1 --- gtsam/geometry/Quaternion.h | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 1557a09db..becb51d24 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -82,7 +82,7 @@ struct traits { using std::sin; if (H) *H = SO3::ExpmapDerivative(omega.template cast()); _Scalar theta2 = omega.dot(omega); - if (theta2 > std::numeric_limits<_Scalar>::epsilon()) { + if (theta2 > 1e-8) { _Scalar theta = std::sqrt(theta2); _Scalar ha = _Scalar(0.5) * theta; Vector3 vec = (sin(ha) / theta) * omega; @@ -100,8 +100,8 @@ struct traits { using std::sqrt; // define these compile time constants to avoid std::abs: - static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, - NearlyNegativeOne = -1.0 + 1e-10; + static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-8, + NearlyNegativeOne = -1.0 + 1e-8; TangentVector omega; @@ -117,13 +117,23 @@ struct traits { omega = (-8. / 3. - 2. / 3. * qw) * q.vec(); } else { // Normal, away from zero case - _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw); - // Important: convert to [-pi,pi] to keep error continuous - if (angle > M_PI) - angle -= twoPi; - else if (angle < -M_PI) - angle += twoPi; - omega = (angle / s) * q.vec(); + if (qw > 0) { + _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw); + // Important: convert to [-pi,pi] to keep error continuous + if (angle > M_PI) + angle -= twoPi; + else if (angle < -M_PI) + angle += twoPi; + omega = (angle / s) * q.vec(); + } else { + // Make sure that we are using a canonical quaternion with w > 0 + _Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw); + if (angle > M_PI) + angle -= twoPi; + else if (angle < -M_PI) + angle += twoPi; + omega = (angle / s) * -q.vec(); + } } if(H) *H = SO3::LogmapDerivative(omega.template cast()); From 578b5e6ec5da7b84c5ef7f07ecdfa12a24c10d2f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 3 Dec 2021 15:21:01 -0500 Subject: [PATCH 71/71] Only keep essentials --- gtsam/geometry/Quaternion.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index becb51d24..2ef47d58e 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -82,7 +82,7 @@ struct traits { using std::sin; if (H) *H = SO3::ExpmapDerivative(omega.template cast()); _Scalar theta2 = omega.dot(omega); - if (theta2 > 1e-8) { + if (theta2 > std::numeric_limits<_Scalar>::epsilon()) { _Scalar theta = std::sqrt(theta2); _Scalar ha = _Scalar(0.5) * theta; Vector3 vec = (sin(ha) / theta) * omega; @@ -100,8 +100,8 @@ struct traits { using std::sqrt; // define these compile time constants to avoid std::abs: - static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-8, - NearlyNegativeOne = -1.0 + 1e-8; + static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, + NearlyNegativeOne = -1.0 + 1e-10; TangentVector omega;