From f68c06a10d2aad69e95951f806b6f566eb9511b5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Sep 2019 18:44:21 -0400 Subject: [PATCH 01/55] wrapped more mEstimators as well as their weight functions --- gtsam.h | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/gtsam.h b/gtsam.h index bf3575580..23590f5f8 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1362,6 +1362,8 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { // enabling serialization functionality void serializable() const; + + double weight(double error) const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { @@ -1370,6 +1372,8 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { // enabling serialization functionality void serializable() const; + + double weight(double error) const; }; virtual class Huber: gtsam::noiseModel::mEstimator::Base { @@ -1378,6 +1382,18 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { // enabling serialization functionality void serializable() const; + + double weight(double error) const; +}; + +virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { + Cauchy(double k); + static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { @@ -1386,6 +1402,8 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { // enabling serialization functionality void serializable() const; + + double weight(double error) const; }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { @@ -1394,8 +1412,21 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { // enabling serialization functionality void serializable() const; + + double weight(double error) const; }; +virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { + GemanMcClure(double k); + static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; +}; + +//TODO DCS and L2WithDeadZone mEstimators }///\namespace mEstimator From c8e18c95d0cd6efb5212515280ae61632000f2a2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Sep 2019 20:08:10 -0400 Subject: [PATCH 02/55] added script to visualize values of different mEstimators --- matlab/gtsam_examples/VisualizeMEstimators.m | 160 +++++++++++++++++++ 1 file changed, 160 insertions(+) create mode 100644 matlab/gtsam_examples/VisualizeMEstimators.m diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m new file mode 100644 index 000000000..b8ffc03c4 --- /dev/null +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -0,0 +1,160 @@ +import gtsam.* + +clear all; +close all; + +x = linspace(-10, 10, 1000); +% x = linspace(-10, 10, 21) + +[rho, psi, w] = fair(x); +plot_rho(x, rho, 1, -1, 30) +title('Fair'); +plot_psi(x, psi, 7, -3, 3); +plot_w(x, w, 13, 3); + +[rho, psi, w] = huber(x); +plot_rho(x, rho, 2, -1, 30); +title('Huber'); +plot_psi(x, psi, 8, -15, 15); +plot_w(x, w, 14, 5); + +[rho, psi, w] = cauchy(x); +plot_rho(x, rho, 3, -0.1, 0.1); +title('Cauchy'); +plot_psi(x, psi, 9, -0.2, 0.2); +plot_w(x, w, 15, 1.5); + +[rho, psi, w] = gemancclure(x); +plot_rho(x, rho, 4, -1, 1); +title('Geman-McClure'); +plot_psi(x, psi, 10, -1, 1); +plot_w(x, w, 16, 1.5); + +[rho, psi, w] = welsch(x); +plot_rho(x, rho, 5, -5, 10); +title('Welsch'); +plot_psi(x, psi, 11, -2, 2); +plot_w(x, w, 17, 2); + +[rho, psi, w] = tukey(x); +plot_rho(x, rho, 6, -5, 10); +title('Tukey'); +plot_psi(x, psi, 12, -2, 2); +plot_w(x, w, 18, 2); + +function plot_rho(x, y, idx, yll, ylu) + subplot(3, 6, idx); + plot(x, y); + xlim([-15, 15]); + ylim([yll, ylu]); +end + +function plot_psi(x, y, idx, yll, ylu) + subplot(3, 6, idx); + plot(x, y); + xlim([-15, 15]); + ylim([yll, ylu]); +end + +function plot_w(x, y, idx, yl) + subplot(3, 6, idx); + plot(x, y); + xlim([-15, 15]); + ylim([-yl, yl]); +end + +function [rho, psi, w] = fair(x) + import gtsam.* + c = 1.3998; + + rho = c^2 * ( (abs(x) ./ c) - log(1 + (abs(x)./c)) ); + est = noiseModel.mEstimator.Fair(c); + + w = zeros(size(x)); + for i = 1:size(x, 2) + w(i) = est.weight(x(i)); + end + + psi = w .* x; +end + +function [rho, psi, w] = huber(x) + import gtsam.* + k = 5; + t = (abs(x) > k); + + rho = (x .^ 2) ./ 2; + rho(t) = k * (abs(x(t)) - (k/2)); + est = noiseModel.mEstimator.Huber(k); + + w = zeros(size(x)); + for i = 1:size(x, 2) + w(i) = est.weight(x(i)); + end + + psi = w .* x; +end + +function [rho, psi, w] = cauchy(x) + import gtsam.* + c = 0.1; + + rho = (c^2 / 2) .* log(1 + ((x./c) .^ 2)); + + est = noiseModel.mEstimator.Cauchy(c); + + w = zeros(size(x)); + for i = 1:size(x, 2) + w(i) = est.weight(x(i)); + end + + psi = w .* x; +end + +function [rho, psi, w] = gemancclure(x) + import gtsam.* + c = 1.0; + rho = ((x .^ 2) ./ 2) ./ (1 + x .^ 2); + + est = noiseModel.mEstimator.GemanMcClure(c); + + w = zeros(size(x)); + for i = 1:size(x, 2) + w(i) = est.weight(x(i)); + end + + psi = w .* x; +end + +function [rho, psi, w] = welsch(x) + import gtsam.* + c = 2.9846; + rho = (c^2 / 2) * ( 1 - exp(-(x ./ c) .^2 )); + + est = noiseModel.mEstimator.Welsh(c); + + w = zeros(size(x)); + for i = 1:size(x, 2) + w(i) = est.weight(x(i)); + end + + psi = w .* x; +end + +function [rho, psi, w] = tukey(x) + import gtsam.* + c = 4.6851; + t = (abs(x) > c); + + rho = (c^2 / 6) * (1 - (1 - (x ./ c) .^ 2 ) .^ 3 ); + rho(t) = c .^ 2 / 6; + + est = noiseModel.mEstimator.Tukey(c); + + w = zeros(size(x)); + for i = 1:size(x, 2) + w(i) = est.weight(x(i)); + end + + psi = w .* x; +end From ab044b693b004ca9e229736cdfd1043f09fa55dd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Sep 2019 23:41:20 -0400 Subject: [PATCH 03/55] different plots for each mEstimator --- matlab/gtsam_examples/VisualizeMEstimators.m | 54 ++++++++++++-------- 1 file changed, 33 insertions(+), 21 deletions(-) diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index b8ffc03c4..8fce828aa 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -7,57 +7,69 @@ x = linspace(-10, 10, 1000); % x = linspace(-10, 10, 21) [rho, psi, w] = fair(x); +figure(1); plot_rho(x, rho, 1, -1, 30) title('Fair'); -plot_psi(x, psi, 7, -3, 3); -plot_w(x, w, 13, 3); +plot_psi(x, psi, 2, -3, 3); +plot_w(x, w, 3, 3); +saveas(figure(1), 'fair.png'); [rho, psi, w] = huber(x); -plot_rho(x, rho, 2, -1, 30); +figure(2); +plot_rho(x, rho, 1, -1, 30); title('Huber'); -plot_psi(x, psi, 8, -15, 15); -plot_w(x, w, 14, 5); +plot_psi(x, psi, 2, -15, 15); +plot_w(x, w, 3, 5); +saveas(figure(2), 'huber.png'); [rho, psi, w] = cauchy(x); -plot_rho(x, rho, 3, -0.1, 0.1); +figure(3); +plot_rho(x, rho, 1, -0.1, 0.1); title('Cauchy'); -plot_psi(x, psi, 9, -0.2, 0.2); -plot_w(x, w, 15, 1.5); +plot_psi(x, psi, 2, -0.2, 0.2); +plot_w(x, w, 3, 1.5); +saveas(figure(3), 'cauchy.png'); [rho, psi, w] = gemancclure(x); -plot_rho(x, rho, 4, -1, 1); +figure(4); +plot_rho(x, rho, 1, -1, 1); title('Geman-McClure'); -plot_psi(x, psi, 10, -1, 1); -plot_w(x, w, 16, 1.5); +plot_psi(x, psi, 2, -1, 1); +plot_w(x, w, 3, 1.5); +saveas(figure(4), 'gemanmcclure.png'); [rho, psi, w] = welsch(x); -plot_rho(x, rho, 5, -5, 10); +figure(5); +plot_rho(x, rho, 1, -5, 10); title('Welsch'); -plot_psi(x, psi, 11, -2, 2); -plot_w(x, w, 17, 2); +plot_psi(x, psi, 2, -2, 2); +plot_w(x, w, 3, 2); +saveas(figure(5), 'welsch.png'); [rho, psi, w] = tukey(x); -plot_rho(x, rho, 6, -5, 10); +figure(6); +plot_rho(x, rho, 1, -5, 10); title('Tukey'); -plot_psi(x, psi, 12, -2, 2); -plot_w(x, w, 18, 2); +plot_psi(x, psi, 2, -2, 2); +plot_w(x, w, 3, 2); +saveas(figure(6), 'tukey.png'); function plot_rho(x, y, idx, yll, ylu) - subplot(3, 6, idx); + subplot(3, 1, idx); plot(x, y); xlim([-15, 15]); ylim([yll, ylu]); end function plot_psi(x, y, idx, yll, ylu) - subplot(3, 6, idx); + subplot(3, 1, idx); plot(x, y); xlim([-15, 15]); ylim([yll, ylu]); end function plot_w(x, y, idx, yl) - subplot(3, 6, idx); + subplot(3, 1, idx); plot(x, y); xlim([-15, 15]); ylim([-yl, yl]); @@ -131,7 +143,7 @@ function [rho, psi, w] = welsch(x) c = 2.9846; rho = (c^2 / 2) * ( 1 - exp(-(x ./ c) .^2 )); - est = noiseModel.mEstimator.Welsh(c); + est = noiseModel.mEstimator.Welsch(c); w = zeros(size(x)); for i = 1:size(x, 2) From f71e156bced9d6a1aed094f004d647fcf71becea Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Sep 2019 16:13:59 -0400 Subject: [PATCH 04/55] Fixed comment --- gtsam/linear/NoiseModel.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 5c540caa3..464bb38ed 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -635,9 +635,9 @@ namespace gtsam { * 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 Date: Thu, 19 Sep 2019 12:58:27 -0400 Subject: [PATCH 05/55] renamed global variables in smallExample.h and added optional noise model parameters --- tests/smallExample.h | 55 +++++++++++++++++++++++--------------------- 1 file changed, 29 insertions(+), 26 deletions(-) diff --git a/tests/smallExample.h b/tests/smallExample.h index 146289dac..d268f2787 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -159,10 +159,10 @@ namespace example { namespace impl { typedef boost::shared_ptr shared_nlf; -static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); -static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); -static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); -static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); +static SharedDiagonal kSigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); +static SharedDiagonal kSigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); +static SharedDiagonal kSigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); +static SharedDiagonal kConstrainedModel = noiseModel::Constrained::All(2); static const Key _l1_=0, _x1_=1, _x2_=2; static const Key _x_=0, _y_=1, _z_=2; @@ -170,40 +170,43 @@ static const Key _x_=0, _y_=1, _z_=2; /* ************************************************************************* */ -inline boost::shared_ptr sharedNonlinearFactorGraph() { +inline boost::shared_ptr +sharedNonlinearFactorGraph(const noiseModel::Base &noiseModel1 = kSigma0_1, + const noiseModel::Base &noiseModel2 = kSigma0_2) { using namespace impl; - using symbol_shorthand::X; using symbol_shorthand::L; + using symbol_shorthand::X; // Create - boost::shared_ptr nlfg( - new NonlinearFactorGraph); + boost::shared_ptr nlfg(new NonlinearFactorGraph); // prior on x1 - Point2 mu(0,0); - shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1))); + Point2 mu(0, 0); + shared_nlf f1(new simulated2D::Prior(mu, noiseModel1, X(1))); nlfg->push_back(f1); // odometry between x1 and x2 Point2 z2(1.5, 0); - shared_nlf f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2))); + shared_nlf f2(new simulated2D::Odometry(z2, noiseModel1, X(1), X(2))); nlfg->push_back(f2); // measurement between x1 and l1 Point2 z3(0, -1); - shared_nlf f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1))); + shared_nlf f3(new simulated2D::Measurement(z3, noiseModel2, X(1), L(1))); nlfg->push_back(f3); // measurement between x2 and l1 Point2 z4(-1.5, -1.); - shared_nlf f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1))); + shared_nlf f4(new simulated2D::Measurement(z4, noiseModel2, X(2), L(1))); nlfg->push_back(f4); return nlfg; } /* ************************************************************************* */ -inline NonlinearFactorGraph createNonlinearFactorGraph() { - return *sharedNonlinearFactorGraph(); +inline NonlinearFactorGraph +createNonlinearFactorGraph(const noiseModel::Base &noiseModel1 = kSigma0_1, + const noiseModel::Base &noiseModel2 = kSigma0_2) { + return *sharedNonlinearFactorGraph(noiseModel1, noiseModel2); } /* ************************************************************************* */ @@ -372,19 +375,19 @@ inline std::pair createNonlinearSmoother(int T) { // prior on x1 Point2 x1(1.0, 0.0); - shared_nlf prior(new simulated2D::Prior(x1, sigma1_0, X(1))); + shared_nlf prior(new simulated2D::Prior(x1, kSigma1_0, X(1))); nlfg.push_back(prior); poses.insert(X(1), x1); for (int t = 2; t <= T; t++) { // odometry between x_t and x_{t-1} Point2 odo(1.0, 0.0); - shared_nlf odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t))); + shared_nlf odometry(new simulated2D::Odometry(odo, kSigma1_0, X(t - 1), X(t))); nlfg.push_back(odometry); // measurement on x_t is like perfect GPS Point2 xt(t, 0); - shared_nlf measurement(new simulated2D::Prior(xt, sigma1_0, X(t))); + shared_nlf measurement(new simulated2D::Prior(xt, kSigma1_0, X(t))); nlfg.push_back(measurement); // initial estimate @@ -412,7 +415,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() { Vector b1(2); b1(0) = 1.0; b1(1) = -1.0; - JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); + JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, kSigma0_1)); // create binary constraint factor // between _x_ and _y_, that is going to be the only factor on _y_ @@ -422,7 +425,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() { Matrix Ay1 = I_2x2 * -1; Vector b2 = Vector2(0.0, 0.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, - constraintModel)); + kConstrainedModel)); // construct the graph GaussianFactorGraph fg; @@ -453,8 +456,8 @@ inline GaussianFactorGraph createSingleConstraintGraph() { Vector b1(2); b1(0) = 1.0; b1(1) = -1.0; - //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1)); - JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); + //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, kSigma0_1->Whiten(Ax), kSigma0_1->whiten(b1), kSigma0_1)); + JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, kSigma0_1)); // create binary constraint factor // between _x_ and _y_, that is going to be the only factor on _y_ @@ -468,7 +471,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() { Matrix Ay1 = I_2x2 * 10; Vector b2 = Vector2(1.0, 2.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, - constraintModel)); + kConstrainedModel)); // construct the graph GaussianFactorGraph fg; @@ -493,7 +496,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() { // unary factor 1 Matrix A = I_2x2; Vector b = Vector2(-2.0, 2.0); - JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); + JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, kSigma0_1)); // constraint 1 Matrix A11(2, 2); @@ -512,7 +515,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() { b1(0) = 1.0; b1(1) = 2.0; JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1, - constraintModel)); + kConstrainedModel)); // constraint 2 Matrix A21(2, 2); @@ -531,7 +534,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() { b2(0) = 3.0; b2(1) = 4.0; JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2, - constraintModel)); + kConstrainedModel)); // construct the graph GaussianFactorGraph fg; From f58d421b71836dbb850913fcf55356282595d18a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 24 Sep 2019 12:48:38 -0400 Subject: [PATCH 06/55] improve m-estimator visualization code --- matlab/gtsam_examples/VisualizeMEstimators.m | 196 ++++++------------- 1 file changed, 63 insertions(+), 133 deletions(-) diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index 8fce828aa..5120934fe 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -1,172 +1,102 @@ -import gtsam.* +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief Plot visualizations of residuals, residual derivatives, and weights for the various mEstimators. +% @author Varun Agrawal +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% import gtsam.* clear all; close all; x = linspace(-10, 10, 1000); -% x = linspace(-10, 10, 21) +% x = linspace(-5, 5, 101); -[rho, psi, w] = fair(x); -figure(1); -plot_rho(x, rho, 1, -1, 30) -title('Fair'); -plot_psi(x, psi, 2, -3, 3); -plot_w(x, w, 3, 3); -saveas(figure(1), 'fair.png'); +c = 1.3998; +rho = fair(x, c); +fairNoiseModel = gtsam.noiseModel.mEstimator.Fair(c); +plot_m_estimator(x, fairNoiseModel, rho, 'Fair', 1, 'fair.png') -[rho, psi, w] = huber(x); -figure(2); -plot_rho(x, rho, 1, -1, 30); -title('Huber'); -plot_psi(x, psi, 2, -15, 15); -plot_w(x, w, 3, 5); -saveas(figure(2), 'huber.png'); +c = 1.345; +rho = huber(x, c); +huberNoiseModel = gtsam.noiseModel.mEstimator.Huber(c); +plot_m_estimator(x, huberNoiseModel, rho, 'Huber', 2, 'huber.png') -[rho, psi, w] = cauchy(x); -figure(3); -plot_rho(x, rho, 1, -0.1, 0.1); -title('Cauchy'); -plot_psi(x, psi, 2, -0.2, 0.2); -plot_w(x, w, 3, 1.5); -saveas(figure(3), 'cauchy.png'); +c = 0.1; +rho = cauchy(x, c); +cauchyNoiseModel = gtsam.noiseModel.mEstimator.Cauchy(c); +plot_m_estimator(x, cauchyNoiseModel, rho, 'Cauchy', 3, 'cauchy.png') -[rho, psi, w] = gemancclure(x); -figure(4); -plot_rho(x, rho, 1, -1, 1); -title('Geman-McClure'); -plot_psi(x, psi, 2, -1, 1); -plot_w(x, w, 3, 1.5); -saveas(figure(4), 'gemanmcclure.png'); +c = 1.0; +rho = gemanmcclure(x, c); +gemanmcclureNoiseModel = gtsam.noiseModel.mEstimator.GemanMcClure(c); +plot_m_estimator(x, gemanmcclureNoiseModel, rho, 'GemanMcClure', 4, 'gemanmcclure.png') -[rho, psi, w] = welsch(x); -figure(5); -plot_rho(x, rho, 1, -5, 10); -title('Welsch'); -plot_psi(x, psi, 2, -2, 2); -plot_w(x, w, 3, 2); -saveas(figure(5), 'welsch.png'); +c = 2.9846; +rho = welsch(x, c); +welschNoiseModel = gtsam.noiseModel.mEstimator.Welsch(c); +plot_m_estimator(x, welschNoiseModel, rho, 'Welsch', 5, 'welsch.png') -[rho, psi, w] = tukey(x); -figure(6); -plot_rho(x, rho, 1, -5, 10); -title('Tukey'); -plot_psi(x, psi, 2, -2, 2); -plot_w(x, w, 3, 2); -saveas(figure(6), 'tukey.png'); +c = 4.6851; +rho = tukey(x, c); +tukeyNoiseModel = gtsam.noiseModel.mEstimator.Tukey(c); +plot_m_estimator(x, tukeyNoiseModel, rho, 'Tukey', 6, 'tukey.png') -function plot_rho(x, y, idx, yll, ylu) - subplot(3, 1, idx); - plot(x, y); - xlim([-15, 15]); - ylim([yll, ylu]); -end - -function plot_psi(x, y, idx, yll, ylu) - subplot(3, 1, idx); - plot(x, y); - xlim([-15, 15]); - ylim([yll, ylu]); -end - -function plot_w(x, y, idx, yl) - subplot(3, 1, idx); - plot(x, y); - xlim([-15, 15]); - ylim([-yl, yl]); -end - -function [rho, psi, w] = fair(x) - import gtsam.* - c = 1.3998; - - rho = c^2 * ( (abs(x) ./ c) - log(1 + (abs(x)./c)) ); - est = noiseModel.mEstimator.Fair(c); - +%% Plot rho, psi and weights of the mEstimator. +function plot_m_estimator(x, model, rho, plot_title, fig_id, filename) w = zeros(size(x)); for i = 1:size(x, 2) - w(i) = est.weight(x(i)); + w(i) = model.weight(x(i)); end psi = w .* x; + + figure(fig_id); + subplot(3, 1, 1); + plot(x, rho); + xlim([-5, 5]); + title(plot_title); + subplot(3, 1, 2); + plot(x, psi); + xlim([-5, 5]); + subplot(3, 1, 3); + plot(x, w); + xlim([-5, 5]); + saveas(figure(fig_id), filename); end -function [rho, psi, w] = huber(x) - import gtsam.* - k = 5; +function rho = fair(x, c) + rho = c^2 * ( (abs(x) ./ c) - log(1 + (abs(x)./c)) ); +end + +function rho = huber(x, k) t = (abs(x) > k); rho = (x .^ 2) ./ 2; rho(t) = k * (abs(x(t)) - (k/2)); - est = noiseModel.mEstimator.Huber(k); - - w = zeros(size(x)); - for i = 1:size(x, 2) - w(i) = est.weight(x(i)); - end - - psi = w .* x; end -function [rho, psi, w] = cauchy(x) - import gtsam.* - c = 0.1; - +function rho = cauchy(x, c) rho = (c^2 / 2) .* log(1 + ((x./c) .^ 2)); - - est = noiseModel.mEstimator.Cauchy(c); - - w = zeros(size(x)); - for i = 1:size(x, 2) - w(i) = est.weight(x(i)); - end - - psi = w .* x; end -function [rho, psi, w] = gemancclure(x) - import gtsam.* - c = 1.0; +function rho = gemanmcclure(x, c) rho = ((x .^ 2) ./ 2) ./ (1 + x .^ 2); - - est = noiseModel.mEstimator.GemanMcClure(c); - - w = zeros(size(x)); - for i = 1:size(x, 2) - w(i) = est.weight(x(i)); - end - - psi = w .* x; end -function [rho, psi, w] = welsch(x) - import gtsam.* - c = 2.9846; +function rho = welsch(x, c) rho = (c^2 / 2) * ( 1 - exp(-(x ./ c) .^2 )); - - est = noiseModel.mEstimator.Welsch(c); - - w = zeros(size(x)); - for i = 1:size(x, 2) - w(i) = est.weight(x(i)); - end - - psi = w .* x; end -function [rho, psi, w] = tukey(x) - import gtsam.* - c = 4.6851; +function rho = tukey(x, c) t = (abs(x) > c); rho = (c^2 / 6) * (1 - (1 - (x ./ c) .^ 2 ) .^ 3 ); rho(t) = c .^ 2 / 6; - - est = noiseModel.mEstimator.Tukey(c); - - w = zeros(size(x)); - for i = 1:size(x, 2) - w(i) = est.weight(x(i)); - end - - psi = w .* x; end From 3eb8e3d9bcb3b2fdc22370828433a018579a5cd1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 25 Sep 2019 23:02:21 -0400 Subject: [PATCH 07/55] fixed function declarations which use globally declared noise models --- tests/smallExample.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tests/smallExample.h b/tests/smallExample.h index d268f2787..2b29a6d10 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -171,8 +171,8 @@ static const Key _x_=0, _y_=1, _z_=2; /* ************************************************************************* */ inline boost::shared_ptr -sharedNonlinearFactorGraph(const noiseModel::Base &noiseModel1 = kSigma0_1, - const noiseModel::Base &noiseModel2 = kSigma0_2) { +sharedNonlinearFactorGraph(const SharedNoiseModel &noiseModel1 = impl::kSigma0_1, + const SharedNoiseModel &noiseModel2 = impl::kSigma0_2) { using namespace impl; using symbol_shorthand::L; using symbol_shorthand::X; @@ -204,8 +204,8 @@ sharedNonlinearFactorGraph(const noiseModel::Base &noiseModel1 = kSigma0_1, /* ************************************************************************* */ inline NonlinearFactorGraph -createNonlinearFactorGraph(const noiseModel::Base &noiseModel1 = kSigma0_1, - const noiseModel::Base &noiseModel2 = kSigma0_2) { +createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1 = impl::kSigma0_1, + const SharedNoiseModel &noiseModel2 = impl::kSigma0_2) { return *sharedNonlinearFactorGraph(noiseModel1, noiseModel2); } From 8280a082bd25f21b2144c79f5ed583fb6d41c93a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 2 Oct 2019 04:04:34 -0400 Subject: [PATCH 08/55] implemented residual functions for common M-estimators and added corresponding tests --- gtsam.h | 6 ++ gtsam/linear/NoiseModel.cpp | 6 ++ gtsam/linear/NoiseModel.h | 40 +++++++++++++ gtsam/linear/tests/testNoiseModel.cpp | 84 +++++++++++++++++++++++++-- 4 files changed, 131 insertions(+), 5 deletions(-) diff --git a/gtsam.h b/gtsam.h index 23590f5f8..35256e92c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1364,6 +1364,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { @@ -1384,6 +1385,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { @@ -1394,6 +1396,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { @@ -1404,6 +1407,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { @@ -1414,6 +1418,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { @@ -1424,6 +1429,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; //TODO DCS and L2WithDeadZone mEstimators diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index a01233fad..d4cf2a5c2 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -858,6 +858,12 @@ double GemanMcClure::weight(double error) const { return c4/(c2error*c2error); } +double GemanMcClure::residual(double error) const { + const double c2 = c_*c_; + const double error2 = error*error; + return 0.5 * (c2 * error2) / (c2 + error2); +} + void GemanMcClure::print(const std::string &s="") const { std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 464bb38ed..2ef830c81 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include @@ -727,6 +728,7 @@ namespace gtsam { Null(const ReweightScheme reweight = Block) : Base(reweight) {} virtual ~Null() {} virtual double weight(double /*error*/) const { return 1.0; } + virtual double residual(double error) const { return error; } virtual void print(const std::string &s) const; virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; @@ -752,6 +754,12 @@ namespace gtsam { double weight(double error) const { return 1.0 / (1.0 + std::abs(error) / c_); } + double residual(double error) const { + double absError = std::abs(error); + double normalizedError = absError / c_; + double val = normalizedError - std::log(1 + normalizedError); + return c_ * c_ * val; + } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; @@ -779,6 +787,14 @@ namespace gtsam { double absError = std::abs(error); return (absError < k_) ? (1.0) : (k_ / absError); } + double residual(double error) const { + double absError = std::abs(error); + if (absError <= k_) { // |x| <= k + return error*error / 2; + } else { // |x| > k + return k_ * (absError - (k_/2)); + } + } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -809,6 +825,11 @@ namespace gtsam { double weight(double error) const { return ksquared_ / (ksquared_ + error*error); } + double residual(double error) const { + double normalizedError = error / k_; + double val = std::log(1 + (normalizedError*normalizedError)); + return ksquared_ * val * 0.5; + } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -839,6 +860,16 @@ namespace gtsam { } return 0.0; } + double residual(double error) const { + double absError = std::abs(error); + if (absError <= c_) { + double t = csquared_ - (error*error); + double val = t * t * t; + return ((csquared_*csquared_*csquared_) - val) / (6.0 * csquared_ * csquared_); + } else { + return csquared_ / 6.0; + } + } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -866,6 +897,10 @@ namespace gtsam { double xc2 = (error*error)/csquared_; return std::exp(-xc2); } + double residual(double error) const { + double xc2 = (error*error)/csquared_; + return csquared_ * 0.5 * (1 - std::exp(-xc2) ); + } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -897,6 +932,10 @@ namespace gtsam { double xc2 = (error*error)/csquared_; return std::exp(-xc2); } + double residual(double error) const { + double xc2 = (error*error)/csquared_; + return csquared_ * 0.5 * (1 - std::exp(-xc2) ); + } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -925,6 +964,7 @@ namespace gtsam { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); virtual ~GemanMcClure() {} virtual double weight(double error) const; + virtual double residual(double error) const; virtual void print(const std::string &s) const; virtual bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 85dc4735d..6419f2b9d 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -457,6 +457,22 @@ TEST(NoiseModel, WhitenInPlace) * penalties using iteratively re-weighted least squares. */ +TEST(NoiseModel, robustFunctionFair) +{ + const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; + const mEstimator::Fair::shared_ptr fair = mEstimator::Fair::Create(k); + DOUBLES_EQUAL(0.8333333333333333, fair->weight(error1), 1e-8); + DOUBLES_EQUAL(0.3333333333333333, fair->weight(error2), 1e-8); + // Test negative value to ensure we take absolute value of error. + 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); +} + TEST(NoiseModel, robustFunctionHuber) { const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; @@ -466,16 +482,74 @@ TEST(NoiseModel, robustFunctionHuber) // Test negative value to ensure we take absolute value of error. 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); +} + +TEST(NoiseModel, robustFunctionCauchy) +{ + const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; + const mEstimator::Cauchy::shared_ptr cauchy = mEstimator::Cauchy::Create(k); + DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error1), 1e-8); + DOUBLES_EQUAL(0.2000, cauchy->weight(error2), 1e-8); + // Test negative value to ensure we take absolute value of error. + 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); } TEST(NoiseModel, robustFunctionGemanMcClure) { - const double k = 1.0, error1 = 1.0, error2 = 10.0; + const double k = 1.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k); - const double weight1 = gmc->weight(error1), - weight2 = gmc->weight(error2); - DOUBLES_EQUAL(0.25 , weight1, 1e-8); - DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8); + DOUBLES_EQUAL(0.25 , gmc->weight(error1), 1e-8); + DOUBLES_EQUAL(9.80296e-5, gmc->weight(error2), 1e-8); + 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); +} + +TEST(NoiseModel, robustFunctionWelsch) +{ + const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; + const mEstimator::Welsch::shared_ptr welsch = mEstimator::Welsch::Create(k); + DOUBLES_EQUAL(0.960789439152323, welsch->weight(error1), 1e-8); + DOUBLES_EQUAL(0.018315638888734, welsch->weight(error2), 1e-8); + // Test negative value to ensure we take absolute value of error. + 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); +} + +TEST(NoiseModel, robustFunctionTukey) +{ + const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; + const mEstimator::Tukey::shared_ptr tukey = mEstimator::Tukey::Create(k); + DOUBLES_EQUAL(0.9216, tukey->weight(error1), 1e-8); + DOUBLES_EQUAL(0.0, tukey->weight(error2), 1e-8); + // Test negative value to ensure we take absolute value of error. + 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); } TEST(NoiseModel, robustFunctionDCS) From 3fad1fa81bda4cf847d6729f6646fa4efbd595af Mon Sep 17 00:00:00 2001 From: Ellon Paiva Mendes Date: Thu, 3 Oct 2019 17:45:58 +0200 Subject: [PATCH 09/55] Install GTSAMConfigVersion.cmake --- cmake/GtsamMakeConfigFile.cmake | 28 ++++++++++++++++++++++++++-- gtsam_extra.cmake.in | 5 ++--- 2 files changed, 28 insertions(+), 5 deletions(-) diff --git a/cmake/GtsamMakeConfigFile.cmake b/cmake/GtsamMakeConfigFile.cmake index 74081b58a..2fff888ef 100644 --- a/cmake/GtsamMakeConfigFile.cmake +++ b/cmake/GtsamMakeConfigFile.cmake @@ -20,13 +20,37 @@ function(GtsamMakeConfigFile PACKAGE_NAME) set(EXTRA_FILE "_does_not_exist_") endif() + # GTSAM defines its version variable as GTSAM_VERSION_STRING while other + # projects may define it as _VERSION. Since this file is + # installed on the system as part of GTSAMCMakeTools and may be useful in + # external projects, we need to handle both cases of version variable naming + # here. + if(NOT DEFINED ${PACKAGE_NAME}_VERSION AND DEFINED ${PACKAGE_NAME}_VERSION_STRING) + set(${PACKAGE_NAME}_VERSION ${${PACKAGE_NAME}_VERSION_STRING}) + endif() + + # Version file + include(CMakePackageConfigHelpers) + write_basic_package_version_file( + "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}ConfigVersion.cmake" + VERSION ${${PACKAGE_NAME}_VERSION} + COMPATIBILITY SameMajorVersion + ) + + # Config file file(RELATIVE_PATH CONF_REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/include") file(RELATIVE_PATH CONF_REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/lib") configure_file(${GTSAM_CONFIG_TEMPLATE_PATH}/Config.cmake.in "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" @ONLY) message(STATUS "Wrote ${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake") - # Install config and exports files (for find scripts) - install(FILES "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" DESTINATION "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}") + # Install config, version and exports files (for find scripts) + install( + FILES + "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" + "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}ConfigVersion.cmake" + DESTINATION + "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" + ) install(EXPORT ${PACKAGE_NAME}-exports DESTINATION ${DEF_INSTALL_CMAKE_DIR}) endfunction() diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index b7990b3cc..8a9a13648 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -1,8 +1,7 @@ # Extra CMake definitions for GTSAM -set (GTSAM_VERSION_MAJOR @GTSAM_VERSION_MAJOR@) -set (GTSAM_VERSION_MINOR @GTSAM_VERSION_MINOR@) -set (GTSAM_VERSION_PATCH @GTSAM_VERSION_PATCH@) +# All version variables are handled by GTSAMConfigVersion.cmake, except these +# two below. We continue to set them here in case someone uses them set (GTSAM_VERSION_NUMERIC @GTSAM_VERSION_NUMERIC@) set (GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@") From bbf007e4e4e6db048604f879bce22255b5f480ac Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 7 Oct 2019 11:15:31 +0200 Subject: [PATCH 10/55] Remove obsolete cmake FindXX modules. Exported config files are preferred over modules, and easier to maintain. --- cmake/obsolete/FindGTSAM.cmake | 88 ------------------------- cmake/obsolete/FindGTSAM_UNSTABLE.cmake | 88 ------------------------- 2 files changed, 176 deletions(-) delete mode 100644 cmake/obsolete/FindGTSAM.cmake delete mode 100644 cmake/obsolete/FindGTSAM_UNSTABLE.cmake diff --git a/cmake/obsolete/FindGTSAM.cmake b/cmake/obsolete/FindGTSAM.cmake deleted file mode 100644 index 895eb853b..000000000 --- a/cmake/obsolete/FindGTSAM.cmake +++ /dev/null @@ -1,88 +0,0 @@ -# This is FindGTSAM.cmake -# DEPRECIATED: Use config file approach to pull in targets from gtsam -# CMake module to locate the GTSAM package -# -# The following cache variables may be set before calling this script: -# -# GTSAM_DIR (or GTSAM_ROOT): (Optional) The install prefix OR source tree of gtsam (e.g. /usr/local or src/gtsam) -# GTSAM_BUILD_NAME: (Optional) If compiling against a source tree, the name of the build directory -# within it (e.g build-debug). Without this defined, this script tries to -# intelligently find the build directory based on the project's build directory name -# or based on the build type (Debug/Release/etc). -# -# The following variables will be defined: -# -# GTSAM_FOUND : TRUE if the package has been successfully found -# GTSAM_INCLUDE_DIR : paths to GTSAM's INCLUDE directories -# GTSAM_LIBS : paths to GTSAM's libraries -# -# NOTES on compiling against an uninstalled GTSAM build tree: -# - A GTSAM source tree will be automatically searched for in the directory -# 'gtsam' next to your project directory, after searching -# CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr. -# - The build directory will be searched first with the same name as your -# project's build directory, e.g. if you build from 'MyProject/build-optimized', -# 'gtsam/build-optimized' will be searched first. Next, a build directory for -# your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is -# 'Release', then 'gtsam/build-release' will be searched next. Finally, plain -# 'gtsam/build' will be searched. -# - You can control the gtsam build directory name directly by defining the CMake -# cache variable 'GTSAM_BUILD_NAME', then only 'gtsam/${GTSAM_BUILD_NAME} will -# be searched. -# - Use the standard CMAKE_PREFIX_PATH, or GTSAM_DIR, to find a specific gtsam -# directory. - -# Get path suffixes to help look for gtsam -if(GTSAM_BUILD_NAME) - set(gtsam_build_names "${GTSAM_BUILD_NAME}/gtsam") -else() - # lowercase build type - string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix) - # build suffix of this project - get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME) - - set(gtsam_build_names "${my_build_name}/gtsam" "build-${build_type_suffix}/gtsam" "build/gtsam") -endif() - -# Use GTSAM_ROOT or GTSAM_DIR equivalently -if(GTSAM_ROOT AND NOT GTSAM_DIR) - set(GTSAM_DIR "${GTSAM_ROOT}") -endif() - -if(GTSAM_DIR) - # Find include dirs - find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h - PATHS "${GTSAM_DIR}/include" "${GTSAM_DIR}" NO_DEFAULT_PATH - DOC "GTSAM include directories") - - # Find libraries - find_library(GTSAM_LIBS NAMES gtsam - HINTS "${GTSAM_DIR}/lib" "${GTSAM_DIR}" NO_DEFAULT_PATH - PATH_SUFFIXES ${gtsam_build_names} - DOC "GTSAM libraries") -else() - # Find include dirs - set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/include /usr/include) - find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h - PATHS ${extra_include_paths} - DOC "GTSAM include directories") - if(NOT GTSAM_INCLUDE_DIR) - message(STATUS "Searched for gtsam headers in default paths plus ${extra_include_paths}") - endif() - - # Find libraries - find_library(GTSAM_LIBS NAMES gtsam - HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/lib /usr/lib - PATH_SUFFIXES ${gtsam_build_names} - DOC "GTSAM libraries") -endif() - -# handle the QUIETLY and REQUIRED arguments and set GTSAM_FOUND to TRUE -# if all listed variables are TRUE -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(GTSAM DEFAULT_MSG - GTSAM_LIBS GTSAM_INCLUDE_DIR) - - - - diff --git a/cmake/obsolete/FindGTSAM_UNSTABLE.cmake b/cmake/obsolete/FindGTSAM_UNSTABLE.cmake deleted file mode 100644 index 42cc9c8b0..000000000 --- a/cmake/obsolete/FindGTSAM_UNSTABLE.cmake +++ /dev/null @@ -1,88 +0,0 @@ -# This is FindGTSAM_UNSTABLE.cmake -# DEPRECIATED: Use config file approach to pull in targets from gtsam -# CMake module to locate the GTSAM_UNSTABLE package -# -# The following cache variables may be set before calling this script: -# -# GTSAM_UNSTABLE_DIR (or GTSAM_UNSTABLE_ROOT): (Optional) The install prefix OR source tree of gtsam_unstable (e.g. /usr/local or src/gtsam_unstable) -# GTSAM_UNSTABLE_BUILD_NAME: (Optional) If compiling against a source tree, the name of the build directory -# within it (e.g build-debug). Without this defined, this script tries to -# intelligently find the build directory based on the project's build directory name -# or based on the build type (Debug/Release/etc). -# -# The following variables will be defined: -# -# GTSAM_UNSTABLE_FOUND : TRUE if the package has been successfully found -# GTSAM_UNSTABLE_INCLUDE_DIR : paths to GTSAM_UNSTABLE's INCLUDE directories -# GTSAM_UNSTABLE_LIBS : paths to GTSAM_UNSTABLE's libraries -# -# NOTES on compiling against an uninstalled GTSAM_UNSTABLE build tree: -# - A GTSAM_UNSTABLE source tree will be automatically searched for in the directory -# 'gtsam_unstable' next to your project directory, after searching -# CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr. -# - The build directory will be searched first with the same name as your -# project's build directory, e.g. if you build from 'MyProject/build-optimized', -# 'gtsam_unstable/build-optimized' will be searched first. Next, a build directory for -# your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is -# 'Release', then 'gtsam_unstable/build-release' will be searched next. Finally, plain -# 'gtsam_unstable/build' will be searched. -# - You can control the gtsam build directory name directly by defining the CMake -# cache variable 'GTSAM_UNSTABLE_BUILD_NAME', then only 'gtsam/${GTSAM_UNSTABLE_BUILD_NAME} will -# be searched. -# - Use the standard CMAKE_PREFIX_PATH, or GTSAM_UNSTABLE_DIR, to find a specific gtsam -# directory. - -# Get path suffixes to help look for gtsam_unstable -if(GTSAM_UNSTABLE_BUILD_NAME) - set(gtsam_unstable_build_names "${GTSAM_UNSTABLE_BUILD_NAME}/gtsam_unstable") -else() - # lowercase build type - string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix) - # build suffix of this project - get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME) - - set(gtsam_unstable_build_names "${my_build_name}/gtsam_unstable" "build-${build_type_suffix}/gtsam_unstable" "build/gtsam_unstable") -endif() - -# Use GTSAM_UNSTABLE_ROOT or GTSAM_UNSTABLE_DIR equivalently -if(GTSAM_UNSTABLE_ROOT AND NOT GTSAM_UNSTABLE_DIR) - set(GTSAM_UNSTABLE_DIR "${GTSAM_UNSTABLE_ROOT}") -endif() - -if(GTSAM_UNSTABLE_DIR) - # Find include dirs - find_path(GTSAM_UNSTABLE_INCLUDE_DIR gtsam_unstable/base/DSF.h - PATHS "${GTSAM_UNSTABLE_DIR}/include" "${GTSAM_UNSTABLE_DIR}" NO_DEFAULT_PATH - DOC "GTSAM_UNSTABLE include directories") - - # Find libraries - find_library(GTSAM_UNSTABLE_LIBS NAMES gtsam_unstable - HINTS "${GTSAM_UNSTABLE_DIR}/lib" "${GTSAM_UNSTABLE_DIR}" NO_DEFAULT_PATH - PATH_SUFFIXES ${gtsam_unstable_build_names} - DOC "GTSAM_UNSTABLE libraries") -else() - # Find include dirs - set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/include /usr/include) - find_path(GTSAM_UNSTABLE_INCLUDE_DIR gtsam_unstable/base/DSF.h - PATHS ${extra_include_paths} - DOC "GTSAM_UNSTABLE include directories") - if(NOT GTSAM_UNSTABLE_INCLUDE_DIR) - message(STATUS "Searched for gtsam_unstable headers in default paths plus ${extra_include_paths}") - endif() - - # Find libraries - find_library(GTSAM_UNSTABLE_LIBS NAMES gtsam_unstable - HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/lib /usr/lib - PATH_SUFFIXES ${gtsam_unstable_build_names} - DOC "GTSAM_UNSTABLE libraries") -endif() - -# handle the QUIETLY and REQUIRED arguments and set GTSAM_UNSTABLE_FOUND to TRUE -# if all listed variables are TRUE -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(GTSAM_UNSTABLE DEFAULT_MSG - GTSAM_UNSTABLE_LIBS GTSAM_UNSTABLE_INCLUDE_DIR) - - - - From b10963802c13893611d5a88894879bed47adf9e0 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 7 Oct 2019 11:21:22 +0200 Subject: [PATCH 11/55] Revert "Fix cmake handling newer boost versions (Closes: #442)" This reverts commit a0fce4257fe4e5fe483ec9d048ee62bac803df8f. --- CMakeLists.txt | 33 ++++++++------------------------- CppUnitLite/CMakeLists.txt | 2 +- gtsam/CMakeLists.txt | 4 ---- wrap/CMakeLists.txt | 13 +++---------- 4 files changed, 12 insertions(+), 40 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b0457cb1c..7e8df35e9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -171,39 +171,22 @@ if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILE message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.") endif() -# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) - -# JLBC: This was once updated to target-based names (Boost::xxx), but it caused -# problems with Boost versions newer than FindBoost.cmake was prepared to handle, -# so we downgraded this to classic filenames-based variables, and manually adding -# the target_include_directories(xxx ${Boost_INCLUDE_DIR}) +# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) set(GTSAM_BOOST_LIBRARIES - optimized ${Boost_SERIALIZATION_LIBRARY_RELEASE} - optimized ${Boost_SYSTEM_LIBRARY_RELEASE} - optimized ${Boost_FILESYSTEM_LIBRARY_RELEASE} - optimized ${Boost_THREAD_LIBRARY_RELEASE} - optimized ${Boost_DATE_TIME_LIBRARY_RELEASE} - optimized ${Boost_REGEX_LIBRARY_RELEASE} - debug ${Boost_SERIALIZATION_LIBRARY_DEBUG} - debug ${Boost_SYSTEM_LIBRARY_DEBUG} - debug ${Boost_FILESYSTEM_LIBRARY_DEBUG} - debug ${Boost_THREAD_LIBRARY_DEBUG} - debug ${Boost_DATE_TIME_LIBRARY_DEBUG} - debug ${Boost_REGEX_LIBRARY_DEBUG} + Boost::serialization + Boost::system + Boost::filesystem + Boost::thread + Boost::date_time + Boost::regex ) -message(STATUS "GTSAM_BOOST_LIBRARIES: ${GTSAM_BOOST_LIBRARIES}") if (GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) else() if(Boost_TIMER_LIBRARY) - list(APPEND GTSAM_BOOST_LIBRARIES - optimized ${Boost_TIMER_LIBRARY_RELEASE} - optimized ${Boost_CHRONO_LIBRARY_RELEASE} - debug ${Boost_TIMER_LIBRARY_DEBUG} - debug ${Boost_CHRONO_LIBRARY_DEBUG} - ) + list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) else() list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt index 72651aca9..f52841274 100644 --- a/CppUnitLite/CMakeLists.txt +++ b/CppUnitLite/CMakeLists.txt @@ -6,7 +6,7 @@ file(GLOB cppunitlite_src "*.cpp") add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers}) list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) -target_include_directories(CppUnitLite PUBLIC ${Boost_INCLUDE_DIR}) # boost/lexical_cast.h +target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index b4a33943e..97f7a2c41 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -103,11 +103,7 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}") # BUILD_SHARED_LIBS automatically defines static/shared libs: add_library(gtsam ${gtsam_srcs}) - -# Boost: target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES}) -target_include_directories(gtsam PUBLIC ${Boost_INCLUDE_DIR}) -# Other deps: target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES}) # Apply build flags: diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 6e046e3ab..ac7a35fcd 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -1,14 +1,9 @@ # Build/install Wrap set(WRAP_BOOST_LIBRARIES - optimized - ${Boost_FILESYSTEM_LIBRARY_RELEASE} - ${Boost_SYSTEM_LIBRARY_RELEASE} - ${Boost_THREAD_LIBRARY_RELEASE} - debug - ${Boost_FILESYSTEM_LIBRARY_DEBUG} - ${Boost_SYSTEM_LIBRARY_DEBUG} - ${Boost_THREAD_LIBRARY_DEBUG} + Boost::system + Boost::filesystem + Boost::thread ) # Allow for disabling serialization to handle errors related to Clang's linker @@ -30,8 +25,6 @@ endif() gtsam_apply_build_flags(wrap_lib) target_link_libraries(wrap_lib ${WRAP_BOOST_LIBRARIES}) -target_include_directories(wrap_lib PUBLIC ${Boost_INCLUDE_DIR}) - gtsam_assign_source_folders(${wrap_srcs} ${wrap_headers}) add_executable(wrap wrap.cpp) target_link_libraries(wrap PRIVATE wrap_lib) From 3e01411010babc96b0d1d755dea932991c08396b Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 7 Oct 2019 11:36:06 +0200 Subject: [PATCH 12/55] Import FindBoost from CMake v3.15.4 --- cmake/FindBoost.cmake | 2337 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 2337 insertions(+) create mode 100644 cmake/FindBoost.cmake diff --git a/cmake/FindBoost.cmake b/cmake/FindBoost.cmake new file mode 100644 index 000000000..078000f22 --- /dev/null +++ b/cmake/FindBoost.cmake @@ -0,0 +1,2337 @@ +# Distributed under the OSI-approved BSD 3-Clause License. See accompanying +# file Copyright.txt or https://cmake.org/licensing for details. + +#[=======================================================================[.rst: +FindBoost +--------- + +Find Boost include dirs and libraries + +Use this module by invoking find_package with the form:: + + find_package(Boost + [version] [EXACT] # Minimum or EXACT version e.g. 1.67.0 + [REQUIRED] # Fail with error if Boost is not found + [COMPONENTS ...] # Boost libraries by their canonical name + # e.g. "date_time" for "libboost_date_time" + [OPTIONAL_COMPONENTS ...] + # Optional Boost libraries by their canonical name) + ) # e.g. "date_time" for "libboost_date_time" + +This module finds headers and requested component libraries OR a CMake +package configuration file provided by a "Boost CMake" build. For the +latter case skip to the "Boost CMake" section below. For the former +case results are reported in variables:: + + Boost_FOUND - True if headers and requested libraries were found + Boost_INCLUDE_DIRS - Boost include directories + Boost_LIBRARY_DIRS - Link directories for Boost libraries + Boost_LIBRARIES - Boost component libraries to be linked + Boost__FOUND - True if component was found ( is upper-case) + Boost__LIBRARY - Libraries to link for component (may include + target_link_libraries debug/optimized keywords) + Boost_VERSION_MACRO - BOOST_VERSION value from boost/version.hpp + Boost_VERSION_STRING - Boost version number in x.y.z format + Boost_VERSION - if CMP0093 NEW => same as Boost_VERSION_STRING + if CMP0093 OLD or unset => same as Boost_VERSION_MACRO + Boost_LIB_VERSION - Version string appended to library filenames + Boost_VERSION_MAJOR - Boost major version number (X in X.y.z) + alias: Boost_MAJOR_VERSION + Boost_VERSION_MINOR - Boost minor version number (Y in x.Y.z) + alias: Boost_MINOR_VERSION + Boost_VERSION_PATCH - Boost subminor version number (Z in x.y.Z) + alias: Boost_SUBMINOR_VERSION + Boost_VERSION_COUNT - Amount of version components (3) + Boost_LIB_DIAGNOSTIC_DEFINITIONS (Windows) + - Pass to add_definitions() to have diagnostic + information about Boost's automatic linking + displayed during compilation + +Note that Boost Python components require a Python version suffix +(Boost 1.67 and later), e.g. ``python36`` or ``python27`` for the +versions built against Python 3.6 and 2.7, respectively. This also +applies to additional components using Python including +``mpi_python`` and ``numpy``. Earlier Boost releases may use +distribution-specific suffixes such as ``2``, ``3`` or ``2.7``. +These may also be used as suffixes, but note that they are not +portable. + +This module reads hints about search locations from variables:: + + BOOST_ROOT - Preferred installation prefix + (or BOOSTROOT) + BOOST_INCLUDEDIR - Preferred include directory e.g. /include + BOOST_LIBRARYDIR - Preferred library directory e.g. /lib + Boost_NO_SYSTEM_PATHS - Set to ON to disable searching in locations not + specified by these hint variables. Default is OFF. + Boost_ADDITIONAL_VERSIONS + - List of Boost versions not known to this module + (Boost install locations may contain the version) + +and saves search results persistently in CMake cache entries:: + + Boost_INCLUDE_DIR - Directory containing Boost headers + Boost_LIBRARY_DIR_RELEASE - Directory containing release Boost libraries + Boost_LIBRARY_DIR_DEBUG - Directory containing debug Boost libraries + Boost__LIBRARY_DEBUG - Component library debug variant + Boost__LIBRARY_RELEASE - Component library release variant + +The following :prop_tgt:`IMPORTED` targets are also defined:: + + Boost::headers - Target for header-only dependencies + (Boost include directory) + alias: Boost::boost + Boost:: - Target for specific component dependency + (shared or static library); is lower- + case + Boost::diagnostic_definitions - interface target to enable diagnostic + information about Boost's automatic linking + during compilation (adds BOOST_LIB_DIAGNOSTIC) + Boost::disable_autolinking - interface target to disable automatic + linking with MSVC (adds BOOST_ALL_NO_LIB) + Boost::dynamic_linking - interface target to enable dynamic linking + linking with MSVC (adds BOOST_ALL_DYN_LINK) + +Implicit dependencies such as ``Boost::filesystem`` requiring +``Boost::system`` will be automatically detected and satisfied, even +if system is not specified when using :command:`find_package` and if +``Boost::system`` is not added to :command:`target_link_libraries`. If using +``Boost::thread``, then ``Threads::Threads`` will also be added automatically. + +It is important to note that the imported targets behave differently +than variables created by this module: multiple calls to +:command:`find_package(Boost)` in the same directory or sub-directories with +different options (e.g. static or shared) will not override the +values of the targets created by the first call. + +Users may set these hints or results as ``CACHE`` entries. Projects +should not read these entries directly but instead use the above +result variables. Note that some hint names start in upper-case +"BOOST". One may specify these as environment variables if they are +not specified as CMake variables or cache entries. + +This module first searches for the ``Boost`` header files using the above +hint variables (excluding ``BOOST_LIBRARYDIR``) and saves the result in +``Boost_INCLUDE_DIR``. Then it searches for requested component libraries +using the above hints (excluding ``BOOST_INCLUDEDIR`` and +``Boost_ADDITIONAL_VERSIONS``), "lib" directories near ``Boost_INCLUDE_DIR``, +and the library name configuration settings below. It saves the +library directories in ``Boost_LIBRARY_DIR_DEBUG`` and +``Boost_LIBRARY_DIR_RELEASE`` and individual library +locations in ``Boost__LIBRARY_DEBUG`` and ``Boost__LIBRARY_RELEASE``. +When one changes settings used by previous searches in the same build +tree (excluding environment variables) this module discards previous +search results affected by the changes and searches again. + +Boost libraries come in many variants encoded in their file name. +Users or projects may tell this module which variant to find by +setting variables:: + + Boost_USE_DEBUG_LIBS - Set to ON or OFF to specify whether to search + and use the debug libraries. Default is ON. + Boost_USE_RELEASE_LIBS - Set to ON or OFF to specify whether to search + and use the release libraries. Default is ON. + Boost_USE_MULTITHREADED - Set to OFF to use the non-multithreaded + libraries ('mt' tag). Default is ON. + Boost_USE_STATIC_LIBS - Set to ON to force the use of the static + libraries. Default is OFF. + Boost_USE_STATIC_RUNTIME - Set to ON or OFF to specify whether to use + libraries linked statically to the C++ runtime + ('s' tag). Default is platform dependent. + Boost_USE_DEBUG_RUNTIME - Set to ON or OFF to specify whether to use + libraries linked to the MS debug C++ runtime + ('g' tag). Default is ON. + Boost_USE_DEBUG_PYTHON - Set to ON to use libraries compiled with a + debug Python build ('y' tag). Default is OFF. + Boost_USE_STLPORT - Set to ON to use libraries compiled with + STLPort ('p' tag). Default is OFF. + Boost_USE_STLPORT_DEPRECATED_NATIVE_IOSTREAMS + - Set to ON to use libraries compiled with + STLPort deprecated "native iostreams" + ('n' tag). Default is OFF. + Boost_COMPILER - Set to the compiler-specific library suffix + (e.g. "-gcc43"). Default is auto-computed + for the C++ compiler in use. A list may be + used if multiple compatible suffixes should + be tested for, in decreasing order of + preference. + Boost_ARCHITECTURE - Set to the architecture-specific library suffix + (e.g. "-x64"). Default is auto-computed for the + C++ compiler in use. + Boost_THREADAPI - Suffix for "thread" component library name, + such as "pthread" or "win32". Names with + and without this suffix will both be tried. + Boost_NAMESPACE - Alternate namespace used to build boost with + e.g. if set to "myboost", will search for + myboost_thread instead of boost_thread. + +Other variables one may set to control this module are:: + + Boost_DEBUG - Set to ON to enable debug output from FindBoost. + Please enable this before filing any bug report. + Boost_REALPATH - Set to ON to resolve symlinks for discovered + libraries to assist with packaging. For example, + the "system" component library may be resolved to + "/usr/lib/libboost_system.so.1.67.0" instead of + "/usr/lib/libboost_system.so". This does not + affect linking and should not be enabled unless + the user needs this information. + Boost_LIBRARY_DIR - Default value for Boost_LIBRARY_DIR_RELEASE and + Boost_LIBRARY_DIR_DEBUG. + +On Visual Studio and Borland compilers Boost headers request automatic +linking to corresponding libraries. This requires matching libraries +to be linked explicitly or available in the link library search path. +In this case setting ``Boost_USE_STATIC_LIBS`` to ``OFF`` may not achieve +dynamic linking. Boost automatic linking typically requests static +libraries with a few exceptions (such as ``Boost.Python``). Use:: + + add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS}) + +to ask Boost to report information about automatic linking requests. + +Example to find Boost headers only:: + + find_package(Boost 1.36.0) + if(Boost_FOUND) + include_directories(${Boost_INCLUDE_DIRS}) + add_executable(foo foo.cc) + endif() + +Example to find Boost libraries and use imported targets:: + + find_package(Boost 1.56 REQUIRED COMPONENTS + date_time filesystem iostreams) + add_executable(foo foo.cc) + target_link_libraries(foo Boost::date_time Boost::filesystem + Boost::iostreams) + +Example to find Boost Python 3.6 libraries and use imported targets:: + + find_package(Boost 1.67 REQUIRED COMPONENTS + python36 numpy36) + add_executable(foo foo.cc) + target_link_libraries(foo Boost::python36 Boost::numpy36) + +Example to find Boost headers and some *static* (release only) libraries:: + + set(Boost_USE_STATIC_LIBS ON) # only find static libs + set(Boost_USE_DEBUG_LIBS OFF) # ignore debug libs and + set(Boost_USE_RELEASE_LIBS ON) # only find release libs + set(Boost_USE_MULTITHREADED ON) + set(Boost_USE_STATIC_RUNTIME OFF) + find_package(Boost 1.66.0 COMPONENTS date_time filesystem system ...) + if(Boost_FOUND) + include_directories(${Boost_INCLUDE_DIRS}) + add_executable(foo foo.cc) + target_link_libraries(foo ${Boost_LIBRARIES}) + endif() + +Boost CMake +^^^^^^^^^^^ + +If Boost was built using the boost-cmake project or from Boost 1.70.0 on +it provides a package configuration file for use with find_package's config mode. +This module looks for the package configuration file called +``BoostConfig.cmake`` or ``boost-config.cmake`` and stores the result in +``CACHE`` entry "Boost_DIR". If found, the package configuration file is loaded +and this module returns with no further action. See documentation of +the Boost CMake package configuration for details on what it provides. + +Set ``Boost_NO_BOOST_CMAKE`` to ``ON``, to disable the search for boost-cmake. +#]=======================================================================] + +# The FPHSA helper provides standard way of reporting final search results to +# the user including the version and component checks. +include(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake) + +# Save project's policies +cmake_policy(PUSH) +cmake_policy(SET CMP0057 NEW) # if IN_LIST + +function(_boost_get_existing_target component target_var) + set(names "${component}") + if(component MATCHES "^([a-z_]*)(python|numpy)([1-9])\\.?([0-9])?$") + # handle pythonXY and numpyXY versioned components and also python X.Y, mpi_python etc. + list(APPEND names + "${CMAKE_MATCH_1}${CMAKE_MATCH_2}" # python + "${CMAKE_MATCH_1}${CMAKE_MATCH_2}${CMAKE_MATCH_3}" # pythonX + "${CMAKE_MATCH_1}${CMAKE_MATCH_2}${CMAKE_MATCH_3}${CMAKE_MATCH_4}" #pythonXY + ) + endif() + # https://github.com/boost-cmake/boost-cmake uses boost::file_system etc. + # So handle similar constructions of target names + string(TOLOWER "${component}" lower_component) + list(APPEND names "${lower_component}") + foreach(prefix Boost boost) + foreach(name IN LISTS names) + if(TARGET "${prefix}::${name}") + # The target may be an INTERFACE library that wraps around a single other + # target for compatibility. Unwrap this layer so we can extract real info. + if("${name}" MATCHES "^(python|numpy|mpi_python)([1-9])([0-9])$") + set(name_nv "${CMAKE_MATCH_1}") + if(TARGET "${prefix}::${name_nv}") + get_property(type TARGET "${prefix}::${name}" PROPERTY TYPE) + if(type STREQUAL "INTERFACE_LIBRARY") + get_property(lib TARGET "${prefix}::${name}" PROPERTY INTERFACE_LINK_LIBRARIES) + if("${lib}" STREQUAL "${prefix}::${name_nv}") + set(${target_var} "${prefix}::${name_nv}" PARENT_SCOPE) + return() + endif() + endif() + endif() + endif() + set(${target_var} "${prefix}::${name}" PARENT_SCOPE) + return() + endif() + endforeach() + endforeach() + set(${target_var} "" PARENT_SCOPE) +endfunction() + +function(_boost_get_canonical_target_name component target_var) + string(TOLOWER "${component}" component) + if(component MATCHES "^([a-z_]*)(python|numpy)([1-9])\\.?([0-9])?$") + # handle pythonXY and numpyXY versioned components and also python X.Y, mpi_python etc. + set(${target_var} "Boost::${CMAKE_MATCH_1}${CMAKE_MATCH_2}" PARENT_SCOPE) + else() + set(${target_var} "Boost::${component}" PARENT_SCOPE) + endif() +endfunction() + +macro(_boost_set_in_parent_scope name value) + # Set a variable in parent scope and make it visibile in current scope + set(${name} "${value}" PARENT_SCOPE) + set(${name} "${value}") +endmacro() + +macro(_boost_set_if_unset name value) + if(NOT ${name}) + _boost_set_in_parent_scope(${name} "${value}") + endif() +endmacro() + +macro(_boost_set_cache_if_unset name value) + if(NOT ${name}) + set(${name} "${value}" CACHE STRING "" FORCE) + endif() +endmacro() + +macro(_boost_append_include_dir target) + get_target_property(inc "${target}" INTERFACE_INCLUDE_DIRECTORIES) + if(inc) + list(APPEND include_dirs "${inc}") + endif() +endmacro() + +function(_boost_set_legacy_variables_from_config) + # Set legacy variables for compatibility if not set + set(include_dirs "") + set(library_dirs "") + set(libraries "") + # Header targets Boost::headers or Boost::boost + foreach(comp headers boost) + _boost_get_existing_target(${comp} target) + if(target) + _boost_append_include_dir("${target}") + endif() + endforeach() + # Library targets + foreach(comp IN LISTS Boost_FIND_COMPONENTS) + string(TOUPPER ${comp} uppercomp) + # Overwrite if set + _boost_set_in_parent_scope(Boost_${uppercomp}_FOUND "${Boost_${comp}_FOUND}") + if(Boost_${comp}_FOUND) + _boost_get_existing_target(${comp} target) + if(NOT target) + if(Boost_DEBUG OR Boost_VERBOSE) + message(WARNING "Could not find imported target for required component '${comp}'. Legacy variables for this component might be missing. Refer to the documentation of your Boost installation for help on variables to use.") + endif() + continue() + endif() + _boost_append_include_dir("${target}") + _boost_set_if_unset(Boost_${uppercomp}_LIBRARY "${target}") + _boost_set_if_unset(Boost_${uppercomp}_LIBRARIES "${target}") # Very old legacy variable + list(APPEND libraries "${target}") + get_property(type TARGET "${target}" PROPERTY TYPE) + if(NOT type STREQUAL "INTERFACE_LIBRARY") + foreach(cfg RELEASE DEBUG) + get_target_property(lib ${target} IMPORTED_LOCATION_${cfg}) + if(lib) + get_filename_component(lib_dir "${lib}" DIRECTORY) + list(APPEND library_dirs ${lib_dir}) + _boost_set_cache_if_unset(Boost_${uppercomp}_LIBRARY_${cfg} "${lib}") + endif() + endforeach() + elseif(Boost_DEBUG OR Boost_VERBOSE) + # For projects using only the Boost::* targets this warning can be safely ignored. + message(WARNING "Imported target '${target}' for required component '${comp}' has no artifact. Legacy variables for this component might be missing. Refer to the documentation of your Boost installation for help on variables to use.") + endif() + _boost_get_canonical_target_name("${comp}" canonical_target) + if(NOT TARGET "${canonical_target}") + add_library("${canonical_target}" INTERFACE IMPORTED) + target_link_libraries("${canonical_target}" INTERFACE "${target}") + endif() + endif() + endforeach() + list(REMOVE_DUPLICATES include_dirs) + list(REMOVE_DUPLICATES library_dirs) + _boost_set_if_unset(Boost_INCLUDE_DIRS "${include_dirs}") + _boost_set_if_unset(Boost_LIBRARY_DIRS "${library_dirs}") + _boost_set_if_unset(Boost_LIBRARIES "${libraries}") + _boost_set_if_unset(Boost_VERSION_STRING "${Boost_VERSION_MAJOR}.${Boost_VERSION_MINOR}.${Boost_VERSION_PATCH}") + find_path(Boost_INCLUDE_DIR + NAMES boost/version.hpp boost/config.hpp + HINTS ${Boost_INCLUDE_DIRS} + NO_DEFAULT_PATH + ) + if(NOT Boost_VERSION_MACRO OR NOT Boost_LIB_VERSION) + set(version_file ${Boost_INCLUDE_DIR}/boost/version.hpp) + if(EXISTS "${version_file}") + file(STRINGS "${version_file}" contents REGEX "#define BOOST_(LIB_)?VERSION ") + if(contents MATCHES "#define BOOST_VERSION ([0-9]+)") + _boost_set_if_unset(Boost_VERSION_MACRO "${CMAKE_MATCH_1}") + endif() + if(contents MATCHES "#define BOOST_LIB_VERSION \"([0-9_]+)\"") + _boost_set_if_unset(Boost_LIB_VERSION "${CMAKE_MATCH_1}") + endif() + endif() + endif() + _boost_set_if_unset(Boost_MAJOR_VERSION ${Boost_VERSION_MAJOR}) + _boost_set_if_unset(Boost_MINOR_VERSION ${Boost_VERSION_MINOR}) + _boost_set_if_unset(Boost_SUBMINOR_VERSION ${Boost_VERSION_PATCH}) + if(WIN32) + _boost_set_if_unset(Boost_LIB_DIAGNOSTIC_DEFINITIONS "-DBOOST_LIB_DIAGNOSTIC") + endif() + if(NOT TARGET Boost::headers) + add_library(Boost::headers INTERFACE IMPORTED) + target_include_directories(Boost::headers INTERFACE ${Boost_INCLUDE_DIRS}) + endif() + # Legacy targets w/o functionality as all handled by defined targets + foreach(lib diagnostic_definitions disable_autolinking dynamic_linking) + if(NOT TARGET Boost::${lib}) + add_library(Boost::${lib} INTERFACE IMPORTED) + endif() + endforeach() + if(NOT TARGET Boost::boost) + add_library(Boost::boost INTERFACE IMPORTED) + target_link_libraries(Boost::boost INTERFACE Boost::headers) + endif() +endfunction() + +#------------------------------------------------------------------------------- +# Before we go searching, check whether a boost cmake package is available, unless +# the user specifically asked NOT to search for one. +# +# If Boost_DIR is set, this behaves as any find_package call would. If not, +# it looks at BOOST_ROOT and BOOSTROOT to find Boost. +# +if (NOT Boost_NO_BOOST_CMAKE) + # If Boost_DIR is not set, look for BOOSTROOT and BOOST_ROOT as alternatives, + # since these are more conventional for Boost. + if ("$ENV{Boost_DIR}" STREQUAL "") + if (NOT "$ENV{BOOST_ROOT}" STREQUAL "") + set(ENV{Boost_DIR} $ENV{BOOST_ROOT}) + elseif (NOT "$ENV{BOOSTROOT}" STREQUAL "") + set(ENV{Boost_DIR} $ENV{BOOSTROOT}) + endif() + endif() + + # Do the same find_package call but look specifically for the CMake version. + # Note that args are passed in the Boost_FIND_xxxxx variables, so there is no + # need to delegate them to this find_package call. + find_package(Boost QUIET NO_MODULE) + mark_as_advanced(Boost_DIR) + + # If we found a boost cmake package, then we're done. Print out what we found. + # Otherwise let the rest of the module try to find it. + if(Boost_FOUND) + # Convert component found variables to standard variables if required + # Necessary for legacy boost-cmake and 1.70 builtin BoostConfig + if(Boost_FIND_COMPONENTS) + foreach(_comp IN LISTS Boost_FIND_COMPONENTS) + if(DEFINED Boost_${_comp}_FOUND) + continue() + endif() + string(TOUPPER ${_comp} _uppercomp) + if(DEFINED Boost${_comp}_FOUND) # legacy boost-cmake project + set(Boost_${_comp}_FOUND ${Boost${_comp}_FOUND}) + elseif(DEFINED Boost_${_uppercomp}_FOUND) # Boost 1.70 + set(Boost_${_comp}_FOUND ${Boost_${_uppercomp}_FOUND}) + endif() + endforeach() + endif() + + find_package_handle_standard_args(Boost HANDLE_COMPONENTS CONFIG_MODE) + _boost_set_legacy_variables_from_config() + + # Restore project's policies + cmake_policy(POP) + return() + endif() +endif() + + +#------------------------------------------------------------------------------- +# FindBoost functions & macros +# + +# +# Print debug text if Boost_DEBUG is set. +# Call example: +# _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "debug message") +# +function(_Boost_DEBUG_PRINT file line text) + if(Boost_DEBUG) + message(STATUS "[ ${file}:${line} ] ${text}") + endif() +endfunction() + +# +# _Boost_DEBUG_PRINT_VAR(file line variable_name [ENVIRONMENT] +# [SOURCE "short explanation of origin of var value"]) +# +# ENVIRONMENT - look up environment variable instead of CMake variable +# +# Print variable name and its value if Boost_DEBUG is set. +# Call example: +# _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" BOOST_ROOT) +# +function(_Boost_DEBUG_PRINT_VAR file line name) + if(Boost_DEBUG) + cmake_parse_arguments(_args "ENVIRONMENT" "SOURCE" "" ${ARGN}) + + unset(source) + if(_args_SOURCE) + set(source " (${_args_SOURCE})") + endif() + + if(_args_ENVIRONMENT) + if(DEFINED ENV{${name}}) + set(value "\"$ENV{${name}}\"") + else() + set(value "") + endif() + set(_name "ENV{${name}}") + else() + if(DEFINED "${name}") + set(value "\"${${name}}\"") + else() + set(value "") + endif() + set(_name "${name}") + endif() + + _Boost_DEBUG_PRINT("${file}" "${line}" "${_name} = ${value}${source}") + endif() +endfunction() + +############################################ +# +# Check the existence of the libraries. +# +############################################ +# This macro was taken directly from the FindQt4.cmake file that is included +# with the CMake distribution. This is NOT my work. All work was done by the +# original authors of the FindQt4.cmake file. Only minor modifications were +# made to remove references to Qt and make this file more generally applicable +# And ELSE/ENDIF pairs were removed for readability. +######################################################################### + +macro(_Boost_ADJUST_LIB_VARS basename) + if(Boost_INCLUDE_DIR ) + if(Boost_${basename}_LIBRARY_DEBUG AND Boost_${basename}_LIBRARY_RELEASE) + # if the generator is multi-config or if CMAKE_BUILD_TYPE is set for + # single-config generators, set optimized and debug libraries + get_property(_isMultiConfig GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG) + if(_isMultiConfig OR CMAKE_BUILD_TYPE) + set(Boost_${basename}_LIBRARY optimized ${Boost_${basename}_LIBRARY_RELEASE} debug ${Boost_${basename}_LIBRARY_DEBUG}) + else() + # For single-config generators where CMAKE_BUILD_TYPE has no value, + # just use the release libraries + set(Boost_${basename}_LIBRARY ${Boost_${basename}_LIBRARY_RELEASE} ) + endif() + # FIXME: This probably should be set for both cases + set(Boost_${basename}_LIBRARIES optimized ${Boost_${basename}_LIBRARY_RELEASE} debug ${Boost_${basename}_LIBRARY_DEBUG}) + endif() + + # if only the release version was found, set the debug variable also to the release version + if(Boost_${basename}_LIBRARY_RELEASE AND NOT Boost_${basename}_LIBRARY_DEBUG) + set(Boost_${basename}_LIBRARY_DEBUG ${Boost_${basename}_LIBRARY_RELEASE}) + set(Boost_${basename}_LIBRARY ${Boost_${basename}_LIBRARY_RELEASE}) + set(Boost_${basename}_LIBRARIES ${Boost_${basename}_LIBRARY_RELEASE}) + endif() + + # if only the debug version was found, set the release variable also to the debug version + if(Boost_${basename}_LIBRARY_DEBUG AND NOT Boost_${basename}_LIBRARY_RELEASE) + set(Boost_${basename}_LIBRARY_RELEASE ${Boost_${basename}_LIBRARY_DEBUG}) + set(Boost_${basename}_LIBRARY ${Boost_${basename}_LIBRARY_DEBUG}) + set(Boost_${basename}_LIBRARIES ${Boost_${basename}_LIBRARY_DEBUG}) + endif() + + # If the debug & release library ends up being the same, omit the keywords + if("${Boost_${basename}_LIBRARY_RELEASE}" STREQUAL "${Boost_${basename}_LIBRARY_DEBUG}") + set(Boost_${basename}_LIBRARY ${Boost_${basename}_LIBRARY_RELEASE} ) + set(Boost_${basename}_LIBRARIES ${Boost_${basename}_LIBRARY_RELEASE} ) + endif() + + if(Boost_${basename}_LIBRARY AND Boost_${basename}_HEADER) + set(Boost_${basename}_FOUND ON) + if("x${basename}" STREQUAL "xTHREAD" AND NOT TARGET Threads::Threads) + string(APPEND Boost_ERROR_REASON_THREAD " (missing dependency: Threads)") + set(Boost_THREAD_FOUND OFF) + endif() + endif() + + endif() + # Make variables changeable to the advanced user + mark_as_advanced( + Boost_${basename}_LIBRARY_RELEASE + Boost_${basename}_LIBRARY_DEBUG + ) +endmacro() + +# Detect changes in used variables. +# Compares the current variable value with the last one. +# In short form: +# v != v_LAST -> CHANGED = 1 +# v is defined, v_LAST not -> CHANGED = 1 +# v is not defined, but v_LAST is -> CHANGED = 1 +# otherwise -> CHANGED = 0 +# CHANGED is returned in variable named ${changed_var} +macro(_Boost_CHANGE_DETECT changed_var) + set(${changed_var} 0) + foreach(v ${ARGN}) + if(DEFINED _Boost_COMPONENTS_SEARCHED) + if(${v}) + if(_${v}_LAST) + string(COMPARE NOTEQUAL "${${v}}" "${_${v}_LAST}" _${v}_CHANGED) + else() + set(_${v}_CHANGED 1) + endif() + elseif(_${v}_LAST) + set(_${v}_CHANGED 1) + endif() + if(_${v}_CHANGED) + set(${changed_var} 1) + endif() + else() + set(_${v}_CHANGED 0) + endif() + endforeach() +endmacro() + +# +# Find the given library (var). +# Use 'build_type' to support different lib paths for RELEASE or DEBUG builds +# +macro(_Boost_FIND_LIBRARY var build_type) + + find_library(${var} ${ARGN}) + + if(${var}) + # If this is the first library found then save Boost_LIBRARY_DIR_[RELEASE,DEBUG]. + if(NOT Boost_LIBRARY_DIR_${build_type}) + get_filename_component(_dir "${${var}}" PATH) + set(Boost_LIBRARY_DIR_${build_type} "${_dir}" CACHE PATH "Boost library directory ${build_type}" FORCE) + endif() + elseif(_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT) + # Try component-specific hints but do not save Boost_LIBRARY_DIR_[RELEASE,DEBUG]. + find_library(${var} HINTS ${_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT} ${ARGN}) + endif() + + # If Boost_LIBRARY_DIR_[RELEASE,DEBUG] is known then search only there. + if(Boost_LIBRARY_DIR_${build_type}) + set(_boost_LIBRARY_SEARCH_DIRS_${build_type} ${Boost_LIBRARY_DIR_${build_type}} NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" + "Boost_LIBRARY_DIR_${build_type}") + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" + "_boost_LIBRARY_SEARCH_DIRS_${build_type}") + endif() +endmacro() + +#------------------------------------------------------------------------------- + +# Convert CMAKE_CXX_COMPILER_VERSION to boost compiler suffix version. +function(_Boost_COMPILER_DUMPVERSION _OUTPUT_VERSION _OUTPUT_VERSION_MAJOR _OUTPUT_VERSION_MINOR) + string(REGEX REPLACE "([0-9]+)\\.([0-9]+)(\\.[0-9]+)?" "\\1" + _boost_COMPILER_VERSION_MAJOR "${CMAKE_CXX_COMPILER_VERSION}") + string(REGEX REPLACE "([0-9]+)\\.([0-9]+)(\\.[0-9]+)?" "\\2" + _boost_COMPILER_VERSION_MINOR "${CMAKE_CXX_COMPILER_VERSION}") + + set(_boost_COMPILER_VERSION "${_boost_COMPILER_VERSION_MAJOR}${_boost_COMPILER_VERSION_MINOR}") + + set(${_OUTPUT_VERSION} ${_boost_COMPILER_VERSION} PARENT_SCOPE) + set(${_OUTPUT_VERSION_MAJOR} ${_boost_COMPILER_VERSION_MAJOR} PARENT_SCOPE) + set(${_OUTPUT_VERSION_MINOR} ${_boost_COMPILER_VERSION_MINOR} PARENT_SCOPE) +endfunction() + +# +# Take a list of libraries with "thread" in it +# and prepend duplicates with "thread_${Boost_THREADAPI}" +# at the front of the list +# +function(_Boost_PREPEND_LIST_WITH_THREADAPI _output) + set(_orig_libnames ${ARGN}) + string(REPLACE "thread" "thread_${Boost_THREADAPI}" _threadapi_libnames "${_orig_libnames}") + set(${_output} ${_threadapi_libnames} ${_orig_libnames} PARENT_SCOPE) +endfunction() + +# +# If a library is found, replace its cache entry with its REALPATH +# +function(_Boost_SWAP_WITH_REALPATH _library _docstring) + if(${_library}) + get_filename_component(_boost_filepathreal ${${_library}} REALPATH) + unset(${_library} CACHE) + set(${_library} ${_boost_filepathreal} CACHE FILEPATH "${_docstring}") + endif() +endfunction() + +function(_Boost_CHECK_SPELLING _var) + if(${_var}) + string(TOUPPER ${_var} _var_UC) + message(FATAL_ERROR "ERROR: ${_var} is not the correct spelling. The proper spelling is ${_var_UC}.") + endif() +endfunction() + +# Guesses Boost's compiler prefix used in built library names +# Returns the guess by setting the variable pointed to by _ret +function(_Boost_GUESS_COMPILER_PREFIX _ret) + if("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xIntel") + if(WIN32) + set (_boost_COMPILER "-iw") + else() + set (_boost_COMPILER "-il") + endif() + elseif (GHSMULTI) + set(_boost_COMPILER "-ghs") + elseif("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xMSVC" OR "x${CMAKE_CXX_SIMULATE_ID}" STREQUAL "xMSVC") + if(MSVC_TOOLSET_VERSION GREATER_EQUAL 150) + # Not yet known. + set(_boost_COMPILER "") + elseif(MSVC_TOOLSET_VERSION GREATER_EQUAL 140) + # MSVC toolset 14.x versions are forward compatible. + set(_boost_COMPILER "") + foreach(v 9 8 7 6 5 4 3 2 1 0) + if(MSVC_TOOLSET_VERSION GREATER_EQUAL 14${v}) + list(APPEND _boost_COMPILER "-vc14${v}") + endif() + endforeach() + elseif(MSVC_TOOLSET_VERSION GREATER_EQUAL 80) + set(_boost_COMPILER "-vc${MSVC_TOOLSET_VERSION}") + elseif(NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 13.10) + set(_boost_COMPILER "-vc71") + elseif(NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 13) # Good luck! + set(_boost_COMPILER "-vc7") # yes, this is correct + else() # VS 6.0 Good luck! + set(_boost_COMPILER "-vc6") # yes, this is correct + endif() + + if("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xClang") + string(REPLACE "." ";" VERSION_LIST "${CMAKE_CXX_COMPILER_VERSION}") + list(GET VERSION_LIST 0 CLANG_VERSION_MAJOR) + set(_boost_COMPILER "-clangw${CLANG_VERSION_MAJOR};${_boost_COMPILER}") + endif() + elseif (BORLAND) + set(_boost_COMPILER "-bcb") + elseif(CMAKE_CXX_COMPILER_ID STREQUAL "SunPro") + set(_boost_COMPILER "-sw") + elseif(CMAKE_CXX_COMPILER_ID STREQUAL "XL") + set(_boost_COMPILER "-xlc") + elseif (MINGW) + if(Boost_VERSION_STRING VERSION_LESS 1.34) + set(_boost_COMPILER "-mgw") # no GCC version encoding prior to 1.34 + else() + _Boost_COMPILER_DUMPVERSION(_boost_COMPILER_VERSION _boost_COMPILER_VERSION_MAJOR _boost_COMPILER_VERSION_MINOR) + set(_boost_COMPILER "-mgw${_boost_COMPILER_VERSION}") + endif() + elseif (UNIX) + _Boost_COMPILER_DUMPVERSION(_boost_COMPILER_VERSION _boost_COMPILER_VERSION_MAJOR _boost_COMPILER_VERSION_MINOR) + if(NOT Boost_VERSION_STRING VERSION_LESS 1.69.0) + # From GCC 5 and clang 4, versioning changes and minor becomes patch. + # For those compilers, patch is exclude from compiler tag in Boost 1.69+ library naming. + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND _boost_COMPILER_VERSION_MAJOR VERSION_GREATER 4) + set(_boost_COMPILER_VERSION "${_boost_COMPILER_VERSION_MAJOR}") + elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND _boost_COMPILER_VERSION_MAJOR VERSION_GREATER 3) + set(_boost_COMPILER_VERSION "${_boost_COMPILER_VERSION_MAJOR}") + endif() + endif() + + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + if(Boost_VERSION_STRING VERSION_LESS 1.34) + set(_boost_COMPILER "-gcc") # no GCC version encoding prior to 1.34 + else() + # Determine which version of GCC we have. + if(APPLE) + if(Boost_VERSION_STRING VERSION_LESS 1.36.0) + # In Boost <= 1.35.0, there is no mangled compiler name for + # the macOS/Darwin version of GCC. + set(_boost_COMPILER "") + else() + # In Boost 1.36.0 and newer, the mangled compiler name used + # on macOS/Darwin is "xgcc". + set(_boost_COMPILER "-xgcc${_boost_COMPILER_VERSION}") + endif() + else() + set(_boost_COMPILER "-gcc${_boost_COMPILER_VERSION}") + endif() + endif() + elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + # TODO: Find out any Boost version constraints vs clang support. + set(_boost_COMPILER "-clang${_boost_COMPILER_VERSION}") + endif() + else() + set(_boost_COMPILER "") + endif() + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" + "_boost_COMPILER" SOURCE "guessed") + set(${_ret} ${_boost_COMPILER} PARENT_SCOPE) +endfunction() + +# +# Get component dependencies. Requires the dependencies to have been +# defined for the Boost release version. +# +# component - the component to check +# _ret - list of library dependencies +# +function(_Boost_COMPONENT_DEPENDENCIES component _ret) + # Note: to add a new Boost release, run + # + # % cmake -DBOOST_DIR=/path/to/boost/source -P Utilities/Scripts/BoostScanDeps.cmake + # + # The output may be added in a new block below. If it's the same as + # the previous release, simply update the version range of the block + # for the previous release. Also check if any new components have + # been added, and add any new components to + # _Boost_COMPONENT_HEADERS. + # + # This information was originally generated by running + # BoostScanDeps.cmake against every boost release to date supported + # by FindBoost: + # + # % for version in /path/to/boost/sources/* + # do + # cmake -DBOOST_DIR=$version -P Utilities/Scripts/BoostScanDeps.cmake + # done + # + # The output was then updated by search and replace with these regexes: + # + # - Strip message(STATUS) prefix dashes + # s;^-- ;; + # - Indent + # s;^set(; set(;; + # - Add conditionals + # s;Scanning /path/to/boost/sources/boost_\(.*\)_\(.*\)_\(.*); elseif(NOT Boost_VERSION_STRING VERSION_LESS \1\.\2\.\3 AND Boost_VERSION_STRING VERSION_LESS xxxx); + # + # This results in the logic seen below, but will require the xxxx + # replacing with the following Boost release version (or the next + # minor version to be released, e.g. 1.59 was the latest at the time + # of writing, making 1.60 the next. Identical consecutive releases + # were then merged together by updating the end range of the first + # block and removing the following redundant blocks. + # + # Running the script against all historical releases should be + # required only if the BoostScanDeps.cmake script logic is changed. + # The addition of a new release should only require it to be run + # against the new release. + + # Handle Python version suffixes + if(component MATCHES "^(python|mpi_python|numpy)([0-9][0-9]?|[0-9]\\.[0-9])\$") + set(component "${CMAKE_MATCH_1}") + set(component_python_version "${CMAKE_MATCH_2}") + endif() + + set(_Boost_IMPORTED_TARGETS TRUE) + if(Boost_VERSION_STRING AND Boost_VERSION_STRING VERSION_LESS 1.33.0) + message(WARNING "Imported targets and dependency information not available for Boost version ${Boost_VERSION_STRING} (all versions older than 1.33)") + set(_Boost_IMPORTED_TARGETS FALSE) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.33.0 AND Boost_VERSION_STRING VERSION_LESS 1.35.0) + set(_Boost_IOSTREAMS_DEPENDENCIES regex thread) + set(_Boost_REGEX_DEPENDENCIES thread) + set(_Boost_WAVE_DEPENDENCIES filesystem thread) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.35.0 AND Boost_VERSION_STRING VERSION_LESS 1.36.0) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_WAVE_DEPENDENCIES filesystem system thread) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.36.0 AND Boost_VERSION_STRING VERSION_LESS 1.38.0) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_WAVE_DEPENDENCIES filesystem system thread) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.38.0 AND Boost_VERSION_STRING VERSION_LESS 1.43.0) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_THREAD_DEPENDENCIES date_time) + set(_Boost_WAVE_DEPENDENCIES filesystem system thread date_time) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.43.0 AND Boost_VERSION_STRING VERSION_LESS 1.44.0) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l random) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_THREAD_DEPENDENCIES date_time) + set(_Boost_WAVE_DEPENDENCIES filesystem system thread date_time) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.44.0 AND Boost_VERSION_STRING VERSION_LESS 1.45.0) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l random serialization) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_THREAD_DEPENDENCIES date_time) + set(_Boost_WAVE_DEPENDENCIES serialization filesystem system thread date_time) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.45.0 AND Boost_VERSION_STRING VERSION_LESS 1.47.0) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l random) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_THREAD_DEPENDENCIES date_time) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread date_time) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.47.0 AND Boost_VERSION_STRING VERSION_LESS 1.48.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l random) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_THREAD_DEPENDENCIES date_time) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread date_time) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.48.0 AND Boost_VERSION_STRING VERSION_LESS 1.50.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l random) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_THREAD_DEPENDENCIES date_time) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread date_time) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.50.0 AND Boost_VERSION_STRING VERSION_LESS 1.53.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l regex random) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.53.0 AND Boost_VERSION_STRING VERSION_LESS 1.54.0) + set(_Boost_ATOMIC_DEPENDENCIES thread chrono system date_time) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l regex random) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.54.0 AND Boost_VERSION_STRING VERSION_LESS 1.55.0) + set(_Boost_ATOMIC_DEPENDENCIES thread chrono system date_time) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES log_setup date_time system filesystem thread regex chrono) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l regex random) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.55.0 AND Boost_VERSION_STRING VERSION_LESS 1.56.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_COROUTINE_DEPENDENCIES context system) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES log_setup date_time system filesystem thread regex chrono) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l regex random) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.56.0 AND Boost_VERSION_STRING VERSION_LESS 1.59.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_COROUTINE_DEPENDENCIES context system) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES log_setup date_time system filesystem thread regex chrono) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_RANDOM_DEPENDENCIES system) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.59.0 AND Boost_VERSION_STRING VERSION_LESS 1.60.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_COROUTINE_DEPENDENCIES context system) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES log_setup date_time system filesystem thread regex chrono atomic) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_RANDOM_DEPENDENCIES system) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.60.0 AND Boost_VERSION_STRING VERSION_LESS 1.61.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_COROUTINE_DEPENDENCIES context system) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_RANDOM_DEPENDENCIES system) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.61.0 AND Boost_VERSION_STRING VERSION_LESS 1.62.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time) + set(_Boost_COROUTINE_DEPENDENCIES context system) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_RANDOM_DEPENDENCIES system) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.62.0 AND Boost_VERSION_STRING VERSION_LESS 1.63.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time) + set(_Boost_COROUTINE_DEPENDENCIES context system) + set(_Boost_FIBER_DEPENDENCIES context thread chrono system date_time) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_RANDOM_DEPENDENCIES system) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.63.0 AND Boost_VERSION_STRING VERSION_LESS 1.65.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time) + set(_Boost_COROUTINE_DEPENDENCIES context system) + set(_Boost_COROUTINE2_DEPENDENCIES context fiber thread chrono system date_time) + set(_Boost_FIBER_DEPENDENCIES context thread chrono system date_time) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_RANDOM_DEPENDENCIES system) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.65.0 AND Boost_VERSION_STRING VERSION_LESS 1.67.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time) + set(_Boost_COROUTINE_DEPENDENCIES context system) + set(_Boost_FIBER_DEPENDENCIES context thread chrono system date_time) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_NUMPY_DEPENDENCIES python${component_python_version}) + set(_Boost_RANDOM_DEPENDENCIES system) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.67.0 AND Boost_VERSION_STRING VERSION_LESS 1.68.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time) + set(_Boost_COROUTINE_DEPENDENCIES context system) + set(_Boost_FIBER_DEPENDENCIES context thread chrono system date_time) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_NUMPY_DEPENDENCIES python${component_python_version}) + set(_Boost_RANDOM_DEPENDENCIES system) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.68.0 AND Boost_VERSION_STRING VERSION_LESS 1.69.0) + set(_Boost_CHRONO_DEPENDENCIES system) + set(_Boost_CONTEXT_DEPENDENCIES thread chrono system date_time) + set(_Boost_CONTRACT_DEPENDENCIES thread chrono system date_time) + set(_Boost_COROUTINE_DEPENDENCIES context system) + set(_Boost_FIBER_DEPENDENCIES context thread chrono system date_time) + set(_Boost_FILESYSTEM_DEPENDENCIES system) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES date_time log_setup system filesystem thread regex chrono atomic) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_NUMPY_DEPENDENCIES python${component_python_version}) + set(_Boost_RANDOM_DEPENDENCIES system) + set(_Boost_THREAD_DEPENDENCIES chrono system date_time atomic) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem system serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.69.0 AND Boost_VERSION_STRING VERSION_LESS 1.70.0) + set(_Boost_CONTRACT_DEPENDENCIES thread chrono date_time) + set(_Boost_COROUTINE_DEPENDENCIES context) + set(_Boost_FIBER_DEPENDENCIES context) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES date_time log_setup filesystem thread regex chrono atomic) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_NUMPY_DEPENDENCIES python${component_python_version}) + set(_Boost_THREAD_DEPENDENCIES chrono date_time atomic) + set(_Boost_TIMER_DEPENDENCIES chrono system) + set(_Boost_WAVE_DEPENDENCIES filesystem serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + elseif(NOT Boost_VERSION_STRING VERSION_LESS 1.70.0) + set(_Boost_CONTRACT_DEPENDENCIES thread chrono date_time) + set(_Boost_COROUTINE_DEPENDENCIES context) + set(_Boost_FIBER_DEPENDENCIES context) + set(_Boost_IOSTREAMS_DEPENDENCIES regex) + set(_Boost_LOG_DEPENDENCIES date_time log_setup filesystem thread regex chrono atomic) + set(_Boost_MATH_DEPENDENCIES math_c99 math_c99f math_c99l math_tr1 math_tr1f math_tr1l atomic) + set(_Boost_MPI_DEPENDENCIES serialization) + set(_Boost_MPI_PYTHON_DEPENDENCIES python${component_python_version} mpi serialization) + set(_Boost_NUMPY_DEPENDENCIES python${component_python_version}) + set(_Boost_THREAD_DEPENDENCIES chrono date_time atomic) + set(_Boost_TIMER_DEPENDENCIES chrono) + set(_Boost_WAVE_DEPENDENCIES filesystem serialization thread chrono date_time atomic) + set(_Boost_WSERIALIZATION_DEPENDENCIES serialization) + if(NOT Boost_VERSION_STRING VERSION_LESS 1.72.0) + message(WARNING "New Boost version may have incorrect or missing dependencies and imported targets") + endif() + endif() + + string(TOUPPER ${component} uppercomponent) + set(${_ret} ${_Boost_${uppercomponent}_DEPENDENCIES} PARENT_SCOPE) + set(_Boost_IMPORTED_TARGETS ${_Boost_IMPORTED_TARGETS} PARENT_SCOPE) + + string(REGEX REPLACE ";" " " _boost_DEPS_STRING "${_Boost_${uppercomponent}_DEPENDENCIES}") + if (NOT _boost_DEPS_STRING) + set(_boost_DEPS_STRING "(none)") + endif() + # message(STATUS "Dependencies for Boost::${component}: ${_boost_DEPS_STRING}") +endfunction() + +# +# Get component headers. This is the primary header (or headers) for +# a given component, and is used to check that the headers are present +# as well as the library itself as an extra sanity check of the build +# environment. +# +# component - the component to check +# _hdrs +# +function(_Boost_COMPONENT_HEADERS component _hdrs) + # Handle Python version suffixes + if(component MATCHES "^(python|mpi_python|numpy)([0-9][0-9]?|[0-9]\\.[0-9])\$") + set(component "${CMAKE_MATCH_1}") + set(component_python_version "${CMAKE_MATCH_2}") + endif() + + # Note: new boost components will require adding here. The header + # must be present in all versions of Boost providing a library. + set(_Boost_ATOMIC_HEADERS "boost/atomic.hpp") + set(_Boost_CHRONO_HEADERS "boost/chrono.hpp") + set(_Boost_CONTAINER_HEADERS "boost/container/container_fwd.hpp") + set(_Boost_CONTRACT_HEADERS "boost/contract.hpp") + if(Boost_VERSION_STRING VERSION_LESS 1.61.0) + set(_Boost_CONTEXT_HEADERS "boost/context/all.hpp") + else() + set(_Boost_CONTEXT_HEADERS "boost/context/detail/fcontext.hpp") + endif() + set(_Boost_COROUTINE_HEADERS "boost/coroutine/all.hpp") + set(_Boost_DATE_TIME_HEADERS "boost/date_time/date.hpp") + set(_Boost_EXCEPTION_HEADERS "boost/exception/exception.hpp") + set(_Boost_FIBER_HEADERS "boost/fiber/all.hpp") + set(_Boost_FILESYSTEM_HEADERS "boost/filesystem/path.hpp") + set(_Boost_GRAPH_HEADERS "boost/graph/adjacency_list.hpp") + set(_Boost_GRAPH_PARALLEL_HEADERS "boost/graph/adjacency_list.hpp") + set(_Boost_IOSTREAMS_HEADERS "boost/iostreams/stream.hpp") + set(_Boost_LOCALE_HEADERS "boost/locale.hpp") + set(_Boost_LOG_HEADERS "boost/log/core.hpp") + set(_Boost_LOG_SETUP_HEADERS "boost/log/detail/setup_config.hpp") + set(_Boost_MATH_HEADERS "boost/math_fwd.hpp") + set(_Boost_MATH_C99_HEADERS "boost/math/tr1.hpp") + set(_Boost_MATH_C99F_HEADERS "boost/math/tr1.hpp") + set(_Boost_MATH_C99L_HEADERS "boost/math/tr1.hpp") + set(_Boost_MATH_TR1_HEADERS "boost/math/tr1.hpp") + set(_Boost_MATH_TR1F_HEADERS "boost/math/tr1.hpp") + set(_Boost_MATH_TR1L_HEADERS "boost/math/tr1.hpp") + set(_Boost_MPI_HEADERS "boost/mpi.hpp") + set(_Boost_MPI_PYTHON_HEADERS "boost/mpi/python/config.hpp") + set(_Boost_NUMPY_HEADERS "boost/python/numpy.hpp") + set(_Boost_PRG_EXEC_MONITOR_HEADERS "boost/test/prg_exec_monitor.hpp") + set(_Boost_PROGRAM_OPTIONS_HEADERS "boost/program_options.hpp") + set(_Boost_PYTHON_HEADERS "boost/python.hpp") + set(_Boost_RANDOM_HEADERS "boost/random.hpp") + set(_Boost_REGEX_HEADERS "boost/regex.hpp") + set(_Boost_SERIALIZATION_HEADERS "boost/serialization/serialization.hpp") + set(_Boost_SIGNALS_HEADERS "boost/signals.hpp") + set(_Boost_STACKTRACE_ADDR2LINE_HEADERS "boost/stacktrace.hpp") + set(_Boost_STACKTRACE_BACKTRACE_HEADERS "boost/stacktrace.hpp") + set(_Boost_STACKTRACE_BASIC_HEADERS "boost/stacktrace.hpp") + set(_Boost_STACKTRACE_NOOP_HEADERS "boost/stacktrace.hpp") + set(_Boost_STACKTRACE_WINDBG_CACHED_HEADERS "boost/stacktrace.hpp") + set(_Boost_STACKTRACE_WINDBG_HEADERS "boost/stacktrace.hpp") + set(_Boost_SYSTEM_HEADERS "boost/system/config.hpp") + set(_Boost_TEST_EXEC_MONITOR_HEADERS "boost/test/test_exec_monitor.hpp") + set(_Boost_THREAD_HEADERS "boost/thread.hpp") + set(_Boost_TIMER_HEADERS "boost/timer.hpp") + set(_Boost_TYPE_ERASURE_HEADERS "boost/type_erasure/config.hpp") + set(_Boost_UNIT_TEST_FRAMEWORK_HEADERS "boost/test/framework.hpp") + set(_Boost_WAVE_HEADERS "boost/wave.hpp") + set(_Boost_WSERIALIZATION_HEADERS "boost/archive/text_wiarchive.hpp") + if(WIN32) + set(_Boost_BZIP2_HEADERS "boost/iostreams/filter/bzip2.hpp") + set(_Boost_ZLIB_HEADERS "boost/iostreams/filter/zlib.hpp") + endif() + + string(TOUPPER ${component} uppercomponent) + set(${_hdrs} ${_Boost_${uppercomponent}_HEADERS} PARENT_SCOPE) + + string(REGEX REPLACE ";" " " _boost_HDRS_STRING "${_Boost_${uppercomponent}_HEADERS}") + if (NOT _boost_HDRS_STRING) + set(_boost_HDRS_STRING "(none)") + endif() + # message(STATUS "Headers for Boost::${component}: ${_boost_HDRS_STRING}") +endfunction() + +# +# Determine if any missing dependencies require adding to the component list. +# +# Sets _Boost_${COMPONENT}_DEPENDENCIES for each required component, +# plus _Boost_IMPORTED_TARGETS (TRUE if imported targets should be +# defined; FALSE if dependency information is unavailable). +# +# componentvar - the component list variable name +# extravar - the indirect dependency list variable name +# +# +function(_Boost_MISSING_DEPENDENCIES componentvar extravar) + # _boost_unprocessed_components - list of components requiring processing + # _boost_processed_components - components already processed (or currently being processed) + # _boost_new_components - new components discovered for future processing + # + list(APPEND _boost_unprocessed_components ${${componentvar}}) + + while(_boost_unprocessed_components) + list(APPEND _boost_processed_components ${_boost_unprocessed_components}) + foreach(component ${_boost_unprocessed_components}) + string(TOUPPER ${component} uppercomponent) + set(${_ret} ${_Boost_${uppercomponent}_DEPENDENCIES} PARENT_SCOPE) + _Boost_COMPONENT_DEPENDENCIES("${component}" _Boost_${uppercomponent}_DEPENDENCIES) + set(_Boost_${uppercomponent}_DEPENDENCIES ${_Boost_${uppercomponent}_DEPENDENCIES} PARENT_SCOPE) + set(_Boost_IMPORTED_TARGETS ${_Boost_IMPORTED_TARGETS} PARENT_SCOPE) + foreach(componentdep ${_Boost_${uppercomponent}_DEPENDENCIES}) + if (NOT ("${componentdep}" IN_LIST _boost_processed_components OR "${componentdep}" IN_LIST _boost_new_components)) + list(APPEND _boost_new_components ${componentdep}) + endif() + endforeach() + endforeach() + set(_boost_unprocessed_components ${_boost_new_components}) + unset(_boost_new_components) + endwhile() + set(_boost_extra_components ${_boost_processed_components}) + if(_boost_extra_components AND ${componentvar}) + list(REMOVE_ITEM _boost_extra_components ${${componentvar}}) + endif() + set(${componentvar} ${_boost_processed_components} PARENT_SCOPE) + set(${extravar} ${_boost_extra_components} PARENT_SCOPE) +endfunction() + +# +# Some boost libraries may require particular set of compler features. +# The very first one was `boost::fiber` introduced in Boost 1.62. +# One can check required compiler features of it in +# - `${Boost_ROOT}/libs/fiber/build/Jamfile.v2`; +# - `${Boost_ROOT}/libs/context/build/Jamfile.v2`. +# +# TODO (Re)Check compiler features on (every?) release ??? +# One may use the following command to get the files to check: +# +# $ find . -name Jamfile.v2 | grep build | xargs grep -l cxx1 +# +function(_Boost_COMPILER_FEATURES component _ret) + # Boost >= 1.62 + if(NOT Boost_VERSION_STRING VERSION_LESS 1.62.0) + set(_Boost_FIBER_COMPILER_FEATURES + cxx_alias_templates + cxx_auto_type + cxx_constexpr + cxx_defaulted_functions + cxx_final + cxx_lambdas + cxx_noexcept + cxx_nullptr + cxx_rvalue_references + cxx_thread_local + cxx_variadic_templates + ) + # Compiler feature for `context` same as for `fiber`. + set(_Boost_CONTEXT_COMPILER_FEATURES ${_Boost_FIBER_COMPILER_FEATURES}) + endif() + + # Boost Contract library available in >= 1.67 + if(NOT Boost_VERSION_STRING VERSION_LESS 1.67.0) + # From `libs/contract/build/boost_contract_build.jam` + set(_Boost_CONTRACT_COMPILER_FEATURES + cxx_lambdas + cxx_variadic_templates + ) + endif() + + string(TOUPPER ${component} uppercomponent) + set(${_ret} ${_Boost_${uppercomponent}_COMPILER_FEATURES} PARENT_SCOPE) +endfunction() + +# +# Update library search directory hint variable with paths used by prebuilt boost binaries. +# +# Prebuilt windows binaries (https://sourceforge.net/projects/boost/files/boost-binaries/) +# have library directories named using MSVC compiler version and architecture. +# This function would append corresponding directories if MSVC is a current compiler, +# so having `BOOST_ROOT` would be enough to specify to find everything. +# +function(_Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS componentlibvar basedir) + if("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xMSVC") + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(_arch_suffix 64) + else() + set(_arch_suffix 32) + endif() + if(MSVC_TOOLSET_VERSION GREATER_EQUAL 150) + # Not yet known. + elseif(MSVC_TOOLSET_VERSION GREATER_EQUAL 140) + # MSVC toolset 14.x versions are forward compatible. + foreach(v 9 8 7 6 5 4 3 2 1 0) + if(MSVC_TOOLSET_VERSION GREATER_EQUAL 14${v}) + list(APPEND ${componentlibvar} ${basedir}/lib${_arch_suffix}-msvc-14.${v}) + endif() + endforeach() + elseif(MSVC_TOOLSET_VERSION GREATER_EQUAL 80) + math(EXPR _toolset_major_version "${MSVC_TOOLSET_VERSION} / 10") + list(APPEND ${componentlibvar} ${basedir}/lib${_arch_suffix}-msvc-${_toolset_major_version}.0) + endif() + set(${componentlibvar} ${${componentlibvar}} PARENT_SCOPE) + endif() +endfunction() + +# +# End functions/macros +# +#------------------------------------------------------------------------------- + +#------------------------------------------------------------------------------- +# main. +#------------------------------------------------------------------------------- + + +# If the user sets Boost_LIBRARY_DIR, use it as the default for both +# configurations. +if(NOT Boost_LIBRARY_DIR_RELEASE AND Boost_LIBRARY_DIR) + set(Boost_LIBRARY_DIR_RELEASE "${Boost_LIBRARY_DIR}") +endif() +if(NOT Boost_LIBRARY_DIR_DEBUG AND Boost_LIBRARY_DIR) + set(Boost_LIBRARY_DIR_DEBUG "${Boost_LIBRARY_DIR}") +endif() + +if(NOT DEFINED Boost_USE_DEBUG_LIBS) + set(Boost_USE_DEBUG_LIBS TRUE) +endif() +if(NOT DEFINED Boost_USE_RELEASE_LIBS) + set(Boost_USE_RELEASE_LIBS TRUE) +endif() +if(NOT DEFINED Boost_USE_MULTITHREADED) + set(Boost_USE_MULTITHREADED TRUE) +endif() +if(NOT DEFINED Boost_USE_DEBUG_RUNTIME) + set(Boost_USE_DEBUG_RUNTIME TRUE) +endif() + +# Check the version of Boost against the requested version. +if(Boost_FIND_VERSION AND NOT Boost_FIND_VERSION_MINOR) + message(SEND_ERROR "When requesting a specific version of Boost, you must provide at least the major and minor version numbers, e.g., 1.34") +endif() + +if(Boost_FIND_VERSION_EXACT) + # The version may appear in a directory with or without the patch + # level, even when the patch level is non-zero. + set(_boost_TEST_VERSIONS + "${Boost_FIND_VERSION_MAJOR}.${Boost_FIND_VERSION_MINOR}.${Boost_FIND_VERSION_PATCH}" + "${Boost_FIND_VERSION_MAJOR}.${Boost_FIND_VERSION_MINOR}") +else() + # The user has not requested an exact version. Among known + # versions, find those that are acceptable to the user request. + # + # Note: When adding a new Boost release, also update the dependency + # information in _Boost_COMPONENT_DEPENDENCIES and + # _Boost_COMPONENT_HEADERS. See the instructions at the top of + # _Boost_COMPONENT_DEPENDENCIES. + set(_Boost_KNOWN_VERSIONS ${Boost_ADDITIONAL_VERSIONS} + "1.71.0" "1.71" "1.70.0" "1.70" "1.69.0" "1.69" + "1.68.0" "1.68" "1.67.0" "1.67" "1.66.0" "1.66" "1.65.1" "1.65.0" "1.65" + "1.64.0" "1.64" "1.63.0" "1.63" "1.62.0" "1.62" "1.61.0" "1.61" "1.60.0" "1.60" + "1.59.0" "1.59" "1.58.0" "1.58" "1.57.0" "1.57" "1.56.0" "1.56" "1.55.0" "1.55" + "1.54.0" "1.54" "1.53.0" "1.53" "1.52.0" "1.52" "1.51.0" "1.51" + "1.50.0" "1.50" "1.49.0" "1.49" "1.48.0" "1.48" "1.47.0" "1.47" "1.46.1" + "1.46.0" "1.46" "1.45.0" "1.45" "1.44.0" "1.44" "1.43.0" "1.43" "1.42.0" "1.42" + "1.41.0" "1.41" "1.40.0" "1.40" "1.39.0" "1.39" "1.38.0" "1.38" "1.37.0" "1.37" + "1.36.1" "1.36.0" "1.36" "1.35.1" "1.35.0" "1.35" "1.34.1" "1.34.0" + "1.34" "1.33.1" "1.33.0" "1.33") + + set(_boost_TEST_VERSIONS) + if(Boost_FIND_VERSION) + set(_Boost_FIND_VERSION_SHORT "${Boost_FIND_VERSION_MAJOR}.${Boost_FIND_VERSION_MINOR}") + # Select acceptable versions. + foreach(version ${_Boost_KNOWN_VERSIONS}) + if(NOT "${version}" VERSION_LESS "${Boost_FIND_VERSION}") + # This version is high enough. + list(APPEND _boost_TEST_VERSIONS "${version}") + elseif("${version}.99" VERSION_EQUAL "${_Boost_FIND_VERSION_SHORT}.99") + # This version is a short-form for the requested version with + # the patch level dropped. + list(APPEND _boost_TEST_VERSIONS "${version}") + endif() + endforeach() + else() + # Any version is acceptable. + set(_boost_TEST_VERSIONS "${_Boost_KNOWN_VERSIONS}") + endif() +endif() + +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_TEST_VERSIONS") +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_USE_MULTITHREADED") +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_USE_STATIC_LIBS") +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_USE_STATIC_RUNTIME") +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_ADDITIONAL_VERSIONS") +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_NO_SYSTEM_PATHS") + +# Supply Boost_LIB_DIAGNOSTIC_DEFINITIONS as a convenience target. It +# will only contain any interface definitions on WIN32, but is created +# on all platforms to keep end user code free from platform dependent +# code. Also provide convenience targets to disable autolinking and +# enable dynamic linking. +if(NOT TARGET Boost::diagnostic_definitions) + add_library(Boost::diagnostic_definitions INTERFACE IMPORTED) + add_library(Boost::disable_autolinking INTERFACE IMPORTED) + add_library(Boost::dynamic_linking INTERFACE IMPORTED) + set_target_properties(Boost::dynamic_linking PROPERTIES + INTERFACE_COMPILE_DEFINITIONS "BOOST_ALL_DYN_LINK") +endif() +if(WIN32) + # In windows, automatic linking is performed, so you do not have + # to specify the libraries. If you are linking to a dynamic + # runtime, then you can choose to link to either a static or a + # dynamic Boost library, the default is to do a static link. You + # can alter this for a specific library "whatever" by defining + # BOOST_WHATEVER_DYN_LINK to force Boost library "whatever" to be + # linked dynamically. Alternatively you can force all Boost + # libraries to dynamic link by defining BOOST_ALL_DYN_LINK. + + # This feature can be disabled for Boost library "whatever" by + # defining BOOST_WHATEVER_NO_LIB, or for all of Boost by defining + # BOOST_ALL_NO_LIB. + + # If you want to observe which libraries are being linked against + # then defining BOOST_LIB_DIAGNOSTIC will cause the auto-linking + # code to emit a #pragma message each time a library is selected + # for linking. + set(Boost_LIB_DIAGNOSTIC_DEFINITIONS "-DBOOST_LIB_DIAGNOSTIC") + set_target_properties(Boost::diagnostic_definitions PROPERTIES + INTERFACE_COMPILE_DEFINITIONS "BOOST_LIB_DIAGNOSTIC") + set_target_properties(Boost::disable_autolinking PROPERTIES + INTERFACE_COMPILE_DEFINITIONS "BOOST_ALL_NO_LIB") +endif() + +cmake_policy(GET CMP0074 _Boost_CMP0074) +if(NOT "x${_Boost_CMP0074}x" STREQUAL "xNEWx") + _Boost_CHECK_SPELLING(Boost_ROOT) +endif() +unset(_Boost_CMP0074) +_Boost_CHECK_SPELLING(Boost_LIBRARYDIR) +_Boost_CHECK_SPELLING(Boost_INCLUDEDIR) + +# Collect environment variable inputs as hints. Do not consider changes. +foreach(v BOOSTROOT BOOST_ROOT BOOST_INCLUDEDIR BOOST_LIBRARYDIR) + set(_env $ENV{${v}}) + if(_env) + file(TO_CMAKE_PATH "${_env}" _ENV_${v}) + else() + set(_ENV_${v} "") + endif() +endforeach() +if(NOT _ENV_BOOST_ROOT AND _ENV_BOOSTROOT) + set(_ENV_BOOST_ROOT "${_ENV_BOOSTROOT}") +endif() + +# Collect inputs and cached results. Detect changes since the last run. +if(NOT BOOST_ROOT AND BOOSTROOT) + set(BOOST_ROOT "${BOOSTROOT}") +endif() +set(_Boost_VARS_DIR + BOOST_ROOT + Boost_NO_SYSTEM_PATHS + ) + +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_ROOT") +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_ROOT" ENVIRONMENT) +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_INCLUDEDIR") +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_INCLUDEDIR" ENVIRONMENT) +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_LIBRARYDIR") +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "BOOST_LIBRARYDIR" ENVIRONMENT) + +# ------------------------------------------------------------------------ +# Search for Boost include DIR +# ------------------------------------------------------------------------ + +set(_Boost_VARS_INC BOOST_INCLUDEDIR Boost_INCLUDE_DIR Boost_ADDITIONAL_VERSIONS) +_Boost_CHANGE_DETECT(_Boost_CHANGE_INCDIR ${_Boost_VARS_DIR} ${_Boost_VARS_INC}) +# Clear Boost_INCLUDE_DIR if it did not change but other input affecting the +# location did. We will find a new one based on the new inputs. +if(_Boost_CHANGE_INCDIR AND NOT _Boost_INCLUDE_DIR_CHANGED) + unset(Boost_INCLUDE_DIR CACHE) +endif() + +if(NOT Boost_INCLUDE_DIR) + set(_boost_INCLUDE_SEARCH_DIRS "") + if(BOOST_INCLUDEDIR) + list(APPEND _boost_INCLUDE_SEARCH_DIRS ${BOOST_INCLUDEDIR}) + elseif(_ENV_BOOST_INCLUDEDIR) + list(APPEND _boost_INCLUDE_SEARCH_DIRS ${_ENV_BOOST_INCLUDEDIR}) + endif() + + if( BOOST_ROOT ) + list(APPEND _boost_INCLUDE_SEARCH_DIRS ${BOOST_ROOT}/include ${BOOST_ROOT}) + elseif( _ENV_BOOST_ROOT ) + list(APPEND _boost_INCLUDE_SEARCH_DIRS ${_ENV_BOOST_ROOT}/include ${_ENV_BOOST_ROOT}) + endif() + + if( Boost_NO_SYSTEM_PATHS) + list(APPEND _boost_INCLUDE_SEARCH_DIRS NO_CMAKE_SYSTEM_PATH NO_SYSTEM_ENVIRONMENT_PATH) + else() + if("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xMSVC") + foreach(ver ${_boost_TEST_VERSIONS}) + string(REPLACE "." "_" ver "${ver}") + list(APPEND _boost_INCLUDE_SEARCH_DIRS PATHS "C:/local/boost_${ver}") + endforeach() + endif() + list(APPEND _boost_INCLUDE_SEARCH_DIRS PATHS + C:/boost/include + C:/boost + /sw/local/include + ) + endif() + + # Try to find Boost by stepping backwards through the Boost versions + # we know about. + # Build a list of path suffixes for each version. + set(_boost_PATH_SUFFIXES) + foreach(_boost_VER ${_boost_TEST_VERSIONS}) + # Add in a path suffix, based on the required version, ideally + # we could read this from version.hpp, but for that to work we'd + # need to know the include dir already + set(_boost_BOOSTIFIED_VERSION) + + # Transform 1.35 => 1_35 and 1.36.0 => 1_36_0 + if(_boost_VER MATCHES "([0-9]+)\\.([0-9]+)\\.([0-9]+)") + set(_boost_BOOSTIFIED_VERSION + "${CMAKE_MATCH_1}_${CMAKE_MATCH_2}_${CMAKE_MATCH_3}") + elseif(_boost_VER MATCHES "([0-9]+)\\.([0-9]+)") + set(_boost_BOOSTIFIED_VERSION + "${CMAKE_MATCH_1}_${CMAKE_MATCH_2}") + endif() + + list(APPEND _boost_PATH_SUFFIXES + "boost-${_boost_BOOSTIFIED_VERSION}" + "boost_${_boost_BOOSTIFIED_VERSION}" + "boost/boost-${_boost_BOOSTIFIED_VERSION}" + "boost/boost_${_boost_BOOSTIFIED_VERSION}" + ) + + endforeach() + + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_INCLUDE_SEARCH_DIRS") + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_PATH_SUFFIXES") + + # Look for a standard boost header file. + find_path(Boost_INCLUDE_DIR + NAMES boost/config.hpp + HINTS ${_boost_INCLUDE_SEARCH_DIRS} + PATH_SUFFIXES ${_boost_PATH_SUFFIXES} + ) +endif() + +# ------------------------------------------------------------------------ +# Extract version information from version.hpp +# ------------------------------------------------------------------------ + +if(Boost_INCLUDE_DIR) + _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" + "location of version.hpp: ${Boost_INCLUDE_DIR}/boost/version.hpp") + + # Extract Boost_VERSION_MACRO and Boost_LIB_VERSION from version.hpp + set(Boost_VERSION_MACRO 0) + set(Boost_LIB_VERSION "") + file(STRINGS "${Boost_INCLUDE_DIR}/boost/version.hpp" _boost_VERSION_HPP_CONTENTS REGEX "#define BOOST_(LIB_)?VERSION ") + if("${_boost_VERSION_HPP_CONTENTS}" MATCHES "#define BOOST_VERSION ([0-9]+)") + set(Boost_VERSION_MACRO "${CMAKE_MATCH_1}") + endif() + if("${_boost_VERSION_HPP_CONTENTS}" MATCHES "#define BOOST_LIB_VERSION \"([0-9_]+)\"") + set(Boost_LIB_VERSION "${CMAKE_MATCH_1}") + endif() + unset(_boost_VERSION_HPP_CONTENTS) + + # Calculate version components + math(EXPR Boost_VERSION_MAJOR "${Boost_VERSION_MACRO} / 100000") + math(EXPR Boost_VERSION_MINOR "${Boost_VERSION_MACRO} / 100 % 1000") + math(EXPR Boost_VERSION_PATCH "${Boost_VERSION_MACRO} % 100") + set(Boost_VERSION_COUNT 3) + + # Define alias variables for backwards compat. + set(Boost_MAJOR_VERSION ${Boost_VERSION_MAJOR}) + set(Boost_MINOR_VERSION ${Boost_VERSION_MINOR}) + set(Boost_SUBMINOR_VERSION ${Boost_VERSION_PATCH}) + + # Define Boost version in x.y.z format + set(Boost_VERSION_STRING "${Boost_VERSION_MAJOR}.${Boost_VERSION_MINOR}.${Boost_VERSION_PATCH}") + + # Define final Boost_VERSION + cmake_policy(GET CMP0093 _Boost_CMP0093 + PARENT_SCOPE # undocumented, do not use outside of CMake + ) + if("x${_Boost_CMP0093}x" STREQUAL "xNEWx") + set(Boost_VERSION ${Boost_VERSION_STRING}) + else() + set(Boost_VERSION ${Boost_VERSION_MACRO}) + endif() + unset(_Boost_CMP0093) + + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION") + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_STRING") + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_MACRO") + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_MAJOR") + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_MINOR") + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_PATCH") + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_VERSION_COUNT") +endif() + +# ------------------------------------------------------------------------ +# Prefix initialization +# ------------------------------------------------------------------------ + +set(Boost_LIB_PREFIX "") +if ( (GHSMULTI AND Boost_USE_STATIC_LIBS) OR + (WIN32 AND Boost_USE_STATIC_LIBS AND NOT CYGWIN) ) + set(Boost_LIB_PREFIX "lib") +endif() + +if ( NOT Boost_NAMESPACE ) + set(Boost_NAMESPACE "boost") +endif() + +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_LIB_PREFIX") +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "Boost_NAMESPACE") + +# ------------------------------------------------------------------------ +# Suffix initialization and compiler suffix detection. +# ------------------------------------------------------------------------ + +set(_Boost_VARS_NAME + Boost_NAMESPACE + Boost_COMPILER + Boost_THREADAPI + Boost_USE_DEBUG_PYTHON + Boost_USE_MULTITHREADED + Boost_USE_STATIC_LIBS + Boost_USE_STATIC_RUNTIME + Boost_USE_STLPORT + Boost_USE_STLPORT_DEPRECATED_NATIVE_IOSTREAMS + ) +_Boost_CHANGE_DETECT(_Boost_CHANGE_LIBNAME ${_Boost_VARS_NAME}) + +# Setting some more suffixes for the library +if (Boost_COMPILER) + set(_boost_COMPILER ${Boost_COMPILER}) + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" + "_boost_COMPILER" SOURCE "user-specified via Boost_COMPILER") +else() + # Attempt to guess the compiler suffix + # NOTE: this is not perfect yet, if you experience any issues + # please report them and use the Boost_COMPILER variable + # to work around the problems. + _Boost_GUESS_COMPILER_PREFIX(_boost_COMPILER) +endif() + +set (_boost_MULTITHREADED "-mt") +if( NOT Boost_USE_MULTITHREADED ) + set (_boost_MULTITHREADED "") +endif() +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_MULTITHREADED") + +#====================== +# Systematically build up the Boost ABI tag for the 'tagged' and 'versioned' layouts +# http://boost.org/doc/libs/1_66_0/more/getting_started/windows.html#library-naming +# http://boost.org/doc/libs/1_66_0/boost/config/auto_link.hpp +# http://boost.org/doc/libs/1_66_0/tools/build/src/tools/common.jam +# http://boost.org/doc/libs/1_66_0/boostcpp.jam +set( _boost_RELEASE_ABI_TAG "-") +set( _boost_DEBUG_ABI_TAG "-") +# Key Use this library when: +# s linking statically to the C++ standard library and +# compiler runtime support libraries. +if(Boost_USE_STATIC_RUNTIME) + set( _boost_RELEASE_ABI_TAG "${_boost_RELEASE_ABI_TAG}s") + set( _boost_DEBUG_ABI_TAG "${_boost_DEBUG_ABI_TAG}s") +endif() +# g using debug versions of the standard and runtime +# support libraries +if(WIN32 AND Boost_USE_DEBUG_RUNTIME) + if("x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xMSVC" + OR "x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xClang" + OR "x${CMAKE_CXX_COMPILER_ID}" STREQUAL "xIntel") + string(APPEND _boost_DEBUG_ABI_TAG "g") + endif() +endif() +# y using special debug build of python +if(Boost_USE_DEBUG_PYTHON) + string(APPEND _boost_DEBUG_ABI_TAG "y") +endif() +# d using a debug version of your code +string(APPEND _boost_DEBUG_ABI_TAG "d") +# p using the STLport standard library rather than the +# default one supplied with your compiler +if(Boost_USE_STLPORT) + string(APPEND _boost_RELEASE_ABI_TAG "p") + string(APPEND _boost_DEBUG_ABI_TAG "p") +endif() +# n using the STLport deprecated "native iostreams" feature +# removed from the documentation in 1.43.0 but still present in +# boost/config/auto_link.hpp +if(Boost_USE_STLPORT_DEPRECATED_NATIVE_IOSTREAMS) + string(APPEND _boost_RELEASE_ABI_TAG "n") + string(APPEND _boost_DEBUG_ABI_TAG "n") +endif() + +# -x86 Architecture and address model tag +# First character is the architecture, then word-size, either 32 or 64 +# Only used in 'versioned' layout, added in Boost 1.66.0 +if(DEFINED Boost_ARCHITECTURE) + set(_boost_ARCHITECTURE_TAG "${Boost_ARCHITECTURE}") + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" + "_boost_ARCHITECTURE_TAG" SOURCE "user-specified via Boost_ARCHITECTURE") +else() + set(_boost_ARCHITECTURE_TAG "") + # {CMAKE_CXX_COMPILER_ARCHITECTURE_ID} is not currently set for all compilers + if(NOT "x${CMAKE_CXX_COMPILER_ARCHITECTURE_ID}" STREQUAL "x" AND NOT Boost_VERSION_STRING VERSION_LESS 1.66.0) + string(APPEND _boost_ARCHITECTURE_TAG "-") + # This needs to be kept in-sync with the section of CMakePlatformId.h.in + # inside 'defined(_WIN32) && defined(_MSC_VER)' + if(CMAKE_CXX_COMPILER_ARCHITECTURE_ID STREQUAL "IA64") + string(APPEND _boost_ARCHITECTURE_TAG "i") + elseif(CMAKE_CXX_COMPILER_ARCHITECTURE_ID STREQUAL "X86" + OR CMAKE_CXX_COMPILER_ARCHITECTURE_ID STREQUAL "x64") + string(APPEND _boost_ARCHITECTURE_TAG "x") + elseif(CMAKE_CXX_COMPILER_ARCHITECTURE_ID MATCHES "^ARM") + string(APPEND _boost_ARCHITECTURE_TAG "a") + elseif(CMAKE_CXX_COMPILER_ARCHITECTURE_ID STREQUAL "MIPS") + string(APPEND _boost_ARCHITECTURE_TAG "m") + endif() + + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + string(APPEND _boost_ARCHITECTURE_TAG "64") + else() + string(APPEND _boost_ARCHITECTURE_TAG "32") + endif() + endif() + _Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" + "_boost_ARCHITECTURE_TAG" SOURCE "detected") +endif() + +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_RELEASE_ABI_TAG") +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_DEBUG_ABI_TAG") + +# ------------------------------------------------------------------------ +# Begin finding boost libraries +# ------------------------------------------------------------------------ + +set(_Boost_VARS_LIB "") +foreach(c DEBUG RELEASE) + set(_Boost_VARS_LIB_${c} BOOST_LIBRARYDIR Boost_LIBRARY_DIR_${c}) + list(APPEND _Boost_VARS_LIB ${_Boost_VARS_LIB_${c}}) + _Boost_CHANGE_DETECT(_Boost_CHANGE_LIBDIR_${c} ${_Boost_VARS_DIR} ${_Boost_VARS_LIB_${c}} Boost_INCLUDE_DIR) + # Clear Boost_LIBRARY_DIR_${c} if it did not change but other input affecting the + # location did. We will find a new one based on the new inputs. + if(_Boost_CHANGE_LIBDIR_${c} AND NOT _Boost_LIBRARY_DIR_${c}_CHANGED) + unset(Boost_LIBRARY_DIR_${c} CACHE) + endif() + + # If Boost_LIBRARY_DIR_[RELEASE,DEBUG] is set, prefer its value. + if(Boost_LIBRARY_DIR_${c}) + set(_boost_LIBRARY_SEARCH_DIRS_${c} ${Boost_LIBRARY_DIR_${c}} NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) + else() + set(_boost_LIBRARY_SEARCH_DIRS_${c} "") + if(BOOST_LIBRARYDIR) + list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} ${BOOST_LIBRARYDIR}) + elseif(_ENV_BOOST_LIBRARYDIR) + list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} ${_ENV_BOOST_LIBRARYDIR}) + endif() + + if(BOOST_ROOT) + list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} ${BOOST_ROOT}/lib ${BOOST_ROOT}/stage/lib) + _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "${BOOST_ROOT}") + elseif(_ENV_BOOST_ROOT) + list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} ${_ENV_BOOST_ROOT}/lib ${_ENV_BOOST_ROOT}/stage/lib) + _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "${_ENV_BOOST_ROOT}") + endif() + + list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} + ${Boost_INCLUDE_DIR}/lib + ${Boost_INCLUDE_DIR}/../lib + ${Boost_INCLUDE_DIR}/stage/lib + ) + _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "${Boost_INCLUDE_DIR}/..") + _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "${Boost_INCLUDE_DIR}") + if( Boost_NO_SYSTEM_PATHS ) + list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} NO_CMAKE_SYSTEM_PATH NO_SYSTEM_ENVIRONMENT_PATH) + else() + foreach(ver ${_boost_TEST_VERSIONS}) + string(REPLACE "." "_" ver "${ver}") + _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "C:/local/boost_${ver}") + endforeach() + _Boost_UPDATE_WINDOWS_LIBRARY_SEARCH_DIRS_WITH_PREBUILT_PATHS(_boost_LIBRARY_SEARCH_DIRS_${c} "C:/boost") + list(APPEND _boost_LIBRARY_SEARCH_DIRS_${c} PATHS + C:/boost/lib + C:/boost + /sw/local/lib + ) + endif() + endif() +endforeach() + +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_LIBRARY_SEARCH_DIRS_RELEASE") +_Boost_DEBUG_PRINT_VAR("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" "_boost_LIBRARY_SEARCH_DIRS_DEBUG") + +# Support preference of static libs by adjusting CMAKE_FIND_LIBRARY_SUFFIXES +if( Boost_USE_STATIC_LIBS ) + set( _boost_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES ${CMAKE_FIND_LIBRARY_SUFFIXES}) + if(WIN32) + list(INSERT CMAKE_FIND_LIBRARY_SUFFIXES 0 .lib .a) + else() + set(CMAKE_FIND_LIBRARY_SUFFIXES .a) + endif() +endif() + +# We want to use the tag inline below without risking double dashes +if(_boost_RELEASE_ABI_TAG) + if(${_boost_RELEASE_ABI_TAG} STREQUAL "-") + set(_boost_RELEASE_ABI_TAG "") + endif() +endif() +if(_boost_DEBUG_ABI_TAG) + if(${_boost_DEBUG_ABI_TAG} STREQUAL "-") + set(_boost_DEBUG_ABI_TAG "") + endif() +endif() + +# The previous behavior of FindBoost when Boost_USE_STATIC_LIBS was enabled +# on WIN32 was to: +# 1. Search for static libs compiled against a SHARED C++ standard runtime library (use if found) +# 2. Search for static libs compiled against a STATIC C++ standard runtime library (use if found) +# We maintain this behavior since changing it could break people's builds. +# To disable the ambiguous behavior, the user need only +# set Boost_USE_STATIC_RUNTIME either ON or OFF. +set(_boost_STATIC_RUNTIME_WORKAROUND false) +if(WIN32 AND Boost_USE_STATIC_LIBS) + if(NOT DEFINED Boost_USE_STATIC_RUNTIME) + set(_boost_STATIC_RUNTIME_WORKAROUND TRUE) + endif() +endif() + +# On versions < 1.35, remove the System library from the considered list +# since it wasn't added until 1.35. +if(Boost_VERSION_STRING AND Boost_FIND_COMPONENTS) + if(Boost_VERSION_STRING VERSION_LESS 1.35.0) + list(REMOVE_ITEM Boost_FIND_COMPONENTS system) + endif() +endif() + +# Additional components may be required via component dependencies. +# Add any missing components to the list. +_Boost_MISSING_DEPENDENCIES(Boost_FIND_COMPONENTS _Boost_EXTRA_FIND_COMPONENTS) + +# If thread is required, get the thread libs as a dependency +if("thread" IN_LIST Boost_FIND_COMPONENTS) + if(Boost_FIND_QUIETLY) + set(_Boost_find_quiet QUIET) + else() + set(_Boost_find_quiet "") + endif() + find_package(Threads ${_Boost_find_quiet}) + unset(_Boost_find_quiet) +endif() + +# If the user changed any of our control inputs flush previous results. +if(_Boost_CHANGE_LIBDIR_DEBUG OR _Boost_CHANGE_LIBDIR_RELEASE OR _Boost_CHANGE_LIBNAME) + foreach(COMPONENT ${_Boost_COMPONENTS_SEARCHED}) + string(TOUPPER ${COMPONENT} UPPERCOMPONENT) + foreach(c DEBUG RELEASE) + set(_var Boost_${UPPERCOMPONENT}_LIBRARY_${c}) + unset(${_var} CACHE) + set(${_var} "${_var}-NOTFOUND") + endforeach() + endforeach() + set(_Boost_COMPONENTS_SEARCHED "") +endif() + +foreach(COMPONENT ${Boost_FIND_COMPONENTS}) + string(TOUPPER ${COMPONENT} UPPERCOMPONENT) + + set( _boost_docstring_release "Boost ${COMPONENT} library (release)") + set( _boost_docstring_debug "Boost ${COMPONENT} library (debug)") + + # Compute component-specific hints. + set(_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT "") + if(${COMPONENT} STREQUAL "mpi" OR ${COMPONENT} STREQUAL "mpi_python" OR + ${COMPONENT} STREQUAL "graph_parallel") + foreach(lib ${MPI_CXX_LIBRARIES} ${MPI_C_LIBRARIES}) + if(IS_ABSOLUTE "${lib}") + get_filename_component(libdir "${lib}" PATH) + string(REPLACE "\\" "/" libdir "${libdir}") + list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT ${libdir}) + endif() + endforeach() + endif() + + # Handle Python version suffixes + unset(COMPONENT_PYTHON_VERSION_MAJOR) + unset(COMPONENT_PYTHON_VERSION_MINOR) + if(${COMPONENT} MATCHES "^(python|mpi_python|numpy)([0-9])\$") + set(COMPONENT_UNVERSIONED "${CMAKE_MATCH_1}") + set(COMPONENT_PYTHON_VERSION_MAJOR "${CMAKE_MATCH_2}") + elseif(${COMPONENT} MATCHES "^(python|mpi_python|numpy)([0-9])\\.?([0-9])\$") + set(COMPONENT_UNVERSIONED "${CMAKE_MATCH_1}") + set(COMPONENT_PYTHON_VERSION_MAJOR "${CMAKE_MATCH_2}") + set(COMPONENT_PYTHON_VERSION_MINOR "${CMAKE_MATCH_3}") + endif() + + unset(_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME) + if (COMPONENT_PYTHON_VERSION_MINOR) + # Boost >= 1.67 + list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}${COMPONENT_PYTHON_VERSION_MAJOR}${COMPONENT_PYTHON_VERSION_MINOR}") + # Debian/Ubuntu (Some versions omit the 2 and/or 3 from the suffix) + list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}${COMPONENT_PYTHON_VERSION_MAJOR}-py${COMPONENT_PYTHON_VERSION_MAJOR}${COMPONENT_PYTHON_VERSION_MINOR}") + list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}-py${COMPONENT_PYTHON_VERSION_MAJOR}${COMPONENT_PYTHON_VERSION_MINOR}") + # Gentoo + list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}-${COMPONENT_PYTHON_VERSION_MAJOR}.${COMPONENT_PYTHON_VERSION_MINOR}") + # RPMs + list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}-${COMPONENT_PYTHON_VERSION_MAJOR}${COMPONENT_PYTHON_VERSION_MINOR}") + endif() + if (COMPONENT_PYTHON_VERSION_MAJOR AND NOT COMPONENT_PYTHON_VERSION_MINOR) + # Boost < 1.67 + list(APPEND _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME "${COMPONENT_UNVERSIONED}${COMPONENT_PYTHON_VERSION_MAJOR}") + endif() + + # Consolidate and report component-specific hints. + if(_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME) + list(REMOVE_DUPLICATES _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME) + _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" + "Component-specific library search names for ${COMPONENT_NAME}: ${_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME}") + endif() + if(_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT) + list(REMOVE_DUPLICATES _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT) + _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" + "Component-specific library search paths for ${COMPONENT}: ${_Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT}") + endif() + + # + # Find headers + # + _Boost_COMPONENT_HEADERS("${COMPONENT}" Boost_${UPPERCOMPONENT}_HEADER_NAME) + # Look for a standard boost header file. + if(Boost_${UPPERCOMPONENT}_HEADER_NAME) + if(EXISTS "${Boost_INCLUDE_DIR}/${Boost_${UPPERCOMPONENT}_HEADER_NAME}") + set(Boost_${UPPERCOMPONENT}_HEADER ON) + else() + set(Boost_${UPPERCOMPONENT}_HEADER OFF) + endif() + else() + set(Boost_${UPPERCOMPONENT}_HEADER ON) + message(WARNING "No header defined for ${COMPONENT}; skipping header check") + endif() + + # + # Find RELEASE libraries + # + unset(_boost_RELEASE_NAMES) + foreach(component IN LISTS _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME COMPONENT) + foreach(compiler IN LISTS _boost_COMPILER) + list(APPEND _boost_RELEASE_NAMES + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG}${_boost_ARCHITECTURE_TAG} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG} ) + endforeach() + list(APPEND _boost_RELEASE_NAMES + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG}${_boost_ARCHITECTURE_TAG} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_ABI_TAG} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component} ) + if(_boost_STATIC_RUNTIME_WORKAROUND) + set(_boost_RELEASE_STATIC_ABI_TAG "-s${_boost_RELEASE_ABI_TAG}") + foreach(compiler IN LISTS _boost_COMPILER) + list(APPEND _boost_RELEASE_NAMES + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG} ) + endforeach() + list(APPEND _boost_RELEASE_NAMES + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_RELEASE_STATIC_ABI_TAG} ) + endif() + endforeach() + if(Boost_THREADAPI AND ${COMPONENT} STREQUAL "thread") + _Boost_PREPEND_LIST_WITH_THREADAPI(_boost_RELEASE_NAMES ${_boost_RELEASE_NAMES}) + endif() + _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" + "Searching for ${UPPERCOMPONENT}_LIBRARY_RELEASE: ${_boost_RELEASE_NAMES}") + + # if Boost_LIBRARY_DIR_RELEASE is not defined, + # but Boost_LIBRARY_DIR_DEBUG is, look there first for RELEASE libs + if(NOT Boost_LIBRARY_DIR_RELEASE AND Boost_LIBRARY_DIR_DEBUG) + list(INSERT _boost_LIBRARY_SEARCH_DIRS_RELEASE 0 ${Boost_LIBRARY_DIR_DEBUG}) + endif() + + # Avoid passing backslashes to _Boost_FIND_LIBRARY due to macro re-parsing. + string(REPLACE "\\" "/" _boost_LIBRARY_SEARCH_DIRS_tmp "${_boost_LIBRARY_SEARCH_DIRS_RELEASE}") + + if(Boost_USE_RELEASE_LIBS) + _Boost_FIND_LIBRARY(Boost_${UPPERCOMPONENT}_LIBRARY_RELEASE RELEASE + NAMES ${_boost_RELEASE_NAMES} + HINTS ${_boost_LIBRARY_SEARCH_DIRS_tmp} + NAMES_PER_DIR + DOC "${_boost_docstring_release}" + ) + endif() + + # + # Find DEBUG libraries + # + unset(_boost_DEBUG_NAMES) + foreach(component IN LISTS _Boost_FIND_LIBRARY_HINTS_FOR_COMPONENT_NAME COMPONENT) + foreach(compiler IN LISTS _boost_COMPILER) + list(APPEND _boost_DEBUG_NAMES + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG}${_boost_ARCHITECTURE_TAG} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG} ) + endforeach() + list(APPEND _boost_DEBUG_NAMES + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG}${_boost_ARCHITECTURE_TAG} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_ABI_TAG} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component} ) + if(_boost_STATIC_RUNTIME_WORKAROUND) + set(_boost_DEBUG_STATIC_ABI_TAG "-s${_boost_DEBUG_ABI_TAG}") + foreach(compiler IN LISTS _boost_COMPILER) + list(APPEND _boost_DEBUG_NAMES + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${compiler}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG} ) + endforeach() + list(APPEND _boost_DEBUG_NAMES + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG}-${Boost_LIB_VERSION} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG}${_boost_ARCHITECTURE_TAG} + ${Boost_LIB_PREFIX}${Boost_NAMESPACE}_${component}${_boost_MULTITHREADED}${_boost_DEBUG_STATIC_ABI_TAG} ) + endif() + endforeach() + if(Boost_THREADAPI AND ${COMPONENT} STREQUAL "thread") + _Boost_PREPEND_LIST_WITH_THREADAPI(_boost_DEBUG_NAMES ${_boost_DEBUG_NAMES}) + endif() + _Boost_DEBUG_PRINT("${CMAKE_CURRENT_LIST_FILE}" "${CMAKE_CURRENT_LIST_LINE}" + "Searching for ${UPPERCOMPONENT}_LIBRARY_DEBUG: ${_boost_DEBUG_NAMES}") + + # if Boost_LIBRARY_DIR_DEBUG is not defined, + # but Boost_LIBRARY_DIR_RELEASE is, look there first for DEBUG libs + if(NOT Boost_LIBRARY_DIR_DEBUG AND Boost_LIBRARY_DIR_RELEASE) + list(INSERT _boost_LIBRARY_SEARCH_DIRS_DEBUG 0 ${Boost_LIBRARY_DIR_RELEASE}) + endif() + + # Avoid passing backslashes to _Boost_FIND_LIBRARY due to macro re-parsing. + string(REPLACE "\\" "/" _boost_LIBRARY_SEARCH_DIRS_tmp "${_boost_LIBRARY_SEARCH_DIRS_DEBUG}") + + if(Boost_USE_DEBUG_LIBS) + _Boost_FIND_LIBRARY(Boost_${UPPERCOMPONENT}_LIBRARY_DEBUG DEBUG + NAMES ${_boost_DEBUG_NAMES} + HINTS ${_boost_LIBRARY_SEARCH_DIRS_tmp} + NAMES_PER_DIR + DOC "${_boost_docstring_debug}" + ) + endif () + + if(Boost_REALPATH) + _Boost_SWAP_WITH_REALPATH(Boost_${UPPERCOMPONENT}_LIBRARY_RELEASE "${_boost_docstring_release}") + _Boost_SWAP_WITH_REALPATH(Boost_${UPPERCOMPONENT}_LIBRARY_DEBUG "${_boost_docstring_debug}" ) + endif() + + _Boost_ADJUST_LIB_VARS(${UPPERCOMPONENT}) + + # Check if component requires some compiler features + _Boost_COMPILER_FEATURES(${COMPONENT} _Boost_${UPPERCOMPONENT}_COMPILER_FEATURES) + +endforeach() + +# Restore the original find library ordering +if( Boost_USE_STATIC_LIBS ) + set(CMAKE_FIND_LIBRARY_SUFFIXES ${_boost_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES}) +endif() + +# ------------------------------------------------------------------------ +# End finding boost libraries +# ------------------------------------------------------------------------ + +set(Boost_INCLUDE_DIRS ${Boost_INCLUDE_DIR}) +set(Boost_LIBRARY_DIRS) +if(Boost_LIBRARY_DIR_RELEASE) + list(APPEND Boost_LIBRARY_DIRS ${Boost_LIBRARY_DIR_RELEASE}) +endif() +if(Boost_LIBRARY_DIR_DEBUG) + list(APPEND Boost_LIBRARY_DIRS ${Boost_LIBRARY_DIR_DEBUG}) +endif() +if(Boost_LIBRARY_DIRS) + list(REMOVE_DUPLICATES Boost_LIBRARY_DIRS) +endif() + +# ------------------------------------------------------------------------ +# Call FPHSA helper, see https://cmake.org/cmake/help/latest/module/FindPackageHandleStandardArgs.html +# ------------------------------------------------------------------------ + +# Define aliases as needed by the component handler in the FPHSA helper below +foreach(_comp IN LISTS Boost_FIND_COMPONENTS) + string(TOUPPER ${_comp} _uppercomp) + if(DEFINED Boost_${_uppercomp}_FOUND) + set(Boost_${_comp}_FOUND ${Boost_${_uppercomp}_FOUND}) + endif() +endforeach() + +find_package_handle_standard_args(Boost + REQUIRED_VARS Boost_INCLUDE_DIR + VERSION_VAR Boost_VERSION_STRING + HANDLE_COMPONENTS) + +if(Boost_FOUND) + if( NOT Boost_LIBRARY_DIRS ) + # Compatibility Code for backwards compatibility with CMake + # 2.4's FindBoost module. + + # Look for the boost library path. + # Note that the user may not have installed any libraries + # so it is quite possible the Boost_LIBRARY_DIRS may not exist. + set(_boost_LIB_DIR ${Boost_INCLUDE_DIR}) + + if("${_boost_LIB_DIR}" MATCHES "boost-[0-9]+") + get_filename_component(_boost_LIB_DIR ${_boost_LIB_DIR} PATH) + endif() + + if("${_boost_LIB_DIR}" MATCHES "/include$") + # Strip off the trailing "/include" in the path. + get_filename_component(_boost_LIB_DIR ${_boost_LIB_DIR} PATH) + endif() + + if(EXISTS "${_boost_LIB_DIR}/lib") + string(APPEND _boost_LIB_DIR /lib) + elseif(EXISTS "${_boost_LIB_DIR}/stage/lib") + string(APPEND _boost_LIB_DIR "/stage/lib") + else() + set(_boost_LIB_DIR "") + endif() + + if(_boost_LIB_DIR AND EXISTS "${_boost_LIB_DIR}") + set(Boost_LIBRARY_DIRS ${_boost_LIB_DIR}) + endif() + + endif() +else() + # Boost headers were not found so no components were found. + foreach(COMPONENT ${Boost_FIND_COMPONENTS}) + string(TOUPPER ${COMPONENT} UPPERCOMPONENT) + set(Boost_${UPPERCOMPONENT}_FOUND 0) + endforeach() +endif() + +# ------------------------------------------------------------------------ +# Add imported targets +# ------------------------------------------------------------------------ + +if(Boost_FOUND) + # The builtin CMake package in Boost 1.70+ introduces a new name + # for the header-only lib, let's provide the same UI in module mode + if(NOT TARGET Boost::headers) + add_library(Boost::headers INTERFACE IMPORTED) + if(Boost_INCLUDE_DIRS) + set_target_properties(Boost::headers PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${Boost_INCLUDE_DIRS}") + endif() + endif() + + # Define the old target name for header-only libraries for backwards + # compat. + if(NOT TARGET Boost::boost) + add_library(Boost::boost INTERFACE IMPORTED) + set_target_properties(Boost::boost + PROPERTIES INTERFACE_LINK_LIBRARIES Boost::headers) + endif() + + foreach(COMPONENT ${Boost_FIND_COMPONENTS}) + if(_Boost_IMPORTED_TARGETS AND NOT TARGET Boost::${COMPONENT}) + string(TOUPPER ${COMPONENT} UPPERCOMPONENT) + if(Boost_${UPPERCOMPONENT}_FOUND) + if(Boost_USE_STATIC_LIBS) + add_library(Boost::${COMPONENT} STATIC IMPORTED) + else() + # Even if Boost_USE_STATIC_LIBS is OFF, we might have static + # libraries as a result. + add_library(Boost::${COMPONENT} UNKNOWN IMPORTED) + endif() + if(Boost_INCLUDE_DIRS) + set_target_properties(Boost::${COMPONENT} PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${Boost_INCLUDE_DIRS}") + endif() + if(EXISTS "${Boost_${UPPERCOMPONENT}_LIBRARY}") + set_target_properties(Boost::${COMPONENT} PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" + IMPORTED_LOCATION "${Boost_${UPPERCOMPONENT}_LIBRARY}") + endif() + if(EXISTS "${Boost_${UPPERCOMPONENT}_LIBRARY_RELEASE}") + set_property(TARGET Boost::${COMPONENT} APPEND PROPERTY + IMPORTED_CONFIGURATIONS RELEASE) + set_target_properties(Boost::${COMPONENT} PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX" + IMPORTED_LOCATION_RELEASE "${Boost_${UPPERCOMPONENT}_LIBRARY_RELEASE}") + endif() + if(EXISTS "${Boost_${UPPERCOMPONENT}_LIBRARY_DEBUG}") + set_property(TARGET Boost::${COMPONENT} APPEND PROPERTY + IMPORTED_CONFIGURATIONS DEBUG) + set_target_properties(Boost::${COMPONENT} PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "CXX" + IMPORTED_LOCATION_DEBUG "${Boost_${UPPERCOMPONENT}_LIBRARY_DEBUG}") + endif() + if(_Boost_${UPPERCOMPONENT}_DEPENDENCIES) + unset(_Boost_${UPPERCOMPONENT}_TARGET_DEPENDENCIES) + foreach(dep ${_Boost_${UPPERCOMPONENT}_DEPENDENCIES}) + list(APPEND _Boost_${UPPERCOMPONENT}_TARGET_DEPENDENCIES Boost::${dep}) + endforeach() + if(COMPONENT STREQUAL "thread") + list(APPEND _Boost_${UPPERCOMPONENT}_TARGET_DEPENDENCIES Threads::Threads) + endif() + set_target_properties(Boost::${COMPONENT} PROPERTIES + INTERFACE_LINK_LIBRARIES "${_Boost_${UPPERCOMPONENT}_TARGET_DEPENDENCIES}") + endif() + if(_Boost_${UPPERCOMPONENT}_COMPILER_FEATURES) + set_target_properties(Boost::${COMPONENT} PROPERTIES + INTERFACE_COMPILE_FEATURES "${_Boost_${UPPERCOMPONENT}_COMPILER_FEATURES}") + endif() + endif() + endif() + endforeach() +endif() + +# ------------------------------------------------------------------------ +# Finalize +# ------------------------------------------------------------------------ + +# Report Boost_LIBRARIES +set(Boost_LIBRARIES "") +foreach(_comp IN LISTS Boost_FIND_COMPONENTS) + string(TOUPPER ${_comp} _uppercomp) + if(Boost_${_uppercomp}_FOUND) + list(APPEND Boost_LIBRARIES ${Boost_${_uppercomp}_LIBRARY}) + if(_comp STREQUAL "thread") + list(APPEND Boost_LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) + endif() + endif() +endforeach() + +# Configure display of cache entries in GUI. +foreach(v BOOSTROOT BOOST_ROOT ${_Boost_VARS_INC} ${_Boost_VARS_LIB}) + get_property(_type CACHE ${v} PROPERTY TYPE) + if(_type) + set_property(CACHE ${v} PROPERTY ADVANCED 1) + if("x${_type}" STREQUAL "xUNINITIALIZED") + if("x${v}" STREQUAL "xBoost_ADDITIONAL_VERSIONS") + set_property(CACHE ${v} PROPERTY TYPE STRING) + else() + set_property(CACHE ${v} PROPERTY TYPE PATH) + endif() + endif() + endif() +endforeach() + +# Record last used values of input variables so we can +# detect on the next run if the user changed them. +foreach(v + ${_Boost_VARS_INC} ${_Boost_VARS_LIB} + ${_Boost_VARS_DIR} ${_Boost_VARS_NAME} + ) + if(DEFINED ${v}) + set(_${v}_LAST "${${v}}" CACHE INTERNAL "Last used ${v} value.") + else() + unset(_${v}_LAST CACHE) + endif() +endforeach() + +# Maintain a persistent list of components requested anywhere since +# the last flush. +set(_Boost_COMPONENTS_SEARCHED "${_Boost_COMPONENTS_SEARCHED}") +list(APPEND _Boost_COMPONENTS_SEARCHED ${Boost_FIND_COMPONENTS}) +list(REMOVE_DUPLICATES _Boost_COMPONENTS_SEARCHED) +list(SORT _Boost_COMPONENTS_SEARCHED) +set(_Boost_COMPONENTS_SEARCHED "${_Boost_COMPONENTS_SEARCHED}" + CACHE INTERNAL "Components requested for this build tree.") + +# Restore project's policies +cmake_policy(POP) From 6badcefac417f0af3c3c06d9b5dd3a79a73796ca Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 7 Oct 2019 11:42:38 +0200 Subject: [PATCH 13/55] Patches to use CMake's FindBoost into gtsam --- cmake/FindBoost.cmake | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/cmake/FindBoost.cmake b/cmake/FindBoost.cmake index 078000f22..5f7cb5ddc 100644 --- a/cmake/FindBoost.cmake +++ b/cmake/FindBoost.cmake @@ -243,7 +243,10 @@ Set ``Boost_NO_BOOST_CMAKE`` to ``ON``, to disable the search for boost-cmake. # The FPHSA helper provides standard way of reporting final search results to # the user including the version and component checks. -include(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake) + +# Patch for GTSAM: +#include(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake) +include(FindPackageHandleStandardArgs) # Save project's policies cmake_policy(PUSH) @@ -1487,7 +1490,11 @@ if(WIN32) INTERFACE_COMPILE_DEFINITIONS "BOOST_ALL_NO_LIB") endif() -cmake_policy(GET CMP0074 _Boost_CMP0074) +# Patch for GTSAM: +if (POLICY CMP0074) + cmake_policy(GET CMP0074 _Boost_CMP0074) +endif() + if(NOT "x${_Boost_CMP0074}x" STREQUAL "xNEWx") _Boost_CHECK_SPELLING(Boost_ROOT) endif() @@ -1640,9 +1647,12 @@ if(Boost_INCLUDE_DIR) set(Boost_VERSION_STRING "${Boost_VERSION_MAJOR}.${Boost_VERSION_MINOR}.${Boost_VERSION_PATCH}") # Define final Boost_VERSION - cmake_policy(GET CMP0093 _Boost_CMP0093 - PARENT_SCOPE # undocumented, do not use outside of CMake - ) + if (POLICY CMP0093) + cmake_policy(GET CMP0093 _Boost_CMP0093 + PARENT_SCOPE # undocumented, do not use outside of CMake + ) + endif() + if("x${_Boost_CMP0093}x" STREQUAL "xNEWx") set(Boost_VERSION ${Boost_VERSION_STRING}) else() From a1d46fab3388d1b41668e79595d6632ab3141e3b Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 7 Oct 2019 11:54:28 +0200 Subject: [PATCH 14/55] Ubuntu PPA packages: use GTSAM_BUILD_WITH_MARCH_NATIVE=OFF --- debian/rules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/debian/rules b/debian/rules index 6a8d21c00..156629fd6 100755 --- a/debian/rules +++ b/debian/rules @@ -21,5 +21,5 @@ export DH_VERBOSE = 1 # dh_make generated override targets # This is example for Cmake (See https://bugs.debian.org/641051 ) override_dh_auto_configure: - dh_auto_configure -- -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_INSTALL_GEOGRAPHICLIB=OFF -DGTSAM_BUILD_TYPE_POSTFIXES=OFF + dh_auto_configure -- -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_INSTALL_GEOGRAPHICLIB=OFF -DGTSAM_BUILD_TYPE_POSTFIXES=OFF -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF From b3dccc6ef3e24eb11b60f03f857f177feff5714f Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 7 Oct 2019 12:07:49 +0200 Subject: [PATCH 15/55] get gtsam version number in synch --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b0457cb1c..1c9f0639a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ endif() # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 0) -set (GTSAM_VERSION_PATCH 0) +set (GTSAM_VERSION_PATCH 2) math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") From 5c883caf168809adf36995067a1cca548e0db5a5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 7 Oct 2019 23:43:01 -0400 Subject: [PATCH 16/55] label subplots for each estimator and hyphenate Geman-McClure --- matlab/gtsam_examples/VisualizeMEstimators.m | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index 5120934fe..1c5a14a70 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -36,7 +36,7 @@ plot_m_estimator(x, cauchyNoiseModel, rho, 'Cauchy', 3, 'cauchy.png') c = 1.0; rho = gemanmcclure(x, c); gemanmcclureNoiseModel = gtsam.noiseModel.mEstimator.GemanMcClure(c); -plot_m_estimator(x, gemanmcclureNoiseModel, rho, 'GemanMcClure', 4, 'gemanmcclure.png') +plot_m_estimator(x, gemanmcclureNoiseModel, rho, 'Geman-McClure', 4, 'gemanmcclure.png') c = 2.9846; rho = welsch(x, c); @@ -59,15 +59,20 @@ function plot_m_estimator(x, model, rho, plot_title, fig_id, filename) figure(fig_id); subplot(3, 1, 1); - plot(x, rho); + plot(x, rho, 'LineWidth',2); + title('rho function'); xlim([-5, 5]); - title(plot_title); subplot(3, 1, 2); - plot(x, psi); + plot(x, psi, 'LineWidth',2); + title('influence function'); xlim([-5, 5]); subplot(3, 1, 3); - plot(x, w); + plot(x, w, 'LineWidth',2); + title('weight function'); xlim([-5, 5]); + + sgtitle(plot_title, 'FontSize', 26); + saveas(figure(fig_id), filename); end From 4be8d8a75853409b44599e0df7bb160896f4838f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2019 20:20:29 -0400 Subject: [PATCH 17/55] Robust Estimator Updates - Moved implementations of residual and weight to NoiseModel.cpp. - Added `const` to function arguments and variables in `residual` and `weight`. - Aliased Welsh to Welsch. - Formatting. --- gtsam/linear/NoiseModel.cpp | 76 ++++++++++++++++++++++-- gtsam/linear/NoiseModel.h | 113 +++++++----------------------------- 2 files changed, 92 insertions(+), 97 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index d4cf2a5c2..67aa20b10 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -718,15 +718,25 @@ void Null::print(const std::string &s="") const Null::shared_ptr Null::Create() { return shared_ptr(new Null()); } +/* ************************************************************************* */ +// Fair +/* ************************************************************************* */ + Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { if (c_ <= 0) { throw runtime_error("mEstimator Fair takes only positive double in constructor."); } } -/* ************************************************************************* */ -// Fair -/* ************************************************************************* */ +double Fair::weight(const double error) const { + return 1.0 / (1.0 + std::abs(error) / c_); +} +double Fair::residual(const double error) const { + const double absError = std::abs(error); + const double normalizedError = absError / c_; + const double c_2 = c_ * c_; + return c_2 * (normalizedError - std::log(1 + normalizedError)); +} void Fair::print(const std::string &s="") const { cout << s << "fair (" << c_ << ")" << endl; } @@ -750,6 +760,20 @@ Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { } } +double Huber::weight(const double error) const { + const double absError = std::abs(error); + return (absError <= k_) ? (1.0) : (k_ / absError); +} + +double Huber::residual(const double error) const { + const double absError = std::abs(error); + if (absError <= k_) { // |x| <= k + return error*error / 2; + } else { // |x| > k + return k_ * (absError - (k_/2)); + } +} + void Huber::print(const std::string &s="") const { cout << s << "huber (" << k_ << ")" << endl; } @@ -774,6 +798,16 @@ Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), } } +double Cauchy::weight(const double error) const { + return ksquared_ / (ksquared_ + error*error); +} + +double Cauchy::residual(const double error) const { + const double xc2 = error / k_; + const double val = std::log(1 + (xc2*xc2)); + return ksquared_ * val * 0.5; +} + void Cauchy::print(const std::string &s="") const { cout << s << "cauchy (" << k_ << ")" << endl; } @@ -791,7 +825,30 @@ Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { /* ************************************************************************* */ // Tukey /* ************************************************************************* */ -Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} + +Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) { + if (c <= 0) { + throw runtime_error("mEstimator Tukey takes only positive double in constructor."); + } +} + +double Tukey::weight(const double error) const { + if (std::abs(error) <= c_) { + const double xc2 = error*error/csquared_; + return (1.0-xc2)*(1.0-xc2); + } + return 0.0; +} +double Tukey::residual(const double error) const { + double absError = std::abs(error); + if (absError <= c_) { + const double xc2 = error*error/csquared_; + const double t = (1 - xc2)*(1 - xc2)*(1 - xc2); + return csquared_ * (1 - t) / 6.0; + } else { + return csquared_ / 6.0; + } +} void Tukey::print(const std::string &s="") const { std::cout << s << ": Tukey (" << c_ << ")" << std::endl; @@ -810,8 +867,19 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { /* ************************************************************************* */ // Welsch /* ************************************************************************* */ + Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} +double Welsch::weight(const double error) const { + const double xc2 = (error*error)/csquared_; + return std::exp(-xc2); +} + +double Welsch::residual(const double error) const { + const double xc2 = (error*error)/csquared_; + return csquared_ * 0.5 * (1 - std::exp(-xc2) ); +} + void Welsch::print(const std::string &s="") const { std::cout << s << ": Welsch (" << c_ << ")" << std::endl; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 2ef830c81..5f957ba01 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -18,7 +18,6 @@ #pragma once -#include #include #include #include @@ -677,7 +676,7 @@ namespace gtsam { * evaluating the total penalty. But for now, I'm leaving this residual method as pure * virtual, so the existing mEstimators can inherit this default fallback behavior. */ - virtual double residual(double error) const { return 0; }; + virtual double residual(const double error) const { return 0; }; /* * This method is responsible for returning the weight function for a given amount of error. @@ -686,7 +685,7 @@ namespace gtsam { * for details. This method is required when optimizing cost functions with robust penalties * using iteratively re-weighted least squares. */ - virtual double weight(double error) const = 0; + virtual double weight(const double error) const = 0; virtual void print(const std::string &s) const = 0; virtual bool equals(const Base& expected, double tol=1e-8) const = 0; @@ -727,8 +726,8 @@ namespace gtsam { Null(const ReweightScheme reweight = Block) : Base(reweight) {} virtual ~Null() {} - virtual double weight(double /*error*/) const { return 1.0; } - virtual double residual(double error) const { return error; } + virtual double weight(const double /*error*/) const { return 1.0; } + virtual double residual(const double error) const { return error; } virtual void print(const std::string &s) const; virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; @@ -751,15 +750,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Fair(double c = 1.3998, const ReweightScheme reweight = Block); - double weight(double error) const { - return 1.0 / (1.0 + std::abs(error) / c_); - } - double residual(double error) const { - double absError = std::abs(error); - double normalizedError = absError / c_; - double val = normalizedError - std::log(1 + normalizedError); - return c_ * c_ * val; - } + double weight(const double error) const; + double residual(const double error) const; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; @@ -783,18 +775,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Huber(double k = 1.345, const ReweightScheme reweight = Block); - double weight(double error) const { - double absError = std::abs(error); - return (absError < k_) ? (1.0) : (k_ / absError); - } - double residual(double error) const { - double absError = std::abs(error); - if (absError <= k_) { // |x| <= k - return error*error / 2; - } else { // |x| > k - return k_ * (absError - (k_/2)); - } - } + double weight(const double error) const; + double residual(const double error) const; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -822,14 +804,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Cauchy(double k = 0.1, const ReweightScheme reweight = Block); - double weight(double error) const { - return ksquared_ / (ksquared_ + error*error); - } - double residual(double error) const { - double normalizedError = error / k_; - double val = std::log(1 + (normalizedError*normalizedError)); - return ksquared_ * val * 0.5; - } + double weight(const double error) const; + double residual(const double error) const; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -853,23 +829,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Tukey(double c = 4.6851, const ReweightScheme reweight = Block); - double weight(double error) const { - if (std::abs(error) <= c_) { - double xc2 = error*error/csquared_; - return (1.0-xc2)*(1.0-xc2); - } - return 0.0; - } - double residual(double error) const { - double absError = std::abs(error); - if (absError <= c_) { - double t = csquared_ - (error*error); - double val = t * t * t; - return ((csquared_*csquared_*csquared_) - val) / (6.0 * csquared_ * csquared_); - } else { - return csquared_ / 6.0; - } - } + double weight(const double error) const; + double residual(const double error) const; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -893,14 +854,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Welsch(double c = 2.9846, const ReweightScheme reweight = Block); - double weight(double error) const { - double xc2 = (error*error)/csquared_; - return std::exp(-xc2); - } - double residual(double error) const { - double xc2 = (error*error)/csquared_; - return csquared_ * 0.5 * (1 - std::exp(-xc2) ); - } + double weight(const double error) const; + double residual(const double error) const; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -920,35 +875,7 @@ namespace gtsam { // Welsh implements the "Welsch" robust error model (Zhang97ivc) // This was misspelled in previous versions of gtsam and should be // removed in the future. - class GTSAM_EXPORT Welsh : public Base { - protected: - double c_, csquared_; - - public: - typedef boost::shared_ptr shared_ptr; - - Welsh(double c = 2.9846, const ReweightScheme reweight = Block); - double weight(double error) const { - double xc2 = (error*error)/csquared_; - return std::exp(-xc2); - } - double residual(double error) const { - double xc2 = (error*error)/csquared_; - return csquared_ * 0.5 * (1 - std::exp(-xc2) ); - } - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; - static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(c_); - } - }; + using Welsh = Welsch; #endif /// GemanMcClure implements the "Geman-McClure" robust error model @@ -963,8 +890,8 @@ namespace gtsam { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); virtual ~GemanMcClure() {} - virtual double weight(double error) const; - virtual double residual(double error) const; + virtual double weight(const double error) const; + virtual double residual(const double error) const; virtual void print(const std::string &s) const; virtual bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -993,7 +920,7 @@ namespace gtsam { DCS(double c = 1.0, const ReweightScheme reweight = Block); virtual ~DCS() {} - virtual double weight(double error) const; + virtual double weight(const double error) const; virtual void print(const std::string &s) const; virtual bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -1024,11 +951,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; L2WithDeadZone(double k, const ReweightScheme reweight = Block); - double residual(double error) const { + double residual(const double error) const { const double abs_error = std::abs(error); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } - double weight(double error) const { + double weight(const double error) const { // note that this code is slightly uglier than above, because there are three distinct // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two // cases (deadzone, non-deadzone) above. From 431c37090a1e133ee400e5316cfd206cf792d82f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2019 21:25:41 -0400 Subject: [PATCH 18/55] removed const declarator from and functions, implemented DCS residual --- gtsam/linear/NoiseModel.cpp | 43 ++++++++++++++++++++------ gtsam/linear/NoiseModel.h | 61 ++++++++++++++++--------------------- 2 files changed, 59 insertions(+), 45 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 67aa20b10..68e579718 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -728,10 +728,10 @@ Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } } -double Fair::weight(const double error) const { +double Fair::weight(double error) const { return 1.0 / (1.0 + std::abs(error) / c_); } -double Fair::residual(const double error) const { +double Fair::residual(double error) const { const double absError = std::abs(error); const double normalizedError = absError / c_; const double c_2 = c_ * c_; @@ -760,12 +760,12 @@ Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { } } -double Huber::weight(const double error) const { +double Huber::weight(double error) const { const double absError = std::abs(error); return (absError <= k_) ? (1.0) : (k_ / absError); } -double Huber::residual(const double error) const { +double Huber::residual(double error) const { const double absError = std::abs(error); if (absError <= k_) { // |x| <= k return error*error / 2; @@ -798,11 +798,11 @@ Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), } } -double Cauchy::weight(const double error) const { +double Cauchy::weight(double error) const { return ksquared_ / (ksquared_ + error*error); } -double Cauchy::residual(const double error) const { +double Cauchy::residual(double error) const { const double xc2 = error / k_; const double val = std::log(1 + (xc2*xc2)); return ksquared_ * val * 0.5; @@ -832,14 +832,14 @@ Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), c } } -double Tukey::weight(const double error) const { +double Tukey::weight(double error) const { if (std::abs(error) <= c_) { const double xc2 = error*error/csquared_; return (1.0-xc2)*(1.0-xc2); } return 0.0; } -double Tukey::residual(const double error) const { +double Tukey::residual(double error) const { double absError = std::abs(error); if (absError <= c_) { const double xc2 = error*error/csquared_; @@ -870,12 +870,12 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} -double Welsch::weight(const double error) const { +double Welsch::weight(double error) const { const double xc2 = (error*error)/csquared_; return std::exp(-xc2); } -double Welsch::residual(const double error) const { +double Welsch::residual(double error) const { const double xc2 = (error*error)/csquared_; return csquared_ * 0.5 * (1 - std::exp(-xc2) ); } @@ -964,6 +964,16 @@ double DCS::weight(double error) const { return 1.0; } +double DCS::residual(double error) const { + // This is the simplified version of Eq 9 from (Agarwal13icra) + // after you simplify and cancel terms. + const double e2 = error*error; + const double e4 = e2*e2; + const double c2 = c_*c_; + + return (c2*e2 + c_*e4) / ((e2 + c_)*(e2 + c_)); +} + void DCS::print(const std::string &s="") const { std::cout << s << ": DCS (" << c_ << ")" << std::endl; } @@ -988,6 +998,19 @@ L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) : Base(r } } +double L2WithDeadZone::residual(double error) const { + const double abs_error = std::abs(error); + return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); +} +double L2WithDeadZone::weight(double error) const { + // note that this code is slightly uglier than residual, because there are three distinct + // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two + // cases (deadzone, non-deadzone) in residual. + if (std::abs(error) <= k_) return 0.0; + else if (error > k_) return (-k_+error)/error; + else return (k_+error)/error; +} + void L2WithDeadZone::print(const std::string &s="") const { std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 5f957ba01..4c289427b 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -676,7 +676,7 @@ namespace gtsam { * evaluating the total penalty. But for now, I'm leaving this residual method as pure * virtual, so the existing mEstimators can inherit this default fallback behavior. */ - virtual double residual(const double error) const { return 0; }; + virtual double residual(double error) const { return 0; }; /* * This method is responsible for returning the weight function for a given amount of error. @@ -685,7 +685,7 @@ namespace gtsam { * for details. This method is required when optimizing cost functions with robust penalties * using iteratively re-weighted least squares. */ - virtual double weight(const double error) const = 0; + virtual double weight(double error) const = 0; virtual void print(const std::string &s) const = 0; virtual bool equals(const Base& expected, double tol=1e-8) const = 0; @@ -726,8 +726,8 @@ namespace gtsam { Null(const ReweightScheme reweight = Block) : Base(reweight) {} virtual ~Null() {} - virtual double weight(const double /*error*/) const { return 1.0; } - virtual double residual(const double error) const { return error; } + virtual double weight(double /*error*/) const { return 1.0; } + virtual double residual(double error) const { return error; } virtual void print(const std::string &s) const; virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; @@ -750,8 +750,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Fair(double c = 1.3998, const ReweightScheme reweight = Block); - double weight(const double error) const; - double residual(const double error) const; + double weight(double error) const; + double residual(double error) const; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; @@ -775,8 +775,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Huber(double k = 1.345, const ReweightScheme reweight = Block); - double weight(const double error) const; - double residual(const double error) const; + double weight(double error) const; + double residual(double error) const; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -804,8 +804,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Cauchy(double k = 0.1, const ReweightScheme reweight = Block); - double weight(const double error) const; - double residual(const double error) const; + double weight(double error) const; + double residual(double error) const; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -829,8 +829,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Tukey(double c = 4.6851, const ReweightScheme reweight = Block); - double weight(const double error) const; - double residual(const double error) const; + double weight(double error) const; + double residual(double error) const; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -854,8 +854,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Welsch(double c = 2.9846, const ReweightScheme reweight = Block); - double weight(const double error) const; - double residual(const double error) const; + double weight(double error) const; + double residual(double error) const; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -889,11 +889,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); - virtual ~GemanMcClure() {} - virtual double weight(const double error) const; - virtual double residual(const double error) const; - virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, double tol=1e-8) const; + ~GemanMcClure() {} + double weight(double error) const; + double residual(double error) const; + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: @@ -919,10 +919,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; DCS(double c = 1.0, const ReweightScheme reweight = Block); - virtual ~DCS() {} - virtual double weight(const double error) const; - virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, double tol=1e-8) const; + ~DCS() {} + double weight(double error) const; + double residual(double error) const; + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: @@ -951,18 +952,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; L2WithDeadZone(double k, const ReweightScheme reweight = Block); - double residual(const double error) const { - const double abs_error = std::abs(error); - return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); - } - double weight(const double error) const { - // note that this code is slightly uglier than above, because there are three distinct - // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two - // cases (deadzone, non-deadzone) above. - if (std::abs(error) <= k_) return 0.0; - else if (error > k_) return (-k_+error)/error; - else return (k_+error)/error; - } + double residual(double error) const; + double weight(double error) const; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block); From 586a825c7c166a4b1e41a53d7ed229cd4d2a40bc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2019 21:25:56 -0400 Subject: [PATCH 19/55] added DCS residual tests --- gtsam/linear/tests/testNoiseModel.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 6419f2b9d..10578627f 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -556,10 +556,12 @@ TEST(NoiseModel, robustFunctionDCS) { const double k = 1.0, error1 = 1.0, error2 = 10.0; const mEstimator::DCS::shared_ptr dcs = mEstimator::DCS::Create(k); - const double weight1 = dcs->weight(error1), - weight2 = dcs->weight(error2); - DOUBLES_EQUAL(1.0 , weight1, 1e-8); - DOUBLES_EQUAL(0.00039211, weight2, 1e-8); + + 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); } TEST(NoiseModel, robustFunctionL2WithDeadZone) From 494005d64a6d1619adc08dc2e8a106bc8455d39a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2019 21:26:11 -0400 Subject: [PATCH 20/55] wrap DCS and L2WithDeadZone estimators --- gtsam.h | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index 35256e92c..04575f84d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1432,7 +1432,27 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { double residual(double error) const; }; -//TODO DCS and L2WithDeadZone mEstimators +virtual class DCS: gtsam::noiseModel::mEstimator::Base { + DCS(double c); + static gtsam::noiseModel::mEstimator::DCS* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double residual(double error) const; +}; + +virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { + L2WithDeadZone(double k); + static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double residual(double error) const; +}; }///\namespace mEstimator From 7501d8a4b08d00270e7fe88fe3bd2157dbae77b2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2019 21:31:09 -0400 Subject: [PATCH 21/55] added override keyword to derived versions of and for mEstimators --- gtsam/linear/NoiseModel.h | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 4c289427b..d93a7f825 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -750,8 +750,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Fair(double c = 1.3998, const ReweightScheme reweight = Block); - double weight(double error) const; - double residual(double error) const; + double weight(double error) const override; + double residual(double error) const override; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; @@ -775,8 +775,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Huber(double k = 1.345, const ReweightScheme reweight = Block); - double weight(double error) const; - double residual(double error) const; + double weight(double error) const override; + double residual(double error) const override; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -804,8 +804,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Cauchy(double k = 0.1, const ReweightScheme reweight = Block); - double weight(double error) const; - double residual(double error) const; + double weight(double error) const override; + double residual(double error) const override; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -829,8 +829,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Tukey(double c = 4.6851, const ReweightScheme reweight = Block); - double weight(double error) const; - double residual(double error) const; + double weight(double error) const override; + double residual(double error) const override; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -854,8 +854,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Welsch(double c = 2.9846, const ReweightScheme reweight = Block); - double weight(double error) const; - double residual(double error) const; + double weight(double error) const override; + double residual(double error) const override; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -890,8 +890,8 @@ namespace gtsam { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); ~GemanMcClure() {} - double weight(double error) const; - double residual(double error) const; + double weight(double error) const override; + double residual(double error) const override; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -920,8 +920,8 @@ namespace gtsam { DCS(double c = 1.0, const ReweightScheme reweight = Block); ~DCS() {} - double weight(double error) const; - double residual(double error) const; + double weight(double error) const override; + double residual(double error) const override; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; @@ -952,8 +952,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; L2WithDeadZone(double k, const ReweightScheme reweight = Block); - double residual(double error) const; - double weight(double error) const; + double residual(double error) const override; + double weight(double error) const override; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block); From 1240339dc2198f6f111ec32e744afae3c06b40d1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 9 Oct 2019 15:33:40 -0400 Subject: [PATCH 22/55] formatting; removed virtual from derived classes; added default values for L2WithDeadZone constructor to allow serialization --- gtsam.h | 4 ++-- gtsam/linear/NoiseModel.cpp | 32 +++++++++----------------------- gtsam/linear/NoiseModel.h | 14 +++++++------- 3 files changed, 18 insertions(+), 32 deletions(-) diff --git a/gtsam.h b/gtsam.h index 04575f84d..09b5b59e6 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1422,8 +1422,8 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { - GemanMcClure(double k); - static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double k); + GemanMcClure(double c); + static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); // enabling serialization functionality void serializable() const; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 68e579718..e0b7c8b17 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -731,6 +731,7 @@ Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { double Fair::weight(double error) const { return 1.0 / (1.0 + std::abs(error) / c_); } + double Fair::residual(double error) const { const double absError = std::abs(error); const double normalizedError = absError / c_; @@ -839,6 +840,7 @@ double Tukey::weight(double error) const { } return 0.0; } + double Tukey::residual(double error) const { double absError = std::abs(error); if (absError <= c_) { @@ -894,24 +896,6 @@ Welsch::shared_ptr Welsch::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Welsch(c, reweight)); } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 -Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} - -void Welsh::print(const std::string &s="") const { - std::cout << s << ": Welsh (" << c_ << ")" << std::endl; -} - -bool Welsh::equals(const Base &expected, double tol) const { - const Welsh* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_) < tol; -} - -Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new Welsh(c, reweight)); -} -#endif - /* ************************************************************************* */ // GemanMcClure /* ************************************************************************* */ @@ -992,16 +976,13 @@ DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { // L2WithDeadZone /* ************************************************************************* */ -L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { +L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) + : Base(reweight), k_(k) { if (k_ <= 0) { throw runtime_error("mEstimator L2WithDeadZone takes only positive double in constructor."); } } -double L2WithDeadZone::residual(double error) const { - const double abs_error = std::abs(error); - return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); -} double L2WithDeadZone::weight(double error) const { // note that this code is slightly uglier than residual, because there are three distinct // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two @@ -1011,6 +992,11 @@ double L2WithDeadZone::weight(double error) const { else return (k_+error)/error; } +double L2WithDeadZone::residual(double error) const { + const double abs_error = std::abs(error); + return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); +} + void L2WithDeadZone::print(const std::string &s="") const { std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index d93a7f825..d6b0a7331 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -725,11 +725,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Null(const ReweightScheme reweight = Block) : Base(reweight) {} - virtual ~Null() {} - virtual double weight(double /*error*/) const { return 1.0; } - virtual double residual(double error) const { return error; } - virtual void print(const std::string &s) const; - virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } + ~Null() {} + double weight(double /*error*/) const { return 1.0; } + double residual(double error) const { return error; } + void print(const std::string &s) const; + bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; private: @@ -951,9 +951,9 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - L2WithDeadZone(double k, const ReweightScheme reweight = Block); - double residual(double error) const override; + L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); double weight(double error) const override; + double residual(double error) const override; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block); From c2b6607d188105a320635e5111655671dba96cb4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 9 Oct 2019 15:46:24 -0400 Subject: [PATCH 23/55] add residual function for Fair mEstimator wrapper --- gtsam.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam.h b/gtsam.h index 09b5b59e6..c9364d9b0 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1375,6 +1375,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; virtual class Huber: gtsam::noiseModel::mEstimator::Base { From 4c9f9ecabf5b048db5775f778186d34334e690d5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 9 Oct 2019 15:48:12 -0400 Subject: [PATCH 24/55] cleaned up Matlab script for visualizing mEstimators --- matlab/gtsam_examples/VisualizeMEstimators.m | 56 ++++---------------- 1 file changed, 11 insertions(+), 45 deletions(-) diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index 1c5a14a70..8a0485334 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -12,47 +12,43 @@ % import gtsam.* -clear all; close all; x = linspace(-10, 10, 1000); -% x = linspace(-5, 5, 101); + +%% Define all the mEstimator models and plot them c = 1.3998; -rho = fair(x, c); fairNoiseModel = gtsam.noiseModel.mEstimator.Fair(c); -plot_m_estimator(x, fairNoiseModel, rho, 'Fair', 1, 'fair.png') +plot_m_estimator(x, fairNoiseModel, 'Fair', 1, 'fair.png') c = 1.345; -rho = huber(x, c); huberNoiseModel = gtsam.noiseModel.mEstimator.Huber(c); -plot_m_estimator(x, huberNoiseModel, rho, 'Huber', 2, 'huber.png') +plot_m_estimator(x, huberNoiseModel, 'Huber', 2, 'huber.png') c = 0.1; -rho = cauchy(x, c); cauchyNoiseModel = gtsam.noiseModel.mEstimator.Cauchy(c); -plot_m_estimator(x, cauchyNoiseModel, rho, 'Cauchy', 3, 'cauchy.png') +plot_m_estimator(x, cauchyNoiseModel, 'Cauchy', 3, 'cauchy.png') c = 1.0; -rho = gemanmcclure(x, c); gemanmcclureNoiseModel = gtsam.noiseModel.mEstimator.GemanMcClure(c); -plot_m_estimator(x, gemanmcclureNoiseModel, rho, 'Geman-McClure', 4, 'gemanmcclure.png') +plot_m_estimator(x, gemanmcclureNoiseModel, 'Geman-McClure', 4, 'gemanmcclure.png') c = 2.9846; -rho = welsch(x, c); welschNoiseModel = gtsam.noiseModel.mEstimator.Welsch(c); -plot_m_estimator(x, welschNoiseModel, rho, 'Welsch', 5, 'welsch.png') +plot_m_estimator(x, welschNoiseModel, 'Welsch', 5, 'welsch.png') c = 4.6851; -rho = tukey(x, c); tukeyNoiseModel = gtsam.noiseModel.mEstimator.Tukey(c); -plot_m_estimator(x, tukeyNoiseModel, rho, 'Tukey', 6, 'tukey.png') +plot_m_estimator(x, tukeyNoiseModel, 'Tukey', 6, 'tukey.png') %% Plot rho, psi and weights of the mEstimator. -function plot_m_estimator(x, model, rho, plot_title, fig_id, filename) +function plot_m_estimator(x, model, plot_title, fig_id, filename) w = zeros(size(x)); + rho = zeros(size(x)); for i = 1:size(x, 2) w(i) = model.weight(x(i)); + rho(i) = model.residual(x(i)); end psi = w .* x; @@ -75,33 +71,3 @@ function plot_m_estimator(x, model, rho, plot_title, fig_id, filename) saveas(figure(fig_id), filename); end - -function rho = fair(x, c) - rho = c^2 * ( (abs(x) ./ c) - log(1 + (abs(x)./c)) ); -end - -function rho = huber(x, k) - t = (abs(x) > k); - - rho = (x .^ 2) ./ 2; - rho(t) = k * (abs(x(t)) - (k/2)); -end - -function rho = cauchy(x, c) - rho = (c^2 / 2) .* log(1 + ((x./c) .^ 2)); -end - -function rho = gemanmcclure(x, c) - rho = ((x .^ 2) ./ 2) ./ (1 + x .^ 2); -end - -function rho = welsch(x, c) - rho = (c^2 / 2) * ( 1 - exp(-(x ./ c) .^2 )); -end - -function rho = tukey(x, c) - t = (abs(x) > c); - - rho = (c^2 / 6) * (1 - (1 - (x ./ c) .^ 2 ) .^ 3 ); - rho(t) = c .^ 2 / 6; -end From e20494324f7c1ef27e2c92d684dd0a5dc8b45cf4 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 9 Oct 2019 15:58:57 -0400 Subject: [PATCH 25/55] Gaussian Factor Graph unittests and linearization (with Python bindings) --- .../gtsam/tests/test_GaussianFactorGraph.py | 91 ++++++++++ gtsam.h | 4 + gtsam/nonlinear/Marginals.cpp | 30 +++- gtsam/nonlinear/Marginals.h | 26 ++- tests/testMarginals.cpp | 162 ++++++++++-------- 5 files changed, 238 insertions(+), 75 deletions(-) create mode 100644 cython/gtsam/tests/test_GaussianFactorGraph.py diff --git a/cython/gtsam/tests/test_GaussianFactorGraph.py b/cython/gtsam/tests/test_GaussianFactorGraph.py new file mode 100644 index 000000000..2e7875af0 --- /dev/null +++ b/cython/gtsam/tests/test_GaussianFactorGraph.py @@ -0,0 +1,91 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for Linear Factor Graphs. +Author: Frank Dellaert & Gerry Chen +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + +import numpy as np + +def create_graph(): + """Create a basic linear factor graph for testing""" + graph = gtsam.GaussianFactorGraph() + + x0 = gtsam.symbol(ord('x'), 0) + x1 = gtsam.symbol(ord('x'), 1) + x2 = gtsam.symbol(ord('x'), 2) + + BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1)) + PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1)) + + graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE) + graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE) + graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE) + + return graph, (x0, x1, x2) + +class TestGaussianFactorGraph(GtsamTestCase): + """Tests for Gaussian Factor Graphs.""" + + def test_fg(self): + """Test solving a linear factor graph""" + graph, X = create_graph() + result = graph.optimize() + + EXPECTEDX = [0, 1, 3] + + # check solutions + self.assertAlmostEqual(EXPECTEDX[0], result.at(X[0]), delta=1e-8) + self.assertAlmostEqual(EXPECTEDX[1], result.at(X[1]), delta=1e-8) + self.assertAlmostEqual(EXPECTEDX[2], result.at(X[2]), delta=1e-8) + + def test_convertNonlinear(self): + """Test converting a linear factor graph to a nonlinear one""" + graph, X = create_graph() + + EXPECTEDM = [1, 2, 3] + + # create nonlinear factor graph for marginalization + nfg = gtsam.LinearContainerFactor.ConvertLinearGraph(graph) + optimizer = gtsam.LevenbergMarquardtOptimizer(nfg, gtsam.Values()) + nlresult = optimizer.optimizeSafely() + + # marginalize + marginals = gtsam.Marginals(nfg, nlresult) + m = [marginals.marginalCovariance(x) for x in X] + + # check linear marginalizations + self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8) + self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) + self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) + + def test_linearMarginalization(self): + """Marginalize a linear factor graph""" + graph, X = create_graph() + result = graph.optimize() + + EXPECTEDM = [1, 2, 3] + + # linear factor graph marginalize + marginals = gtsam.Marginals(graph, result) + m = [marginals.marginalCovariance(x) for x in X] + + # check linear marginalizations + self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8) + self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) + self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) + +if __name__ == '__main__': + unittest.main() diff --git a/gtsam.h b/gtsam.h index bf3575580..1a1149084 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2011,6 +2011,10 @@ class Values { class Marginals { Marginals(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::VectorValues& solutionvec); void print(string s) const; Matrix marginalCovariance(size_t variable) const; diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index fcbbf2f44..11e123f13 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -28,15 +28,35 @@ namespace gtsam { /* ************************************************************************* */ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, EliminateableFactorGraph::OptionalOrdering ordering) -{ + : values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); - - // Linearize graph graph_ = *graph.linearize(solution); + computeBayesTree(ordering); +} - // Store values - values_ = solution; +/* ************************************************************************* */ +Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, + EliminateableFactorGraph::OptionalOrdering ordering) + : graph_(graph), factorization_(factorization) { + gttic(MarginalsConstructor); + Values vals; + for (const VectorValues::KeyValuePair& v: solution) { + vals.insert(v.first, v.second); + } + values_ = vals; + computeBayesTree(ordering); +} +/* ************************************************************************* */ +Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, + EliminateableFactorGraph::OptionalOrdering ordering) + : graph_(graph), values_(solution), factorization_(factorization) { + gttic(MarginalsConstructor); + computeBayesTree(ordering); +} + +/* ************************************************************************* */ +void Marginals::computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering) { // Compute BayesTree factorization_ = factorization; if(factorization_ == CHOLESKY) diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index e0a78042d..54a290196 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -51,7 +51,7 @@ public: /// Default constructor only for Cython wrapper Marginals(){} - /** Construct a marginals class. + /** Construct a marginals class from a nonlinear factor graph. * @param graph The factor graph defining the full joint density on all variables. * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). @@ -60,6 +60,24 @@ public: Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + /** Construct a marginals class from a linear factor graph. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The solution point to compute Gaussian marginals. + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + * @param ordering An optional variable ordering for elimination. + */ + Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, + EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + + /** Construct a marginals class from a linear factor graph. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The solution point to compute Gaussian marginals. + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + * @param ordering An optional variable ordering for elimination. + */ + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY, + EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + /** print */ void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -81,6 +99,12 @@ public: /** Optimize the bayes tree */ VectorValues optimize() const; + +protected: + + /** Compute the Bayes Tree as a helper function to the constructor */ + void computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering); + }; /** diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index 4bdbd9387..3c35c6bc0 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -87,6 +87,12 @@ TEST(Marginals, planarSLAMmarginals) { soln.insert(x3, Pose2(4.0, 0.0, 0.0)); soln.insert(l1, Point2(2.0, 2.0)); soln.insert(l2, Point2(4.0, 2.0)); + VectorValues soln_lin; + soln_lin.insert(x1, Vector3(0.0, 0.0, 0.0)); + soln_lin.insert(x2, Vector3(2.0, 0.0, 0.0)); + soln_lin.insert(x3, Vector3(4.0, 0.0, 0.0)); + soln_lin.insert(l1, Vector2(2.0, 2.0)); + soln_lin.insert(l2, Vector2(4.0, 2.0)); Matrix expectedx1(3,3); expectedx1 << @@ -110,71 +116,80 @@ TEST(Marginals, planarSLAMmarginals) { Matrix expectedl2(2,2); expectedl2 << 0.293870968, -0.104516129, - -0.104516129, 0.391935484; + -0.104516129, 0.391935484; - // Check marginals covariances for all variables (Cholesky mode) - Marginals marginals(graph, soln, Marginals::CHOLESKY); - EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8)); - EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8)); - EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8)); - EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8)); - EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8)); + auto testMarginals = [&] (Marginals marginals) { + EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8)); + EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8)); + EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8)); + EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8)); + EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8)); + }; - // Check marginals covariances for all variables (QR mode) + auto testJointMarginals = [&] (Marginals marginals) { + // Check joint marginals for 3 variables + Matrix expected_l2x1x3(8,8); + expected_l2x1x3 << + 0.293871159514111, -0.104516127560770, 0.090000180000270, -0.000000000000000, -0.020000000000000, 0.151935669757191, -0.104516127560770, -0.050967744878460, + -0.104516127560770, 0.391935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193, + 0.090000180000270, 0.000000000000000, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.090000180000270, 0.000000000000000, 0.000000000000000, + -0.000000000000000, 0.090000180000270, -0.000000000000000, 0.090000180000270, 0.000000000000000, -0.000000000000000, 0.090000180000270, 0.000000000000000, + -0.020000000000000, 0.040000000000000, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.000000000000000, 0.040000000000000, 0.010000000000000, + 0.151935669757191, 0.007741936219615, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.160967924878730, 0.007741936219615, 0.004516127560770, + -0.104516127560770, 0.351935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193, + -0.050967744878460, 0.056129031890193, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.004516127560770, 0.056129031890193, 0.027741936219615; + KeyVector variables {x1, l2, x3}; + JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,0,3,2)), Matrix(joint_l2x1x3(x1,l2)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,0,3,2)), Matrix(joint_l2x1x3(x3,l2)), 1e-6)); + + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,2,2,3)), Matrix(joint_l2x1x3(l2,x1)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,2,3,3)), Matrix(joint_l2x1x3(x1,x1)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,2,3,3)), Matrix(joint_l2x1x3(x3,x1)), 1e-6)); + + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,5,2,3)), Matrix(joint_l2x1x3(l2,x3)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,5,3,3)), Matrix(joint_l2x1x3(x1,x3)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,5,3,3)), Matrix(joint_l2x1x3(x3,x3)), 1e-6)); + + // Check joint marginals for 2 variables (different code path than >2 variable case above) + Matrix expected_l2x1(5,5); + expected_l2x1 << + 0.293871159514111, -0.104516127560770, 0.090000180000270, -0.000000000000000, -0.020000000000000, + -0.104516127560770, 0.391935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, + 0.090000180000270, 0.000000000000000, 0.090000180000270, -0.000000000000000, 0.000000000000000, + -0.000000000000000, 0.090000180000270, -0.000000000000000, 0.090000180000270, 0.000000000000000, + -0.020000000000000, 0.040000000000000, 0.000000000000000, 0.000000000000000, 0.010000000000000; + variables.resize(2); + variables[0] = l2; + variables[1] = x1; + JointMarginal joint_l2x1 = marginals.jointMarginalCovariance(variables); + EXPECT(assert_equal(Matrix(expected_l2x1.block(0,0,2,2)), Matrix(joint_l2x1(l2,l2)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1.block(2,0,3,2)), Matrix(joint_l2x1(x1,l2)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1.block(0,2,2,3)), Matrix(joint_l2x1(l2,x1)), 1e-6)); + EXPECT(assert_equal(Matrix(expected_l2x1.block(2,2,3,3)), Matrix(joint_l2x1(x1,x1)), 1e-6)); + + // Check joint marginal for 1 variable (different code path than >1 variable cases above) + variables.resize(1); + variables[0] = x1; + JointMarginal joint_x1 = marginals.jointMarginalCovariance(variables); + EXPECT(assert_equal(expectedx1, Matrix(joint_l2x1(x1,x1)), 1e-6)); + }; + + Marginals marginals; + + marginals = Marginals(graph, soln, Marginals::CHOLESKY); + testMarginals(marginals); marginals = Marginals(graph, soln, Marginals::QR); - EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8)); - EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8)); - EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8)); - EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8)); - EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8)); + testMarginals(marginals); + testJointMarginals(marginals); - // Check joint marginals for 3 variables - Matrix expected_l2x1x3(8,8); - expected_l2x1x3 << - 0.293871159514111, -0.104516127560770, 0.090000180000270, -0.000000000000000, -0.020000000000000, 0.151935669757191, -0.104516127560770, -0.050967744878460, - -0.104516127560770, 0.391935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193, - 0.090000180000270, 0.000000000000000, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.090000180000270, 0.000000000000000, 0.000000000000000, - -0.000000000000000, 0.090000180000270, -0.000000000000000, 0.090000180000270, 0.000000000000000, -0.000000000000000, 0.090000180000270, 0.000000000000000, - -0.020000000000000, 0.040000000000000, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.000000000000000, 0.040000000000000, 0.010000000000000, - 0.151935669757191, 0.007741936219615, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.160967924878730, 0.007741936219615, 0.004516127560770, - -0.104516127560770, 0.351935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193, - -0.050967744878460, 0.056129031890193, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.004516127560770, 0.056129031890193, 0.027741936219615; - KeyVector variables {x1, l2, x3}; - JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables); - EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6)); - EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,0,3,2)), Matrix(joint_l2x1x3(x1,l2)), 1e-6)); - EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,0,3,2)), Matrix(joint_l2x1x3(x3,l2)), 1e-6)); - - EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,2,2,3)), Matrix(joint_l2x1x3(l2,x1)), 1e-6)); - EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,2,3,3)), Matrix(joint_l2x1x3(x1,x1)), 1e-6)); - EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,2,3,3)), Matrix(joint_l2x1x3(x3,x1)), 1e-6)); - - EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,5,2,3)), Matrix(joint_l2x1x3(l2,x3)), 1e-6)); - EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,5,3,3)), Matrix(joint_l2x1x3(x1,x3)), 1e-6)); - EXPECT(assert_equal(Matrix(expected_l2x1x3.block(5,5,3,3)), Matrix(joint_l2x1x3(x3,x3)), 1e-6)); - - // Check joint marginals for 2 variables (different code path than >2 variable case above) - Matrix expected_l2x1(5,5); - expected_l2x1 << - 0.293871159514111, -0.104516127560770, 0.090000180000270, -0.000000000000000, -0.020000000000000, - -0.104516127560770, 0.391935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, - 0.090000180000270, 0.000000000000000, 0.090000180000270, -0.000000000000000, 0.000000000000000, - -0.000000000000000, 0.090000180000270, -0.000000000000000, 0.090000180000270, 0.000000000000000, - -0.020000000000000, 0.040000000000000, 0.000000000000000, 0.000000000000000, 0.010000000000000; - variables.resize(2); - variables[0] = l2; - variables[1] = x1; - JointMarginal joint_l2x1 = marginals.jointMarginalCovariance(variables); - EXPECT(assert_equal(Matrix(expected_l2x1.block(0,0,2,2)), Matrix(joint_l2x1(l2,l2)), 1e-6)); - EXPECT(assert_equal(Matrix(expected_l2x1.block(2,0,3,2)), Matrix(joint_l2x1(x1,l2)), 1e-6)); - EXPECT(assert_equal(Matrix(expected_l2x1.block(0,2,2,3)), Matrix(joint_l2x1(l2,x1)), 1e-6)); - EXPECT(assert_equal(Matrix(expected_l2x1.block(2,2,3,3)), Matrix(joint_l2x1(x1,x1)), 1e-6)); - - // Check joint marginal for 1 variable (different code path than >1 variable cases above) - variables.resize(1); - variables[0] = x1; - JointMarginal joint_x1 = marginals.jointMarginalCovariance(variables); - EXPECT(assert_equal(expectedx1, Matrix(joint_l2x1(x1,x1)), 1e-6)); + GaussianFactorGraph gfg = *graph.linearize(soln); + marginals = Marginals(gfg, soln_lin, Marginals::CHOLESKY); + testMarginals(marginals); + marginals = Marginals(gfg, soln_lin, Marginals::QR); + testMarginals(marginals); + testJointMarginals(marginals); } /* ************************************************************************* */ @@ -222,17 +237,26 @@ TEST(Marginals, order) { vals.at(3).bearing(vals.at(101)), vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2)); + auto testMarginals = [&] (Marginals marginals, KeySet set) { + KeyVector keys(set.begin(), set.end()); + JointMarginal joint = marginals.jointMarginalCovariance(keys); + + LONGS_EQUAL(3, (long)joint(0,0).rows()); + LONGS_EQUAL(3, (long)joint(1,1).rows()); + LONGS_EQUAL(3, (long)joint(2,2).rows()); + LONGS_EQUAL(3, (long)joint(3,3).rows()); + LONGS_EQUAL(2, (long)joint(100,100).rows()); + LONGS_EQUAL(2, (long)joint(101,101).rows()); + }; + Marginals marginals(fg, vals); KeySet set = fg.keys(); - KeyVector keys(set.begin(), set.end()); - JointMarginal joint = marginals.jointMarginalCovariance(keys); + testMarginals(marginals, set); - LONGS_EQUAL(3, (long)joint(0,0).rows()); - LONGS_EQUAL(3, (long)joint(1,1).rows()); - LONGS_EQUAL(3, (long)joint(2,2).rows()); - LONGS_EQUAL(3, (long)joint(3,3).rows()); - LONGS_EQUAL(2, (long)joint(100,100).rows()); - LONGS_EQUAL(2, (long)joint(101,101).rows()); + GaussianFactorGraph gfg = *fg.linearize(vals); + marginals = Marginals(gfg, vals); + set = gfg.keys(); + testMarginals(marginals, set); } /* ************************************************************************* */ From bc511a26eca5053097d1d09cd2b3931b1a95d282 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 9 Oct 2019 17:12:18 -0400 Subject: [PATCH 26/55] `optional` and `auto` modernization --- gtsam/nonlinear/Marginals.cpp | 17 ++++++++--------- gtsam/nonlinear/Marginals.h | 8 ++++---- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 11e123f13..7f554b5e5 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -27,7 +27,7 @@ namespace gtsam { /* ************************************************************************* */ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) + boost::optional ordering) : values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); graph_ = *graph.linearize(solution); @@ -36,12 +36,12 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, /* ************************************************************************* */ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) + boost::optional ordering) : graph_(graph), factorization_(factorization) { gttic(MarginalsConstructor); Values vals; - for (const VectorValues::KeyValuePair& v: solution) { - vals.insert(v.first, v.second); + for (const auto& keyValue: solution) { + vals.insert(keyValue.first, keyValue.second); } values_ = vals; computeBayesTree(ordering); @@ -49,16 +49,15 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut /* ************************************************************************* */ Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) + boost::optional ordering) : graph_(graph), values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); computeBayesTree(ordering); } /* ************************************************************************* */ -void Marginals::computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering) { +void Marginals::computeBayesTree(boost::optional ordering) { // Compute BayesTree - factorization_ = factorization; if(factorization_ == CHOLESKY) bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); else if(factorization_ == QR) @@ -145,7 +144,7 @@ JointMarginal Marginals::jointMarginalInformation(const KeyVector& variables) co // Get dimensions from factor graph std::vector dims; dims.reserve(variablesSorted.size()); - for(Key key: variablesSorted) { + for(const auto& key: variablesSorted) { dims.push_back(values_.at(key).dim()); } @@ -162,7 +161,7 @@ VectorValues Marginals::optimize() const { void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const { cout << s << "Joint marginal on keys "; bool first = true; - for(Key key: keys_) { + for(const auto& key: keys_) { if(!first) cout << ", "; else diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 54a290196..4e5921544 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -58,7 +58,7 @@ public: * @param ordering An optional variable ordering for elimination. */ Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + boost::optional ordering = boost::none); /** Construct a marginals class from a linear factor graph. * @param graph The factor graph defining the full joint density on all variables. @@ -67,7 +67,7 @@ public: * @param ordering An optional variable ordering for elimination. */ Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + boost::optional ordering = boost::none); /** Construct a marginals class from a linear factor graph. * @param graph The factor graph defining the full joint density on all variables. @@ -76,7 +76,7 @@ public: * @param ordering An optional variable ordering for elimination. */ Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + boost::optional ordering = boost::none); /** print */ void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -103,7 +103,7 @@ public: protected: /** Compute the Bayes Tree as a helper function to the constructor */ - void computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering); + void computeBayesTree(boost::optional ordering); }; From 5900eaa8ec35467b41ca98fa6b5f289f8ed8a560 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 10 Oct 2019 01:02:38 -0400 Subject: [PATCH 27/55] Change ordering back to OptionalOrdering typedef --- gtsam/nonlinear/Marginals.cpp | 8 ++++---- gtsam/nonlinear/Marginals.h | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 7f554b5e5..c24cb6143 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -27,7 +27,7 @@ namespace gtsam { /* ************************************************************************* */ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, - boost::optional ordering) + EliminateableFactorGraph::OptionalOrdering ordering) : values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); graph_ = *graph.linearize(solution); @@ -36,7 +36,7 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, /* ************************************************************************* */ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, - boost::optional ordering) + EliminateableFactorGraph::OptionalOrdering ordering) : graph_(graph), factorization_(factorization) { gttic(MarginalsConstructor); Values vals; @@ -49,14 +49,14 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut /* ************************************************************************* */ Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, - boost::optional ordering) + EliminateableFactorGraph::OptionalOrdering ordering) : graph_(graph), values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); computeBayesTree(ordering); } /* ************************************************************************* */ -void Marginals::computeBayesTree(boost::optional ordering) { +void Marginals::computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering) { // Compute BayesTree if(factorization_ == CHOLESKY) bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 4e5921544..54a290196 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -58,7 +58,7 @@ public: * @param ordering An optional variable ordering for elimination. */ Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, - boost::optional ordering = boost::none); + EliminateableFactorGraph::OptionalOrdering ordering = boost::none); /** Construct a marginals class from a linear factor graph. * @param graph The factor graph defining the full joint density on all variables. @@ -67,7 +67,7 @@ public: * @param ordering An optional variable ordering for elimination. */ Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, - boost::optional ordering = boost::none); + EliminateableFactorGraph::OptionalOrdering ordering = boost::none); /** Construct a marginals class from a linear factor graph. * @param graph The factor graph defining the full joint density on all variables. @@ -76,7 +76,7 @@ public: * @param ordering An optional variable ordering for elimination. */ Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY, - boost::optional ordering = boost::none); + EliminateableFactorGraph::OptionalOrdering ordering = boost::none); /** print */ void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -103,7 +103,7 @@ public: protected: /** Compute the Bayes Tree as a helper function to the constructor */ - void computeBayesTree(boost::optional ordering); + void computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering); }; From 76367bd946a39c89580f52d0f07b242a8bcd0ac6 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Thu, 10 Oct 2019 10:34:07 -0700 Subject: [PATCH 28/55] Fix Metis ordering for empty graph and single node --- gtsam/inference/Ordering.cpp | 12 +++++++++++- gtsam/inference/Ordering.h | 5 ++++- gtsam/inference/tests/testOrdering.cpp | 22 ++++++++++++++++++++++ 3 files changed, 37 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index da61ca57e..0875755aa 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -213,11 +213,21 @@ Ordering Ordering::Metis(const MetisIndex& met) { #ifdef GTSAM_SUPPORT_NESTED_DISSECTION gttic(Ordering_METIS); + idx_t size = met.nValues(); + if (size == 0) + { + return Ordering(); + } + + if (size == 1) + { + return Ordering(KeyVector(1, met.intToKey(0))); + } + vector xadj = met.xadj(); vector adj = met.adj(); vector perm, iperm; - idx_t size = met.nValues(); for (idx_t i = 0; i < size; i++) { perm.push_back(0); iperm.push_back(0); diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index f770c7da1..a2d165792 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -191,7 +191,10 @@ public: template static Ordering Metis(const FACTOR_GRAPH& graph) { - return Metis(MetisIndex(graph)); + if (graph.empty()) + return Ordering(); + else + return Metis(MetisIndex(graph)); } /// @} diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 0d23d9655..0305218af 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -294,6 +294,28 @@ TEST(Ordering, MetisLoop) { } #endif /* ************************************************************************* */ +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION +TEST(Ordering, MetisEmptyGraph) { + SymbolicFactorGraph symbolicGraph; + + Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph); + Ordering expected; + EXPECT(assert_equal(expected, actual)); +} +#endif +/* ************************************************************************* */ +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION +TEST(Ordering, MetisSingleNode) { + // create graph with a single node + SymbolicFactorGraph symbolicGraph; + symbolicGraph.push_factor(7); + + Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph); + Ordering expected = Ordering(list_of(7)); + EXPECT(assert_equal(expected, actual)); +} +#endif +/* ************************************************************************* */ TEST(Ordering, Create) { // create chain graph From f44ba996e02ea75ec0d144cce8c4f41ca495663d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 12 Oct 2019 14:13:31 -0400 Subject: [PATCH 29/55] Add override to resolve warnings on clang --- gtsam/linear/NoiseModel.h | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index d6b0a7331..f73fde51c 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -752,8 +752,8 @@ namespace gtsam { Fair(double c = 1.3998, const ReweightScheme reweight = Block); double weight(double error) const override; double residual(double error) const override; - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; + 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) ; private: @@ -777,8 +777,8 @@ namespace gtsam { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double error) const override; double residual(double error) const override; - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; + 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) ; private: @@ -806,8 +806,8 @@ namespace gtsam { Cauchy(double k = 0.1, const ReweightScheme reweight = Block); double weight(double error) const override; double residual(double error) const override; - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; + 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) ; private: @@ -831,8 +831,8 @@ namespace gtsam { Tukey(double c = 4.6851, const ReweightScheme reweight = Block); double weight(double error) const override; double residual(double error) const override; - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; + 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) ; private: @@ -856,8 +856,8 @@ namespace gtsam { Welsch(double c = 2.9846, const ReweightScheme reweight = Block); double weight(double error) const override; double residual(double error) const override; - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; + 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) ; private: @@ -892,8 +892,8 @@ namespace gtsam { ~GemanMcClure() {} double weight(double error) const override; double residual(double error) const override; - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; + 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) ; protected: @@ -922,8 +922,8 @@ namespace gtsam { ~DCS() {} double weight(double error) const override; double residual(double error) const override; - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; + 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) ; protected: @@ -954,8 +954,8 @@ namespace gtsam { L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); double weight(double error) const override; double residual(double error) const override; - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; + 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); private: From 4a1491876d5a96ea791c0e17204ace05f6eb206a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 12 Oct 2019 14:14:10 -0400 Subject: [PATCH 30/55] Replace deprecated `ptr_fun` with lambda expression --- gtsam/linear/GaussianBayesNet.cpp | 12 ++++++------ gtsam/linear/GaussianBayesTree.cpp | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index bc96452b9..e9938ceb6 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -187,16 +187,16 @@ namespace gtsam { } /* ************************************************************************* */ - double GaussianBayesNet::logDeterminant() const - { + double GaussianBayesNet::logDeterminant() const { double logDet = 0.0; - for(const sharedConditional& cg: *this) { - if(cg->get_model()) { + for (const sharedConditional& cg : *this) { + if (cg->get_model()) { Vector diag = cg->R().diagonal(); cg->get_model()->whitenInPlace(diag); - logDet += diag.unaryExpr(ptr_fun(log)).sum(); + logDet += diag.unaryExpr([](double x) { return log(x); }).sum(); } else { - logDet += cg->R().diagonal().unaryExpr(ptr_fun(log)).sum(); + logDet += + cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); } } return logDet; diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 8d0fafb61..13c19bce6 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -40,11 +40,11 @@ namespace gtsam { parentSum += clique->conditional() ->R() .diagonal() - .unaryExpr(std::ptr_fun(log)) + .unaryExpr([](double x) { return log(x); }) .sum(); return 0; } - } + } // namespace internal /* ************************************************************************* */ bool GaussianBayesTree::equals(const This& other, double tol) const From fc6d6794848fcf62b0680419f5b1f2ff12c442ea Mon Sep 17 00:00:00 2001 From: Peter Mullen Date: Sat, 12 Oct 2019 22:56:34 -0700 Subject: [PATCH 31/55] fix compile error --- timing/timeGaussianFactorGraph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/timing/timeGaussianFactorGraph.cpp b/timing/timeGaussianFactorGraph.cpp index 3258edb49..2c1e11586 100644 --- a/timing/timeGaussianFactorGraph.cpp +++ b/timing/timeGaussianFactorGraph.cpp @@ -40,7 +40,7 @@ double timeKalmanSmoother(int T) { /* ************************************************************************* */ // Create a planar factor graph and optimize double timePlanarSmoother(int N, bool old = true) { - GaussianFactorGraph fg = planarGraph(N).get<0>(); + GaussianFactorGraph fg = planarGraph(N).first; clock_t start = clock(); fg.optimize(); clock_t end = clock (); @@ -51,7 +51,7 @@ double timePlanarSmoother(int N, bool old = true) { /* ************************************************************************* */ // Create a planar factor graph and eliminate double timePlanarSmootherEliminate(int N, bool old = true) { - GaussianFactorGraph fg = planarGraph(N).get<0>(); + GaussianFactorGraph fg = planarGraph(N).first; clock_t start = clock(); fg.eliminateMultifrontal(); clock_t end = clock (); From 5a358489dc91daeaafd6b6dcb6f84670d479e104 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 13 Oct 2019 23:54:22 -0400 Subject: [PATCH 32/55] Some low hanging fruit changing `func(..., boost::optional)` to overloaded `func(...)` and `func(..., const Class &)` not all done Also, although it currently passes all tests, I think some more tests may be needed... --- gtsam/base/TestableAssertions.h | 8 +- gtsam/inference/EliminateableFactorGraph.h | 4 +- gtsam/linear/GaussianBayesNet.cpp | 21 +-- gtsam/linear/GaussianBayesNet.h | 9 +- gtsam/linear/GaussianFactorGraph.cpp | 45 +++++- gtsam/linear/GaussianFactorGraph.h | 47 +++++- gtsam/linear/HessianFactor.cpp | 4 +- gtsam/linear/HessianFactor.h | 6 +- gtsam/linear/IterativeSolver.cpp | 21 ++- gtsam/linear/IterativeSolver.h | 16 +- gtsam/linear/JacobianFactor.cpp | 172 +++++++++++++++------ gtsam/linear/JacobianFactor.h | 36 ++++- gtsam/linear/RegularHessianFactor.h | 8 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 8 +- 14 files changed, 304 insertions(+), 101 deletions(-) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 1a31aa284..a698f8f98 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -71,12 +71,8 @@ bool assert_equal(const V& expected, const boost::optional& actual, double to } template -bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { - if (!actual) { - std::cout << "actual is boost::none" << std::endl; - return false; - } - return assert_equal(expected, *actual, tol); +bool assert_equal(const V& expected, double tol = 1e-9) { + return false; } /** diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 8084aa75b..249c594b6 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -89,10 +89,10 @@ namespace gtsam { typedef boost::function Eliminate; /// Typedef for an optional ordering as an argument to elimination functions - typedef boost::optional OptionalOrdering; + typedef const boost::optional& OptionalOrdering; /// Typedef for an optional variable index as an argument to elimination functions - typedef boost::optional OptionalVariableIndex; + typedef const boost::optional& OptionalVariableIndex; /// Typedef for an optional ordering type typedef boost::optional OptionalOrderingType; diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index e9938ceb6..04094d593 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -156,16 +156,17 @@ namespace gtsam { } /* ************************************************************************* */ - pair GaussianBayesNet::matrix(boost::optional ordering) const { - if (ordering) { - // Convert to a GaussianFactorGraph and use its machinery - GaussianFactorGraph factorGraph(*this); - return factorGraph.jacobian(ordering); - } else { - // recursively call with default ordering - const auto defaultOrdering = this->ordering(); - return matrix(defaultOrdering); - } + pair GaussianBayesNet::matrix(const Ordering& ordering) const { + // Convert to a GaussianFactorGraph and use its machinery + GaussianFactorGraph factorGraph(*this); + return factorGraph.jacobian(ordering); + } + + /* ************************************************************************* */ + pair GaussianBayesNet::matrix() const { + // recursively call with default ordering + const auto defaultOrdering = this->ordering(); + return matrix(defaultOrdering); } ///* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 669c8a964..9da7a1609 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -92,7 +92,14 @@ namespace gtsam { * Will return upper-triangular matrix only when using 'ordering' above. * In case Bayes net is incomplete zero columns are added to the end. */ - std::pair matrix(boost::optional ordering = boost::none) const; + std::pair matrix(const Ordering& ordering) const; + + /** + * Return (dense) upper-triangular matrix representation + * Will return upper-triangular matrix only when using 'ordering' above. + * In case Bayes net is incomplete zero columns are added to the end. + */ + std::pair matrix() const; /** * Optimize along the gradient direction, with a closed-form computation to perform the line diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 9281c7e7a..fe0a9b2df 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -190,33 +190,62 @@ namespace gtsam { /* ************************************************************************* */ Matrix GaussianFactorGraph::augmentedJacobian( - boost::optional optionalOrdering) const { + const Ordering& ordering) const { // combine all factors - JacobianFactor combined(*this, optionalOrdering); + JacobianFactor combined(*this, ordering); + return combined.augmentedJacobian(); + } + + /* ************************************************************************* */ + Matrix GaussianFactorGraph::augmentedJacobian() const { + // combine all factors + JacobianFactor combined(*this); return combined.augmentedJacobian(); } /* ************************************************************************* */ pair GaussianFactorGraph::jacobian( - boost::optional optionalOrdering) const { - Matrix augmented = augmentedJacobian(optionalOrdering); + const Ordering& ordering) const { + Matrix augmented = augmentedJacobian(ordering); + return make_pair(augmented.leftCols(augmented.cols() - 1), + augmented.col(augmented.cols() - 1)); + } + + /* ************************************************************************* */ + pair GaussianFactorGraph::jacobian() const { + Matrix augmented = augmentedJacobian(); return make_pair(augmented.leftCols(augmented.cols() - 1), augmented.col(augmented.cols() - 1)); } /* ************************************************************************* */ Matrix GaussianFactorGraph::augmentedHessian( - boost::optional optionalOrdering) const { + const Ordering& ordering) const { // combine all factors and get upper-triangular part of Hessian - Scatter scatter(*this, optionalOrdering); + Scatter scatter(*this, ordering); + HessianFactor combined(*this, scatter); + return combined.info().selfadjointView();; + } + + /* ************************************************************************* */ + Matrix GaussianFactorGraph::augmentedHessian() const { + // combine all factors and get upper-triangular part of Hessian + Scatter scatter(*this); HessianFactor combined(*this, scatter); return combined.info().selfadjointView();; } /* ************************************************************************* */ pair GaussianFactorGraph::hessian( - boost::optional optionalOrdering) const { - Matrix augmented = augmentedHessian(optionalOrdering); + const Ordering& ordering) const { + Matrix augmented = augmentedHessian(ordering); + size_t n = augmented.rows() - 1; + return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1)); + } + + /* ************************************************************************* */ + pair GaussianFactorGraph::hessian() const { + Matrix augmented = augmentedHessian(); size_t n = augmented.rows() - 1; return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1)); } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 49cba482d..8bdf4e475 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -201,7 +201,16 @@ namespace gtsam { * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. */ - Matrix augmentedJacobian(boost::optional optionalOrdering = boost::none) const; + Matrix augmentedJacobian(const Ordering& ordering) const; + + /** + * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ + * Jacobian matrix, augmented with b with the noise models baked + * into A and b. The negative log-likelihood is + * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. + */ + Matrix augmentedJacobian() const; /** * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, @@ -210,7 +219,16 @@ namespace gtsam { * GaussianFactorGraph::augmentedJacobian and * GaussianFactorGraph::sparseJacobian. */ - std::pair jacobian(boost::optional optionalOrdering = boost::none) const; + std::pair jacobian(const Ordering& ordering) const; + + /** + * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, + * with the noise models baked into A and b. The negative log-likelihood + * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::augmentedJacobian and + * GaussianFactorGraph::sparseJacobian. + */ + std::pair jacobian() const; /** * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian @@ -223,7 +241,20 @@ namespace gtsam { and the negative log-likelihood is \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. */ - Matrix augmentedHessian(boost::optional optionalOrdering = boost::none) const; + Matrix augmentedHessian(const Ordering& ordering) const; + + /** + * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian + * matrix, augmented with the information vector \f$ \eta \f$. The + * augmented Hessian is + \f[ \left[ \begin{array}{ccc} + \Lambda & \eta \\ + \eta^T & c + \end{array} \right] \f] + and the negative log-likelihood is + \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. + */ + Matrix augmentedHessian() const; /** * Return the dense Hessian \f$ \Lambda \f$ and information vector @@ -231,7 +262,15 @@ namespace gtsam { * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also * GaussianFactorGraph::augmentedHessian. */ - std::pair hessian(boost::optional optionalOrdering = boost::none) const; + std::pair hessian(const Ordering& ordering) const; + + /** + * Return the dense Hessian \f$ \Lambda \f$ and information vector + * \f$ \eta \f$, with the noise models baked in. The negative log-likelihood + * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also + * GaussianFactorGraph::augmentedHessian. + */ + std::pair hessian() const; /** Return only the diagonal of the Hessian A'*A, as a VectorValues */ virtual VectorValues hessianDiagonal() const; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index c208259b8..cc82e2fa0 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -250,10 +250,10 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, - boost::optional scatter) { + const Scatter& scatter) { gttic(HessianFactor_MergeConstructor); - Allocate(scatter ? *scatter : Scatter(factors)); + Allocate(scatter); // Form A' * A gttic(update); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index cb9da0f1a..f0c79d8fb 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -176,7 +176,11 @@ namespace gtsam { /** Combine a set of factors into a single dense HessianFactor */ explicit HessianFactor(const GaussianFactorGraph& factors, - boost::optional scatter = boost::none); + const Scatter& scatter); + + /** Combine a set of factors into a single dense HessianFactor */ + explicit HessianFactor(const GaussianFactorGraph& factors) + : HessianFactor(factors, Scatter(factors)) {} /** Destructor */ virtual ~HessianFactor() {} diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index c7d4e5405..9478f6fbf 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -84,16 +84,25 @@ string IterativeOptimizationParameters::verbosityTranslator( /*****************************************************************************/ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - boost::optional keyInfo, - boost::optional&> lambda) { - return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), - lambda ? *lambda : std::map()); + const KeyInfo &keyInfo, const std::map &lambda) { + return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } /*****************************************************************************/ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda) { - return optimize(gfg, keyInfo, lambda, keyInfo.x0()); + const KeyInfo& keyInfo) { + return optimize(gfg, keyInfo, std::map()); +} + +/*****************************************************************************/ +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, + const std::map& lambda) { + return optimize(gfg, KeyInfo(gfg), lambda); +} + +/*****************************************************************************/ +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg) { + return optimize(gfg, KeyInfo(gfg), std::map()); } /****************************************************************************/ diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 758d2aec9..84ebe52cb 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -91,15 +91,21 @@ public: virtual ~IterativeSolver() { } - /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, - boost::optional = boost::none, - boost::optional&> lambda = boost::none); - /* interface to the nonlinear optimizer, without initial estimate */ GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda); + /* interface to the nonlinear optimizer, without damping and initial estimate */ + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo& keyInfo); + + /* interface to the nonlinear optimizer, without metadata and initial estimate */ + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, + const std::map& lambda); + + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg); + /* interface to the nonlinear optimizer that the subclasses have to implement */ virtual VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 06f6b3bed..9be010ff5 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -220,60 +220,13 @@ FastVector _convertOrCastToJacobians( } /* ************************************************************************* */ -JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, - boost::optional ordering, - boost::optional p_variableSlots) { - gttic(JacobianFactor_combine_constructor); - - // Compute VariableSlots if one was not provided - // Binds reference, does not copy VariableSlots - const VariableSlots & variableSlots = - p_variableSlots ? p_variableSlots.get() : VariableSlots(graph); +void JacobianFactor::JacobianFactorHelper(const GaussianFactorGraph& graph, + const FastVector& orderedSlots) { // Cast or convert to Jacobians FastVector jacobians = _convertOrCastToJacobians( graph); - gttic(Order_slots); - // Order variable slots - we maintain the vector of ordered slots, as well as keep a list - // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then - // be added after all of the ordered variables. - FastVector orderedSlots; - orderedSlots.reserve(variableSlots.size()); - if (ordering) { - // If an ordering is provided, arrange the slots first that ordering - FastList unorderedSlots; - size_t nOrderingSlotsUsed = 0; - orderedSlots.resize(ordering->size()); - FastMap inverseOrdering = ordering->invert(); - for (VariableSlots::const_iterator item = variableSlots.begin(); - item != variableSlots.end(); ++item) { - FastMap::const_iterator orderingPosition = - inverseOrdering.find(item->first); - if (orderingPosition == inverseOrdering.end()) { - unorderedSlots.push_back(item); - } else { - orderedSlots[orderingPosition->second] = item; - ++nOrderingSlotsUsed; - } - } - if (nOrderingSlotsUsed != ordering->size()) - throw std::invalid_argument( - "The ordering provided to the JacobianFactor combine constructor\n" - "contained extra variables that did not appear in the factors to combine."); - // Add the remaining slots - for(VariableSlots::const_iterator item: unorderedSlots) { - orderedSlots.push_back(item); - } - } else { - // If no ordering is provided, arrange the slots as they were, which will be sorted - // numerically since VariableSlots uses a map sorting on Key. - for (VariableSlots::const_iterator item = variableSlots.begin(); - item != variableSlots.end(); ++item) - orderedSlots.push_back(item); - } - gttoc(Order_slots); - // Count dimensions FastVector varDims; DenseIndex m, n; @@ -344,6 +297,127 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, this->setModel(anyConstrained, *sigmas); } +/* ************************************************************************* */ +// Order variable slots - we maintain the vector of ordered slots, as well as keep a list +// 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then +// be added after all of the ordered variables. +FastVector orderedSlotsHelper( + const Ordering& ordering, + const VariableSlots& variableSlots) { + gttic(Order_slots); + + FastVector orderedSlots; + orderedSlots.reserve(variableSlots.size()); + + // If an ordering is provided, arrange the slots first that ordering + FastList unorderedSlots; + size_t nOrderingSlotsUsed = 0; + orderedSlots.resize(ordering.size()); + FastMap inverseOrdering = ordering.invert(); + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) { + FastMap::const_iterator orderingPosition = + inverseOrdering.find(item->first); + if (orderingPosition == inverseOrdering.end()) { + unorderedSlots.push_back(item); + } else { + orderedSlots[orderingPosition->second] = item; + ++nOrderingSlotsUsed; + } + } + if (nOrderingSlotsUsed != ordering.size()) + throw std::invalid_argument( + "The ordering provided to the JacobianFactor combine constructor\n" + "contained extra variables that did not appear in the factors to combine."); + // Add the remaining slots + for(VariableSlots::const_iterator item: unorderedSlots) { + orderedSlots.push_back(item); + } + + gttoc(Order_slots); + + return orderedSlots; +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph) { + gttic(JacobianFactor_combine_constructor); + + // Compute VariableSlots if one was not provided + // Binds reference, does not copy VariableSlots + const VariableSlots & variableSlots = VariableSlots(graph); + + gttic(Order_slots); + // Order variable slots - we maintain the vector of ordered slots, as well as keep a list + // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then + // be added after all of the ordered variables. + FastVector orderedSlots; + orderedSlots.reserve(variableSlots.size()); + + // If no ordering is provided, arrange the slots as they were, which will be sorted + // numerically since VariableSlots uses a map sorting on Key. + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) + orderedSlots.push_back(item); + gttoc(Order_slots); + + JacobianFactorHelper(graph, orderedSlots); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, + const VariableSlots& p_variableSlots) { + gttic(JacobianFactor_combine_constructor); + + // Binds reference, does not copy VariableSlots + const VariableSlots & variableSlots = p_variableSlots; + + gttic(Order_slots); + // Order variable slots - we maintain the vector of ordered slots, as well as keep a list + // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then + // be added after all of the ordered variables. + FastVector orderedSlots; + orderedSlots.reserve(variableSlots.size()); + + // If no ordering is provided, arrange the slots as they were, which will be sorted + // numerically since VariableSlots uses a map sorting on Key. + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) + orderedSlots.push_back(item); + gttoc(Order_slots); + + JacobianFactorHelper(graph, orderedSlots); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, + const Ordering& ordering) { + gttic(JacobianFactor_combine_constructor); + + // Compute VariableSlots if one was not provided + // Binds reference, does not copy VariableSlots + const VariableSlots & variableSlots = VariableSlots(graph); + + // Order variable slots + FastVector orderedSlots = + orderedSlotsHelper(ordering, variableSlots); + + JacobianFactorHelper(graph, orderedSlots); +} + +/* ************************************************************************* */ +JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, + const Ordering& ordering, + const VariableSlots& p_variableSlots) { + gttic(JacobianFactor_combine_constructor); + + // Order variable slots + FastVector orderedSlots = + orderedSlotsHelper(ordering, p_variableSlots); + + JacobianFactorHelper(graph, orderedSlots); +} + /* ************************************************************************* */ void JacobianFactor::print(const string& s, const KeyFormatter& formatter) const { diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 36d1e12da..c8d77c554 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -22,6 +22,7 @@ #include #include #include +#include #include @@ -147,14 +148,37 @@ namespace gtsam { JacobianFactor( const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); + /** + * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots + * structure computed for \c graph is already available, providing it will reduce the amount of + * computation performed. */ + explicit JacobianFactor( + const GaussianFactorGraph& graph); + /** * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots * structure computed for \c graph is already available, providing it will reduce the amount of * computation performed. */ explicit JacobianFactor( const GaussianFactorGraph& graph, - boost::optional ordering = boost::none, - boost::optional p_variableSlots = boost::none); + const VariableSlots& p_variableSlots); + + /** + * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots + * structure computed for \c graph is already available, providing it will reduce the amount of + * computation performed. */ + explicit JacobianFactor( + const GaussianFactorGraph& graph, + const Ordering& ordering); + + /** + * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots + * structure computed for \c graph is already available, providing it will reduce the amount of + * computation performed. */ + explicit JacobianFactor( + const GaussianFactorGraph& graph, + const Ordering& ordering, + const VariableSlots& p_variableSlots); /** Virtual destructor */ virtual ~JacobianFactor() {} @@ -356,6 +380,14 @@ namespace gtsam { private: + /** + * Helper function for public constructors: + * Build a dense joint factor from all the factors in a factor graph. Takes in + * ordered variable slots */ + void JacobianFactorHelper( + const GaussianFactorGraph& graph, + const FastVector& orderedSlots); + /** Unsafe Constructor that creates an uninitialized Jacobian of right size * @param keys in some order * @param diemnsions of the variables in same order diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h index 1fe7009bc..a24acfacd 100644 --- a/gtsam/linear/RegularHessianFactor.h +++ b/gtsam/linear/RegularHessianFactor.h @@ -77,11 +77,17 @@ public: /// Construct from a GaussianFactorGraph RegularHessianFactor(const GaussianFactorGraph& factors, - boost::optional scatter = boost::none) + const Scatter& scatter) : HessianFactor(factors, scatter) { checkInvariants(); } + /// Construct from a GaussianFactorGraph + RegularHessianFactor(const GaussianFactorGraph& factors) + : HessianFactor(factors) { + checkInvariants(); + } + private: /// Check invariants after construction diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index e1efa2061..b12dcf70a 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -128,17 +128,17 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, const NonlinearOptimizerParams& params) const { // solution of linear solver is an update to the linearization point VectorValues delta; - boost::optional optionalOrdering; + Ordering ordering; if (params.ordering) - optionalOrdering.reset(*params.ordering); + ordering = *params.ordering; // Check which solver we are using if (params.isMultifrontal()) { // Multifrontal QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.optimize(optionalOrdering, params.getEliminationFunction()); + delta = gfg.optimize(ordering, params.getEliminationFunction()); } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.eliminateSequential(optionalOrdering, params.getEliminationFunction(), boost::none, + delta = gfg.eliminateSequential(ordering, params.getEliminationFunction(), boost::none, params.orderingType)->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams From dcb82920eaa237b24de4bb03db06328c9cfd44d3 Mon Sep 17 00:00:00 2001 From: duyanwei Date: Thu, 17 Oct 2019 21:32:21 +0800 Subject: [PATCH 33/55] fix default parameters in constructor of IncrementalFixedLagSmoother.h --- .../nonlinear/IncrementalFixedLagSmoother.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index fe89c5b26..4a1a9cb4a 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -39,8 +39,8 @@ public: /** default constructor */ IncrementalFixedLagSmoother(double smootherLag = 0.0, - const ISAM2Params& parameters = ISAM2Params()) : - FixedLagSmoother(smootherLag), isam_(parameters) { + const boost::optional& parameters = boost::none) : + FixedLagSmoother(smootherLag), isam_(parameters ? (*parameters) : getDefaultParams()) { } /** destructor */ @@ -114,6 +114,14 @@ public: const ISAM2Result& getISAM2Result() const{ return isamResult_; } protected: + + /* Create default parameters */ + ISAM2Params getDefaultParams() const { + ISAM2Params params; + params.findUnusedFactorSlots = true; + return params; + } + /** An iSAM2 object used to perform inference. The smoother lag is controlled * by what factors are removed each iteration */ ISAM2 isam_; From 38982ae4aa31bd9948c17bb79b742848ecd26960 Mon Sep 17 00:00:00 2001 From: duyanwei Date: Fri, 18 Oct 2019 00:23:00 +0800 Subject: [PATCH 34/55] changed function name and made it static --- gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 4a1a9cb4a..3cf6c16d3 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -39,8 +39,8 @@ public: /** default constructor */ IncrementalFixedLagSmoother(double smootherLag = 0.0, - const boost::optional& parameters = boost::none) : - FixedLagSmoother(smootherLag), isam_(parameters ? (*parameters) : getDefaultParams()) { + const ISAM2Params& parameters = DefaultISAM2Params()) : + FixedLagSmoother(smootherLag), isam_(parameters) { } /** destructor */ @@ -115,8 +115,8 @@ public: protected: - /* Create default parameters */ - ISAM2Params getDefaultParams() const { + /** Create default parameters */ + static ISAM2Params DefaultISAM2Params() { ISAM2Params params; params.findUnusedFactorSlots = true; return params; From 730199b67819aa32dc390085c1b2bd86dea9e182 Mon Sep 17 00:00:00 2001 From: Jean-Philippe Tardif Date: Fri, 18 Oct 2019 10:55:02 -0400 Subject: [PATCH 35/55] Fix python wrappers for BetweenFactorVector and PreintegratedCombinedMeasurements and added Vector type to the BetweenFactor generation Created PreintegrationCombinedParams to add constructor for PreintegratedCombinedMeasurements --- gtsam.h | 29 +++++++- gtsam/navigation/CombinedImuFactor.h | 103 +++++++++++++++------------ 2 files changed, 84 insertions(+), 48 deletions(-) diff --git a/gtsam.h b/gtsam.h index e500edbd9..9ab4f1d99 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2405,7 +2405,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { #include -template +template virtual class BetweenFactor : gtsam::NoiseModelFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; @@ -2812,7 +2812,34 @@ virtual class ImuFactor: gtsam::NonlinearFactor { }; #include +//typedef PreintegratedCombinedMeasurements::Params PreintegrationCombinedParams; +virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { + PreintegrationCombinedParams(Vector n_gravity); + + static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); + + void setBiasAccCovariance(Matrix cov); + void setBiasOmegaCovariance(Matrix cov); + void setBiasAccOmegaInt(Matrix cov); + + Matrix getBiasAccCovariance() const ; + Matrix getBiasOmegaCovariance() const ; + Matrix getBiasAccOmegaInt() const; + +}; + class PreintegratedCombinedMeasurements { +// Constructors + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, + const gtsam::imuBias::ConstantBias& bias); // Testable void print(string s) const; bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 2ad71cb3c..d37d5fe84 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -29,12 +29,6 @@ namespace gtsam { -#ifdef GTSAM_TANGENT_PREINTEGRATION -typedef TangentPreintegration PreintegrationType; -#else -typedef ManifoldPreintegration PreintegrationType; -#endif - /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating @@ -54,6 +48,61 @@ typedef ManifoldPreintegration PreintegrationType; * Robotics: Science and Systems (RSS), 2015. */ +struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { + Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk + Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk + Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration + + /// See two named constructors below for good values of n_gravity in body frame +PreintegrationCombinedParams(const Vector3& n_gravity) : + PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( + I_3x3), biasAccOmegaInt(I_6x6) { + } + + // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis + static boost::shared_ptr MakeSharedD(double g = 9.81) { + return boost::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, g))); + } + + // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + static boost::shared_ptr MakeSharedU(double g = 9.81) { + return boost::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, -g))); + } + + void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } + void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } + void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; } + + const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } + const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } + const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; } + +private: + /// Default constructor makes unitialized params struct + PreintegrationCombinedParams() {} + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); + ar& BOOST_SERIALIZATION_NVP(biasAccCovariance); + ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); + ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt); + } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + + +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + + /** * PreintegratedCombinedMeasurements integrates the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. @@ -67,47 +116,7 @@ typedef ManifoldPreintegration PreintegrationType; class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType { public: - - /// Parameters for pre-integration: - /// Usage: Create just a single Params and pass a shared pointer to the constructor - struct Params : PreintegrationParams { - Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk - Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk - Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration - - /// See two named constructors below for good values of n_gravity in body frame - Params(const Vector3& n_gravity) : - PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( - I_3x3), biasAccOmegaInt(I_6x6) { - } - - // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis - static boost::shared_ptr MakeSharedD(double g = 9.81) { - return boost::shared_ptr(new Params(Vector3(0, 0, g))); - } - - // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis - static boost::shared_ptr MakeSharedU(double g = 9.81) { - return boost::shared_ptr(new Params(Vector3(0, 0, -g))); - } - - private: - /// Default constructor makes unitialized params struct - Params() {} - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); - ar& BOOST_SERIALIZATION_NVP(biasAccCovariance); - ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); - ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; + typedef PreintegrationCombinedParams Params; protected: /* Covariance matrix of the preintegrated measurements From f58a5f89f22fba829d5c13649d7912ca7ff20a51 Mon Sep 17 00:00:00 2001 From: Jean-Philippe Tardif Date: Fri, 18 Oct 2019 14:15:19 -0400 Subject: [PATCH 36/55] clean up commented code --- gtsam.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index 9ab4f1d99..708753749 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2812,7 +2812,6 @@ virtual class ImuFactor: gtsam::NonlinearFactor { }; #include -//typedef PreintegratedCombinedMeasurements::Params PreintegrationCombinedParams; virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { PreintegrationCombinedParams(Vector n_gravity); From ca02a03c08f0076fb0f4ea2d706de7efd094de46 Mon Sep 17 00:00:00 2001 From: Jean-Philippe Tardif Date: Fri, 18 Oct 2019 14:16:07 -0400 Subject: [PATCH 37/55] Move back code,, comments in the code --- gtsam/navigation/CombinedImuFactor.h | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index d37d5fe84..ca9b2ca1a 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -29,6 +29,12 @@ namespace gtsam { +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating @@ -48,6 +54,8 @@ namespace gtsam { * Robotics: Science and Systems (RSS), 2015. */ +/// Parameters for pre-integration using PreintegratedCombinedMeasurements: +/// Usage: Create just a single Params and pass a shared pointer to the constructor struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk @@ -95,13 +103,6 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - -#ifdef GTSAM_TANGENT_PREINTEGRATION -typedef TangentPreintegration PreintegrationType; -#else -typedef ManifoldPreintegration PreintegrationType; -#endif - /** * PreintegratedCombinedMeasurements integrates the IMU measurements From 8df79815649b1f7964d59a628aac7e718afd4690 Mon Sep 17 00:00:00 2001 From: mxie32 Date: Sat, 19 Oct 2019 17:15:56 -0400 Subject: [PATCH 38/55] expose GPSFactor in python wrapper, and add example for it --- cython/gtsam/examples/GPSFactorExample.py | 56 +++++++++++++++++++++++ gtsam.h | 25 ++++++++++ 2 files changed, 81 insertions(+) create mode 100644 cython/gtsam/examples/GPSFactorExample.py diff --git a/cython/gtsam/examples/GPSFactorExample.py b/cython/gtsam/examples/GPSFactorExample.py new file mode 100644 index 000000000..493a07725 --- /dev/null +++ b/cython/gtsam/examples/GPSFactorExample.py @@ -0,0 +1,56 @@ +""" +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +Simple robot motion example, with prior and one GPS measurements +Author: Mandy Xie +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import numpy as np + +import gtsam + +import matplotlib.pyplot as plt +import gtsam.utils.plot as gtsam_plot + +# ENU Origin is where the plane was in hold next to runway +lat0 = 33.86998 +lon0 = -84.30626 +h0 = 274 + +# Create noise models +GPS_NOISE = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) +PRIOR_NOISE = gtsam.noiseModel_Isotropic.Sigma(6, 0.25) + +# Create an empty nonlinear factor graph +graph = gtsam.NonlinearFactorGraph() + +# Add a prior on the first point, setting it to the origin +# A prior factor consists of a mean and a noise model (covariance matrix) +priorMean = gtsam.Pose3() # prior at origin +graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) + +# Add GPS factors +gps = gtsam.Point3(lat0, lon0, h0) +graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) +print("\nFactor Graph:\n{}".format(graph)) + +# Create the data structure to hold the initialEstimate estimate to the solution +# For illustrative purposes, these have been deliberately set to incorrect values +initial = gtsam.Values() +initial.insert(1, gtsam.Pose3()) +print("\nInitial Estimate:\n{}".format(initial)) + +# optimize using Levenberg-Marquardt optimization +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) +result = optimizer.optimize() +print("\nFinal Result:\n{}".format(result)) + diff --git a/gtsam.h b/gtsam.h index 708753749..070a3c42f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2936,6 +2936,31 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { gtsam::Unit3 bRef() const; }; +#include +virtual class GPSFactor : gtsam::NonlinearFactor{ + GPSFactor(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s) const; + bool equals(const gtsam::GPSFactor& expected, double tol); + + // Standard Interface + gtsam::Point3 measurementIn() const; +}; + +virtual class GPSFactor2 : gtsam::NonlinearFactor { + GPSFactor2(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s) const; + bool equals(const gtsam::GPSFactor2& expected, double tol); + + // Standard Interface + gtsam::Point3 measurementIn() const; +}; + #include virtual class Scenario { gtsam::Pose3 pose(double t) const; From 939aba362181ce57cd7da448e8bec63f9c7bad7b Mon Sep 17 00:00:00 2001 From: mxie32 Date: Sat, 19 Oct 2019 17:51:15 -0400 Subject: [PATCH 39/55] upgrade colamd to suite-sparse-5.4.0 --- gtsam/3rdparty/CCOLAMD/Source/ccolamd.c | 11 ++-- gtsam/3rdparty/SuiteSparse_config/Makefile | 4 +- gtsam/3rdparty/SuiteSparse_config/README.txt | 3 +- .../SuiteSparse_config/SuiteSparse_config.c | 2 +- .../SuiteSparse_config/SuiteSparse_config.h | 41 ++------------ .../SuiteSparse_config/SuiteSparse_config.mk | 56 +++++++++++++------ 6 files changed, 52 insertions(+), 65 deletions(-) diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c index d5e8da2f0..9a08e3ea8 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c @@ -1560,9 +1560,8 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ Int *dead_cols ; Int set1 ; Int set2 ; -#ifndef NDEBUG Int cs ; -#endif + int ok ; #ifndef NDEBUG @@ -1910,10 +1909,8 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ p [k] = col ; ASSERT (A [col] == EMPTY) ; -#ifndef NDEBUG - cs = CMEMBER (col) ; + cs = CMEMBER (col) ; ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ; -#endif A [col] = k ; k++ ; @@ -1929,11 +1926,11 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ if (A [col] == EMPTY) { k = Col [col].shared2.order ; + cs = CMEMBER (col) ; #ifndef NDEBUG - cs = CMEMBER (col) ; dead_cols [cs]-- ; - ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ; #endif + ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ; DEBUG1 (("ccolamd output ordering: k "ID" col "ID " (dense or null col)\n", k, col)) ; p [k] = col ; diff --git a/gtsam/3rdparty/SuiteSparse_config/Makefile b/gtsam/3rdparty/SuiteSparse_config/Makefile index aa858aeab..695a327a4 100644 --- a/gtsam/3rdparty/SuiteSparse_config/Makefile +++ b/gtsam/3rdparty/SuiteSparse_config/Makefile @@ -7,8 +7,8 @@ export SUITESPARSE # version of SuiteSparse_config is also version of SuiteSparse meta-package LIBRARY = libsuitesparseconfig -VERSION = 4.5.6 -SO_VERSION = 4 +VERSION = 5.4.0 +SO_VERSION = 5 default: library diff --git a/gtsam/3rdparty/SuiteSparse_config/README.txt b/gtsam/3rdparty/SuiteSparse_config/README.txt index 8129f5a04..8555cc459 100644 --- a/gtsam/3rdparty/SuiteSparse_config/README.txt +++ b/gtsam/3rdparty/SuiteSparse_config/README.txt @@ -1,4 +1,4 @@ -SuiteSparse_config, 2017, Timothy A. Davis, http://www.suitesparse.com +SuiteSparse_config, 2018, Timothy A. Davis, http://www.suitesparse.com (formerly the UFconfig package) This directory contains a default SuiteSparse_config.mk file. It tries to @@ -37,6 +37,7 @@ SuiteSparse_config is not required by these packages: CSparse a Concise Sparse matrix package MATLAB_Tools toolboxes for use in MATLAB + GraphBLAS graph algorithms in the language of linear algebra In addition, the xerbla/ directory contains Fortan and C versions of the BLAS/LAPACK xerbla routine, which is called when an invalid input is passed to diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c index b491539fe..595e46781 100644 --- a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c @@ -4,7 +4,7 @@ /* SuiteSparse configuration : memory manager and printf functions. */ -/* Copyright (c) 2013, Timothy A. Davis. No licensing restrictions +/* Copyright (c) 2013-2018, Timothy A. Davis. No licensing restrictions * apply to this file or to the SuiteSparse_config directory. * Author: Timothy A. Davis. */ diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h index 7d4de65d3..9e28c0530 100644 --- a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h @@ -177,38 +177,7 @@ int SuiteSparse_divcomplex /* SuiteSparse is not a package itself, but a collection of packages, some of * which must be used together (UMFPACK requires AMD, CHOLMOD requires AMD, * COLAMD, CAMD, and CCOLAMD, etc). A version number is provided here for the - * collection itself. The versions of packages within each version of - * SuiteSparse are meant to work together. Combining one package from one - * version of SuiteSparse, with another package from another version of - * SuiteSparse, may or may not work. - * - * SuiteSparse contains the following packages: - * - * SuiteSparse_config version 4.5.6 (version always the same as SuiteSparse) - * AMD version 2.4.6 - * BTF version 1.2.6 - * CAMD version 2.4.6 - * CCOLAMD version 2.9.6 - * CHOLMOD version 3.0.11 - * COLAMD version 2.9.6 - * CSparse version 3.1.9 - * CXSparse version 3.1.9 - * GPUQREngine version 1.0.5 - * KLU version 1.3.8 - * LDL version 2.2.6 - * RBio version 2.2.6 - * SPQR version 2.0.8 - * SuiteSparse_GPURuntime version 1.0.5 - * UMFPACK version 5.7.6 - * MATLAB_Tools various packages & M-files - * xerbla version 1.0.3 - * - * Other package dependencies: - * BLAS required by CHOLMOD and UMFPACK - * LAPACK required by CHOLMOD - * METIS 5.1.0 required by CHOLMOD (optional) and KLU (optional) - * CUBLAS, CUDART NVIDIA libraries required by CHOLMOD and SPQR when - * they are compiled with GPU acceleration. + * collection itself, which is also the version number of SuiteSparse_config. */ int SuiteSparse_version /* returns SUITESPARSE_VERSION */ @@ -233,11 +202,11 @@ int SuiteSparse_version /* returns SUITESPARSE_VERSION */ */ #define SUITESPARSE_HAS_VERSION_FUNCTION -#define SUITESPARSE_DATE "Oct 3, 2017" +#define SUITESPARSE_DATE "Dec 28, 2018" #define SUITESPARSE_VER_CODE(main,sub) ((main) * 1000 + (sub)) -#define SUITESPARSE_MAIN_VERSION 4 -#define SUITESPARSE_SUB_VERSION 5 -#define SUITESPARSE_SUBSUB_VERSION 6 +#define SUITESPARSE_MAIN_VERSION 5 +#define SUITESPARSE_SUB_VERSION 4 +#define SUITESPARSE_SUBSUB_VERSION 0 #define SUITESPARSE_VERSION \ SUITESPARSE_VER_CODE(SUITESPARSE_MAIN_VERSION,SUITESPARSE_SUB_VERSION) diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk index 2c13a6010..19a39032a 100644 --- a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk @@ -3,9 +3,11 @@ #=============================================================================== # This file contains all configuration settings for all packages in SuiteSparse, -# except for CSparse (which is stand-alone) and the packages in MATLAB_Tools. +# except for CSparse (which is stand-alone), the packages in MATLAB_Tools, +# and GraphBLAS. The configuration settings for GraphBLAS are determined by +# GraphBLAS/CMakeLists.txt -SUITESPARSE_VERSION = 4.5.6 +SUITESPARSE_VERSION = 5.4.0 #=============================================================================== # Options you can change without editing this file: @@ -57,6 +59,15 @@ SUITESPARSE_VERSION = 4.5.6 INSTALL_INCLUDE ?= $(INSTALL)/include INSTALL_DOC ?= $(INSTALL)/share/doc/suitesparse-$(SUITESPARSE_VERSION) + CMAKE_OPTIONS ?= -DCMAKE_INSTALL_PREFIX=$(INSTALL) + + #--------------------------------------------------------------------------- + # parallel make + #--------------------------------------------------------------------------- + + # sequential make's by default + JOBS ?= 1 + #--------------------------------------------------------------------------- # optimization level #--------------------------------------------------------------------------- @@ -78,19 +89,11 @@ SUITESPARSE_VERSION = 4.5.6 CXX = g++ BLAS = -lrefblas -lgfortran -lstdc++ LAPACK = -llapack - CFLAGS += --coverage - OPTIMIZATION = -g - LDFLAGS += --coverage + CFLAGS += --coverage + OPTIMIZATION = -g + LDFLAGS += --coverage endif - #--------------------------------------------------------------------------- - # CFLAGS for the C/C++ compiler - #--------------------------------------------------------------------------- - - # The CF macro is used by SuiteSparse Makefiles as a combination of - # CFLAGS, CPPFLAGS, TARGET_ARCH, and system-dependent settings. - CF ?= $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) $(OPTIMIZATION) -fexceptions -fPIC - #--------------------------------------------------------------------------- # OpenMP is used in CHOLMOD #--------------------------------------------------------------------------- @@ -112,10 +115,12 @@ SUITESPARSE_VERSION = 4.5.6 ifneq ($(AUTOCC),no) ifneq ($(shell which icc 2>/dev/null),) # use the Intel icc compiler for C codes, and -qopenmp for OpenMP - CC = icc -D_GNU_SOURCE - CXX = $(CC) + CC = icc + CFLAGS += -D_GNU_SOURCE + CXX = icpc CFOPENMP = -qopenmp -I$(MKLROOT)/include - LDFLAGS += -openmp + LDFLAGS += -qopenmp + LDLIBS += -lm -lirc endif ifneq ($(shell which ifort 2>/dev/null),) # use the Intel ifort compiler for Fortran codes @@ -123,6 +128,16 @@ SUITESPARSE_VERSION = 4.5.6 endif endif + CMAKE_OPTIONS += -DCMAKE_CXX_COMPILER=$(CXX) -DCMAKE_C_COMPILER=$(CC) + + #--------------------------------------------------------------------------- + # CFLAGS for the C/C++ compiler + #--------------------------------------------------------------------------- + + # The CF macro is used by SuiteSparse Makefiles as a combination of + # CFLAGS, CPPFLAGS, TARGET_ARCH, and system-dependent settings. + CF ?= $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) $(OPTIMIZATION) -fexceptions -fPIC + #--------------------------------------------------------------------------- # code formatting (for Tcov on Linux only) #--------------------------------------------------------------------------- @@ -157,7 +172,7 @@ SUITESPARSE_VERSION = 4.5.6 # $(MKLROOT)/lib/intel64/libmkl_intel_thread.a \ # -Wl,--end-group -lpthread -lm # using dynamic linking: - BLAS = -lmkl_intel_lp64 -lmkl_core -lmkl_intel_thread -lpthread -lm + BLAS = -lmkl_intel_lp64 -lmkl_core -lmkl_intel_thread -liomp5 -lpthread -lm LAPACK = else # use the OpenBLAS at http://www.openblas.net @@ -223,12 +238,16 @@ SUITESPARSE_VERSION = 4.5.6 CUBLAS_LIB = $(CUDA_PATH)/lib64/libcublas.so CUDA_INC_PATH = $(CUDA_PATH)/include/ CUDA_INC = -I$(CUDA_INC_PATH) + MAGMA_INC = -I/opt/magma-2.4.0/include/ + MAGMA_LIB = -L/opt/magma-2.4.0/lib/ -lmagma NVCC = $(CUDA_PATH)/bin/nvcc NVCCFLAGS = -Xcompiler -fPIC -O3 \ -gencode=arch=compute_30,code=sm_30 \ -gencode=arch=compute_35,code=sm_35 \ -gencode=arch=compute_50,code=sm_50 \ - -gencode=arch=compute_50,code=compute_50 + -gencode=arch=compute_53,code=sm_53 \ + -gencode=arch=compute_53,code=sm_53 \ + -gencode=arch=compute_60,code=compute_60 endif #--------------------------------------------------------------------------- @@ -555,6 +574,7 @@ config: @echo 'Install include files in: INSTALL_INCLUDE=' '$(INSTALL_INCLUDE)' @echo 'Install documentation in: INSTALL_DOC= ' '$(INSTALL_DOC)' @echo 'Optimization level: OPTIMIZATION= ' '$(OPTIMIZATION)' + @echo 'parallel make jobs: JOBS= ' '$(JOBS)' @echo 'BLAS library: BLAS= ' '$(BLAS)' @echo 'LAPACK library: LAPACK= ' '$(LAPACK)' @echo 'Intel TBB library: TBB= ' '$(TBB)' From 1733f3ac98323cd24c042e0f2cc774e2da2c45dc Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 20 Oct 2019 01:15:20 -0400 Subject: [PATCH 40/55] convert all optional Ordering to function overloads compiles and passes tests, but some potentially code-breaking changes in: Marginals.h - order of arguments had to change since `factorization` has a default value EliminatableFactorGraph.h - marginalMultifrontalBayesNet and marginalMultifrontalBayesTree no longer accept `boost::none` as a placeholder to specify later arguments Notes: EliminateableFactorGraph.h - `orderingType` is no longer needed in function overloads that specify ordering, but I left it for the time being to avoid potential code breaking --- gtsam/inference/BayesTree-inst.h | 4 +- gtsam/inference/BayesTreeCliqueBase-inst.h | 2 +- .../inference/EliminateableFactorGraph-inst.h | 315 ++++++++++-------- gtsam/inference/EliminateableFactorGraph.h | 95 ++++-- gtsam/linear/GaussianFactorGraph.cpp | 9 +- gtsam/linear/GaussianFactorGraph.h | 9 +- gtsam/linear/IterativeSolver.cpp | 25 +- gtsam/linear/IterativeSolver.h | 16 +- gtsam/linear/Scatter.cpp | 34 +- gtsam/linear/Scatter.h | 14 +- gtsam/nonlinear/Marginals.cpp | 64 +++- gtsam/nonlinear/Marginals.h | 41 ++- gtsam_unstable/discrete/CSP.cpp | 9 +- gtsam_unstable/discrete/CSP.h | 5 +- tests/testGaussianFactorGraphB.cpp | 4 +- 15 files changed, 406 insertions(+), 240 deletions(-) diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 4df234004..639bcbab0 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -262,7 +262,7 @@ namespace gtsam { // Now, marginalize out everything that is not variable j BayesNetType marginalBN = *cliqueMarginal.marginalMultifrontalBayesNet( - Ordering(cref_list_of<1,Key>(j)), boost::none, function); + Ordering(cref_list_of<1,Key>(j)), function); // The Bayes net should contain only one conditional for variable j, so return it return marginalBN.front(); @@ -383,7 +383,7 @@ namespace gtsam { } // now, marginalize out everything that is not variable j1 or j2 - return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), boost::none, function); + return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), function); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 6bcfb434d..e762786f5 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -171,7 +171,7 @@ namespace gtsam { // The variables we want to keepSet are exactly the ones in S KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); - cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function); + cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function); } } diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index af2a91257..a77d96537 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -28,33 +28,19 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - OptionalOrdering ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const - { - if(ordering && variableIndex) { - gttic(eliminateSequential); - // Do elimination - EliminationTreeType etree(asDerived(), *variableIndex, *ordering); - boost::shared_ptr bayesNet; - boost::shared_ptr factorGraph; - boost::tie(bayesNet,factorGraph) = etree.eliminate(function); - // If any factors are remaining, the ordering was incomplete - if(!factorGraph->empty()) - throw InconsistentEliminationRequested(); - // Return the Bayes net - return bayesNet; - } - else if(!variableIndex) { + const Eliminate& function, OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) 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. + // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex + // before creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(ordering, function, computedVariableIndex, orderingType); + return eliminateSequential(function, computedVariableIndex, orderingType); } - else /*if(!ordering)*/ { - // If no Ordering provided, compute one and call this function again. We are guaranteed to - // have a VariableIndex already here because we computed one if needed in the previous 'else' - // block. + 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); @@ -67,15 +53,73 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr::BayesTreeType> - EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrdering ordering, const Eliminate& function, + boost::shared_ptr::BayesNetType> + EliminateableFactorGraph::eliminateSequential( + const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { - if(ordering && variableIndex) { + if(!variableIndex) { + // If no VariableIndex provided, compute one and call this function again + VariableIndex computedVariableIndex(asDerived()); + return eliminateSequential(ordering, function, computedVariableIndex, orderingType); + } else { + gttic(eliminateSequential); + // Do elimination + EliminationTreeType etree(asDerived(), *variableIndex, ordering); + boost::shared_ptr bayesNet; + boost::shared_ptr factorGraph; + boost::tie(bayesNet,factorGraph) = etree.eliminate(function); + // If any factors are remaining, the ordering was incomplete + if(!factorGraph->empty()) + throw InconsistentEliminationRequested(); + // Return the Bayes net + return bayesNet; + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesTreeType> + EliminateableFactorGraph::eliminateMultifrontal( + const Eliminate& function, OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) 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. + if (orderingType == Ordering::METIS) { + Ordering computedOrdering = Ordering::Metis(asDerived()); + return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + } else { + Ordering computedOrdering = Ordering::Colamd(*variableIndex); + return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + } + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesTreeType> + EliminateableFactorGraph::eliminateMultifrontal( + const Ordering& ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + { + if(!variableIndex) { + // If no VariableIndex provided, compute one and call this function again + VariableIndex computedVariableIndex(asDerived()); + return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType); + } else { gttic(eliminateMultifrontal); // Do elimination with given ordering - EliminationTreeType etree(asDerived(), *variableIndex, *ordering); + EliminationTreeType etree(asDerived(), *variableIndex, ordering); JunctionTreeType junctionTree(etree); boost::shared_ptr bayesTree; boost::shared_ptr factorGraph; @@ -86,25 +130,6 @@ namespace gtsam { // Return the Bayes tree return bayesTree; } - else 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. - VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType); - } - else /*if(!ordering)*/ { - // If no Ordering provided, compute one and call this function again. We are guaranteed to - // have a VariableIndex already here because we computed one if needed in the previous 'else' - // block. - if (orderingType == Ordering::METIS) { - Ordering computedOrdering = Ordering::Metis(asDerived()); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); - } else { - Ordering computedOrdering = Ordering::Colamd(*variableIndex); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); - } - } } /* ************************************************************************* */ @@ -191,57 +216,65 @@ namespace gtsam { boost::shared_ptr::BayesNetType> EliminateableFactorGraph::marginalMultifrontalBayesNet( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering, const Eliminate& function, OptionalVariableIndex variableIndex) const { - if(variableIndex) - { - if(marginalizedVariableOrdering) - { - gttic(marginalMultifrontalBayesNet); - // An ordering was provided for the marginalized variables, so we can first eliminate them - // in the order requested. - boost::shared_ptr bayesTree; - boost::shared_ptr factorGraph; - boost::tie(bayesTree,factorGraph) = - eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); - - if(const Ordering* varsAsOrdering = boost::get(&variables)) - { - // An ordering was also provided for the unmarginalized variables, so we can also - // eliminate them in the order requested. - return factorGraph->eliminateSequential(*varsAsOrdering, function); - } - else - { - // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateSequential(boost::none, function); - } - } - else - { - // No ordering was provided for the marginalized variables, so order them using constrained - // COLAMD. - bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); - const KeyVector* variablesOrOrdering = - unmarginalizedAreOrdered ? - boost::get(&variables) : boost::get(&variables); - - Ordering totalOrdering = - Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); - - // Split up ordering - const size_t nVars = variablesOrOrdering->size(); - Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); - Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); - - // Call this function again with the computed orderings - return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); - } + if(!variableIndex) { + // If no variable index is provided, compute one and call this function again + VariableIndex index(asDerived()); + return marginalMultifrontalBayesNet(variables, function, index); } else { + // No ordering was provided for the marginalized variables, so order them using constrained + // COLAMD. + bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); + const KeyVector* variablesOrOrdering = + unmarginalizedAreOrdered ? + boost::get(&variables) : boost::get(&variables); + + Ordering totalOrdering = + Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + + // Split up ordering + const size_t nVars = variablesOrOrdering->size(); + Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); + Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); + + // Call this function again with the computed orderings + return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesNetType> + EliminateableFactorGraph::marginalMultifrontalBayesNet( + boost::variant variables, + const Ordering& marginalizedVariableOrdering, + const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex index(asDerived()); return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index); + } else { + gttic(marginalMultifrontalBayesNet); + // An ordering was provided for the marginalized variables, so we can first eliminate them + // in the order requested. + boost::shared_ptr bayesTree; + boost::shared_ptr factorGraph; + boost::tie(bayesTree,factorGraph) = + eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); + + if(const Ordering* varsAsOrdering = boost::get(&variables)) + { + // An ordering was also provided for the unmarginalized variables, so we can also + // eliminate them in the order requested. + return factorGraph->eliminateSequential(*varsAsOrdering, function); + } + else + { + // No ordering was provided for the unmarginalized variables, so order them with COLAMD. + return factorGraph->eliminateSequential(function); + } } } @@ -250,57 +283,65 @@ namespace gtsam { boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::marginalMultifrontalBayesTree( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering, const Eliminate& function, OptionalVariableIndex variableIndex) const { - if(variableIndex) - { - if(marginalizedVariableOrdering) - { - gttic(marginalMultifrontalBayesTree); - // An ordering was provided for the marginalized variables, so we can first eliminate them - // in the order requested. - boost::shared_ptr bayesTree; - boost::shared_ptr factorGraph; - boost::tie(bayesTree,factorGraph) = - eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); - - if(const Ordering* varsAsOrdering = boost::get(&variables)) - { - // An ordering was also provided for the unmarginalized variables, so we can also - // eliminate them in the order requested. - return factorGraph->eliminateMultifrontal(*varsAsOrdering, function); - } - else - { - // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateMultifrontal(boost::none, function); - } - } - else - { - // No ordering was provided for the marginalized variables, so order them using constrained - // COLAMD. - bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); - const KeyVector* variablesOrOrdering = - unmarginalizedAreOrdered ? - boost::get(&variables) : boost::get(&variables); - - Ordering totalOrdering = - Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); - - // Split up ordering - const size_t nVars = variablesOrOrdering->size(); - Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); - Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); - - // Call this function again with the computed orderings - return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); - } + if(!variableIndex) { + // If no variable index is provided, compute one and call this function again + VariableIndex computedVariableIndex(asDerived()); + return marginalMultifrontalBayesTree(variables, function, computedVariableIndex); } else { + // No ordering was provided for the marginalized variables, so order them using constrained + // COLAMD. + bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); + const KeyVector* variablesOrOrdering = + unmarginalizedAreOrdered ? + boost::get(&variables) : boost::get(&variables); + + Ordering totalOrdering = + Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + + // Split up ordering + const size_t nVars = variablesOrOrdering->size(); + Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); + Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); + + // Call this function again with the computed orderings + return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesTreeType> + EliminateableFactorGraph::marginalMultifrontalBayesTree( + boost::variant variables, + const Ordering& marginalizedVariableOrdering, + const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex); + } else { + gttic(marginalMultifrontalBayesTree); + // An ordering was provided for the marginalized variables, so we can first eliminate them + // in the order requested. + boost::shared_ptr bayesTree; + boost::shared_ptr factorGraph; + boost::tie(bayesTree,factorGraph) = + eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); + + if(const Ordering* varsAsOrdering = boost::get(&variables)) + { + // An ordering was also provided for the unmarginalized variables, so we can also + // eliminate them in the order requested. + return factorGraph->eliminateMultifrontal(*varsAsOrdering, function); + } + else + { + // No ordering was provided for the unmarginalized variables, so order them with COLAMD. + return factorGraph->eliminateMultifrontal(function); + } } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 249c594b6..8a216918c 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -88,9 +88,6 @@ namespace gtsam { /// The function type that does a single dense elimination step on a subgraph. typedef boost::function Eliminate; - /// Typedef for an optional ordering as an argument to elimination functions - typedef const boost::optional& OptionalOrdering; - /// Typedef for an optional variable index as an argument to elimination functions typedef const boost::optional& OptionalVariableIndex; @@ -108,25 +105,40 @@ namespace gtsam { * Example - METIS ordering for elimination * \code * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); - * - * Example - Full QR elimination in specified order: - * \code - * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, myOrdering); * \endcode * * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: * \code * VariableIndex varIndex(graph); // Build variable index * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index - * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, boost::none, varIndex); + * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, varIndex, boost::none); * \endcode * */ boost::shared_ptr eliminateSequential( - OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none, OptionalOrderingType orderingType = boost::none) const; + /** Do sequential elimination of all variables to produce a Bayes net. + * + * Example - Full QR elimination in specified order: + * \code + * boost::shared_ptr result = graph.eliminateSequential(myOrdering, EliminateQR); + * \endcode + * + * Example - Reusing an existing VariableIndex to improve performance: + * \code + * VariableIndex varIndex(graph); // Build variable index + * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index + * boost::shared_ptr result = graph.eliminateSequential(myOrdering, EliminateQR, varIndex, boost::none); + * \endcode + * */ + boost::shared_ptr eliminateSequential( + const Ordering& ordering, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; // orderingType is not necessary anymore, kept for backwards compatibility + /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on * the parameter orderingType (Ordering::COLAMD or Ordering::METIS) @@ -136,11 +148,6 @@ namespace gtsam { * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateCholesky); * \endcode * - * Example - Full QR elimination in specified order: - * \code - * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); - * \endcode - * * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: * \code * VariableIndex varIndex(graph); // Build variable index @@ -149,11 +156,25 @@ namespace gtsam { * \endcode * */ boost::shared_ptr eliminateMultifrontal( - OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none, OptionalOrderingType orderingType = boost::none) const; + /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not + * provided, the ordering will be computed using either COLAMD or METIS, dependeing on + * the parameter orderingType (Ordering::COLAMD or Ordering::METIS) + * + * Example - Full QR elimination in specified order: + * \code + * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); + * \endcode + * */ + boost::shared_ptr eliminateMultifrontal( + const Ordering& ordering, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; // orderingType no longer needed + /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, * where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$ @@ -194,20 +215,47 @@ namespace gtsam { const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; - /** Compute the marginal of the requested variables and return the result as a Bayes net. + /** Compute the marginal of the requested variables and return the result as a Bayes net. Uses + * COLAMD marginalization ordering by default * @param variables Determines the variables whose marginal to compute, if provided as an * Ordering they will be ordered in the returned BayesNet as specified, and if provided * as a KeyVector they will be ordered using constrained COLAMD. - * @param marginalizedVariableOrdering Optional ordering for the variables being marginalized - * out, i.e. all variables not in \c variables. If this is boost::none, the ordering - * will be computed with COLAMD. * @param function Optional dense elimination function, if not provided the default will be * used. * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesNet( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering = boost::none, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Compute the marginal of the requested variables and return the result as a Bayes net. + * @param variables Determines the variables whose marginal to compute, if provided as an + * Ordering they will be ordered in the returned BayesNet as specified, and if provided + * as a KeyVector they will be ordered using constrained COLAMD. + * @param marginalizedVariableOrdering Ordering for the variables being marginalized out, + * i.e. all variables not in \c variables. + * @param function Optional dense elimination function, if not provided the default will be + * used. + * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not + * provided one will be computed. */ + boost::shared_ptr marginalMultifrontalBayesNet( + boost::variant variables, + const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Compute the marginal of the requested variables and return the result as a Bayes tree. Uses + * COLAMD marginalization order by default + * @param variables Determines the variables whose marginal to compute, if provided as an + * Ordering they will be ordered in the returned BayesNet as specified, and if provided + * as a KeyVector they will be ordered using constrained COLAMD. + * @param function Optional dense elimination function, if not provided the default will be + * used. + * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not + * provided one will be computed. */ + boost::shared_ptr marginalMultifrontalBayesTree( + boost::variant variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; @@ -215,16 +263,15 @@ namespace gtsam { * @param variables Determines the variables whose marginal to compute, if provided as an * Ordering they will be ordered in the returned BayesNet as specified, and if provided * as a KeyVector they will be ordered using constrained COLAMD. - * @param marginalizedVariableOrdering Optional ordering for the variables being marginalized - * out, i.e. all variables not in \c variables. If this is boost::none, the ordering - * will be computed with COLAMD. + * @param marginalizedVariableOrdering Ordering for the variables being marginalized out, + * i.e. all variables not in \c variables. * @param function Optional dense elimination function, if not provided the default will be * used. * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesTree( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering = boost::none, + const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index fe0a9b2df..faa3f8bd6 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -282,8 +282,13 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues GaussianFactorGraph::optimize(OptionalOrdering ordering, const Eliminate& function) const - { + VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const { + gttic(GaussianFactorGraph_optimize); + return BaseEliminateable::eliminateMultifrontal(function)->optimize(); + } + + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::optimize(const Ordering& ordering, const Eliminate& function) const { gttic(GaussianFactorGraph_optimize); return BaseEliminateable::eliminateMultifrontal(ordering, function)->optimize(); } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 8bdf4e475..f24fb602d 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -282,7 +282,14 @@ namespace gtsam { * the dense elimination function specified in \c function (default EliminatePreferCholesky), * followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent * to calling graph.eliminateMultifrontal()->optimize(). */ - VectorValues optimize(OptionalOrdering ordering = boost::none, + VectorValues optimize( + const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + + /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using + * the dense elimination function specified in \c function (default EliminatePreferCholesky), + * followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent + * to calling graph.eliminateMultifrontal()->optimize(). */ + VectorValues optimize(const Ordering&, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; /** diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 9478f6fbf..c7d4e5405 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -82,29 +82,20 @@ string IterativeOptimizationParameters::verbosityTranslator( return "UNKNOWN"; } +/*****************************************************************************/ +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, + boost::optional keyInfo, + boost::optional&> lambda) { + return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), + lambda ? *lambda : std::map()); +} + /*****************************************************************************/ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) { return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } -/*****************************************************************************/ -VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - const KeyInfo& keyInfo) { - return optimize(gfg, keyInfo, std::map()); -} - -/*****************************************************************************/ -VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - const std::map& lambda) { - return optimize(gfg, KeyInfo(gfg), lambda); -} - -/*****************************************************************************/ -VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg) { - return optimize(gfg, KeyInfo(gfg), std::map()); -} - /****************************************************************************/ KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) : ordering_(ordering) { diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 84ebe52cb..758d2aec9 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -91,21 +91,15 @@ public: virtual ~IterativeSolver() { } + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, + boost::optional = boost::none, + boost::optional&> lambda = boost::none); + /* interface to the nonlinear optimizer, without initial estimate */ GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda); - /* interface to the nonlinear optimizer, without damping and initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, - const KeyInfo& keyInfo); - - /* interface to the nonlinear optimizer, without metadata and initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, - const std::map& lambda); - - /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg); - /* interface to the nonlinear optimizer that the subclasses have to implement */ virtual VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 5312da34b..448120cdd 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -34,17 +34,7 @@ string SlotEntry::toString() const { } /* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering) { - gttic(Scatter_Constructor); - - // If we have an ordering, pre-fill the ordered variables first - if (ordering) { - for (Key key : *ordering) { - add(key, 0); - } - } - +void Scatter::ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart) { // Now, find dimensions of variables and/or extend for (const auto& factor : gfg) { if (!factor) @@ -68,10 +58,30 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, // To keep the same behavior as before, sort the keys after the ordering iterator first = begin(); - if (ordering) first += ordering->size(); + first += sortStart; if (first != end()) std::sort(first, end()); } +/* ************************************************************************* */ +Scatter::Scatter(const GaussianFactorGraph& gfg) { + gttic(Scatter_Constructor); + + ScatterHelper(gfg, 0); +} + +/* ************************************************************************* */ +Scatter::Scatter(const GaussianFactorGraph& gfg, + const Ordering& ordering) { + gttic(Scatter_Constructor); + + // pre-fill the ordered variables first + for (Key key : ordering) { + add(key, 0); + } + + ScatterHelper(gfg, ordering.size()); +} + /* ************************************************************************* */ void Scatter::add(Key key, size_t dim) { emplace_back(SlotEntry(key, dim)); diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 793961c59..8936f827c 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -23,8 +23,6 @@ #include #include -#include - namespace gtsam { class GaussianFactorGraph; @@ -53,15 +51,21 @@ class Scatter : public FastVector { /// Default Constructor Scatter() {} - /// Construct from gaussian factor graph, with optional (partial or complete) ordering - Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering = boost::none); + /// Construct from gaussian factor graph, without ordering + explicit Scatter(const GaussianFactorGraph& gfg); + + /// Construct from gaussian factor graph, with (partial or complete) ordering + explicit Scatter(const GaussianFactorGraph& gfg, const Ordering& ordering); /// Add a key/dim pair void add(Key key, size_t dim); private: + /// Helper function for constructors, adds/finds dimensions of variables and + // sorts starting from sortStart + void ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart); + /// Find the SlotEntry with the right key (linear time worst case) iterator find(Key key); }; diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index c24cb6143..c29a79623 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -26,8 +26,16 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) +Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) + : values_(solution), factorization_(factorization) { + gttic(MarginalsConstructor); + graph_ = *graph.linearize(solution); + computeBayesTree(); +} + +/* ************************************************************************* */ +Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, + Factorization factorization) : values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); graph_ = *graph.linearize(solution); @@ -35,28 +43,52 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, } /* ************************************************************************* */ -Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) - : graph_(graph), factorization_(factorization) { +Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization) + : graph_(graph), values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); - Values vals; - for (const auto& keyValue: solution) { - vals.insert(keyValue.first, keyValue.second); - } - values_ = vals; - computeBayesTree(ordering); + computeBayesTree(); } /* ************************************************************************* */ -Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) +Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, + Factorization factorization) : graph_(graph), values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); computeBayesTree(ordering); } /* ************************************************************************* */ -void Marginals::computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering) { +Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization) + : graph_(graph), factorization_(factorization) { + gttic(MarginalsConstructor); + for (const auto& keyValue: solution) { + values_.insert(keyValue.first, keyValue.second); + } + computeBayesTree(); +} + +/* ************************************************************************* */ +Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, + Factorization factorization) + : graph_(graph), factorization_(factorization) { + gttic(MarginalsConstructor); + for (const auto& keyValue: solution) { + values_.insert(keyValue.first, keyValue.second); + } + computeBayesTree(ordering); +} + +/* ************************************************************************* */ +void Marginals::computeBayesTree() { + // Compute BayesTree + if(factorization_ == CHOLESKY) + bayesTree_ = *graph_.eliminateMultifrontal(EliminatePreferCholesky); + else if(factorization_ == QR) + bayesTree_ = *graph_.eliminateMultifrontal(EliminateQR); +} + +/* ************************************************************************* */ +void Marginals::computeBayesTree(const Ordering& ordering) { // Compute BayesTree if(factorization_ == CHOLESKY) bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); @@ -128,9 +160,9 @@ JointMarginal Marginals::jointMarginalInformation(const KeyVector& variables) co jointFG = *bayesTree_.joint(variables[0], variables[1], EliminateQR); } else { if(factorization_ == CHOLESKY) - jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminatePreferCholesky)); + jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, EliminatePreferCholesky)); else if(factorization_ == QR) - jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminateQR)); + jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, EliminateQR)); } // Get information matrix diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 54a290196..abad71ea7 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -55,10 +55,33 @@ public: * @param graph The factor graph defining the full joint density on all variables. * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). - * @param ordering An optional variable ordering for elimination. */ - Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); + + /** Construct a marginals class from a nonlinear factor graph. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + * @param ordering The ordering for elimination. + */ + Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Factorization factorization = CHOLESKY); + + /** Construct a marginals class from a linear factor graph. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The solution point to compute Gaussian marginals. + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + */ + Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); + + /** Construct a marginals class from a linear factor graph. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The solution point to compute Gaussian marginals. + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + * @param ordering The ordering for elimination. + */ + Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. * @param graph The factor graph defining the full joint density on all variables. @@ -66,8 +89,7 @@ public: * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering An optional variable ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. * @param graph The factor graph defining the full joint density on all variables. @@ -75,8 +97,8 @@ public: * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering An optional variable ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Factorization factorization = CHOLESKY); /** print */ void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -103,7 +125,10 @@ public: protected: /** Compute the Bayes Tree as a helper function to the constructor */ - void computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering); + void computeBayesTree(); + + /** Compute the Bayes Tree as a helper function to the constructor */ + void computeBayesTree(const Ordering& ordering); }; diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index 0223250b5..525abd098 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -14,7 +14,14 @@ using namespace std; namespace gtsam { /// Find the best total assignment - can be expensive - CSP::sharedValues CSP::optimalAssignment(OptionalOrdering ordering) const { + 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; diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index bbdadd3dc..9e843f667 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -60,7 +60,10 @@ namespace gtsam { // } /// Find the best total assignment - can be expensive - sharedValues optimalAssignment(OptionalOrdering ordering = boost::none) const; + sharedValues optimalAssignment() const; + + /// Find the best total assignment - can be expensive + sharedValues optimalAssignment(const Ordering& ordering) const; // /* // * Perform loopy belief propagation diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index c4e9d26f5..bf968c8d7 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -206,7 +206,7 @@ TEST(GaussianFactorGraph, optimize_Cholesky) { GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual = fg.optimize(boost::none, EliminateCholesky); + VectorValues actual = fg.optimize(EliminateCholesky); // verify VectorValues expected = createCorrectDelta(); @@ -220,7 +220,7 @@ TEST( GaussianFactorGraph, optimize_QR ) GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual = fg.optimize(boost::none, EliminateQR); + VectorValues actual = fg.optimize(EliminateQR); // verify VectorValues expected = createCorrectDelta(); From 4877bdb4f3d7ec7daeb2f605997418fb93a52efb Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 20 Oct 2019 03:20:14 -0400 Subject: [PATCH 41/55] Caught some stragglers using boost::optional --- gtsam/discrete/DiscreteConditional.cpp | 19 ++++-- gtsam/discrete/DiscreteConditional.h | 6 +- gtsam/linear/Scatter.cpp | 7 +-- gtsam/linear/Scatter.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 53 +++++++++++----- gtsam/nonlinear/NonlinearFactorGraph.h | 22 +++++-- gtsam/nonlinear/NonlinearOptimizerParams.h | 2 +- .../nonlinear/NonlinearClusterTree.h | 60 +++++++++++++++---- tests/testNonlinearFactorGraph.cpp | 2 +- 9 files changed, 131 insertions(+), 42 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index f12cd2567..71d04f246 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -46,16 +46,25 @@ DiscreteConditional::DiscreteConditional(const size_t nrFrontals, /* ******************************************************************************** */ DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal, const boost::optional& orderedKeys) : + const DecisionTreeFactor& marginal) : BaseFactor( ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional( joint.size()-marginal.size()) { if (ISDEBUG("DiscreteConditional::DiscreteConditional")) cout << (firstFrontalKey()) << endl; //TODO Print all keys - if (orderedKeys) { - keys_.clear(); - keys_.insert(keys_.end(), orderedKeys->begin(), orderedKeys->end()); - } +} + +/* ******************************************************************************** */ +DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal, const Ordering& orderedKeys) : + BaseFactor( + ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional( + joint.size()-marginal.size()) { + if (ISDEBUG("DiscreteConditional::DiscreteConditional")) + cout << (firstFrontalKey()) << endl; //TODO Print all keys + + keys_.clear(); + keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end()); } /* ******************************************************************************** */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 88c3e5620..3da8d0a82 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -60,7 +60,11 @@ public: /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal, const boost::optional& orderedKeys = boost::none); + const DecisionTreeFactor& marginal); + + /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ + DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal, const Ordering& orderedKeys); /** * Combine several conditional into a single one. diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 448120cdd..d201dfa78 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -34,8 +34,7 @@ string SlotEntry::toString() const { } /* ************************************************************************* */ -void Scatter::ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart) { - // Now, find dimensions of variables and/or extend +void Scatter::setDimensions(const GaussianFactorGraph& gfg, size_t sortStart) { for (const auto& factor : gfg) { if (!factor) continue; @@ -66,7 +65,7 @@ void Scatter::ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart) { Scatter::Scatter(const GaussianFactorGraph& gfg) { gttic(Scatter_Constructor); - ScatterHelper(gfg, 0); + setDimensions(gfg, 0); } /* ************************************************************************* */ @@ -79,7 +78,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, add(key, 0); } - ScatterHelper(gfg, ordering.size()); + setDimensions(gfg, ordering.size()); } /* ************************************************************************* */ diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 8936f827c..1583aa2f0 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -64,7 +64,7 @@ class Scatter : public FastVector { /// Helper function for constructors, adds/finds dimensions of variables and // sorts starting from sortStart - void ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart); + void setDimensions(const GaussianFactorGraph& gfg, size_t sortStart); /// Find the SlotEntry with the right key (linear time worst case) iterator find(Key key); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index a4bdd83e3..3ad9cd9a6 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -344,23 +344,31 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li } /* ************************************************************************* */ -static Scatter scatterFromValues(const Values& values, boost::optional ordering) { +static Scatter scatterFromValues(const Values& values) { gttic(scatterFromValues); Scatter scatter; scatter.reserve(values.size()); - if (!ordering) { - // use "natural" ordering with keys taken from the initial values - for (const auto& key_value : values) { - scatter.add(key_value.key, key_value.value.dim()); - } - } else { - // copy ordering into keys and lookup dimension in values, is O(n*log n) - for (Key key : *ordering) { - const Value& value = values.at(key); - scatter.add(key, value.dim()); - } + // use "natural" ordering with keys taken from the initial values + for (const auto& key_value : values) { + scatter.add(key_value.key, key_value.value.dim()); + } + + return scatter; +} + +/* ************************************************************************* */ +static Scatter scatterFromValues(const Values& values, const Ordering& ordering) { + gttic(scatterFromValues); + + Scatter scatter; + scatter.reserve(values.size()); + + // copy ordering into keys and lookup dimension in values, is O(n*log n) + for (Key key : ordering) { + const Value& value = values.at(key); + scatter.add(key, value.dim()); } return scatter; @@ -368,7 +376,15 @@ static Scatter scatterFromValues(const Values& values, boost::optional ordering, const Dampen& dampen) const { + const Values& values, const Dampen& dampen) const { + KeyVector keys = values.keys(); + Ordering defaultOrdering(keys); + return linearizeToHessianFactor(values, defaultOrdering, dampen); +} + +/* ************************************************************************* */ +HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( + const Values& values, const Ordering& ordering, const Dampen& dampen) const { gttic(NonlinearFactorGraph_linearizeToHessianFactor); Scatter scatter = scatterFromValues(values, ordering); @@ -395,7 +411,16 @@ HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( /* ************************************************************************* */ Values NonlinearFactorGraph::updateCholesky(const Values& values, - boost::optional ordering, + const Dampen& dampen) const { + gttic(NonlinearFactorGraph_updateCholesky); + auto hessianFactor = linearizeToHessianFactor(values, dampen); + VectorValues delta = hessianFactor->solve(); + return values.retract(delta); +} + +/* ************************************************************************* */ +Values NonlinearFactorGraph::updateCholesky(const Values& values, + const Ordering& ordering, const Dampen& dampen) const { gttic(NonlinearFactorGraph_updateCholesky); auto hessianFactor = linearizeToHessianFactor(values, ordering, dampen); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 323461459..0b0db1b7b 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -149,17 +149,31 @@ namespace gtsam { * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing * a new graph, and hence useful in case a dense solve is appropriate for your problem. - * An optional ordering can be given that still decides how the Hessian is laid out. * An optional lambda function can be used to apply damping on the filled Hessian. * No parallelism is exploited, because all the factors write in the same memory. */ boost::shared_ptr linearizeToHessianFactor( - const Values& values, boost::optional ordering = boost::none, - const Dampen& dampen = nullptr) const; + const Values& values, const Dampen& dampen = nullptr) const; + + /** + * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly + * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing + * a new graph, and hence useful in case a dense solve is appropriate for your problem. + * An ordering is given that still decides how the Hessian is laid out. + * An optional lambda function can be used to apply damping on the filled Hessian. + * No parallelism is exploited, because all the factors write in the same memory. + */ + boost::shared_ptr linearizeToHessianFactor( + const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const; /// Linearize and solve in one pass. /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values. - Values updateCholesky(const Values& values, boost::optional ordering = boost::none, + Values updateCholesky(const Values& values, + const Dampen& dampen = nullptr) const; + + /// Linearize and solve in one pass. + /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values. + Values updateCholesky(const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const; /// Clone() performs a deep-copy of the graph, including all of the factors diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 180f4fb84..9757cca73 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -82,7 +82,7 @@ public: }; LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer - boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) + boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. inline bool isMultifrontal() const { diff --git a/gtsam_unstable/nonlinear/NonlinearClusterTree.h b/gtsam_unstable/nonlinear/NonlinearClusterTree.h index a483c9521..5d089f123 100644 --- a/gtsam_unstable/nonlinear/NonlinearClusterTree.h +++ b/gtsam_unstable/nonlinear/NonlinearClusterTree.h @@ -46,21 +46,26 @@ class NonlinearClusterTree : public ClusterTree { // linearize local custer factors straight into hessianFactor, which is returned // If no ordering given, uses colamd HessianFactor::shared_ptr linearizeToHessianFactor( - const Values& values, boost::optional ordering = boost::none, + const Values& values, const NonlinearFactorGraph::Dampen& dampen = nullptr) const { - if (!ordering) - ordering.reset(Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true)); - return factors.linearizeToHessianFactor(values, *ordering, dampen); + Ordering ordering; + ordering = Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true); + return factors.linearizeToHessianFactor(values, ordering, dampen); } - // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent + // linearize local custer factors straight into hessianFactor, which is returned + // If no ordering given, uses colamd + HessianFactor::shared_ptr linearizeToHessianFactor( + const Values& values, const Ordering& ordering, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + return factors.linearizeToHessianFactor(values, ordering, dampen); + } + + // Helper function: recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent // TODO(frank): Use TBB to support deep trees and parallelism std::pair linearizeAndEliminate( - const Values& values, boost::optional ordering = boost::none, - const NonlinearFactorGraph::Dampen& dampen = nullptr) const { - // Linearize and create HessianFactor f(front,separator) - HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen); - + const Values& values, + const HessianFactor::shared_ptr& localFactor) const { // Get contributions f(front) from children, as well as p(children|front) GaussianBayesNet bayesNet; for (const auto& child : children) { @@ -72,12 +77,45 @@ class NonlinearClusterTree : public ClusterTree { return {bayesNet, localFactor}; } + // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent + // TODO(frank): Use TBB to support deep trees and parallelism + std::pair linearizeAndEliminate( + const Values& values, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + // Linearize and create HessianFactor f(front,separator) + HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, dampen); + return linearizeAndEliminate(values, localFactor); + } + + // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent + // TODO(frank): Use TBB to support deep trees and parallelism + std::pair linearizeAndEliminate( + const Values& values, const Ordering& ordering, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + // Linearize and create HessianFactor f(front,separator) + HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen); + return linearizeAndEliminate(values, localFactor); + } + // Recursively eliminate subtree rooted at this Cluster // Version that updates existing Bayes net and returns a new Hessian factor on parent clique // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor HessianFactor::shared_ptr linearizeAndEliminate( const Values& values, GaussianBayesNet* bayesNet, - boost::optional ordering = boost::none, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + auto bayesNet_newFactor_pair = linearizeAndEliminate(values, dampen); + if (bayesNet) { + bayesNet->push_back(bayesNet_newFactor_pair.first); + } + return bayesNet_newFactor_pair.second; + } + + // Recursively eliminate subtree rooted at this Cluster + // Version that updates existing Bayes net and returns a new Hessian factor on parent clique + // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor + HessianFactor::shared_ptr linearizeAndEliminate( + const Values& values, GaussianBayesNet* bayesNet, + const Ordering& ordering, const NonlinearFactorGraph::Dampen& dampen = nullptr) const { auto bayesNet_newFactor_pair = linearizeAndEliminate(values, ordering, dampen); if (bayesNet) { diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 290f0138e..6a7a963bc 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -198,7 +198,7 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { } } }; - EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); + EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6)); } /* ************************************************************************* */ From 8e62a1405e5cc7b4a059b691ffda6e96d2b328f7 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 28 Oct 2019 17:41:16 -0400 Subject: [PATCH 42/55] deprecated functions for backwards compatibility also removed some edits that were tangential to the PR. --- gtsam/base/TestableAssertions.h | 8 +- gtsam/discrete/DiscreteConditional.cpp | 7 +- .../inference/EliminateableFactorGraph-inst.h | 16 ++-- gtsam/inference/EliminateableFactorGraph.h | 73 ++++++++++++++++--- gtsam/linear/GaussianFactorGraph.cpp | 7 ++ gtsam/linear/GaussianFactorGraph.h | 6 ++ gtsam/linear/Scatter.cpp | 35 ++++----- gtsam/linear/Scatter.h | 5 -- gtsam/nonlinear/Marginals.h | 21 +++++- gtsam/nonlinear/NonlinearFactorGraph.cpp | 32 ++++---- gtsam/nonlinear/NonlinearFactorGraph.h | 20 +++++ gtsam/nonlinear/NonlinearOptimizer.cpp | 16 ++-- tests/testGaussianFactorGraphB.cpp | 2 + tests/testNonlinearFactorGraph.cpp | 2 +- 14 files changed, 172 insertions(+), 78 deletions(-) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index a698f8f98..1a31aa284 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -71,8 +71,12 @@ bool assert_equal(const V& expected, const boost::optional& actual, double to } template -bool assert_equal(const V& expected, double tol = 1e-9) { - return false; +bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { + if (!actual) { + std::cout << "actual is boost::none" << std::endl; + return false; + } + return assert_equal(expected, *actual, tol); } /** diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 71d04f246..cf2997ce0 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -57,12 +57,7 @@ DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, /* ******************************************************************************** */ DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, const DecisionTreeFactor& marginal, const Ordering& orderedKeys) : - BaseFactor( - ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional( - joint.size()-marginal.size()) { - if (ISDEBUG("DiscreteConditional::DiscreteConditional")) - cout << (firstFrontalKey()) << endl; //TODO Print all keys - + DiscreteConditional(joint, marginal) { keys_.clear(); keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end()); } diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index a77d96537..81f4047a1 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -28,8 +28,8 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - const Eliminate& function, OptionalVariableIndex variableIndex, - OptionalOrderingType orderingType) const { + 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 @@ -56,12 +56,12 @@ namespace gtsam { boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( const Ordering& ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalVariableIndex variableIndex) const { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(ordering, function, computedVariableIndex, orderingType); + return eliminateSequential(ordering, function, computedVariableIndex); } else { gttic(eliminateSequential); // Do elimination @@ -81,8 +81,8 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - const Eliminate& function, OptionalVariableIndex variableIndex, - OptionalOrderingType orderingType) const + OptionalOrderingType orderingType, const Eliminate& function, + OptionalVariableIndex variableIndex) const { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check @@ -110,12 +110,12 @@ namespace gtsam { boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( const Ordering& ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalVariableIndex variableIndex) const { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType); + return eliminateMultifrontal(ordering, function, computedVariableIndex); } else { gttic(eliminateMultifrontal); // Do elimination with given ordering diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 8a216918c..316ca8ee4 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -89,7 +89,7 @@ namespace gtsam { typedef boost::function Eliminate; /// Typedef for an optional variable index as an argument to elimination functions - typedef const boost::optional& OptionalVariableIndex; + typedef boost::optional OptionalVariableIndex; /// Typedef for an optional ordering type typedef boost::optional OptionalOrderingType; @@ -115,9 +115,9 @@ namespace gtsam { * \endcode * */ boost::shared_ptr eliminateSequential( + OptionalOrderingType orderingType = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalVariableIndex variableIndex = boost::none) const; /** Do sequential elimination of all variables to produce a Bayes net. * @@ -136,8 +136,7 @@ namespace gtsam { boost::shared_ptr eliminateSequential( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; // orderingType is not necessary anymore, kept for backwards compatibility + OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on @@ -156,9 +155,9 @@ namespace gtsam { * \endcode * */ boost::shared_ptr eliminateMultifrontal( + OptionalOrderingType orderingType = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on @@ -172,8 +171,7 @@ namespace gtsam { boost::shared_ptr eliminateMultifrontal( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; // orderingType no longer needed + OptionalVariableIndex variableIndex = boost::none) const; /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, @@ -241,7 +239,7 @@ namespace gtsam { * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesNet( boost::variant variables, - const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking + const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; @@ -271,7 +269,7 @@ namespace gtsam { * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesTree( boost::variant variables, - const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking + const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; @@ -288,6 +286,59 @@ namespace gtsam { // Access the derived factor graph class FactorGraphType& asDerived() { return static_cast(*this); } + + public: + /** \deprecated ordering and orderingType shouldn't both be specified */ + boost::shared_ptr eliminateSequential( + const Ordering& ordering, + const Eliminate& function, + OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) const { + return eliminateSequential(ordering, function, variableIndex); + } + + /** \deprecated orderingType specified first for consistency */ + boost::shared_ptr eliminateSequential( + const Eliminate& function, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const { + return eliminateSequential(orderingType, function, variableIndex); + } + + /** \deprecated ordering and orderingType shouldn't both be specified */ + boost::shared_ptr eliminateMultifrontal( + const Ordering& ordering, + const Eliminate& function, + OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) const { + return eliminateMultifrontal(ordering, function, variableIndex); + } + + /** \deprecated orderingType specified first for consistency */ + boost::shared_ptr eliminateMultifrontal( + const Eliminate& function, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const { + return eliminateMultifrontal(orderingType, function, variableIndex); + } + + /** \deprecated */ + boost::shared_ptr marginalMultifrontalBayesNet( + boost::variant variables, + boost::none_t, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const { + return marginalMultifrontalBayesNet(variables, function, variableIndex); + } + + /** \deprecated */ + boost::shared_ptr marginalMultifrontalBayesTree( + boost::variant variables, + boost::none_t, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const { + return marginalMultifrontalBayesTree(variables, function, variableIndex); + } }; } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index faa3f8bd6..8dc3600c6 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -494,4 +494,11 @@ namespace gtsam { return e; } + /* ************************************************************************* */ + /** \deprecated */ + VectorValues GaussianFactorGraph::optimize(boost::none_t, + const Eliminate& function) const { + return optimize(function); + } + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index f24fb602d..2b9e8e675 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -375,6 +375,12 @@ namespace gtsam { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } + public: + + /** \deprecated */ + VectorValues optimize(boost::none_t, + const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + }; /** diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index d201dfa78..07ecaf483 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -33,8 +33,19 @@ string SlotEntry::toString() const { return oss.str(); } +Scatter::Scatter(const GaussianFactorGraph& gfg) : Scatter(gfg, Ordering()) {} + /* ************************************************************************* */ -void Scatter::setDimensions(const GaussianFactorGraph& gfg, size_t sortStart) { +Scatter::Scatter(const GaussianFactorGraph& gfg, + const Ordering& ordering) { + gttic(Scatter_Constructor); + + // If we have an ordering, pre-fill the ordered variables first + for (Key key : ordering) { + add(key, 0); + } + + // Now, find dimensions of variables and/or extend for (const auto& factor : gfg) { if (!factor) continue; @@ -57,30 +68,10 @@ void Scatter::setDimensions(const GaussianFactorGraph& gfg, size_t sortStart) { // To keep the same behavior as before, sort the keys after the ordering iterator first = begin(); - first += sortStart; + first += ordering.size(); if (first != end()) std::sort(first, end()); } -/* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg) { - gttic(Scatter_Constructor); - - setDimensions(gfg, 0); -} - -/* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg, - const Ordering& ordering) { - gttic(Scatter_Constructor); - - // pre-fill the ordered variables first - for (Key key : ordering) { - add(key, 0); - } - - setDimensions(gfg, ordering.size()); -} - /* ************************************************************************* */ void Scatter::add(Key key, size_t dim) { emplace_back(SlotEntry(key, dim)); diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 1583aa2f0..3b34d170c 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -61,11 +61,6 @@ class Scatter : public FastVector { void add(Key key, size_t dim); private: - - /// Helper function for constructors, adds/finds dimensions of variables and - // sorts starting from sortStart - void setDimensions(const GaussianFactorGraph& gfg, size_t sortStart); - /// Find the SlotEntry with the right key (linear time worst case) iterator find(Key key); }; diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index abad71ea7..4e201cc38 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -64,7 +64,7 @@ public: * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering The ordering for elimination. */ - Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. @@ -80,7 +80,7 @@ public: * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering The ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. @@ -97,7 +97,7 @@ public: * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering An optional variable ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, Factorization factorization = CHOLESKY); /** print */ @@ -121,7 +121,7 @@ public: /** Optimize the bayes tree */ VectorValues optimize() const; - + protected: /** Compute the Bayes Tree as a helper function to the constructor */ @@ -130,6 +130,19 @@ protected: /** Compute the Bayes Tree as a helper function to the constructor */ void computeBayesTree(const Ordering& ordering); +public: + /** \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) {} + + /** \deprecated argument order changed due to removing boost::optional */ + 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, + const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} + }; /** diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 3ad9cd9a6..d4b9fbb68 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -376,19 +376,7 @@ static Scatter scatterFromValues(const Values& values, const Ordering& ordering) /* ************************************************************************* */ HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( - const Values& values, const Dampen& dampen) const { - KeyVector keys = values.keys(); - Ordering defaultOrdering(keys); - return linearizeToHessianFactor(values, defaultOrdering, dampen); -} - -/* ************************************************************************* */ -HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( - const Values& values, const Ordering& ordering, const Dampen& dampen) const { - gttic(NonlinearFactorGraph_linearizeToHessianFactor); - - Scatter scatter = scatterFromValues(values, ordering); - + const Values& values, const Scatter& scatter, const Dampen& dampen) const { // NOTE(frank): we are heavily leaning on friendship below HessianFactor::shared_ptr hessianFactor(new HessianFactor(scatter)); @@ -409,6 +397,24 @@ HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( return hessianFactor; } +/* ************************************************************************* */ +HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( + const Values& values, const Ordering& order, const Dampen& dampen) const { + gttic(NonlinearFactorGraph_linearizeToHessianFactor); + + Scatter scatter = scatterFromValues(values, order); + return linearizeToHessianFactor(values, scatter, dampen); +} + +/* ************************************************************************* */ +HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( + const Values& values, const Dampen& dampen) const { + gttic(NonlinearFactorGraph_linearizeToHessianFactor); + + Scatter scatter = scatterFromValues(values); + return linearizeToHessianFactor(values, scatter, dampen); +} + /* ************************************************************************* */ Values NonlinearFactorGraph::updateCholesky(const Values& values, const Dampen& dampen) const { diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 0b0db1b7b..902a47a0f 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -204,6 +204,13 @@ namespace gtsam { private: + /** + * Linearize from Scatter rather than from Ordering. Made private because + * it doesn't include gttic. + */ + boost::shared_ptr linearizeToHessianFactor( + const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const; + /** Serialization function */ friend class boost::serialization::access; template @@ -211,6 +218,19 @@ namespace gtsam { ar & boost::serialization::make_nvp("NonlinearFactorGraph", boost::serialization::base_object(*this)); } + + public: + + /** \deprecated */ + boost::shared_ptr 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, + const Dampen& dampen = nullptr) const + {return updateCholesky(values, dampen);} + }; /// traits diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index b12dcf70a..ad3b9c4cc 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -128,18 +128,22 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, const NonlinearOptimizerParams& params) const { // solution of linear solver is an update to the linearization point VectorValues delta; - Ordering ordering; - if (params.ordering) - ordering = *params.ordering; // Check which solver we are using if (params.isMultifrontal()) { // Multifrontal QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.optimize(ordering, params.getEliminationFunction()); + if (params.ordering) + delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); + else + delta = gfg.optimize(params.getEliminationFunction()); } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.eliminateSequential(ordering, params.getEliminationFunction(), boost::none, - params.orderingType)->optimize(); + if (params.ordering) + delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), + boost::none, params.orderingType)->optimize(); + else + delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none, + params.orderingType)->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams if (!params.iterativeParams) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index bf968c8d7..9596f4af5 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -206,6 +206,7 @@ TEST(GaussianFactorGraph, optimize_Cholesky) { GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph + VectorValues actual1 = fg.optimize(boost::none, EliminateCholesky); VectorValues actual = fg.optimize(EliminateCholesky); // verify @@ -220,6 +221,7 @@ TEST( GaussianFactorGraph, optimize_QR ) GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph + VectorValues actual1 = fg.optimize(boost::none, EliminateQR); VectorValues actual = fg.optimize(EliminateQR); // verify diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 6a7a963bc..290f0138e 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -198,7 +198,7 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { } } }; - EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6)); + EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); } /* ************************************************************************* */ From e40a5008590e427c2d56b5a5c91a432b0cf6342a Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 28 Oct 2019 17:43:56 -0400 Subject: [PATCH 43/55] Update test cases for new preferred syntax --- tests/testGaussianFactorGraphB.cpp | 2 -- tests/testNonlinearFactorGraph.cpp | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 9596f4af5..bf968c8d7 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -206,7 +206,6 @@ TEST(GaussianFactorGraph, optimize_Cholesky) { GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual1 = fg.optimize(boost::none, EliminateCholesky); VectorValues actual = fg.optimize(EliminateCholesky); // verify @@ -221,7 +220,6 @@ TEST( GaussianFactorGraph, optimize_QR ) GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual1 = fg.optimize(boost::none, EliminateQR); VectorValues actual = fg.optimize(EliminateQR); // verify diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 290f0138e..6a7a963bc 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -198,7 +198,7 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { } } }; - EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); + EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6)); } /* ************************************************************************* */ From 8572cd17ad637b8e7c00f15346202fa920c8cba1 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Sat, 9 Nov 2019 16:58:44 -0800 Subject: [PATCH 44/55] Fix bug when constructing VariableSlots from a GaussianFactorGraph that has null factors. --- gtsam/inference/VariableSlots.h | 6 ++++-- gtsam_unstable/examples/FixedLagSmootherExample.cpp | 13 +++++++++++++ 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index f9297d300..a665890c2 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -99,7 +99,9 @@ VariableSlots::VariableSlots(const FG& factorGraph) // factor does not involve that variable. size_t jointFactorPos = 0; for(const typename FG::sharedFactor& factor: factorGraph) { - assert(factor); + if (!factor) { + continue; + } size_t factorVarSlot = 0; for(const Key involvedVariable: *factor) { // Set the slot in this factor for this variable. If the @@ -111,7 +113,7 @@ VariableSlots::VariableSlots(const FG& factorGraph) iterator thisVarSlots; bool inserted; boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector())); if(inserted) - thisVarSlots->second.resize(factorGraph.size(), Empty); + thisVarSlots->second.resize(factorGraph.nrFactors(), Empty); thisVarSlots->second[jointFactorPos] = factorVarSlot; if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl; ++ factorVarSlot; diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 71153ee31..a6d5b699d 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -145,5 +145,18 @@ int main(int argc, char** argv) { cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl; } + // get the linearization point + Values result = smootherISAM2.calculateEstimate(); + + // create the factor graph + auto &factor_graph = smootherISAM2.getFactors(); + + // linearize to a Gaussian factor graph + boost::shared_ptr linear_graph = factor_graph.linearize(result); + + // this is where the segmentation fault occurs + Matrix A = linear_graph->jacobian().first; + cout << " A = " << A << endl; + return 0; } From fab3b01756a53553a3efaca7e8447ff92e07f018 Mon Sep 17 00:00:00 2001 From: Michael Bosse Date: Sat, 9 Nov 2019 19:49:59 -0800 Subject: [PATCH 45/55] updates from code review --- .../examples/FixedLagSmootherExample.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index a6d5b699d..dc9b00580 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -145,18 +145,19 @@ int main(int argc, char** argv) { cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl; } - // get the linearization point + // Here is an example of how to get the full Jacobian of the problem. + // First, get the linearization point. Values result = smootherISAM2.calculateEstimate(); - // create the factor graph - auto &factor_graph = smootherISAM2.getFactors(); + // Get the factor graph + auto &factorGraph = smootherISAM2.getFactors(); - // linearize to a Gaussian factor graph - boost::shared_ptr linear_graph = factor_graph.linearize(result); + // Linearize to a Gaussian factor graph + boost::shared_ptr linearGraph = factorGraph.linearize(result); - // this is where the segmentation fault occurs - Matrix A = linear_graph->jacobian().first; - cout << " A = " << A << endl; + // Converts the linear graph into a Jacobian factor and extracts the Jacobian matrix + Matrix jacobian = linearGraph->jacobian().first; + cout << " Jacobian: " << jacobian << endl; return 0; } From 3135cd6623c924fa3ec51920acde78e1b747ddc1 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 11 Nov 2019 08:34:52 +0100 Subject: [PATCH 46/55] Fix broken dependencies in Debian packages --- debian/control | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/debian/control b/debian/control index 9a37e4377..9b3ae5308 100644 --- a/debian/control +++ b/debian/control @@ -10,6 +10,6 @@ Vcs-Browser: https://github.com/borglab/gtsam Package: libgtsam-dev Architecture: any -Depends: ${shlibs:Depends}, ${misc:Depends} +Depends: ${shlibs:Depends}, ${misc:Depends}, libboost-serialization-dev, libboost-system-dev, libboost-filesystem-dev, libboost-thread-dev, libboost-program-options-dev, libboost-date-time-dev, libboost-timer-dev, libboost-chrono-dev, libboost-regex-dev Description: Georgia Tech Smoothing and Mapping Library gtsam: Georgia Tech Smoothing and Mapping library for SLAM type applications From 99856befce90757eb6cb59d62f309522aaf23f4f Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sat, 12 Oct 2019 18:21:54 +0200 Subject: [PATCH 47/55] Use gcc -fPIC flag when building gtsam libraries This flag is required to ensure proper code generation for shared libraries. --- cmake/GtsamBuildTypes.cmake | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index aa35fea05..06b25ab06 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -94,7 +94,8 @@ if(MSVC) set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING /MD /O2 CACHE STRING "(User editable) Private compiler flags for Timing configuration.") else() # Common to all configurations, next for each configuration: - set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON -Wall CACHE STRING "(User editable) Private compiler flags for all configurations.") + # "-fPIC" is to ensure proper code generation for shared libraries + set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON -Wall -fPIC CACHE STRING "(User editable) Private compiler flags for all configurations.") set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG -g -fno-inline CACHE STRING "(User editable) Private compiler flags for Debug configuration.") set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO -g -O3 CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.") set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE -O3 CACHE STRING "(User editable) Private compiler flags for Release configuration.") From 96f90c3f13dbecae8469be714f6156ac55c73f01 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 12 Nov 2019 19:16:34 -0500 Subject: [PATCH 48/55] Fix doxygen "Creating a new factor" I think NonlinearFactor1, NonlinearFactor2, ... is supposed to say NoiseModelFactor1, NoiseModelFactor2, ... --- gtsam/mainpage.dox | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/mainpage.dox b/gtsam/mainpage.dox index db42b1277..59e7f9fa6 100644 --- a/gtsam/mainpage.dox +++ b/gtsam/mainpage.dox @@ -17,11 +17,11 @@ To use GTSAM to solve your own problems, you will often have to create new facto -# The number of variables your factor involves is unknown at compile time - derive from NoiseModelFactor and implement NoiseModelFactor::unwhitenedError() - This is a factor expressing the sum-of-squares error between a measurement \f$ z \f$ and a measurement prediction function \f$ h(x) \f$, on which the errors are expected to follow some distribution specified by a noise model (see noiseModel). --# The number of variables your factor involves is known at compile time and is between 1 and 6 - derive from NonlinearFactor1, NonlinearFactor2, NonlinearFactor3, NonlinearFactor4, NonlinearFactor5, or NonlinearFactor6, and implement \c evaluateError() +-# The number of variables your factor involves is known at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement \c evaluateError() - This factor expresses the same sum-of-squares error with a noise model, but makes the implementation task slightly easier than with %NoiseModelFactor. -# Derive from NonlinearFactor - This is more advanced and allows creating factors without an explicit noise model, or that linearize to HessianFactor instead of JacobianFactor. */ -} \ No newline at end of file +} From 041c4518bf6258caad9013d4db7bcd5b71c8135c Mon Sep 17 00:00:00 2001 From: Erik Nelson Date: Fri, 22 Nov 2019 13:04:48 -0800 Subject: [PATCH 49/55] Fix an infinite loop in GTSAM's nonlinear optimizer --- gtsam/nonlinear/NonlinearOptimizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index ad3b9c4cc..0c5d99c0f 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -101,7 +101,7 @@ void NonlinearOptimizer::defaultOptimize() { cout << "newError: " << error() << endl; } while (iterations() < params.maxIterations && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, - currentError, error(), params.verbosity)); + currentError, error(), params.verbosity) && std::isfinite(currentError)); // Printing if verbose if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) { From 30a139c1180fd8d61b706b95230b676cb26853a4 Mon Sep 17 00:00:00 2001 From: Erik Nelson Date: Fri, 22 Nov 2019 13:18:53 -0800 Subject: [PATCH 50/55] Add missing boost::shared_ptr include to Values.h --- gtsam/nonlinear/Values.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 4b0fceaf9..e48b11b32 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -39,6 +39,7 @@ #pragma GCC diagnostic pop #endif #include +#include #include #include From 660234e4ad7790d6e32c177c06b968c247d18ac6 Mon Sep 17 00:00:00 2001 From: Erik Nelson Date: Fri, 22 Nov 2019 13:39:09 -0800 Subject: [PATCH 51/55] Make attitude factor copy and move assignable --- gtsam/navigation/AttitudeFactor.h | 25 ++++++++++--------------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 19c1f8139..db588008e 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -35,7 +35,7 @@ class AttitudeFactor { protected: - const Unit3 nZ_, bRef_; ///< Position measurement in + Unit3 nZ_, bRef_; ///< Position measurement in public: @@ -56,12 +56,19 @@ public: Vector attitudeError(const Rot3& p, OptionalJacobian<2,3> H = boost::none) const; + const Unit3& nZ() const { + return nZ_; + } + const Unit3& bRef() const { + return bRef_; + } + /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("nZ_", const_cast(nZ_)); - ar & boost::serialization::make_nvp("bRef_", const_cast(bRef_)); + ar & boost::serialization::make_nvp("nZ_", nZ_); + ar & boost::serialization::make_nvp("bRef_", bRef_); } }; @@ -118,12 +125,6 @@ public: boost::optional H = boost::none) const { return attitudeError(nRb, H); } - Unit3 nZ() const { - return nZ_; - } - Unit3 bRef() const { - return bRef_; - } private: @@ -204,12 +205,6 @@ public: } return e; } - Unit3 nZ() const { - return nZ_; - } - Unit3 bRef() const { - return bRef_; - } private: From cb5de5be5813a42be89e1c4ee5ed42d6df70c6c5 Mon Sep 17 00:00:00 2001 From: Erik Nelson Date: Fri, 22 Nov 2019 13:41:55 -0800 Subject: [PATCH 52/55] Add unit tests --- gtsam/navigation/tests/testAttitudeFactor.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 70b78c916..24b7f2311 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -75,6 +75,21 @@ TEST(Rot3AttitudeFactor, Serialization) { EXPECT(serializationTestHelpers::equalsBinary(factor)); } +/* ************************************************************************* */ +TEST(Rot3AttitudeFactor, CopyAndMove) { + Unit3 nDown(0, 0, -1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); + Rot3AttitudeFactor factor(0, nDown, model); + + // Copy assignable. + Rot3AttitudeFactor factor_copied = factor; + EXPECT(assert_equal(factor, factor_copied)); + + // Move assignable. + Rot3AttitudeFactor factor_moved = std::move(factor_copied); + EXPECT(assert_equal(factor, factor_moved)); +} + // ************************************************************************* TEST( Pose3AttitudeFactor, Constructor ) { @@ -119,6 +134,21 @@ TEST(Pose3AttitudeFactor, Serialization) { EXPECT(serializationTestHelpers::equalsBinary(factor)); } +/* ************************************************************************* */ +TEST(Pose3AttitudeFactor, CopyAndMove) { + Unit3 nDown(0, 0, -1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); + Pose3AttitudeFactor factor(0, nDown, model); + + // Copy assignable. + Pose3AttitudeFactor factor_copied = factor; + EXPECT(assert_equal(factor, factor_copied)); + + // Move assignable. + Pose3AttitudeFactor factor_moved = std::move(factor_copied); + EXPECT(assert_equal(factor, factor_moved)); +} + // ************************************************************************* int main() { TestResult tr; From 3f064894c4fdf80163f2945075acb51050b4782a Mon Sep 17 00:00:00 2001 From: Erik Nelson Date: Fri, 22 Nov 2019 13:59:23 -0800 Subject: [PATCH 53/55] Explicitly check for copy and move assignable --- gtsam/navigation/tests/testAttitudeFactor.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 24b7f2311..38f16f55f 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -82,10 +82,12 @@ TEST(Rot3AttitudeFactor, CopyAndMove) { Rot3AttitudeFactor factor(0, nDown, model); // Copy assignable. + EXPECT(std::is_copy_assignable::value); Rot3AttitudeFactor factor_copied = factor; EXPECT(assert_equal(factor, factor_copied)); // Move assignable. + EXPECT(std::is_move_assignable::value); Rot3AttitudeFactor factor_moved = std::move(factor_copied); EXPECT(assert_equal(factor, factor_moved)); } @@ -141,10 +143,12 @@ TEST(Pose3AttitudeFactor, CopyAndMove) { Pose3AttitudeFactor factor(0, nDown, model); // Copy assignable. + EXPECT(std::is_copy_assignable::value); Pose3AttitudeFactor factor_copied = factor; EXPECT(assert_equal(factor, factor_copied)); // Move assignable. + EXPECT(std::is_move_assignable::value); Pose3AttitudeFactor factor_moved = std::move(factor_copied); EXPECT(assert_equal(factor, factor_moved)); } From 9b402e478c2544d5316ed98c328225969b9297a3 Mon Sep 17 00:00:00 2001 From: Cong Li Date: Mon, 25 Nov 2019 14:49:40 -0800 Subject: [PATCH 54/55] compute numerical derivative for 6-argument function and add test code for this, also fix bugs in computing numerical derivative in 5-argument function --- gtsam/base/numericalDerivative.h | 180 ++++++++++++++++++- gtsam/base/tests/testNumericalDerivative.cpp | 80 +++++++++ 2 files changed, 257 insertions(+), 3 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index a9a088108..831bb6bcc 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -453,7 +453,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative52( } template -inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), +inline typename internal::FixedSizeMatrix::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { return numericalDerivative52(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); } @@ -509,7 +509,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative54( } template -inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), +inline typename internal::FixedSizeMatrix::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { return numericalDerivative54(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); } @@ -537,11 +537,185 @@ typename internal::FixedSizeMatrix::type numericalDerivative55( } template -inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), +inline typename internal::FixedSizeMatrix::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { return numericalDerivative55(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); } +/** + * Compute numerical derivative in argument 1 of 6-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param x6 sixth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative61( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + return numericalDerivative61(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); +} + +/** + * Compute numerical derivative in argument 2 of 6-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param x6 sixth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative62( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + return numericalDerivative62(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); +} + +/** + * Compute numerical derivative in argument 3 of 6-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param x6 sixth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative63( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + return numericalDerivative63(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); +} + +/** + * Compute numerical derivative in argument 4 of 6-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param x6 sixth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative64( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + return numericalDerivative64(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); +} + +/** + * Compute numerical derivative in argument 5 of 6-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param x6 sixth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative65( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + return numericalDerivative65(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); +} + +/** + * Compute numerical derivative in argument 6 of 6-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param x6 sixth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative66( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { + return numericalDerivative66(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6); +} + /** * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar * function. This is implemented simply as the derivative of the gradient. diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index 1cbf71e1f..d27e43040 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -135,6 +135,86 @@ TEST(testNumericalDerivative, numericalHessian311) { EXPECT(assert_equal(expected33, actual33, 1e-5)); } +/* ************************************************************************* */ +Vector6 f6(const double x1, const double x2, const double x3, const double x4, + const double x5, const double x6) { + Vector6 result; + result << sin(x1), cos(x2), x3 * x3, x4 * x4 * x4, sqrt(x5), sin(x6) - cos(x6); + return result; +} + +/* ************************************************************************* */ +// +TEST(testNumericalDerivative, numeriDerivative61) { + double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6; + + Matrix expected61 = (Matrix(6, 1) << cos(x1), 0, 0, 0, 0, 0).finished(); + Matrix61 actual61 = numericalDerivative61(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected61, actual61, 1e-5)); +} + +/* ************************************************************************* */ +// +TEST(testNumericalDerivative, numeriDerivative62) { + double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6; + + Matrix expected62 = (Matrix(6, 1) << 0, -sin(x2), 0, 0, 0, 0).finished(); + Matrix61 actual62 = numericalDerivative62(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected62, actual62, 1e-5)); +} + +/* ************************************************************************* */ +// +TEST(testNumericalDerivative, numeriDerivative63) { + double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6; + + Matrix expected63 = (Matrix(6, 1) << 0, 0, 2 * x3, 0, 0, 0).finished(); + Matrix61 actual63 = numericalDerivative63(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected63, actual63, 1e-5)); +} + +/* ************************************************************************* */ +// +TEST(testNumericalDerivative, numeriDerivative64) { + double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6; + + Matrix expected64 = (Matrix(6, 1) << 0, 0, 0, 3 * x4 * x4, 0, 0).finished(); + Matrix61 actual64 = numericalDerivative64(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected64, actual64, 1e-5)); +} + +/* ************************************************************************* */ +// +TEST(testNumericalDerivative, numeriDerivative65) { + double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6; + + Matrix expected65 = (Matrix(6, 1) << 0, 0, 0, 0, 0.5 / sqrt(x5), 0).finished(); + Matrix61 actual65 = numericalDerivative65(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected65, actual65, 1e-5)); +} + +/* ************************************************************************* */ +// +TEST(testNumericalDerivative, numeriDerivative66) { + double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6; + + Matrix expected66 = (Matrix(6, 1) << 0, 0, 0, 0, 0, cos(x6) + sin(x6)).finished(); + Matrix61 actual66 = numericalDerivative66(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected66, actual66, 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr; From fb4dc7ccb9d1b4eebdab6a3f17a601b4c05e2890 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 26 Nov 2019 16:48:28 -0800 Subject: [PATCH 55/55] Trigger travis --- gtsam/mainpage.dox | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/mainpage.dox b/gtsam/mainpage.dox index 59e7f9fa6..ee7bd9924 100644 --- a/gtsam/mainpage.dox +++ b/gtsam/mainpage.dox @@ -1,7 +1,6 @@ // This causes Doxygen to find classes inside the gtsam namespace without // explicitly specifying it when writing class names. namespace gtsam { - /** \mainpage GTSAM @@ -23,5 +22,4 @@ To use GTSAM to solve your own problems, you will often have to create new facto - This is more advanced and allows creating factors without an explicit noise model, or that linearize to HessianFactor instead of JacobianFactor. */ - }