From 39286f667214a4f7fd88c224943bfb850d5e2a32 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 19 Dec 2021 10:41:07 -0500 Subject: [PATCH 01/61] added clone to play well with gnc --- gtsam_unstable/slam/PoseToPointFactor.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index cab48e506..b9b2ad5ce 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -61,6 +61,12 @@ class PoseToPointFactor : public NoiseModelFactor2 { traits::Equals(this->measured_, e->measured_, tol); } + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + /** implement functions needed to derive from Factor */ /** vector of errors From c59fbc825fe2f77c96d2e8a1bd7c509c3bf7189c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 5 Jan 2022 23:01:57 -0500 Subject: [PATCH 02/61] ruled out corner case where weights are outside [0,1] in TLS --- gtsam/nonlinear/GncOptimizer.h | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3025d2468..9601e5f8c 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -439,13 +439,11 @@ class GTSAM_EXPORT GncOptimizer { for (size_t k : unknownWeights) { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual - if (u2_k >= upperbound) { + weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu; + if (u2_k >= upperbound || weights[k] < 0) { weights[k] = 0; - } else if (u2_k <= lowerbound) { + } else if (u2_k <= lowerbound || weights[k] > 1) { weights[k] = 1; - } else { - weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - - mu; } } } From 859e4cb37af125b9351db22dc86837077d5c62f9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 12 Jan 2022 21:31:22 -0500 Subject: [PATCH 03/61] thresholded mu to avoid case mu = 0 in TLS. improved verbosity handling --- gtsam/nonlinear/GncOptimizer.h | 25 +++++++++++++++++++------ gtsam/nonlinear/GncParams.h | 2 ++ 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3025d2468..6c8519aac 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -206,9 +206,11 @@ class GTSAM_EXPORT GncOptimizer { std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers" << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::MU) { + std::cout << "mu: " << mu << std::endl; + } if (params_.verbosity >= GncParameters::Verbosity::VALUES) { result.print("result\n"); - std::cout << "mu: " << mu << std::endl; } return result; } @@ -217,12 +219,16 @@ class GTSAM_EXPORT GncOptimizer { for (iter = 0; iter < params_.maxIterations; iter++) { // display info - if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + if (params_.verbosity >= GncParameters::Verbosity::MU) { std::cout << "iter: " << iter << std::endl; - result.print("result\n"); std::cout << "mu: " << mu << std::endl; + } + if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { std::cout << "weights: " << weights_ << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + result.print("result\n"); + } // weights update weights_ = calculateWeights(result, mu); @@ -253,10 +259,12 @@ class GTSAM_EXPORT GncOptimizer { if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "final iterations: " << iter << std::endl; std::cout << "final mu: " << mu << std::endl; - std::cout << "final weights: " << weights_ << std::endl; std::cout << "previous cost: " << prev_cost << std::endl; std::cout << "current cost: " << cost << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { + std::cout << "final weights: " << weights_ << std::endl; + } return result; } @@ -291,6 +299,9 @@ class GTSAM_EXPORT GncOptimizer { std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; } } + if (mu_init >= 0 && mu_init < 1e-6) + mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors, + // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold // and there is no need to robustify (TLS = least squares) @@ -338,8 +349,10 @@ class GTSAM_EXPORT GncOptimizer { bool checkCostConvergence(const double cost, const double prev_cost) const { bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7) < params_.relativeCostTol; - if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) - std::cout << "checkCostConvergence = true " << std::endl; + if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY){ + std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost + << ", curr. cost = " << cost << ")" << std::endl; + } return costConverged; } diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 086f08acc..1f324ae38 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -48,6 +48,8 @@ class GTSAM_EXPORT GncParams { enum Verbosity { SILENT = 0, SUMMARY, + MU, + WEIGHTS, VALUES }; From 7b0356cbea59c3660c8e85e81f07917b007d0e9c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 22 Mar 2022 20:40:08 -0400 Subject: [PATCH 04/61] edited --- gtsam/nonlinear/GncOptimizer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index c5c5d1883..96b03e803 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -447,11 +447,11 @@ class GTSAM_EXPORT GncOptimizer { return weights; } case GncLossType::TLS: { // use eq (14) in GNC paper - double upperbound = (mu + 1) / mu * barcSq_.maxCoeff(); - double lowerbound = mu / (mu + 1) * barcSq_.minCoeff(); for (size_t k : unknownWeights) { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual + double upperbound = (mu + 1) / mu * barcSq_[k]; + double lowerbound = mu / (mu + 1) * barcSq_[k]; weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu; if (u2_k >= upperbound || weights[k] < 0) { weights[k] = 0; From 215682e64f4063911639320dcab209aab2f062fb Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Thu, 7 Apr 2022 21:33:54 -0400 Subject: [PATCH 05/61] FIX: Robust loss error calculation --- gtsam/linear/NoiseModel.h | 6 +++++ tests/testRobust.cpp | 53 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 59 insertions(+) create mode 100644 tests/testRobust.cpp diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 5c379beb8..5a29e5d7d 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -697,6 +697,12 @@ namespace gtsam { return robust_->loss(std::sqrt(squared_distance)); } + // NOTE: This is special because in whiten the base version will do the reweighting + // which is incorrect! + double squaredMahalanobisDistance(const Vector& v) const override { + return noise_->squaredMahalanobisDistance(v); + } + // These are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; void WhitenSystem(std::vector& A, Vector& b) const override; diff --git a/tests/testRobust.cpp b/tests/testRobust.cpp new file mode 100644 index 000000000..ad361f6d9 --- /dev/null +++ b/tests/testRobust.cpp @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testRobust.cpp + * @brief Unit tests for Robust loss functions + * @author Fan Jiang + * @author Yetong Zhang + * @date Apr 7, 2022 + **/ + +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +TEST(RobustNoise, loss) { + // Keys. + gtsam::Key x1_key = 1; + gtsam::Key x2_key = 2; + + auto gm = noiseModel::mEstimator::GemanMcClure::Create(1.0); + auto noise = noiseModel::Robust::Create(gm, noiseModel::Unit::Create(1)); + + auto factor = PriorFactor(x1_key, 0.0, noise); + auto between_factor = BetweenFactor(x1_key, x2_key, 0.0, noise); + + Values values; + values.insert(x1_key, 10.0); + values.insert(x2_key, 0.0); + + EXPECT_DOUBLES_EQUAL(0.49505, factor.error(values), 1e-5); + EXPECT_DOUBLES_EQUAL(0.49505, between_factor.error(values), 1e-5); + EXPECT_DOUBLES_EQUAL(0.49505, gm->loss(10.0), 1e-5); +} + +int main() { + TestResult tr; + + return TestRegistry::runAllTests(tr); +} From 7a47815e0b0c69828a1c5ebf8f43ec9a38502b21 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 8 Apr 2022 16:30:40 -0400 Subject: [PATCH 06/61] Fix the test --- gtsam/linear/tests/testNoiseModel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index b974b6cd5..92774a133 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -668,7 +668,7 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone) for (int i = 0; i < 5; i++) { Vector3 error = Vector3(i, 0, 0); DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i, - robust->squaredMahalanobisDistance(error), 1e-8); + robust->loss(robust->squaredMahalanobisDistance(error)), 1e-8); } } From aee494a24f967f652aae6783df95601cf7bab2c2 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Tue, 14 Jun 2022 17:07:29 -0400 Subject: [PATCH 07/61] Expose GNC params to python --- gtsam/nonlinear/GncOptimizer.h | 8 ++++---- gtsam/nonlinear/nonlinear.i | 17 +++++++++++++++++ python/gtsam/preamble/nonlinear.h | 3 ++- 3 files changed, 23 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index cc3fdaf34..69067a915 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -246,7 +246,9 @@ class GTSAM_EXPORT GncOptimizer { prev_cost = cost; // display info - if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { + std::cout << "iter: " << iter << std::endl; + std::cout << "mu: " << mu << std::endl; std::cout << "previous cost: " << prev_cost << std::endl; std::cout << "current cost: " << cost << std::endl; } @@ -255,9 +257,7 @@ class GTSAM_EXPORT GncOptimizer { if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "final iterations: " << iter << std::endl; std::cout << "final mu: " << mu << std::endl; - std::cout << "final weights: " << weights_ << std::endl; - std::cout << "previous cost: " << prev_cost << std::endl; - std::cout << "current cost: " << cost << std::endl; + std::cout << "final cost: " << cost << std::endl; } return result; } diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 033e5ced2..21470060c 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -544,11 +544,23 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { }; #include +enum GncLossType { + GM /*Geman McClure*/, + TLS /*Truncated least squares*/ +}; + template virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); + void setLossType(const GncLossType type); + void setMaxIterations(const size_t maxIter); + void setMuStep(const double step); + void setRelativeCostTol(double value); + void setWeightsTol(double value); void setVerbosityGNC(const This::Verbosity value); + void setKnownInliers(const std::vector& knownIn); + void setKnownOutliers(const std::vector& knownOut); void print(const string& str) const; enum Verbosity { @@ -597,6 +609,11 @@ virtual class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const PARAMS& params); + void setInlierCostThresholds(const double inth); + const Vector& getInlierCostThresholds(); + void setInlierCostThresholdsAtProbability(const double alpha); + void setWeights(const Vector w); + const Vector& getWeights(); gtsam::Values optimize(); }; diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index a34e73058..34b864027 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -9,4 +9,5 @@ * automatic STL binding, such that the raw objects can be accessed in Python. * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. - */ \ No newline at end of file + */ +#include \ No newline at end of file From c49ad326d1f34327e9d751b5e2fdbacd4f71ba38 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Fri, 17 Jun 2022 10:57:51 -0700 Subject: [PATCH 08/61] initial LOST implementation tested --- gtsam/geometry/tests/testTriangulation.cpp | 18 +++++++++ gtsam/geometry/triangulation.cpp | 46 ++++++++++++++++++++++ gtsam/geometry/triangulation.h | 45 +++++++++++++++++++++ 3 files changed, 109 insertions(+) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index fb66fb6a2..838a5e07f 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -96,6 +96,24 @@ TEST(triangulation, twoPoses) { EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } +TEST(triangulation, twoCamerasLOST) { + std::vector> cameras = {camera1, camera2}; + std::vector measurements = {z1, z2}; + + // 1. Test simple triangulation, perfect in no noise situation + Point3 actual1 = // + triangulateLOSTPoint3(cameras, measurements); + EXPECT(assert_equal(landmark, actual1, 1e-12)); + + // 3. Add some noise and try again: result should be ~ (4.995, + // 0.499167, 1.19814) + measurements[0] += Point2(0.1, 0.5); + measurements[1] += Point2(-0.2, 0.3); + Point3 actual2 = // + triangulateLOSTPoint3(cameras, measurements); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4)); +} + //****************************************************************************** // Simple test with a well-behaved two camera situation with Cal3DS2 calibration. TEST(triangulation, twoPosesCal3DS2) { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 026afef24..a02da2eff 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -39,6 +39,11 @@ Vector4 triangulateHomogeneousDLT( const Point2& p = measurements.at(i); // build system of equations + // [A_1; A_2; A_3] x = [b_1; b_2; b_3] + // [b_3 * A_1 - b_1 * A_3] x = 0 + // [b_3 * A_2 - b_2 * A_3] x = 0 + // A' x = 0 + // A' 2x4 = [b_3 * A_1 - b_1 * A_3; b_3 * A_2 - b_2 * A_3] A.row(row) = p.x() * projection.row(2) - projection.row(0); A.row(row + 1) = p.y() * projection.row(2) - projection.row(1); } @@ -53,6 +58,47 @@ Vector4 triangulateHomogeneousDLT( return v; } +Vector3 triangulateLOSTHomogeneous( + const std::vector& poses, + const std::vector& calibrated_measurements) { + + // TODO(akshay-krishnan): make this an argument. + const double sigma_x = 1e-3; + + size_t m = calibrated_measurements.size(); + assert(m == poses.size()); + + // Construct the system matrices. + Matrix A = Matrix::Zero(m * 2, 3); + Matrix b = Matrix::Zero(m * 2, 1); + + for (size_t i = 0; i < m; i++) { + const Pose3& wTi = poses[i]; + // TODO(akshay-krishnan): are there better ways to select j? + const int j = (i + 1) % m; + const Pose3& wTj = poses[j]; + + Point3 d_ij = wTj.translation() - wTi.translation(); + + Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); + Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]); + + double numerator = w_measurement_i.cross(w_measurement_j).norm(); + double denominator = d_ij.cross(w_measurement_j).norm(); + + double q_i = numerator / (sigma_x * denominator); + Matrix23 coefficient_mat = q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); + + A.row(2 * i) = coefficient_mat.row(0); + A.row(2 * i + 1) = coefficient_mat.row(1); + Point2 p = coefficient_mat * wTi.translation(); + b(2 * i) = p.x(); + b(2 * i + 1) = p.y(); + } + // Solve Ax = b, using QR decomposition + return A.colPivHouseholderQr().solve(b); +} + Vector4 triangulateHomogeneousDLT( const std::vector>& projection_matrices, const std::vector& measurements, double rank_tol) { diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 401fd2d0b..3b834c615 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -62,6 +62,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( const std::vector>& projection_matrices, const Point2Vector& measurements, double rank_tol = 1e-9); +/** + * @brief + * + * @param projection_matrices + * @param measurements + * @param rank_tol + * @return GTSAM_EXPORT + */ +GTSAM_EXPORT Vector3 +triangulateLOSTHomogeneous(const std::vector& poses, + const std::vector& calibrated_measurements); + /** * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) @@ -382,6 +394,39 @@ Point3 triangulatePoint3(const std::vector& poses, return point; } +template +Point3 triangulateLOSTPoint3(const std::vector>& cameras, + const std::vector& measurements) { + const size_t num_cameras = cameras.size(); + assert(measurements.size() == num_cameras); + + if (num_cameras < 2) throw(TriangulationUnderconstrainedException()); + + // Convert measurements to image plane coordinates. + std::vector calibrated_measurements; + calibrated_measurements.reserve(measurements.size()); + for (int i = 0; i < measurements.size(); ++i) { + Point2 p = cameras[i].calibration().calibrate(measurements[i]); + calibrated_measurements.emplace_back(p.x(), p.y(), 1.0); + } + + std::vector poses; + poses.reserve(cameras.size()); + for (const auto& camera : cameras) poses.push_back(camera.pose()); + + Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements); + +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + // verify that the triangulated point lies in front of all cameras + for (const auto& camera : cameras) { + const Point3& p_local = camera.pose().transformTo(point); + if (p_local.z() <= 0) throw(TriangulationCheiralityException()); + } +#endif + + return point; +} + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. This function is similar to the one From 5ea8f2529fc91b82a414a770c027eac14fc3a205 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 20 Jun 2022 23:20:29 -0700 Subject: [PATCH 09/61] make noise input, add LOST vs DLT unit test --- gtsam/geometry/tests/testTriangulation.cpp | 39 +++++++++++++++++++++- gtsam/geometry/triangulation.cpp | 19 +++++------ gtsam/geometry/triangulation.h | 11 +++--- 3 files changed, 54 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 838a5e07f..0b885d434 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -109,11 +109,48 @@ TEST(triangulation, twoCamerasLOST) { // 0.499167, 1.19814) measurements[0] += Point2(0.1, 0.5); measurements[1] += Point2(-0.2, 0.3); + const double measurement_sigma = 1e-3; Point3 actual2 = // - triangulateLOSTPoint3(cameras, measurements); + triangulateLOSTPoint3(cameras, measurements, measurement_sigma); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4)); } +TEST(triangulation, twoCamerasLOSTvsDLT) { + // LOST has been shown to preform better when the point is much closer to one + // camera compared to another. This unit test checks this configuration. + Cal3_S2 identity_K; + Pose3 pose_1; + Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); + PinholeCamera camera_1(pose_1, identity_K); + PinholeCamera camera_2(pose_2, identity_K); + + Point3 gt_point(0, 0, 1); + Point2 x1 = camera_1.project(gt_point); + Point2 x2 = camera_2.project(gt_point); + + Point2 x1_noisy = x1 + Point2(0.00817, 0.00977); + Point2 x2_noisy = x2 + Point2(-0.00610, 0.01969); + + const double measurement_sigma = 1e-2; + Point3 estimate_lost = triangulateLOSTPoint3( + {camera_1, camera_2}, {x1_noisy, x2_noisy}, measurement_sigma); + + // These values are from a MATLAB implementation. + EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), estimate_lost, 1e-3)); + + double rank_tol = 1e-9; + + Pose3Vector poses = {pose_1, pose_2}; + Point2Vector points = {x1_noisy, x2_noisy}; + boost::shared_ptr cal = boost::make_shared(identity_K); + boost::optional estimate_dlt = + triangulatePoint3(poses, cal, points, rank_tol, false); + + // The LOST estimate should have a smaller error. + EXPECT((gt_point - estimate_lost).norm() <= + (gt_point - *estimate_dlt).norm()); +} + //****************************************************************************** // Simple test with a well-behaved two camera situation with Cal3DS2 calibration. TEST(triangulation, twoPosesCal3DS2) { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index a02da2eff..bec239240 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -60,11 +60,8 @@ Vector4 triangulateHomogeneousDLT( Vector3 triangulateLOSTHomogeneous( const std::vector& poses, - const std::vector& calibrated_measurements) { - - // TODO(akshay-krishnan): make this an argument. - const double sigma_x = 1e-3; - + const std::vector& calibrated_measurements, + const double measurement_sigma) { size_t m = calibrated_measurements.size(); assert(m == poses.size()); @@ -78,16 +75,18 @@ Vector3 triangulateLOSTHomogeneous( const int j = (i + 1) % m; const Pose3& wTj = poses[j]; - Point3 d_ij = wTj.translation() - wTi.translation(); + Point3 d_ij = wTj.translation() - wTi.translation(); Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]); - - double numerator = w_measurement_i.cross(w_measurement_j).norm(); + + double numerator = w_measurement_i.cross(w_measurement_j).norm(); double denominator = d_ij.cross(w_measurement_j).norm(); - double q_i = numerator / (sigma_x * denominator); - Matrix23 coefficient_mat = q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); + double q_i = numerator / (measurement_sigma * denominator); + Matrix23 coefficient_mat = + q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * + wTi.rotation().matrix().transpose(); A.row(2 * i) = coefficient_mat.row(0); A.row(2 * i + 1) = coefficient_mat.row(1); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 3b834c615..d31630e67 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -72,7 +72,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( */ GTSAM_EXPORT Vector3 triangulateLOSTHomogeneous(const std::vector& poses, - const std::vector& calibrated_measurements); + const std::vector& calibrated_measurements, + const double measurement_sigma); /** * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors @@ -395,8 +396,10 @@ Point3 triangulatePoint3(const std::vector& poses, } template -Point3 triangulateLOSTPoint3(const std::vector>& cameras, - const std::vector& measurements) { +Point3 triangulateLOSTPoint3( + const std::vector>& cameras, + const std::vector& measurements, + const double measurement_sigma = 1e-2) { const size_t num_cameras = cameras.size(); assert(measurements.size() == num_cameras); @@ -414,7 +417,7 @@ Point3 triangulateLOSTPoint3(const std::vector>& came poses.reserve(cameras.size()); for (const auto& camera : cameras) poses.push_back(camera.pose()); - Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements); + Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements, measurement_sigma); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras From a1e45e0df502b8d02f3f9b8fdf6d456dfb6fc956 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 25 Jun 2022 23:34:16 -0400 Subject: [PATCH 10/61] add this-> to fix ROS issue --- gtsam/sam/BearingRangeFactor.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index da352f2e9..9874760c4 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -79,9 +79,8 @@ class BearingRangeFactor { std::vector Hs(2); const auto &keys = Factor::keys(); - const Vector error = unwhitenedError( - {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, - Hs); + const Vector error = this->unwhitenedError( + {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, Hs); if (H1) *H1 = Hs[0]; if (H2) *H2 = Hs[1]; return error; From ca4b450e7ee98a2d6344df2120148c958e6ab410 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 28 Jun 2022 00:59:20 -0700 Subject: [PATCH 11/61] experimenting with LS and TLS --- gtsam/geometry/triangulation.cpp | 66 +++++++++++++++++++++++++------- gtsam/geometry/triangulation.h | 17 ++++++-- 2 files changed, 65 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index bec239240..9d8325d68 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -65,6 +65,50 @@ Vector3 triangulateLOSTHomogeneous( size_t m = calibrated_measurements.size(); assert(m == poses.size()); + // Construct the system matrices. + Matrix A = Matrix::Zero(m * 2, 4); + + for (size_t i = 0; i < m; i++) { + const Pose3& wTi = poses[i]; + // TODO(akshay-krishnan): are there better ways to select j? + const int j = (i + 1) % m; + const Pose3& wTj = poses[j]; + + const Point3 d_ij = wTj.translation() - wTi.translation(); + + const Point3 w_measurement_i = + wTi.rotation().rotate(calibrated_measurements[i]); + const Point3 w_measurement_j = + wTj.rotation().rotate(calibrated_measurements[j]); + + const double q_i = w_measurement_i.cross(w_measurement_j).norm() / + (measurement_sigma * d_ij.cross(w_measurement_j).norm()); + const Matrix23 coefficient_mat = + q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * + wTi.rotation().matrix().transpose(); + + A.block<2, 4>(2 * i, 0) << coefficient_mat, -coefficient_mat * wTi.translation(); + } + + const double rank_tol = 1e-6; + int rank; + double error; + Vector v; + boost::tie(rank, error, v) = DLT(A, rank_tol); + + if (rank < 3) + throw(TriangulationUnderconstrainedException()); + + return Point3(v.head<3>() / v[3]); +} + +Vector3 triangulateLOSTHomogeneousLS( + const std::vector& poses, + const std::vector& calibrated_measurements, + const double measurement_sigma) { + size_t m = calibrated_measurements.size(); + assert(m == poses.size()); + // Construct the system matrices. Matrix A = Matrix::Zero(m * 2, 3); Matrix b = Matrix::Zero(m * 2, 1); @@ -75,26 +119,20 @@ Vector3 triangulateLOSTHomogeneous( const int j = (i + 1) % m; const Pose3& wTj = poses[j]; - Point3 d_ij = wTj.translation() - wTi.translation(); + const Point3 d_ij = wTj.translation() - wTi.translation(); - Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); - Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]); + const Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); + const Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]); - double numerator = w_measurement_i.cross(w_measurement_j).norm(); - double denominator = d_ij.cross(w_measurement_j).norm(); - - double q_i = numerator / (measurement_sigma * denominator); - Matrix23 coefficient_mat = + const double q_i = w_measurement_i.cross(w_measurement_j).norm() / + (measurement_sigma * d_ij.cross(w_measurement_j).norm()); + const Matrix23 coefficient_mat = q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); - A.row(2 * i) = coefficient_mat.row(0); - A.row(2 * i + 1) = coefficient_mat.row(1); - Point2 p = coefficient_mat * wTi.translation(); - b(2 * i) = p.x(); - b(2 * i + 1) = p.y(); + A.block<2, 3>(2*i, 0) = coefficient_mat; + b.block<2, 1>(2*i, 0) = coefficient_mat * wTi.translation(); } - // Solve Ax = b, using QR decomposition return A.colPivHouseholderQr().solve(b); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index d31630e67..5bea4d93c 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -75,6 +75,11 @@ triangulateLOSTHomogeneous(const std::vector& poses, const std::vector& calibrated_measurements, const double measurement_sigma); +GTSAM_EXPORT Vector3 +triangulateLOSTHomogeneousLS(const std::vector& poses, + const std::vector& calibrated_measurements, + const double measurement_sigma); + /** * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) @@ -397,9 +402,9 @@ Point3 triangulatePoint3(const std::vector& poses, template Point3 triangulateLOSTPoint3( - const std::vector>& cameras, - const std::vector& measurements, - const double measurement_sigma = 1e-2) { + const CameraSet>& cameras, + const Point2Vector& measurements, + const double measurement_sigma = 1e-2, bool use_dlt = false) { const size_t num_cameras = cameras.size(); assert(measurements.size() == num_cameras); @@ -417,7 +422,11 @@ Point3 triangulateLOSTPoint3( poses.reserve(cameras.size()); for (const auto& camera : cameras) poses.push_back(camera.pose()); - Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements, measurement_sigma); + Point3 point = use_dlt + ? triangulateLOSTHomogeneous( + poses, calibrated_measurements, measurement_sigma) + : triangulateLOSTHomogeneousLS( + poses, calibrated_measurements, measurement_sigma); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras From 0d501b6de153bfc13044816480bc5752044e3bf6 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 30 Jun 2022 23:38:34 -0400 Subject: [PATCH 12/61] fix testNoiseModel --- gtsam/linear/tests/testNoiseModel.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 92774a133..f83ba7831 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -661,14 +661,15 @@ TEST(NoiseModel, robustNoiseDCS) TEST(NoiseModel, robustNoiseL2WithDeadZone) { double dead_zone_size = 1.0; - SharedNoiseModel robust = noiseModel::Robust::Create( + auto robust = noiseModel::Robust::Create( noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size), Unit::Create(3)); for (int i = 0; i < 5; i++) { - Vector3 error = Vector3(i, 0, 0); + Vector error = Vector3(i, 0, 0); + robust->WhitenSystem(error); DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i, - robust->loss(robust->squaredMahalanobisDistance(error)), 1e-8); + robust->squaredMahalanobisDistance(error), 1e-8); } } From f947b973cdc5d59214f95b80531f17c4f538b771 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Jul 2022 16:02:42 -0400 Subject: [PATCH 13/61] addressed comments in PR --- gtsam/geometry/triangulation.h | 4 ++-- gtsam/nonlinear/GncOptimizer.h | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index af01d3a36..a4c6b1a13 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -107,7 +107,7 @@ std::pair triangulationGraph( const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = unit2) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; @@ -117,7 +117,7 @@ std::pair triangulationGraph( typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); graph.emplace_shared > // - (camera_i, measurements[i], model? model : unit2, landmarkKey); + (camera_i, measurements[i], model, landmarkKey); } return std::make_pair(graph, values); } diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 96b03e803..e5b4718d6 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -299,9 +299,11 @@ class GTSAM_EXPORT GncOptimizer { std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; } } - if (mu_init >= 0 && mu_init < 1e-6) + if (mu_init >= 0 && mu_init < 1e-6){ mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors, - // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 + // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 + } + return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold // and there is no need to robustify (TLS = least squares) From 79305c1a9d51bdb62f43993a20ad32b5b6fc8ad2 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Jul 2022 17:05:14 -0400 Subject: [PATCH 14/61] fixed compile issue --- gtsam/geometry/triangulation.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index a4c6b1a13..486850903 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -107,11 +107,10 @@ std::pair triangulationGraph( const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate, - const SharedNoiseModel& model = unit2) { + const SharedNoiseModel& model = noiseModel::Unit::Create(2)) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; typedef PinholePose Camera; From aab6af3368ead2be9ef0aa885de5a4777e0c79a4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 3 Jul 2022 11:09:46 -0400 Subject: [PATCH 15/61] replace boost::function with std::function --- gtsam/basis/Chebyshev2.h | 4 +--- gtsam/basis/tests/testChebyshev2.cpp | 2 +- gtsam/discrete/DecisionTree.h | 2 +- gtsam/geometry/tests/testPoint3.cpp | 2 -- 4 files changed, 3 insertions(+), 7 deletions(-) diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index e306c93d5..4c3542d56 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -36,8 +36,6 @@ #include #include -#include - namespace gtsam { /** @@ -134,7 +132,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { * Create matrix of values at Chebyshev points given vector-valued function. */ template - static Matrix matrix(boost::function(double)> f, + static Matrix matrix(std::function(double)> f, size_t N, double a = -1, double b = 1) { Matrix Xmat(M, N); for (size_t j = 0; j < N; j++) { diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 9090757f4..73339e0f1 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -118,7 +118,7 @@ TEST(Chebyshev2, InterpolateVector) { EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); // Check derivative - boost::function)> f = boost::bind( + std::function)> f = boost::bind( &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); Matrix numericalH = numericalDerivative11, 2 * N>(f, X); diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 7449ae995..dede2a2f0 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 315391ac8..e373e1d52 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -19,8 +19,6 @@ #include #include -#include - using namespace std::placeholders; using namespace gtsam; From b0369c48c8cab250589e28c12062c869699ca762 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 3 Jul 2022 11:10:27 -0400 Subject: [PATCH 16/61] import VectorSerialization to Point3 to allow serialization for downstream classes --- gtsam/geometry/Point3.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 835c53d7c..f47531963 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include From 3595e8588c84a6e8488cd948664c765bce92bf40 Mon Sep 17 00:00:00 2001 From: yotams Date: Mon, 4 Jul 2022 11:27:49 +0300 Subject: [PATCH 17/61] added jacobians for all pose3 methods in python wrappers --- gtsam/geometry/Pose3.cpp | 5 +++++ gtsam/geometry/Pose3.h | 8 ++++++++ gtsam/geometry/geometry.i | 33 +++++++++++++++++++++++++++++++++ 3 files changed, 46 insertions(+) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2da51a625..f261a2302 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -489,6 +489,11 @@ boost::optional align(const Point3Pairs &baPointPairs) { } #endif +/* ************************************************************************* */ +Pose3 Pose3::interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const { + return interpolate(*this, other, t, Hx, Hy); +} + /* ************************************************************************* */ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { // Both Rot3 and Point3 have ostream definitions so we use them. diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index c36047349..e2128b28d 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -379,6 +379,14 @@ public: return std::make_pair(0, 2); } + /** + * @brief Spherical Linear interpolation between *this and other + * @param s a value between 0 and 1.5 + * @param other final point of interpolation geodesic on manifold + */ + Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, + OptionalJacobian<6, 6> Hy = boost::none) const; + /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 517558265..4deb5c48a 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -446,24 +446,43 @@ class Pose3 { // Group static gtsam::Pose3 identity(); gtsam::Pose3 inverse() const; + gtsam::Pose3 inverse(Eigen::Ref H) const; gtsam::Pose3 compose(const gtsam::Pose3& pose) const; + gtsam::Pose3 compose(const gtsam::Pose3& pose, + Eigen::Ref H1, + Eigen::Ref H2) const; gtsam::Pose3 between(const gtsam::Pose3& pose) const; + gtsam::Pose3 between(const gtsam::Pose3& pose, + Eigen::Ref H1, + Eigen::Ref H2) const; + gtsam::Pose3 interp(double t, const gtsam::Pose3& pose) const; + gtsam::Pose3 interp(double t, const gtsam::Pose3& pose, + Eigen::Ref Hx, + Eigen::Ref Hy) const; // Operator Overloads gtsam::Pose3 operator*(const gtsam::Pose3& pose) const; // Manifold gtsam::Pose3 retract(Vector v) const; + gtsam::Pose3 retract(Vector v, Eigen::Ref Hxi) const; Vector localCoordinates(const gtsam::Pose3& pose) const; + Vector localCoordinates(const gtsam::Pose3& pose, Eigen::Ref Hxi) const; // Lie Group static gtsam::Pose3 Expmap(Vector v); + static gtsam::Pose3 Expmap(Vector v, Eigen::Ref Hxi); static Vector Logmap(const gtsam::Pose3& pose); + static Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref Hpose); gtsam::Pose3 expmap(Vector v); Vector logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; + Vector Adjoint(Vector xi, Eigen::Ref H_this, + Eigen::Ref H_xib) const; Vector AdjointTranspose(Vector xi) const; + Vector AdjointTranspose(Vector xi, Eigen::Ref H_this, + Eigen::Ref H_x) const; static Matrix adjointMap(Vector xi); static Vector adjoint(Vector xi, Vector y); static Matrix adjointMap_(Vector xi); @@ -476,7 +495,11 @@ class Pose3 { // Group Action on Point3 gtsam::Point3 transformFrom(const gtsam::Point3& point) const; + gtsam::Point3 transformFrom(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint) const; gtsam::Point3 transformTo(const gtsam::Point3& point) const; + gtsam::Point3 transformTo(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint) const; // Matrix versions Matrix transformFrom(const Matrix& points) const; @@ -484,15 +507,25 @@ class Pose3 { // Standard Interface gtsam::Rot3 rotation() const; + gtsam::Rot3 rotation(Eigen::Ref Hself) const; gtsam::Point3 translation() const; + gtsam::Point3 translation(Eigen::Ref Hself) const; double x() const; double y() const; double z() const; Matrix matrix() const; gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose, Eigen::Ref Hself, + Eigen::Ref HaTb) const; gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose, Eigen::Ref Hself, + Eigen::Ref HwTb) const; double range(const gtsam::Point3& point); + double range(const gtsam::Point3& point, Eigen::Ref Hself, + Eigen::Ref Hpoint); double range(const gtsam::Pose3& pose); + double range(const gtsam::Pose3& pose, Eigen::Ref Hself, + Eigen::Ref Hpose); // enabling serialization functionality void serialize() const; From aaeeccf8f59e26c479b39d6e8a12c78008d4e43f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Jul 2022 14:55:39 -0400 Subject: [PATCH 18/61] Squashed 'wrap/' changes from 1a7dc9722..ca357ccdd ca357ccdd Merge pull request #149 from borglab/install-package 886846724 set the GTWRAP_PATH_SEPARATOR properly for MatlabWrap 4abed7fa0 install the python package explicitly git-subtree-dir: wrap git-subtree-split: ca357ccdd27f0661e73ff7a1771768dc4bf8f746 --- cmake/MatlabWrap.cmake | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/cmake/MatlabWrap.cmake b/cmake/MatlabWrap.cmake index 3cb058102..eaffcc059 100644 --- a/cmake/MatlabWrap.cmake +++ b/cmake/MatlabWrap.cmake @@ -242,6 +242,13 @@ function(wrap_library_internal interfaceHeader moduleName linkLibraries extraInc find_package(PythonInterp ${WRAP_PYTHON_VERSION} EXACT) find_package(PythonLibs ${WRAP_PYTHON_VERSION} EXACT) + # Set the path separator for PYTHONPATH + if(UNIX) + set(GTWRAP_PATH_SEPARATOR ":") + else() + set(GTWRAP_PATH_SEPARATOR ";") + endif() + add_custom_command( OUTPUT ${generated_cpp_file} DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets} From 7348586f60b04c464fe674ee939449980ae0efc2 Mon Sep 17 00:00:00 2001 From: yotams Date: Thu, 7 Jul 2022 14:09:10 +0300 Subject: [PATCH 19/61] 1. changed interp name to slerp 2. added python tests for jacobians for some pose3 apis --- gtsam/geometry/Pose3.cpp | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/geometry/geometry.i | 4 +- python/gtsam/tests/test_Pose3.py | 118 ++++++++++++++++++++++++++++++- 4 files changed, 121 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f261a2302..2938e90f5 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -490,7 +490,7 @@ boost::optional align(const Point3Pairs &baPointPairs) { #endif /* ************************************************************************* */ -Pose3 Pose3::interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const { +Pose3 Pose3::slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const { return interpolate(*this, other, t, Hx, Hy); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index e2128b28d..33fb55250 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -384,7 +384,7 @@ public: * @param s a value between 0 and 1.5 * @param other final point of interpolation geodesic on manifold */ - Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, + Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, OptionalJacobian<6, 6> Hy = boost::none) const; /// Output stream operator diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 4deb5c48a..92d9ec0a3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -455,8 +455,8 @@ class Pose3 { gtsam::Pose3 between(const gtsam::Pose3& pose, Eigen::Ref H1, Eigen::Ref H2) const; - gtsam::Pose3 interp(double t, const gtsam::Pose3& pose) const; - gtsam::Pose3 interp(double t, const gtsam::Pose3& pose, + gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose) const; + gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose, Eigen::Ref Hx, Eigen::Ref Hy) const; diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index cde71de53..cdc214a2c 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -19,6 +19,62 @@ from gtsam import Point3, Pose3, Rot3, Point3Pairs from gtsam.utils.test_case import GtsamTestCase +def numerical_derivative_pose(pose, method, delta=1e-5): + jacobian = np.zeros((6, 6)) + for idx in range(6): + xplus = np.zeros(6) + xplus[idx] = delta + xminus = np.zeros(6) + xminus[idx] = -delta + pose_plus = pose.retract(xplus).__getattribute__(method)() + pose_minus = pose.retract(xminus).__getattribute__(method)() + jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta) + return jacobian + + +def numerical_derivative_2_poses(pose, other_pose, method, delta=1e-5, inputs=()): + jacobian = np.zeros((6, 6)) + other_jacobian = np.zeros((6, 6)) + for idx in range(6): + xplus = np.zeros(6) + xplus[idx] = delta + xminus = np.zeros(6) + xminus[idx] = -delta + + pose_plus = pose.retract(xplus).__getattribute__(method)(*inputs, other_pose) + pose_minus = pose.retract(xminus).__getattribute__(method)(*inputs, other_pose) + jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta) + + other_pose_plus = pose.__getattribute__(method)(*inputs, other_pose.retract(xplus)) + other_pose_minus = pose.__getattribute__(method)(*inputs, other_pose.retract(xminus)) + other_jacobian[:, idx] = other_pose_minus.localCoordinates(other_pose_plus) / (2 * delta) + return jacobian, other_jacobian + + +def numerical_derivative_pose_point(pose, point, method, delta=1e-5): + jacobian = np.zeros((3, 6)) + point_jacobian = np.zeros((3, 3)) + for idx in range(6): + xplus = np.zeros(6) + xplus[idx] = delta + xminus = np.zeros(6) + xminus[idx] = -delta + + point_plus = pose.retract(xplus).__getattribute__(method)(point) + point_minus = pose.retract(xminus).__getattribute__(method)(point) + jacobian[:, idx] = (point_plus - point_minus) / (2 * delta) + + if idx < 3: + xplus = np.zeros(3) + xplus[idx] = delta + xminus = np.zeros(3) + xminus[idx] = -delta + point_plus = pose.__getattribute__(method)(point + xplus) + point_minus = pose.__getattribute__(method)(point + xminus) + point_jacobian[:, idx] = (point_plus - point_minus) / (2 * delta) + return jacobian, point_jacobian + + class TestPose3(GtsamTestCase): """Test selected Pose3 methods.""" @@ -30,6 +86,47 @@ class TestPose3(GtsamTestCase): actual = T2.between(T3) self.gtsamAssertEquals(actual, expected, 1e-6) + #test jacobians + jacobian = np.zeros((6, 6), order='F') + jacobian_other = np.zeros((6, 6), order='F') + T2.between(T3, jacobian, jacobian_other) + jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(T2, T3, 'between') + self.gtsamAssertEquals(jacobian, jacobian_numerical) + self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other) + + def test_inverse(self): + """Test between method.""" + pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) + expected = Pose3(Rot3.Rodrigues(0, 0, math.pi/2), Point3(4, -2, 0)) + actual = pose.inverse() + self.gtsamAssertEquals(actual, expected, 1e-6) + + #test jacobians + jacobian = np.zeros((6, 6), order='F') + pose.inverse(jacobian) + jacobian_numerical = numerical_derivative_pose(pose, 'inverse') + self.gtsamAssertEquals(jacobian, jacobian_numerical) + + def test_slerp(self): + """Test slerp method.""" + pose0 = gtsam.Pose3() + pose1 = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) + actual_alpha_0 = pose0.slerp(0, pose1) + self.gtsamAssertEquals(actual_alpha_0, pose0) + actual_alpha_1 = pose0.slerp(1, pose1) + self.gtsamAssertEquals(actual_alpha_1, pose1) + actual_alpha_half = pose0.slerp(0.5, pose1) + expected_alpha_half = Pose3(Rot3.Rodrigues(0, 0, -math.pi/4), Point3(0.17157288, 2.41421356, 0)) + self.gtsamAssertEquals(actual_alpha_half, expected_alpha_half, tol=1e-6) + + # test jacobians + jacobian = np.zeros((6, 6), order='F') + jacobian_other = np.zeros((6, 6), order='F') + pose0.slerp(0.5, pose1, jacobian, jacobian_other) + jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(pose0, pose1, 'slerp', inputs=[0.5]) + self.gtsamAssertEquals(jacobian, jacobian_numerical) + self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other) + def test_transformTo(self): """Test transformTo method.""" pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) @@ -37,6 +134,15 @@ class TestPose3(GtsamTestCase): expected = Point3(2, 1, 10) self.gtsamAssertEquals(actual, expected, 1e-6) + #test jacobians + point = Point3(3, 2, 10) + jacobian_pose = np.zeros((3, 6), order='F') + jacobian_point = np.zeros((3, 3), order='F') + pose.transformTo(point, jacobian_pose, jacobian_point) + jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformTo') + self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose) + self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point) + # multi-point version points = np.stack([Point3(3, 2, 10), Point3(3, 2, 10)]).T actual_array = pose.transformTo(points) @@ -51,6 +157,15 @@ class TestPose3(GtsamTestCase): expected = Point3(3, 2, 10) self.gtsamAssertEquals(actual, expected, 1e-6) + #test jacobians + point = Point3(3, 2, 10) + jacobian_pose = np.zeros((3, 6), order='F') + jacobian_point = np.zeros((3, 3), order='F') + pose.transformFrom(point, jacobian_pose, jacobian_point) + jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformFrom') + self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose) + self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point) + # multi-point version points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T actual_array = pose.transformFrom(points) @@ -122,4 +237,5 @@ class TestPose3(GtsamTestCase): if __name__ == "__main__": - unittest.main() + # unittest.main() + TestPose3().test_slerp() \ No newline at end of file From 7c077f8536c1a06710c251d16b1760ef55be11aa Mon Sep 17 00:00:00 2001 From: yotams Date: Thu, 7 Jul 2022 14:20:26 +0300 Subject: [PATCH 20/61] bugfix --- python/gtsam/tests/test_Pose3.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index cdc214a2c..4464e8d47 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -237,5 +237,4 @@ class TestPose3(GtsamTestCase): if __name__ == "__main__": - # unittest.main() - TestPose3().test_slerp() \ No newline at end of file + unittest.main() \ No newline at end of file From 4333f9a186f26aa7e181a7c0aced34fb42b5d681 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Thu, 7 Jul 2022 13:21:54 -0400 Subject: [PATCH 21/61] Revert changes to verbosity --- gtsam/nonlinear/GncOptimizer.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 69067a915..cc3fdaf34 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -246,9 +246,7 @@ class GTSAM_EXPORT GncOptimizer { prev_cost = cost; // display info - if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { - std::cout << "iter: " << iter << std::endl; - std::cout << "mu: " << mu << std::endl; + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { std::cout << "previous cost: " << prev_cost << std::endl; std::cout << "current cost: " << cost << std::endl; } @@ -257,7 +255,9 @@ class GTSAM_EXPORT GncOptimizer { if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "final iterations: " << iter << std::endl; std::cout << "final mu: " << mu << std::endl; - std::cout << "final cost: " << cost << std::endl; + std::cout << "final weights: " << weights_ << std::endl; + std::cout << "previous cost: " << prev_cost << std::endl; + std::cout << "current cost: " << cost << std::endl; } return result; } From f5ec070f9fc87d52076846ff394b79b001f4bd13 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 7 Jul 2022 15:09:55 -0400 Subject: [PATCH 22/61] wrap BearingRange classes and factors --- gtsam/geometry/geometry.i | 6 ++++++ gtsam/sam/sam.i | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 92d9ec0a3..f3f8a5c79 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1279,5 +1279,11 @@ class BearingRange { typedef gtsam::BearingRange BearingRange2D; +typedef gtsam::BearingRange + BearingRangePose2; +typedef gtsam::BearingRange + BearingRange3D; +typedef gtsam::BearingRange + BearingRangePose3; } // namespace gtsam diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i index 90c319ede..a46a6de9e 100644 --- a/gtsam/sam/sam.i +++ b/gtsam/sam/sam.i @@ -108,5 +108,11 @@ typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; +typedef gtsam::BearingRangeFactor + BearingRangeFactor3D; +typedef gtsam::BearingRangeFactor + BearingRangeFactorPose3; } // namespace gtsam From 2ab09a580fa059b7dd5cd0750365927f81f5a5e0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 7 Jul 2022 15:24:56 -0400 Subject: [PATCH 23/61] add Python unit tests --- gtsam/linear/linear.i | 2 +- python/gtsam/tests/test_Factors.py | 22 ++++++++++++++++++++-- python/gtsam/tests/test_sam.py | 28 ++++++++++++++++++++++++++++ 3 files changed, 49 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 943b661d8..fdf156ff9 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -279,7 +279,6 @@ virtual class GaussianFactor { virtual class JacobianFactor : gtsam::GaussianFactor { //Constructors JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor& factor); JacobianFactor(Vector b_in); JacobianFactor(size_t i1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); @@ -295,6 +294,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { JacobianFactor(const gtsam::GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const gtsam::VariableSlots& p_variableSlots); + JacobianFactor(const gtsam::GaussianFactor& factor); //Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = diff --git a/python/gtsam/tests/test_Factors.py b/python/gtsam/tests/test_Factors.py index 3ec8648a4..a5688eec8 100644 --- a/python/gtsam/tests/test_Factors.py +++ b/python/gtsam/tests/test_Factors.py @@ -11,9 +11,8 @@ Author: Varun Agrawal """ import unittest -import numpy as np - import gtsam +import numpy as np from gtsam.utils.test_case import GtsamTestCase @@ -21,6 +20,7 @@ class TestNonlinearEquality2Factor(GtsamTestCase): """ Test various instantiations of NonlinearEquality2. """ + def test_point3(self): """Test for Point3 version.""" factor = gtsam.NonlinearEquality2Point3(0, 1) @@ -30,5 +30,23 @@ class TestNonlinearEquality2Factor(GtsamTestCase): np.testing.assert_allclose(error, np.zeros(3)) +class TestJacobianFactor(GtsamTestCase): + """Test JacobianFactor""" + + def test_gaussian_factor_graph(self): + """Test construction from GaussianFactorGraph.""" + gfg = gtsam.GaussianFactorGraph() + jf = gtsam.JacobianFactor(gfg) + self.assertIsInstance(jf, gtsam.JacobianFactor) + + nfg = gtsam.NonlinearFactorGraph() + nfg.push_back(gtsam.PriorFactorDouble(1, 0.0, gtsam.noiseModel.Isotropic.Sigma(1, 1.0))) + values = gtsam.Values() + values.insert(1, 0.0) + gfg = nfg.linearize(values) + jf = gtsam.JacobianFactor(gfg) + self.assertIsInstance(jf, gtsam.JacobianFactor) + + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/tests/test_sam.py b/python/gtsam/tests/test_sam.py index e01b9c1d1..fd8da5308 100644 --- a/python/gtsam/tests/test_sam.py +++ b/python/gtsam/tests/test_sam.py @@ -50,6 +50,34 @@ class TestSam(GtsamTestCase): self.assertEqual(range_measurement, measurement.range()) self.gtsamAssertEquals(bearing_measurement, measurement.bearing()) + def test_BearingRangeFactor3D(self): + """ + Test that `measured` works as expected for BearingRangeFactor3D. + """ + bearing_measurement = gtsam.Unit3() + range_measurement = 10.0 + factor = gtsam.BearingRangeFactor3D( + 1, 2, bearing_measurement, range_measurement, + gtsam.noiseModel.Isotropic.Sigma(3, 1)) + measurement = factor.measured() + + self.assertEqual(range_measurement, measurement.range()) + self.gtsamAssertEquals(bearing_measurement, measurement.bearing()) + + def test_BearingRangeFactorPose3(self): + """ + Test that `measured` works as expected for BearingRangeFactorPose3. + """ + range_measurement = 10.0 + bearing_measurement = gtsam.Unit3() + factor = gtsam.BearingRangeFactorPose3( + 1, 2, bearing_measurement, range_measurement, + gtsam.noiseModel.Isotropic.Sigma(3, 1)) + measurement = factor.measured() + + self.assertEqual(range_measurement, measurement.range()) + self.gtsamAssertEquals(bearing_measurement, measurement.bearing()) + if __name__ == "__main__": unittest.main() From 26dd94bdaf554c18c2f9027a6bb04d2a4448a744 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Fri, 8 Jul 2022 12:33:47 -0400 Subject: [PATCH 24/61] Expose remaining attributes, add tests --- gtsam/nonlinear/nonlinear.i | 12 +++- python/gtsam/tests/test_NonlinearOptimizer.py | 60 ++++++++++++++++++- 2 files changed, 68 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 21470060c..913be1aed 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -553,6 +553,16 @@ template virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); + BaseOptimizerParameters baseOptimizerParams; + GncLossType lossType; + size_t maxIterations; + double muStep; + double relativeCostTol; + double weightsTol; + Verbosity verbosity; + std::vector knownInliers; + std::vector knownOutliers; + void setLossType(const GncLossType type); void setMaxIterations(const size_t maxIter); void setMuStep(const double step); @@ -561,7 +571,7 @@ virtual class GncParams { void setVerbosityGNC(const This::Verbosity value); void setKnownInliers(const std::vector& knownIn); void setKnownOutliers(const std::vector& knownOut); - void print(const string& str) const; + void print(const string& str = "") const; enum Verbosity { SILENT, diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 37afd9e1c..47d41edb2 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -16,9 +16,10 @@ import unittest import gtsam from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, - GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer, - LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph, - Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values) + GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLossType, + GncLMOptimizer, LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, + PriorFactorPoint2, Values) from gtsam.utils.test_case import GtsamTestCase KEY1 = 1 @@ -83,6 +84,59 @@ class TestScenario(GtsamTestCase): actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize() self.assertAlmostEqual(0, self.fg.error(actual)) + def test_gnc_params(self): + base_params = LevenbergMarquardtParams() + # Test base params + for base_max_iters in (50, 100): + base_params.setMaxIterations(base_max_iters) + params = GncLMParams(base_params) + self.assertEqual(params.baseOptimizerParams.getMaxIterations(), base_max_iters) + # Test printing + str(params) + # Test each parameter + for loss_type in (GncLossType.TLS, GncLossType.GM): + params.setLossType(loss_type) # Default is TLS + self.assertEqual(params.lossType, loss_type) + for max_iter in (1, 10, 100): + params.setMaxIterations(max_iter) + self.assertEqual(params.maxIterations, max_iter) + for mu_step in (1.1, 1.2, 1.5): + params.setMuStep(mu_step) + self.assertEqual(params.muStep, mu_step) + for rel_cost_tol in (1e-5, 1e-6, 1e-7): + params.setRelativeCostTol(rel_cost_tol) + self.assertEqual(params.relativeCostTol, rel_cost_tol) + for weights_tol in (1e-4, 1e-3, 1e-2): + params.setWeightsTol(weights_tol) + self.assertEqual(params.weightsTol, weights_tol) + for i in (0, 1, 2): + verb = GncLMParams.Verbosity(i) + params.setVerbosityGNC(verb) + self.assertEqual(params.verbosity, verb) + for inl in ([], [10], [0, 100]): + params.setKnownInliers(inl) + self.assertEqual(params.knownInliers, inl) + params.knownInliers = [] + for out in ([], [1], [0, 10]): + params.setKnownInliers(out) + self.assertEqual(params.knownInliers, out) + params.knownInliers = [] + # Test optimizer params + optimizer = GncLMOptimizer(self.fg, self.initial_values, params) + for ict_factor in (0.9, 1.1): + new_ict = ict_factor * optimizer.getInlierCostThresholds() + optimizer.setInlierCostThresholds(new_ict) + self.assertAlmostEqual(optimizer.getInlierCostThresholds(), new_ict) + for w_factor in (0.8, 0.9): + new_weights = w_factor * optimizer.getWeights() + optimizer.setWeights(new_weights) + self.assertAlmostEqual(optimizer.getWeights(), new_weights) + optimizer.setInlierCostThresholdsAtProbability(0.9) + w1 = optimizer.getInlierCostThresholds() + optimizer.setInlierCostThresholdsAtProbability(0.8) + w2 = optimizer.getInlierCostThresholds() + self.assertLess(w2, w1) + def test_iteration_hook(self): # set up iteration hook to track some testable values iteration_count = 0 From 9dd1f8ffaf0112a8ff053ad347157c1382010e47 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Fri, 8 Jul 2022 16:48:52 -0400 Subject: [PATCH 25/61] Adress review comments --- gtsam/nonlinear/GncParams.h | 2 +- gtsam/nonlinear/nonlinear.i | 2 +- python/gtsam/tests/test_NonlinearOptimizer.py | 14 +++++++++++++- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 086f08acc..600a90976 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -161,7 +161,7 @@ class GTSAM_EXPORT GncParams { std::cout << "knownInliers: " << knownInliers[i] << "\n"; for (size_t i = 0; i < knownOutliers.size(); i++) std::cout << "knownOutliers: " << knownOutliers[i] << "\n"; - baseOptimizerParams.print(str); + baseOptimizerParams.print("Base optimizer params: "); } }; diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 913be1aed..b6d0f31be 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -571,7 +571,7 @@ virtual class GncParams { void setVerbosityGNC(const This::Verbosity value); void setKnownInliers(const std::vector& knownIn); void setKnownOutliers(const std::vector& knownOut); - void print(const string& str = "") const; + void print(const string& str = "GncParams: ") const; enum Verbosity { SILENT, diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 47d41edb2..3db2bb196 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -91,8 +91,19 @@ class TestScenario(GtsamTestCase): base_params.setMaxIterations(base_max_iters) params = GncLMParams(base_params) self.assertEqual(params.baseOptimizerParams.getMaxIterations(), base_max_iters) + # Test printing - str(params) + params_str = str(params) + for s in ( + "lossType", + "maxIterations", + "muStep", + "relativeCostTol", + "weightsTol", + "verbosity", + ): + self.assertTrue(s in params_str) + # Test each parameter for loss_type in (GncLossType.TLS, GncLossType.GM): params.setLossType(loss_type) # Default is TLS @@ -121,6 +132,7 @@ class TestScenario(GtsamTestCase): params.setKnownInliers(out) self.assertEqual(params.knownInliers, out) params.knownInliers = [] + # Test optimizer params optimizer = GncLMOptimizer(self.fg, self.initial_values, params) for ict_factor in (0.9, 1.1): From b4fdda4740c39a78306f9189bab2c80ffedf8ffd Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Fri, 8 Jul 2022 16:49:04 -0400 Subject: [PATCH 26/61] Run yapf --- python/gtsam/tests/test_NonlinearOptimizer.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 3db2bb196..a47c5ad62 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -15,11 +15,12 @@ from __future__ import print_function import unittest import gtsam -from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, - GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLossType, - GncLMOptimizer, LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, - PriorFactorPoint2, Values) +from gtsam import ( + DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer, + GaussNewtonParams, GncLMParams, GncLossType, GncLMOptimizer, LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, + PriorFactorPoint2, Values +) from gtsam.utils.test_case import GtsamTestCase KEY1 = 1 @@ -28,7 +29,6 @@ KEY2 = 2 class TestScenario(GtsamTestCase): """Do trivial test with three optimizer variants.""" - def setUp(self): """Set up the optimization problem and ordering""" # create graph @@ -154,11 +154,13 @@ class TestScenario(GtsamTestCase): iteration_count = 0 final_error = 0 final_values = None + def iteration_hook(iter, error_before, error_after): nonlocal iteration_count, final_error, final_values iteration_count = iter final_error = error_after final_values = optimizer.values() + # optimize params = LevenbergMarquardtParams.CeresDefaults() params.setOrdering(self.ordering) @@ -170,5 +172,6 @@ class TestScenario(GtsamTestCase): self.assertEqual(self.fg.error(actual), final_error) self.assertEqual(optimizer.iterations(), iteration_count) + if __name__ == "__main__": unittest.main() From 63e0446f0b04fe23e1224b19e9f819c0f9c9a249 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 10 Jul 2022 16:49:15 -0700 Subject: [PATCH 27/61] update triangulation LOST API --- gtsam/geometry/Point3.h | 1 + gtsam/geometry/tests/testTriangulation.cpp | 63 +++--- gtsam/geometry/triangulation.cpp | 159 +++++++-------- gtsam/geometry/triangulation.h | 218 +++++++++++++-------- 4 files changed, 247 insertions(+), 194 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 835c53d7c..b1ac6195b 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -33,6 +33,7 @@ namespace gtsam { /// As of GTSAM 4, in order to make GTSAM more lean, /// it is now possible to just typedef Point3 to Vector3 typedef Vector3 Point3; +typedef std::vector > Point3Vector; // Convenience typedef using Point3Pair = std::pair; diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0b885d434..731558327 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -96,23 +96,33 @@ TEST(triangulation, twoPoses) { EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } -TEST(triangulation, twoCamerasLOST) { - std::vector> cameras = {camera1, camera2}; - std::vector measurements = {z1, z2}; +TEST(triangulation, twoCamerasUsingLOST) { + CameraSet> cameras; + cameras.push_back(camera1); + cameras.push_back(camera2); + + Point2Vector measurements = {z1, z2}; + SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4); + double rank_tol = 1e-9; // 1. Test simple triangulation, perfect in no noise situation - Point3 actual1 = // - triangulateLOSTPoint3(cameras, measurements); - EXPECT(assert_equal(landmark, actual1, 1e-12)); + boost::optional actual1 = // + triangulatePoint3>( + cameras, measurements, rank_tol, + /*optimize=*/false, measurementNoise, + /*use_lost_triangulation=*/true); + EXPECT(assert_equal(landmark, *actual1, 1e-12)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) measurements[0] += Point2(0.1, 0.5); measurements[1] += Point2(-0.2, 0.3); - const double measurement_sigma = 1e-3; - Point3 actual2 = // - triangulateLOSTPoint3(cameras, measurements, measurement_sigma); - EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4)); + boost::optional actual2 = // + triangulatePoint3>( + cameras, measurements, rank_tol, + /*optimize=*/false, measurementNoise, + /*use_lost_triangulation=*/true); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual2, 1e-4)); } TEST(triangulation, twoCamerasLOSTvsDLT) { @@ -121,33 +131,32 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { Cal3_S2 identity_K; Pose3 pose_1; Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); - PinholeCamera camera_1(pose_1, identity_K); - PinholeCamera camera_2(pose_2, identity_K); + CameraSet> cameras; + cameras.emplace_back(pose_1, identity_K); + cameras.emplace_back(pose_2, identity_K); Point3 gt_point(0, 0, 1); - Point2 x1 = camera_1.project(gt_point); - Point2 x2 = camera_2.project(gt_point); + Point2 x1_noisy = cameras[0].project(gt_point) + Point2(0.00817, 0.00977); + Point2 x2_noisy = cameras[1].project(gt_point) + Point2(-0.00610, 0.01969); + Point2Vector measurements = {x1_noisy, x2_noisy}; - Point2 x1_noisy = x1 + Point2(0.00817, 0.00977); - Point2 x2_noisy = x2 + Point2(-0.00610, 0.01969); + double rank_tol = 1e-9; + SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); - const double measurement_sigma = 1e-2; - Point3 estimate_lost = triangulateLOSTPoint3( - {camera_1, camera_2}, {x1_noisy, x2_noisy}, measurement_sigma); + boost::optional estimate_lost = // + triangulatePoint3>( + cameras, measurements, rank_tol, + /*optimize=*/false, measurementNoise, + /*use_lost_triangulation=*/true); // These values are from a MATLAB implementation. - EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), estimate_lost, 1e-3)); - - double rank_tol = 1e-9; + EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimate_lost, 1e-3)); - Pose3Vector poses = {pose_1, pose_2}; - Point2Vector points = {x1_noisy, x2_noisy}; - boost::shared_ptr cal = boost::make_shared(identity_K); boost::optional estimate_dlt = - triangulatePoint3(poses, cal, points, rank_tol, false); + triangulatePoint3(cameras, measurements, rank_tol, false); // The LOST estimate should have a smaller error. - EXPECT((gt_point - estimate_lost).norm() <= + EXPECT((gt_point - *estimate_lost).norm() <= (gt_point - *estimate_dlt).norm()); } diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 9d8325d68..c2c798d8b 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -24,9 +24,9 @@ namespace gtsam { Vector4 triangulateHomogeneousDLT( - const std::vector>& projection_matrices, + const std::vector>& + projection_matrices, const Point2Vector& measurements, double rank_tol) { - // number of cameras size_t m = projection_matrices.size(); @@ -52,61 +52,75 @@ Vector4 triangulateHomogeneousDLT( Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - if (rank < 3) - throw(TriangulationUnderconstrainedException()); + if (rank < 3) throw(TriangulationUnderconstrainedException()); return v; } -Vector3 triangulateLOSTHomogeneous( - const std::vector& poses, - const std::vector& calibrated_measurements, - const double measurement_sigma) { - size_t m = calibrated_measurements.size(); - assert(m == poses.size()); +Vector4 triangulateHomogeneousDLT( + const std::vector>& + projection_matrices, + const std::vector& measurements, double rank_tol) { + // number of cameras + size_t m = projection_matrices.size(); - // Construct the system matrices. + // Allocate DLT matrix Matrix A = Matrix::Zero(m * 2, 4); for (size_t i = 0; i < m; i++) { - const Pose3& wTi = poses[i]; - // TODO(akshay-krishnan): are there better ways to select j? - const int j = (i + 1) % m; - const Pose3& wTj = poses[j]; + size_t row = i * 2; + const Matrix34& projection = projection_matrices.at(i); + const Point3& p = + measurements.at(i) + .point3(); // to get access to x,y,z of the bearing vector - const Point3 d_ij = wTj.translation() - wTi.translation(); - - const Point3 w_measurement_i = - wTi.rotation().rotate(calibrated_measurements[i]); - const Point3 w_measurement_j = - wTj.rotation().rotate(calibrated_measurements[j]); - - const double q_i = w_measurement_i.cross(w_measurement_j).norm() / - (measurement_sigma * d_ij.cross(w_measurement_j).norm()); - const Matrix23 coefficient_mat = - q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * - wTi.rotation().matrix().transpose(); - - A.block<2, 4>(2 * i, 0) << coefficient_mat, -coefficient_mat * wTi.translation(); + // build system of equations + A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); + A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1); } - - const double rank_tol = 1e-6; int rank; double error; Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - if (rank < 3) - throw(TriangulationUnderconstrainedException()); + if (rank < 3) throw(TriangulationUnderconstrainedException()); + + return v; +} + +Point3 triangulateDLT(const std::vector& poses, + const Point3Vector& homogenousMeasurements, + double rank_tol) { + // number of cameras + size_t m = poses.size(); + + // Allocate DLT matrix + Matrix A = Matrix::Zero(m * 2, 4); + + for (size_t i = 0; i < m; i++) { + size_t row = i * 2; + const Matrix34 projection = poses[i].matrix().block(0, 0, 3, 4); + const Point3& p = homogenousMeasurements[i]; // to get access to x,y,z of + // the bearing vector + + // build system of equations + A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); + A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1); + } + int rank; + double error; + Vector v; + boost::tie(rank, error, v) = DLT(A, rank_tol); + + if (rank < 3) throw(TriangulationUnderconstrainedException()); return Point3(v.head<3>() / v[3]); } -Vector3 triangulateLOSTHomogeneousLS( - const std::vector& poses, - const std::vector& calibrated_measurements, - const double measurement_sigma) { - size_t m = calibrated_measurements.size(); +Point3 triangulateLOST(const std::vector& poses, + const Point3Vector& calibratedMeasurements, + const SharedIsotropic& measurementNoise) { + size_t m = calibratedMeasurements.size(); assert(m == poses.size()); // Construct the system matrices. @@ -121,70 +135,43 @@ Vector3 triangulateLOSTHomogeneousLS( const Point3 d_ij = wTj.translation() - wTi.translation(); - const Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); - const Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]); + const Point3 w_measurement_i = + wTi.rotation().rotate(calibratedMeasurements[i]); + const Point3 w_measurement_j = + wTj.rotation().rotate(calibratedMeasurements[j]); - const double q_i = w_measurement_i.cross(w_measurement_j).norm() / - (measurement_sigma * d_ij.cross(w_measurement_j).norm()); + double q_i = + w_measurement_i.cross(w_measurement_j).norm() / + (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm()); const Matrix23 coefficient_mat = - q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * + q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); - A.block<2, 3>(2*i, 0) = coefficient_mat; - b.block<2, 1>(2*i, 0) = coefficient_mat * wTi.translation(); + A.block<2, 3>(2 * i, 0) << coefficient_mat; + b.block<2, 1>(2 * i, 0) << coefficient_mat * wTi.translation(); } return A.colPivHouseholderQr().solve(b); } -Vector4 triangulateHomogeneousDLT( - const std::vector>& projection_matrices, - const std::vector& measurements, double rank_tol) { - - // number of cameras - size_t m = projection_matrices.size(); - - // Allocate DLT matrix - Matrix A = Matrix::Zero(m * 2, 4); - - for (size_t i = 0; i < m; i++) { - size_t row = i * 2; - const Matrix34& projection = projection_matrices.at(i); - const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector - - // build system of equations - A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); - A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1); - } - int rank; - double error; - Vector v; - boost::tie(rank, error, v) = DLT(A, rank_tol); - - if (rank < 3) - throw(TriangulationUnderconstrainedException()); - - return v; -} - Point3 triangulateDLT( - const std::vector>& projection_matrices, + const std::vector>& + projection_matrices, const Point2Vector& measurements, double rank_tol) { - - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, - rank_tol); + Vector4 v = + triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); // Create 3D point from homogeneous coordinates return Point3(v.head<3>() / v[3]); } Point3 triangulateDLT( - const std::vector>& projection_matrices, + const std::vector>& + projection_matrices, const std::vector& measurements, double rank_tol) { - // contrary to previous triangulateDLT, this is now taking Unit3 inputs - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, - rank_tol); - // Create 3D point from homogeneous coordinates - return Point3(v.head<3>() / v[3]); + Vector4 v = + triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); + // Create 3D point from homogeneous coordinates + return Point3(v.head<3>() / v[3]); } /// @@ -215,4 +202,4 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, return result.at(landmarkKey); } -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 5bea4d93c..9e59a92cb 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -62,24 +62,6 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( const std::vector>& projection_matrices, const Point2Vector& measurements, double rank_tol = 1e-9); -/** - * @brief - * - * @param projection_matrices - * @param measurements - * @param rank_tol - * @return GTSAM_EXPORT - */ -GTSAM_EXPORT Vector3 -triangulateLOSTHomogeneous(const std::vector& poses, - const std::vector& calibrated_measurements, - const double measurement_sigma); - -GTSAM_EXPORT Vector3 -triangulateLOSTHomogeneousLS(const std::vector& poses, - const std::vector& calibrated_measurements, - const double measurement_sigma); - /** * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) @@ -112,6 +94,20 @@ GTSAM_EXPORT Point3 triangulateDLT( const std::vector& measurements, double rank_tol = 1e-9); +/** + * @brief Triangulation using the LOST (Linear Optimal Sine Triangulation) + * algorithm proposed in https://arxiv.org/pdf/2205.12197.pdf by Sebastien Henry + * and John Christian. + * @param poses camera poses in world frame + * @param calibratedMeasurements measurements in homogeneous coordinates in each + * camera pose + * @param measurementNoise isotropic noise model for the measurements + * @return triangulated point in world coordinates + */ +GTSAM_EXPORT Point3 triangulateLOST(const std::vector& poses, + const Point3Vector& calibratedMeasurements, + const SharedIsotropic& measurementNoise); + /** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses @@ -320,8 +316,8 @@ template typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = cameras.size(); - assert(num_meas == measurements.size()); + const size_t num_meas = measurements.size(); + assert(num_meas == cameras.size()); typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); for (size_t ii = 0; ii < num_meas; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that @@ -349,6 +345,65 @@ inline SphericalCamera::MeasurementVector undistortMeasurements( return measurements; } +/** Convert pixel measurements in image to homogeneous measurements in the image + * plane using shared camera intrinsics. + * + * @tparam CALIBRATION Calibration type to use. + * @param cal Calibration with which measurements were taken. + * @param measurements Vector of measurements to undistort. + * @return homogeneous measurements in image plane + */ +template +inline Point3Vector calibrateMeasurementsShared( + const CALIBRATION& cal, const Point2Vector& measurements) { + Point3Vector calibratedMeasurements; + // Calibrate with cal and uncalibrate with pinhole version of cal so that + // measurements are undistorted. + std::transform(measurements.begin(), measurements.end(), + std::back_inserter(calibratedMeasurements), + [&cal](const Point2& measurement) { + Point3 p; + p << cal.calibrate(measurement), 1.0; + return p; + }); + return calibratedMeasurements; +} + +/** Convert pixel measurements in image to homogeneous measurements in the image + * plane using camera intrinsics of each measurement. + * + * @tparam CAMERA Camera type to use. + * @param cameras Cameras corresponding to each measurement. + * @param measurements Vector of measurements to undistort. + * @return homogeneous measurements in image plane + */ +template +inline Point3Vector calibrateMeasurements( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements) { + const size_t num_meas = measurements.size(); + assert(num_meas == cameras.size()); + Point3Vector calibratedMeasurements(num_meas); + for (size_t ii = 0; ii < num_meas; ++ii) { + calibratedMeasurements[ii] + << cameras[ii].calibration().calibrate(measurements[ii]), + 1.0; + } + return calibratedMeasurements; +} + +/** Specialize for SphericalCamera to do nothing. */ +template +inline Point3Vector calibrateMeasurements( + const CameraSet& cameras, + const SphericalCamera::MeasurementVector& measurements) { + Point3Vector calibratedMeasurements(measurements.size()); + for (size_t ii = 0; ii < measurements.size(); ++ii) { + calibratedMeasurements[ii] << measurements[ii].point3(); + } + return calibratedMeasurements; +} + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -359,6 +414,7 @@ inline SphericalCamera::MeasurementVector undistortMeasurements( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation + * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT * @return Returns a Point3 */ template @@ -366,22 +422,36 @@ Point3 triangulatePoint3(const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = true) { assert(poses.size() == measurements.size()); if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); - - // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = - undistortMeasurements(*sharedCal, measurements); - // Triangulate linearly - Point3 point = - triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + Point3 point; + if(use_lost_triangulation) { + // Reduce input noise model to an isotropic noise model using the mean of + // the diagonal. + const double measurementSigma = model ? model->sigmas().mean() : 1e-4; + SharedIsotropic measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); + // calibrate the measurements to obtain homogenous coordinates in image plane. + auto calibratedMeasurements = + calibrateMeasurementsShared(*sharedCal, measurements); + + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + } else { + // construct projection matrices from poses & calibration + auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); + + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = + undistortMeasurements(*sharedCal, measurements); + + point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + } // Then refine using non-linear optimization if (optimize) @@ -400,45 +470,6 @@ Point3 triangulatePoint3(const std::vector& poses, return point; } -template -Point3 triangulateLOSTPoint3( - const CameraSet>& cameras, - const Point2Vector& measurements, - const double measurement_sigma = 1e-2, bool use_dlt = false) { - const size_t num_cameras = cameras.size(); - assert(measurements.size() == num_cameras); - - if (num_cameras < 2) throw(TriangulationUnderconstrainedException()); - - // Convert measurements to image plane coordinates. - std::vector calibrated_measurements; - calibrated_measurements.reserve(measurements.size()); - for (int i = 0; i < measurements.size(); ++i) { - Point2 p = cameras[i].calibration().calibrate(measurements[i]); - calibrated_measurements.emplace_back(p.x(), p.y(), 1.0); - } - - std::vector poses; - poses.reserve(cameras.size()); - for (const auto& camera : cameras) poses.push_back(camera.pose()); - - Point3 point = use_dlt - ? triangulateLOSTHomogeneous( - poses, calibrated_measurements, measurement_sigma) - : triangulateLOSTHomogeneousLS( - poses, calibrated_measurements, measurement_sigma); - -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies in front of all cameras - for (const auto& camera : cameras) { - const Point3& p_local = camera.pose().transformTo(point); - if (p_local.z() <= 0) throw(TriangulationCheiralityException()); - } -#endif - - return point; -} - /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. This function is similar to the one @@ -449,6 +480,7 @@ Point3 triangulateLOSTPoint3( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation + * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT * @return Returns a Point3 */ template @@ -456,7 +488,8 @@ Point3 triangulatePoint3( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, bool optimize = false, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation=false) { size_t m = cameras.size(); assert(measurements.size() == m); @@ -464,19 +497,41 @@ Point3 triangulatePoint3( if (m < 2) throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - auto projection_matrices = projectionMatricesFromCameras(cameras); + // Triangulate linearly + Point3 point; + if(use_lost_triangulation) { + // Reduce input noise model to an isotropic noise model using the mean of + // the diagonal. + const double measurementSigma = model ? model->sigmas().mean() : 1e-4; + SharedIsotropic measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); - // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = - undistortMeasurements(cameras, measurements); + // construct poses from cameras. + std::vector poses; + poses.reserve(cameras.size()); + for (const auto& camera : cameras) poses.push_back(camera.pose()); - Point3 point = - triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + // calibrate the measurements to obtain homogenous coordinates in image + // plane. + auto calibratedMeasurements = + calibrateMeasurements(cameras, measurements); - // The n refine using non-linear optimization - if (optimize) + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + } else { + // construct projection matrices from poses & calibration + auto projection_matrices = projectionMatricesFromCameras(cameras); + + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = + undistortMeasurements(cameras, measurements); + + point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + } + + // Then refine using non-linear optimization + if (optimize) { point = triangulateNonlinear(cameras, measurements, point, model); + } #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras @@ -496,9 +551,10 @@ Point3 triangulatePoint3( const CameraSet >& cameras, const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = false) { return triangulatePoint3 > // - (cameras, measurements, rank_tol, optimize, model); + (cameras, measurements, rank_tol, optimize, model, use_lost_triangulation); } struct GTSAM_EXPORT TriangulationParameters { From ed5fa923cfe04ddfed1b9e74da9d268ca8009b41 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 10 Jul 2022 16:49:30 -0700 Subject: [PATCH 28/61] Example to run LOST and compare to DLT --- examples/TriangulationLOSTExample.cpp | 161 ++++++++++++++++++++++++++ 1 file changed, 161 insertions(+) create mode 100644 examples/TriangulationLOSTExample.cpp diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp new file mode 100644 index 000000000..a903c625a --- /dev/null +++ b/examples/TriangulationLOSTExample.cpp @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TriangulationLOSTExample.cpp + * @author Akshay Krishnan + * @brief This example runs triangulation several times using 3 different + * approaches: LOST, DLT, and DLT with optimization. It reports the covariance + * and the runtime for each approach. + * + * @date 2022-07-10 + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static std::mt19937 rng(42); + +void PrintCovarianceStats(const Matrix& mat, const std::string& method) { + Matrix centered = mat.rowwise() - mat.colwise().mean(); + Matrix cov = (centered.adjoint() * centered) / double(mat.rows() - 1); + std::cout << method << " covariance: " << std::endl; + std::cout << cov << std::endl; + std::cout << "Trace sqrt: " << sqrt(cov.trace()) << std::endl << std::endl; +} + +void PrintDuration(const std::chrono::nanoseconds dur, double num_samples, + const std::string& method) { + double nanoseconds = dur.count() / num_samples; + std::cout << "Time taken by " << method << ": " << nanoseconds * 1e-3 + << std::endl; +} + +void GetLargeCamerasDataset(CameraSet>* cameras, + std::vector* poses, Point3* point, + Point2Vector* measurements) { + const double min_xy = -10, max_xy = 10; + const double min_z = -20, max_z = 0; + const int num_cameras = 500; + cameras->reserve(num_cameras); + poses->reserve(num_cameras); + measurements->reserve(num_cameras); + *point = Point3(0.0, 0.0, 10.0); + + std::uniform_real_distribution rand_xy(min_xy, max_xy); + std::uniform_real_distribution rand_z(min_z, max_z); + Cal3_S2 identity_K; + + for (int i = 0; i < num_cameras; ++i) { + Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng)); + Pose3 wTi(Rot3(), wti); + + poses->push_back(wTi); + cameras->emplace_back(wTi, identity_K); + measurements->push_back(cameras->back().project(*point)); + } +} + +void GetSmallCamerasDataset(CameraSet>* cameras, + std::vector* poses, Point3* point, + Point2Vector* measurements) { + Pose3 pose_1; + Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); + Cal3_S2 identity_K; + PinholeCamera camera_1(pose_1, identity_K); + PinholeCamera camera_2(pose_2, identity_K); + + *point = Point3(0, 0, 1); + cameras->push_back(camera_1); + cameras->push_back(camera_2); + *poses = {pose_1, pose_2}; + *measurements = {camera_1.project(*point), camera_2.project(*point)}; +} + +Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements, + const double measurement_sigma) { + std::normal_distribution normal(0.0, measurement_sigma); + + Point2Vector noisy_measurements; + noisy_measurements.reserve(measurements.size()); + for (const auto& p : measurements) { + noisy_measurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); + } + return noisy_measurements; +} + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + CameraSet> cameras; + std::vector poses; + Point3 gt_point; + Point2Vector measurements; + GetLargeCamerasDataset(&cameras, &poses, >_point, &measurements); + // GetSmallCamerasDataset(&cameras, &poses, >_point, &measurements); + const double measurement_sigma = 1e-2; + SharedNoiseModel measurement_noise = + noiseModel::Isotropic::Sigma(2, measurement_sigma); + + const long int num_trials = 1000; + Matrix dlt_errors = Matrix::Zero(num_trials, 3); + Matrix lost_errors = Matrix::Zero(num_trials, 3); + Matrix dlt_opt_errors = Matrix::Zero(num_trials, 3); + + double rank_tol = 1e-9; + boost::shared_ptr calib = boost::make_shared(); + std::chrono::nanoseconds dlt_duration; + std::chrono::nanoseconds dlt_opt_duration; + std::chrono::nanoseconds lost_duration; + std::chrono::nanoseconds lost_tls_duration; + + for (int i = 0; i < num_trials; i++) { + Point2Vector noisy_measurements = + AddNoiseToMeasurements(measurements, measurement_sigma); + + auto lost_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_lost = triangulatePoint3( + cameras, noisy_measurements, rank_tol, false, measurement_noise, true); + lost_duration += std::chrono::high_resolution_clock::now() - lost_start; + + auto dlt_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_dlt = triangulatePoint3( + cameras, noisy_measurements, rank_tol, false, measurement_noise, false); + dlt_duration += std::chrono::high_resolution_clock::now() - dlt_start; + + auto dlt_opt_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_dlt_opt = triangulatePoint3( + cameras, noisy_measurements, rank_tol, true, measurement_noise, false); + dlt_opt_duration += + std::chrono::high_resolution_clock::now() - dlt_opt_start; + + lost_errors.row(i) = *estimate_lost - gt_point; + dlt_errors.row(i) = *estimate_dlt - gt_point; + dlt_opt_errors.row(i) = *estimate_dlt_opt - gt_point; + } + PrintCovarianceStats(lost_errors, "LOST"); + PrintCovarianceStats(dlt_errors, "DLT"); + PrintCovarianceStats(dlt_opt_errors, "DLT_OPT"); + + PrintDuration(lost_duration, num_trials, "LOST"); + PrintDuration(dlt_duration, num_trials, "DLT"); + PrintDuration(dlt_opt_duration, num_trials, "DLT_OPT"); +} \ No newline at end of file From 7d9cf475794e1176d6a31f9b695bd1ade1e27301 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 10 Jul 2022 22:00:18 -0700 Subject: [PATCH 29/61] unit tests pass --- gtsam/geometry/triangulation.cpp | 32 +--------------- gtsam/geometry/triangulation.h | 64 ++++++++++++++++---------------- 2 files changed, 32 insertions(+), 64 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index c2c798d8b..c68fffb82 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -88,35 +88,6 @@ Vector4 triangulateHomogeneousDLT( return v; } -Point3 triangulateDLT(const std::vector& poses, - const Point3Vector& homogenousMeasurements, - double rank_tol) { - // number of cameras - size_t m = poses.size(); - - // Allocate DLT matrix - Matrix A = Matrix::Zero(m * 2, 4); - - for (size_t i = 0; i < m; i++) { - size_t row = i * 2; - const Matrix34 projection = poses[i].matrix().block(0, 0, 3, 4); - const Point3& p = homogenousMeasurements[i]; // to get access to x,y,z of - // the bearing vector - - // build system of equations - A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); - A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1); - } - int rank; - double error; - Vector v; - boost::tie(rank, error, v) = DLT(A, rank_tol); - - if (rank < 3) throw(TriangulationUnderconstrainedException()); - - return Point3(v.head<3>() / v[3]); -} - Point3 triangulateLOST(const std::vector& poses, const Point3Vector& calibratedMeasurements, const SharedIsotropic& measurementNoise) { @@ -140,7 +111,7 @@ Point3 triangulateLOST(const std::vector& poses, const Point3 w_measurement_j = wTj.rotation().rotate(calibratedMeasurements[j]); - double q_i = + const double q_i = w_measurement_i.cross(w_measurement_j).norm() / (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm()); const Matrix23 coefficient_mat = @@ -174,7 +145,6 @@ Point3 triangulateDLT( return Point3(v.head<3>() / v[3]); } -/// /** * Optimize for triangulation * @param graph nonlinear factors for projection diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 9e59a92cb..8a709ccd0 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -414,30 +414,30 @@ inline Point3Vector calibrateMeasurements( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT + * @param use_lost_triangulation whether to use the LOST algorithm instead of + * DLT * @return Returns a Point3 */ -template +template Point3 triangulatePoint3(const std::vector& poses, - boost::shared_ptr sharedCal, - const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = true) { - + boost::shared_ptr sharedCal, + const Point2Vector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = false) { assert(poses.size() == measurements.size()); - if (poses.size() < 2) - throw(TriangulationUnderconstrainedException()); + if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); // Triangulate linearly Point3 point; - if(use_lost_triangulation) { + if (use_lost_triangulation) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma); - // calibrate the measurements to obtain homogenous coordinates in image plane. + // calibrate the measurements to obtain homogenous coordinates in image + // plane. auto calibratedMeasurements = calibrateMeasurementsShared(*sharedCal, measurements); @@ -450,20 +450,20 @@ Point3 triangulatePoint3(const std::vector& poses, auto undistortedMeasurements = undistortMeasurements(*sharedCal, measurements); - point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); } // Then refine using non-linear optimization if (optimize) - point = triangulateNonlinear // + point = triangulateNonlinear // (poses, sharedCal, measurements, point, model); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras - for(const Pose3& pose: poses) { + for (const Pose3& pose : poses) { const Point3& p_local = pose.transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } #endif @@ -480,26 +480,24 @@ Point3 triangulatePoint3(const std::vector& poses, * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT + * @param use_lost_triangulation whether to use the LOST algorithm instead of + * DLT * @return Returns a Point3 */ -template -Point3 triangulatePoint3( - const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation=false) { - +template +Point3 triangulatePoint3(const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = false) { size_t m = cameras.size(); assert(measurements.size() == m); - if (m < 2) - throw(TriangulationUnderconstrainedException()); + if (m < 2) throw(TriangulationUnderconstrainedException()); // Triangulate linearly Point3 point; - if(use_lost_triangulation) { + if (use_lost_triangulation) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; @@ -525,7 +523,8 @@ Point3 triangulatePoint3( auto undistortedMeasurements = undistortMeasurements(cameras, measurements); - point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); } // Then refine using non-linear optimization @@ -535,10 +534,9 @@ Point3 triangulatePoint3( #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras - for(const CAMERA& camera: cameras) { + for (const CAMERA& camera : cameras) { const Point3& p_local = camera.pose().transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } #endif From 223ea468d6109273ccadbd2ec84ee0c0ed75e294 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 18:45:46 -0700 Subject: [PATCH 30/61] change global variable names in test --- gtsam/geometry/tests/testTriangulation.cpp | 240 ++++++++++----------- 1 file changed, 120 insertions(+), 120 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 731558327..f6408fab9 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -38,24 +38,24 @@ using namespace boost::assign; // Some common constants -static const boost::shared_ptr sharedCal = // +static const boost::shared_ptr kSharedCal = // boost::make_shared(1500, 1200, 0.1, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); -static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); -PinholeCamera camera1(pose1, *sharedCal); +static const Pose3 kPose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); +static const PinholeCamera kCamera1(kPose1, *kSharedCal); // create second camera 1 meter to the right of first camera -static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); -PinholeCamera camera2(pose2, *sharedCal); +static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0)); +static const PinholeCamera kCamera2(kPose2, *kSharedCal); // landmark ~5 meters infront of camera -static const Point3 landmark(5, 0.5, 1.2); +static const Point3 kLandmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate -Point2 z1 = camera1.project(landmark); -Point2 z2 = camera2.project(landmark); +static const Point2 kZ1 = kCamera1.project(kLandmark); +static const Point2 kZ2 = kCamera2.project(kLandmark); //****************************************************************************** // Simple test with a well-behaved two camera situation @@ -63,22 +63,22 @@ TEST(triangulation, twoPoses) { vector poses; Point2Vector measurements; - poses += pose1, pose2; - measurements += z1, z2; + poses += kPose1, kPose2; + measurements += kZ1, kZ2; double rank_tol = 1e-9; // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -86,22 +86,22 @@ TEST(triangulation, twoPoses) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } TEST(triangulation, twoCamerasUsingLOST) { CameraSet> cameras; - cameras.push_back(camera1); - cameras.push_back(camera2); + cameras.push_back(kCamera1); + cameras.push_back(kCamera2); - Point2Vector measurements = {z1, z2}; + Point2Vector measurements = {kZ1, kZ2}; SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4); double rank_tol = 1e-9; @@ -111,7 +111,7 @@ TEST(triangulation, twoCamerasUsingLOST) { cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, /*use_lost_triangulation=*/true); - EXPECT(assert_equal(landmark, *actual1, 1e-12)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-12)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -167,18 +167,18 @@ TEST(triangulation, twoPosesCal3DS2) { boost::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); - PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + PinholeCamera camera1Distorted(kPose1, *sharedDistortedCal); - PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + PinholeCamera camera2Distorted(kPose2, *sharedDistortedCal); // 0. Project two landmarks into two cameras and triangulate - Point2 z1Distorted = camera1Distorted.project(landmark); - Point2 z2Distorted = camera2Distorted.project(landmark); + Point2 z1Distorted = camera1Distorted.project(kLandmark); + Point2 z2Distorted = camera2Distorted.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1Distorted, z2Distorted; double rank_tol = 1e-9; @@ -188,14 +188,14 @@ TEST(triangulation, twoPosesCal3DS2) { boost::optional actual1 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -224,18 +224,18 @@ TEST(triangulation, twoPosesFisheye) { boost::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, 0.0001, -0.0003); - PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + PinholeCamera camera1Distorted(kPose1, *sharedDistortedCal); - PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + PinholeCamera camera2Distorted(kPose2, *sharedDistortedCal); // 0. Project two landmarks into two cameras and triangulate - Point2 z1Distorted = camera1Distorted.project(landmark); - Point2 z2Distorted = camera2Distorted.project(landmark); + Point2 z1Distorted = camera1Distorted.project(kLandmark); + Point2 z2Distorted = camera2Distorted.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1Distorted, z2Distorted; double rank_tol = 1e-9; @@ -245,14 +245,14 @@ TEST(triangulation, twoPosesFisheye) { boost::optional actual1 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -277,17 +277,17 @@ TEST(triangulation, twoPosesFisheye) { TEST(triangulation, twoPosesBundler) { boost::shared_ptr bundlerCal = // boost::make_shared(1500, 0.1, 0.2, 640, 480); - PinholeCamera camera1(pose1, *bundlerCal); - PinholeCamera camera2(pose2, *bundlerCal); + PinholeCamera camera1(kPose1, *bundlerCal); + PinholeCamera camera2(kPose2, *bundlerCal); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1, z2; bool optimize = true; @@ -296,7 +296,7 @@ TEST(triangulation, twoPosesBundler) { boost::optional actual = // triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual, 1e-7)); // Add some noise and try again measurements.at(0) += Point2(0.1, 0.5); @@ -313,12 +313,12 @@ TEST(triangulation, fourPoses) { vector poses; Point2Vector measurements; - poses += pose1, pose2; - measurements += z1, z2; + poses += kPose1, kPose2; + measurements += kZ1, kZ2; boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -326,37 +326,37 @@ TEST(triangulation, fourPoses) { measurements.at(1) += Point2(-0.2, 0.3); boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual2, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); poses += pose3; measurements += z3 + Point2(0.1, -0.1); boost::optional triangulated_3cameras = // - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization boost::optional triangulated_3cameras_opt = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); - EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - PinholeCamera camera4(pose4, *sharedCal); + PinholeCamera camera4(pose4, *kSharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); + CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); poses += pose4; measurements += Point2(400, 400); - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationCheiralityException); #endif } @@ -364,33 +364,33 @@ TEST(triangulation, fourPoses) { //****************************************************************************** TEST(triangulation, threePoses_robustNoiseModel) { - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2, pose3; - measurements += z1, z2, z3; + poses += kPose1, kPose2, pose3; + measurements += kZ1, kZ2, z3; // noise free, so should give exactly the landmark boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // Add outlier measurements.at(0) += Point2(100, 120); // very large pixel noise! // now estimate does not match landmark boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); + triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.26m): - EXPECT( (landmark - *actual2).norm() >= 0.2); - EXPECT( (landmark - *actual2).norm() <= 0.5); + EXPECT( (kLandmark - *actual2).norm() >= 0.2); + EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization boost::optional actual3 = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -398,27 +398,27 @@ TEST(triangulation, threePoses_robustNoiseModel) { auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); boost::optional actual4 = triangulatePoint3( - poses, sharedCal, measurements, 1e-9, true, model); + poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! - EXPECT(assert_equal(landmark, *actual4, 0.05)); + EXPECT(assert_equal(kLandmark, *actual4, 0.05)); } //****************************************************************************** TEST(triangulation, fourPoses_robustNoiseModel) { - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1 - measurements += z1, z1, z2, z3; + poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1 + measurements += kZ1, kZ1, kZ2, z3; // noise free, so should give exactly the landmark boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // Add outlier measurements.at(0) += Point2(100, 120); // very large pixel noise! @@ -429,14 +429,14 @@ TEST(triangulation, fourPoses_robustNoiseModel) { // now estimate does not match landmark boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); + triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.17m): - EXPECT( (landmark - *actual2).norm() >= 0.1); - EXPECT( (landmark - *actual2).norm() <= 0.5); + EXPECT( (kLandmark - *actual2).norm() >= 0.1); + EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization boost::optional actual3 = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -444,24 +444,24 @@ TEST(triangulation, fourPoses_robustNoiseModel) { auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); boost::optional actual4 = triangulatePoint3( - poses, sharedCal, measurements, 1e-9, true, model); + poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! - EXPECT(assert_equal(landmark, *actual4, 0.05)); + EXPECT(assert_equal(kLandmark, *actual4, 0.05)); } //****************************************************************************** TEST(triangulation, fourPoses_distinct_Ks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -471,7 +471,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { boost::optional actual = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -480,25 +480,25 @@ TEST(triangulation, fourPoses_distinct_Ks) { boost::optional actual2 = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual2, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); PinholeCamera camera3(pose3, K3); - Point2 z3 = camera3.project(landmark); + Point2 z3 = camera3.project(kLandmark); cameras += camera3; measurements += z3 + Point2(0.1, -0.1); boost::optional triangulated_3cameras = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization boost::optional triangulated_3cameras_opt = triangulatePoint3(cameras, measurements, 1e-9, true); - EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); @@ -506,7 +506,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { PinholeCamera camera4(pose4, K4); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); + CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); cameras += camera4; measurements += Point2(400, 400); @@ -519,15 +519,15 @@ TEST(triangulation, fourPoses_distinct_Ks) { TEST(triangulation, fourPoses_distinct_Ks_distortion) { Cal3DS2 K1(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3DS2 K2(1600, 1300, 0, 650, 440, -.2, 0.05, 0.0002, -0.0001); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -537,22 +537,22 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) { boost::optional actual = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); } //****************************************************************************** TEST(triangulation, outliersAndFarLandmarks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -565,7 +565,7 @@ TEST(triangulation, outliersAndFarLandmarks) { 1.0, false, landmarkDistanceThreshold); // all default except // landmarkDistanceThreshold TriangulationResult actual = triangulateSafe(cameras, measurements, params); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); EXPECT(actual.valid()); landmarkDistanceThreshold = 4; // landmark is farther than that @@ -577,10 +577,10 @@ TEST(triangulation, outliersAndFarLandmarks) { // 3. Add a slightly rotated third camera above with a wrong measurement // (OUTLIER) - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); PinholeCamera camera3(pose3, K3); - Point2 z3 = camera3.project(landmark); + Point2 z3 = camera3.project(kLandmark); cameras += camera3; measurements += z3 + Point2(10, -10); @@ -603,18 +603,18 @@ TEST(triangulation, outliersAndFarLandmarks) { //****************************************************************************** TEST(triangulation, twoIdenticalPoses) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, *sharedCal); + PinholeCamera camera1(kPose1, *kSharedCal); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); + Point2 z1 = camera1.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose1; + poses += kPose1, kPose1; measurements += z1, z1; - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); } @@ -629,7 +629,7 @@ TEST(triangulation, onePose) { poses += Pose3(); measurements += Point2(0, 0); - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); } @@ -745,12 +745,12 @@ TEST(triangulation, twoPoses_sphericalCamera) { std::vector measurements; // Project landmark into two cameras and triangulate - SphericalCamera cam1(pose1); - SphericalCamera cam2(pose2); - Unit3 u1 = cam1.project(landmark); - Unit3 u2 = cam2.project(landmark); + SphericalCamera cam1(kPose1); + SphericalCamera cam2(kPose2); + Unit3 u1 = cam1.project(kLandmark); + Unit3 u2 = cam2.project(kLandmark); - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += u1, u2; CameraSet cameras; @@ -762,25 +762,25 @@ TEST(triangulation, twoPoses_sphericalCamera) { // 1. Test linear triangulation via DLT auto projection_matrices = projectionMatricesFromCameras(cameras); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - EXPECT(assert_equal(landmark, point, 1e-7)); + EXPECT(assert_equal(kLandmark, point, 1e-7)); // 2. Test nonlinear triangulation point = triangulateNonlinear(cameras, measurements, point); - EXPECT(assert_equal(landmark, point, 1e-7)); + EXPECT(assert_equal(kLandmark, point, 1e-7)); // 3. Test simple DLT, now within triangulatePoint3 bool optimize = false; boost::optional actual1 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 4. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 5. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -825,7 +825,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2, 1e-7)); // behind and to the right of PoseB - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += u1, u2; CameraSet cameras; From da78cc202a33f1eb9ba77aed8edbcb2bb7c37311 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 18:47:37 -0700 Subject: [PATCH 31/61] add author --- gtsam/geometry/tests/testTriangulation.cpp | 1 + gtsam/geometry/triangulation.cpp | 1 + gtsam/geometry/triangulation.h | 1 + 3 files changed, 3 insertions(+) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index f6408fab9..0ce67083e 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -15,6 +15,7 @@ * @date July 30th, 2013 * @author Chris Beall (cbeall3) * @author Luca Carlone + * @author Akshay Krishnan */ #include diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index c68fffb82..4a8bb279d 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -14,6 +14,7 @@ * @brief Functions for triangulation * @date July 31, 2013 * @author Chris Beall + * @author Akshay Krishnan */ #include diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 8a709ccd0..1b0970fe2 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -15,6 +15,7 @@ * @date July 31, 2013 * @author Chris Beall * @author Luca Carlone + * @author Akshay Krishnan */ #pragma once From b52aaeab22400f9d496611f0a431466d117ad767 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 19:14:26 -0700 Subject: [PATCH 32/61] update to camelcase names in test --- gtsam/geometry/tests/testTriangulation.cpp | 27 +++++++++++----------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0ce67083e..d9e605d55 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -129,36 +129,35 @@ TEST(triangulation, twoCamerasUsingLOST) { TEST(triangulation, twoCamerasLOSTvsDLT) { // LOST has been shown to preform better when the point is much closer to one // camera compared to another. This unit test checks this configuration. - Cal3_S2 identity_K; - Pose3 pose_1; - Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); + const Cal3_S2 identityK; + const Pose3 pose1; + const Pose3 pose2(Rot3(), Point3(5., 0., -5.)); CameraSet> cameras; - cameras.emplace_back(pose_1, identity_K); - cameras.emplace_back(pose_2, identity_K); + cameras.emplace_back(pose1, identityK); + cameras.emplace_back(pose2, identityK); - Point3 gt_point(0, 0, 1); - Point2 x1_noisy = cameras[0].project(gt_point) + Point2(0.00817, 0.00977); - Point2 x2_noisy = cameras[1].project(gt_point) + Point2(-0.00610, 0.01969); + Point3 landmark(0, 0, 1); + Point2 x1_noisy = cameras[0].project(landmark) + Point2(0.00817, 0.00977); + Point2 x2_noisy = cameras[1].project(landmark) + Point2(-0.00610, 0.01969); Point2Vector measurements = {x1_noisy, x2_noisy}; - double rank_tol = 1e-9; + const double rank_tol = 1e-9; SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); - boost::optional estimate_lost = // + boost::optional estimateLOST = // triangulatePoint3>( cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, /*use_lost_triangulation=*/true); // These values are from a MATLAB implementation. - EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimate_lost, 1e-3)); + EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3)); - boost::optional estimate_dlt = + boost::optional estimateDLT = triangulatePoint3(cameras, measurements, rank_tol, false); // The LOST estimate should have a smaller error. - EXPECT((gt_point - *estimate_lost).norm() <= - (gt_point - *estimate_dlt).norm()); + EXPECT((landmark - *estimateLOST).norm() <= (landmark - *estimateDLT).norm()); } //****************************************************************************** From f347209d4ce4d36db373519d62ebc47b76c67e2b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 19:30:41 -0700 Subject: [PATCH 33/61] more camelcase changes --- gtsam/geometry/tests/testTriangulation.cpp | 16 ++++++------ gtsam/geometry/triangulation.cpp | 19 +++++++------- gtsam/geometry/triangulation.h | 30 ++++++++++------------ 3 files changed, 31 insertions(+), 34 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index d9e605d55..bc843ad75 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -108,10 +108,10 @@ TEST(triangulation, twoCamerasUsingLOST) { // 1. Test simple triangulation, perfect in no noise situation boost::optional actual1 = // - triangulatePoint3>( - cameras, measurements, rank_tol, - /*optimize=*/false, measurementNoise, - /*use_lost_triangulation=*/true); + triangulatePoint3>(cameras, measurements, rank_tol, + /*optimize=*/false, + measurementNoise, + /*useLOST=*/true); EXPECT(assert_equal(kLandmark, *actual1, 1e-12)); // 3. Add some noise and try again: result should be ~ (4.995, @@ -145,10 +145,10 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); boost::optional estimateLOST = // - triangulatePoint3>( - cameras, measurements, rank_tol, - /*optimize=*/false, measurementNoise, - /*use_lost_triangulation=*/true); + triangulatePoint3>(cameras, measurements, rank_tol, + /*optimize=*/false, + measurementNoise, + /*useLOST=*/true); // These values are from a MATLAB implementation. EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3)); diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 4a8bb279d..acb0ddf0e 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -107,20 +107,19 @@ Point3 triangulateLOST(const std::vector& poses, const Point3 d_ij = wTj.translation() - wTi.translation(); - const Point3 w_measurement_i = - wTi.rotation().rotate(calibratedMeasurements[i]); - const Point3 w_measurement_j = - wTj.rotation().rotate(calibratedMeasurements[j]); + const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]); + const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); - const double q_i = - w_measurement_i.cross(w_measurement_j).norm() / - (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm()); - const Matrix23 coefficient_mat = + // Note: Setting q_i = 1.0 gives same results as DLT. + const double q_i = wZi.cross(wZj).norm() / + (measurementNoise->sigma() * d_ij.cross(wZj).norm()); + + const Matrix23 coefficientMat = q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); - A.block<2, 3>(2 * i, 0) << coefficient_mat; - b.block<2, 1>(2 * i, 0) << coefficient_mat * wTi.translation(); + A.block<2, 3>(2 * i, 0) << coefficientMat; + b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation(); } return A.colPivHouseholderQr().solve(b); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1b0970fe2..aa9433484 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -415,8 +415,7 @@ inline Point3Vector calibrateMeasurements( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of - * DLT + * @param useLOST whether to use the LOST algorithm instead of DLT * @return Returns a Point3 */ template @@ -425,13 +424,13 @@ Point3 triangulatePoint3(const std::vector& poses, const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { + const bool useLOST = false) { assert(poses.size() == measurements.size()); if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); // Triangulate linearly Point3 point; - if (use_lost_triangulation) { + if (useLOST) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; @@ -481,7 +480,7 @@ Point3 triangulatePoint3(const std::vector& poses, * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of + * @param useLOST whether to use the LOST algorithm instead of * DLT * @return Returns a Point3 */ @@ -490,7 +489,7 @@ Point3 triangulatePoint3(const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { + const bool useLOST = false) { size_t m = cameras.size(); assert(measurements.size() == m); @@ -498,7 +497,7 @@ Point3 triangulatePoint3(const CameraSet& cameras, // Triangulate linearly Point3 point; - if (use_lost_triangulation) { + if (useLOST) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; @@ -545,15 +544,14 @@ Point3 triangulatePoint3(const CameraSet& cameras, } /// Pinhole-specific version -template -Point3 triangulatePoint3( - const CameraSet >& cameras, - const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { - return triangulatePoint3 > // - (cameras, measurements, rank_tol, optimize, model, use_lost_triangulation); +template +Point3 triangulatePoint3(const CameraSet>& cameras, + const Point2Vector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool useLOST = false) { + return triangulatePoint3> // + (cameras, measurements, rank_tol, optimize, model, useLOST); } struct GTSAM_EXPORT TriangulationParameters { From 897dae4436e9d8ce5b9518c23cca7abe9e5b7f4b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 20:03:02 -0700 Subject: [PATCH 34/61] more renames to camelcase --- examples/TriangulationLOSTExample.cpp | 130 +++++++++++++------------- gtsam/geometry/triangulation.h | 16 ++-- 2 files changed, 72 insertions(+), 74 deletions(-) diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp index a903c625a..a862026e0 100644 --- a/examples/TriangulationLOSTExample.cpp +++ b/examples/TriangulationLOSTExample.cpp @@ -53,24 +53,24 @@ void PrintDuration(const std::chrono::nanoseconds dur, double num_samples, void GetLargeCamerasDataset(CameraSet>* cameras, std::vector* poses, Point3* point, Point2Vector* measurements) { - const double min_xy = -10, max_xy = 10; - const double min_z = -20, max_z = 0; - const int num_cameras = 500; - cameras->reserve(num_cameras); - poses->reserve(num_cameras); - measurements->reserve(num_cameras); + const double minXY = -10, maxXY = 10; + const double minZ = -20, maxZ = 0; + const int nrCameras = 500; + cameras->reserve(nrCameras); + poses->reserve(nrCameras); + measurements->reserve(nrCameras); *point = Point3(0.0, 0.0, 10.0); - std::uniform_real_distribution rand_xy(min_xy, max_xy); - std::uniform_real_distribution rand_z(min_z, max_z); - Cal3_S2 identity_K; + std::uniform_real_distribution rand_xy(minXY, maxXY); + std::uniform_real_distribution rand_z(minZ, maxZ); + Cal3_S2 identityK; - for (int i = 0; i < num_cameras; ++i) { + for (int i = 0; i < nrCameras; ++i) { Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng)); Pose3 wTi(Rot3(), wti); poses->push_back(wTi); - cameras->emplace_back(wTi, identity_K); + cameras->emplace_back(wTi, identityK); measurements->push_back(cameras->back().project(*point)); } } @@ -78,84 +78,82 @@ void GetLargeCamerasDataset(CameraSet>* cameras, void GetSmallCamerasDataset(CameraSet>* cameras, std::vector* poses, Point3* point, Point2Vector* measurements) { - Pose3 pose_1; - Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); - Cal3_S2 identity_K; - PinholeCamera camera_1(pose_1, identity_K); - PinholeCamera camera_2(pose_2, identity_K); + Pose3 pose1; + Pose3 pose2(Rot3(), Point3(5., 0., -5.)); + Cal3_S2 identityK; + PinholeCamera camera1(pose1, identityK); + PinholeCamera camera2(pose2, identityK); *point = Point3(0, 0, 1); - cameras->push_back(camera_1); - cameras->push_back(camera_2); - *poses = {pose_1, pose_2}; - *measurements = {camera_1.project(*point), camera_2.project(*point)}; + cameras->push_back(camera1); + cameras->push_back(camera2); + *poses = {pose1, pose2}; + *measurements = {camera1.project(*point), camera2.project(*point)}; } Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements, - const double measurement_sigma) { - std::normal_distribution normal(0.0, measurement_sigma); + const double measurementSigma) { + std::normal_distribution normal(0.0, measurementSigma); - Point2Vector noisy_measurements; - noisy_measurements.reserve(measurements.size()); + Point2Vector noisyMeasurements; + noisyMeasurements.reserve(measurements.size()); for (const auto& p : measurements) { - noisy_measurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); + noisyMeasurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); } - return noisy_measurements; + return noisyMeasurements; } /* ************************************************************************* */ int main(int argc, char* argv[]) { CameraSet> cameras; std::vector poses; - Point3 gt_point; + Point3 landmark; Point2Vector measurements; - GetLargeCamerasDataset(&cameras, &poses, >_point, &measurements); - // GetSmallCamerasDataset(&cameras, &poses, >_point, &measurements); - const double measurement_sigma = 1e-2; - SharedNoiseModel measurement_noise = - noiseModel::Isotropic::Sigma(2, measurement_sigma); + GetLargeCamerasDataset(&cameras, &poses, &landmark, &measurements); + // GetSmallCamerasDataset(&cameras, &poses, &landmark, &measurements); + const double measurementSigma = 1e-2; + SharedNoiseModel measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); - const long int num_trials = 1000; - Matrix dlt_errors = Matrix::Zero(num_trials, 3); - Matrix lost_errors = Matrix::Zero(num_trials, 3); - Matrix dlt_opt_errors = Matrix::Zero(num_trials, 3); + const long int nrTrials = 1000; + Matrix errorsDLT = Matrix::Zero(nrTrials, 3); + Matrix errorsLOST = Matrix::Zero(nrTrials, 3); + Matrix errorsDLTOpt = Matrix::Zero(nrTrials, 3); double rank_tol = 1e-9; boost::shared_ptr calib = boost::make_shared(); - std::chrono::nanoseconds dlt_duration; - std::chrono::nanoseconds dlt_opt_duration; - std::chrono::nanoseconds lost_duration; - std::chrono::nanoseconds lost_tls_duration; + std::chrono::nanoseconds durationDLT; + std::chrono::nanoseconds durationDLTOpt; + std::chrono::nanoseconds durationLOST; - for (int i = 0; i < num_trials; i++) { - Point2Vector noisy_measurements = - AddNoiseToMeasurements(measurements, measurement_sigma); + for (int i = 0; i < nrTrials; i++) { + Point2Vector noisyMeasurements = + AddNoiseToMeasurements(measurements, measurementSigma); - auto lost_start = std::chrono::high_resolution_clock::now(); - boost::optional estimate_lost = triangulatePoint3( - cameras, noisy_measurements, rank_tol, false, measurement_noise, true); - lost_duration += std::chrono::high_resolution_clock::now() - lost_start; + auto lostStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateLOST = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, false, measurementNoise, true); + durationLOST += std::chrono::high_resolution_clock::now() - lostStart; - auto dlt_start = std::chrono::high_resolution_clock::now(); - boost::optional estimate_dlt = triangulatePoint3( - cameras, noisy_measurements, rank_tol, false, measurement_noise, false); - dlt_duration += std::chrono::high_resolution_clock::now() - dlt_start; + auto dltStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateDLT = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, false, measurementNoise, false); + durationDLT += std::chrono::high_resolution_clock::now() - dltStart; - auto dlt_opt_start = std::chrono::high_resolution_clock::now(); - boost::optional estimate_dlt_opt = triangulatePoint3( - cameras, noisy_measurements, rank_tol, true, measurement_noise, false); - dlt_opt_duration += - std::chrono::high_resolution_clock::now() - dlt_opt_start; + auto dltOptStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateDLTOpt = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, true, measurementNoise, false); + durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart; - lost_errors.row(i) = *estimate_lost - gt_point; - dlt_errors.row(i) = *estimate_dlt - gt_point; - dlt_opt_errors.row(i) = *estimate_dlt_opt - gt_point; + errorsLOST.row(i) = *estimateLOST - landmark; + errorsDLT.row(i) = *estimateDLT - landmark; + errorsDLTOpt.row(i) = *estimateDLTOpt - landmark; } - PrintCovarianceStats(lost_errors, "LOST"); - PrintCovarianceStats(dlt_errors, "DLT"); - PrintCovarianceStats(dlt_opt_errors, "DLT_OPT"); + PrintCovarianceStats(errorsLOST, "LOST"); + PrintCovarianceStats(errorsDLT, "DLT"); + PrintCovarianceStats(errorsDLTOpt, "DLT_OPT"); - PrintDuration(lost_duration, num_trials, "LOST"); - PrintDuration(dlt_duration, num_trials, "DLT"); - PrintDuration(dlt_opt_duration, num_trials, "DLT_OPT"); -} \ No newline at end of file + PrintDuration(durationLOST, nrTrials, "LOST"); + PrintDuration(durationDLT, nrTrials, "DLT"); + PrintDuration(durationDLTOpt, nrTrials, "DLT_OPT"); +} diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index aa9433484..f6c919b24 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -317,10 +317,10 @@ template typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = measurements.size(); - assert(num_meas == cameras.size()); - typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); - for (size_t ii = 0; ii < num_meas; ++ii) { + const size_t nrMeasurements = measurements.size(); + assert(nrMeasurements == cameras.size()); + typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); + for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that // measurements are undistorted. undistortedMeasurements[ii] = @@ -382,10 +382,10 @@ template inline Point3Vector calibrateMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = measurements.size(); - assert(num_meas == cameras.size()); - Point3Vector calibratedMeasurements(num_meas); - for (size_t ii = 0; ii < num_meas; ++ii) { + const size_t nrMeasurements = measurements.size(); + assert(nrMeasurements == cameras.size()); + Point3Vector calibratedMeasurements(nrMeasurements); + for (size_t ii = 0; ii < nrMeasurements; ++ii) { calibratedMeasurements[ii] << cameras[ii].calibration().calibrate(measurements[ii]), 1.0; From a3ee2660e5809ef40dcd5f9a2d56f32cf7b2efa3 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 22:50:58 -0700 Subject: [PATCH 35/61] LOST in python wrapper --- gtsam/geometry/geometry.i | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 517558265..597f6a169 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1129,7 +1129,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, @@ -1151,7 +1152,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, @@ -1173,7 +1175,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, @@ -1195,7 +1198,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Fisheye* sharedCal, const gtsam::Point2Vector& measurements, @@ -1217,7 +1221,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Unified* sharedCal, const gtsam::Point2Vector& measurements, From 01ea31633654a137ed1cc7f74ffac5c2bec37648 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Wed, 13 Jul 2022 08:33:09 -0400 Subject: [PATCH 36/61] Make vector opaque --- python/gtsam/preamble/nonlinear.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index 34b864027..b50059a2e 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,4 +10,6 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ -#include \ No newline at end of file +#include + +PYBIND11_MAKE_OPAQUE(std::vector); From e381a7c9d21d7e63d315ace33612cfe46cbd28e9 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Wed, 13 Jul 2022 23:27:00 -0400 Subject: [PATCH 37/61] Revert pybind stl include --- python/gtsam/preamble/nonlinear.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index b50059a2e..d07a75f6f 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,6 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ -#include - -PYBIND11_MAKE_OPAQUE(std::vector); From b8fb26a0b03f096214c350b57d288d71104aedd7 Mon Sep 17 00:00:00 2001 From: Scott Date: Sat, 16 Jul 2022 15:18:45 -0700 Subject: [PATCH 38/61] Clear ConcurrentMap before loading archive data --- gtsam/base/ConcurrentMap.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index 2d7cbd6db..cf4537d4c 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -113,6 +113,7 @@ private: template void load(Archive& ar, const unsigned int /*version*/) { + this->clear(); // Load into STL container and then fill our map FastVector > map; ar & BOOST_SERIALIZATION_NVP(map); From f332d21dd7a1a3cd5ccced3cb5634fb3b1f8b470 Mon Sep 17 00:00:00 2001 From: Scott Date: Sun, 17 Jul 2022 19:00:24 -0700 Subject: [PATCH 39/61] Add unit test for ConcurrentMap serialization --- gtsam/base/tests/testSerializationBase.cpp | 34 ++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index f7aa97b31..d2f4d5f51 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -18,6 +18,7 @@ #include +#include #include #include #include @@ -106,6 +107,39 @@ TEST (Serialization, matrix_vector) { EXPECT(equalityBinary((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished())); } +/* ************************************************************************* */ +TEST (Serialization, ConcurrentMap) { + + ConcurrentMap map; + + map.insert(make_pair(1, "apple")); + map.insert(make_pair(2, "banana")); + + std::string binaryPath = "saved_map.dat"; + try { + std::ofstream outputStream(binaryPath); + boost::archive::binary_oarchive outputArchive(outputStream); + outputArchive << map; + } catch(...) { + EXPECT(false); + } + + // Verify that the existing map contents are replaced by the archive data + ConcurrentMap mapFromDisk; + mapFromDisk.insert(make_pair(3, "clam")); + EXPECT(mapFromDisk.exists(3)); + try { + std::ifstream ifs(binaryPath); + boost::archive::binary_iarchive inputArchive(ifs); + inputArchive >> mapFromDisk; + } catch(...) { + EXPECT(false); + } + EXPECT(mapFromDisk.exists(1)); + EXPECT(mapFromDisk.exists(2)); + EXPECT(!mapFromDisk.exists(3)); +} + /* ************************************************************************* */ From 3d3703441c60f49abe93314add486d35efeee623 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 19 Jul 2022 14:08:31 -0400 Subject: [PATCH 40/61] wrap custom factor in a separate file --- gtsam/nonlinear/custom.i | 38 +++++++++++++++++++++++++++ matlab/CMakeLists.txt | 1 + python/CMakeLists.txt | 1 + python/gtsam/preamble/custom.h | 12 +++++++++ python/gtsam/preamble/nonlinear.h | 1 + python/gtsam/specializations/custom.h | 12 +++++++++ 6 files changed, 65 insertions(+) create mode 100644 gtsam/nonlinear/custom.i create mode 100644 python/gtsam/preamble/custom.h create mode 100644 python/gtsam/specializations/custom.h diff --git a/gtsam/nonlinear/custom.i b/gtsam/nonlinear/custom.i new file mode 100644 index 000000000..0d195a91b --- /dev/null +++ b/gtsam/nonlinear/custom.i @@ -0,0 +1,38 @@ +//************************************************************************* +// Custom Factor wrapping +//************************************************************************* + +namespace gtsam { + +#include +virtual class CustomFactor : gtsam::NoiseModelFactor { + /* + * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting + * machinery there. This is achieved by adding `gtsam::CustomFactor` to the + * ignore list in `matlab/CMakeLists.txt`. + */ + CustomFactor(); + /* + * Example: + * ``` + * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + * + * if not H is None: + * + * H[0] = J1 # 2-d numpy array for a Jacobian block + * H[1] = J2 + * ... + * return error # 1-d numpy array + * + * cf = CustomFactor(noise_model, keys, error_func) + * ``` + */ + CustomFactor(const gtsam::SharedNoiseModel& noiseModel, + const gtsam::KeyVector& keys, + const gtsam::CustomErrorFunction& errorFunction); + + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); +}; + +} \ No newline at end of file diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index a657c6be7..b83bf1f0f 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -73,6 +73,7 @@ set(interface_files ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i + ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/custom.i ${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i ${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i ${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index c14f02dda..f79800296 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -61,6 +61,7 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i ${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i + ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/custom.i ${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i ${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i diff --git a/python/gtsam/preamble/custom.h b/python/gtsam/preamble/custom.h new file mode 100644 index 000000000..d07a75f6f --- /dev/null +++ b/python/gtsam/preamble/custom.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index d07a75f6f..661215e59 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,3 +10,4 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ +#include diff --git a/python/gtsam/specializations/custom.h b/python/gtsam/specializations/custom.h new file mode 100644 index 000000000..d46ccdc66 --- /dev/null +++ b/python/gtsam/specializations/custom.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file From 31d174d640001c50d531ec5078017a8441d98624 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 19 Jul 2022 14:09:39 -0400 Subject: [PATCH 41/61] add gtsam namespace --- gtsam/nonlinear/CustomFactor.h | 2 -- gtsam/nonlinear/nonlinear.i | 43 +++++----------------------------- 2 files changed, 6 insertions(+), 39 deletions(-) diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 615b5418e..8580a4949 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -19,8 +19,6 @@ #include -using namespace gtsam; - namespace gtsam { using JacobianVector = std::vector; diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index b6d0f31be..c79333236 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -99,11 +99,11 @@ class NonlinearFactorGraph { string dot( const gtsam::Values& values, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, - const GraphvizFormatting& writer = GraphvizFormatting()); + const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting()); void saveGraph( const string& s, const gtsam::Values& values, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, - const GraphvizFormatting& writer = GraphvizFormatting()) const; + const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting()) const; // enabling serialization functionality void serialize() const; @@ -135,37 +135,6 @@ virtual class NoiseModelFactor : gtsam::NonlinearFactor { Vector whitenedError(const gtsam::Values& x) const; }; -#include -virtual class CustomFactor : gtsam::NoiseModelFactor { - /* - * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting - * machinery there. This is achieved by adding `gtsam::CustomFactor` to the - * ignore list in `matlab/CMakeLists.txt`. - */ - CustomFactor(); - /* - * Example: - * ``` - * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): - * - * if not H is None: - * - * H[0] = J1 # 2-d numpy array for a Jacobian block - * H[1] = J2 - * ... - * return error # 1-d numpy array - * - * cf = CustomFactor(noise_model, keys, error_func) - * ``` - */ - CustomFactor(const gtsam::SharedNoiseModel& noiseModel, - const gtsam::KeyVector& keys, - const gtsam::CustomErrorFunction& errorFunction); - - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); -}; - #include class Values { Values(); @@ -554,7 +523,7 @@ virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); BaseOptimizerParameters baseOptimizerParams; - GncLossType lossType; + gtsam::GncLossType lossType; size_t maxIterations; double muStep; double relativeCostTol; @@ -563,12 +532,12 @@ virtual class GncParams { std::vector knownInliers; std::vector knownOutliers; - void setLossType(const GncLossType type); + void setLossType(const gtsam::GncLossType type); void setMaxIterations(const size_t maxIter); void setMuStep(const double step); void setRelativeCostTol(double value); void setWeightsTol(double value); - void setVerbosityGNC(const This::Verbosity value); + void setVerbosityGNC(const gtsam::This::Verbosity value); void setKnownInliers(const std::vector& knownIn); void setKnownOutliers(const std::vector& knownOut); void print(const string& str = "GncParams: ") const; @@ -900,7 +869,7 @@ template , gtsam::imuBias::ConstantBias}> virtual class NonlinearEquality2 : gtsam::NoiseModelFactor { - NonlinearEquality2(Key key1, Key key2, double mu = 1e4); + NonlinearEquality2(gtsam::Key key1, gtsam::Key key2, double mu = 1e4); gtsam::Vector evaluateError(const T& x1, const T& x2); }; From 90723d933edf9bd68135444f1bc70ad678da1aaa Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Tue, 19 Jul 2022 19:32:21 -0400 Subject: [PATCH 42/61] Remove pybind/stl.h again --- python/gtsam/preamble/nonlinear.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index 661215e59..a34e73058 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -9,5 +9,4 @@ * automatic STL binding, such that the raw objects can be accessed in Python. * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. - */ -#include + */ \ No newline at end of file From 07c33c46e526c63149f7fad9c8d755bf0fcaf686 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 20 Jul 2022 18:58:41 -0400 Subject: [PATCH 43/61] Set more swap space on linux since gcc runs out of memory --- .github/workflows/build-python.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 3fc2d662f..f42939bc2 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -112,6 +112,11 @@ jobs: run: | echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" + - name: Set Swap Space + if: runner.os == 'Linux' + uses: pierotofy/set-swap-space@master + with: + swap-size-gb: 6 - name: Build (Linux) if: runner.os == 'Linux' run: | From 12b35b41424d1564162fde59b47b474b23d26678 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 19 Jul 2022 18:59:58 -0400 Subject: [PATCH 44/61] remove pybind stl.h --- python/gtsam/preamble/nonlinear.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index a34e73058..d07a75f6f 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -9,4 +9,4 @@ * automatic STL binding, such that the raw objects can be accessed in Python. * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. - */ \ No newline at end of file + */ From 7eb9a95c5fbf0e60813d69443e2e9ab7dcd50c58 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Jul 2022 05:35:09 -0400 Subject: [PATCH 45/61] minor fixes --- gtsam/nonlinear/NonlinearFactor.cpp | 3 ++- python/gtsam/preamble/nonlinear.h | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 3d572e970..debff54ac 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -167,8 +167,9 @@ boost::shared_ptr NoiseModelFactor::linearize( return GaussianFactor::shared_ptr( new JacobianFactor(terms, b, boost::static_pointer_cast(noiseModel_)->unit())); - else + else { return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); + } } /* ************************************************************************* */ diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index d07a75f6f..56a07cfdd 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,3 +10,5 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ + +#include From 31cfce0b8dfc284fb593c2e27f5c0b9c0f374ae7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Jul 2022 05:15:40 -0400 Subject: [PATCH 46/61] update macos CI env --- .github/workflows/build-macos.yml | 10 +++++----- .github/workflows/build-python.yml | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 462723222..7b7646328 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -19,16 +19,16 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - macOS-10.15-xcode-11.3.1, + macos-11-xcode-13.4.1, ] build_type: [Debug, Release] build_unstable: [ON] include: - - name: macOS-10.15-xcode-11.3.1 - os: macOS-10.15 + - name: macos-11-xcode-13.4.1 + os: macos-11 compiler: xcode - version: "11.3.1" + version: "13.4.1" steps: - name: Checkout @@ -43,7 +43,7 @@ jobs: echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else - sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app + sudo xcode-select -switch /Applications/Xcode.app echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f42939bc2..f80b9362c 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -22,7 +22,7 @@ jobs: ubuntu-18.04-gcc-5, ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9, - macOS-10.15-xcode-11.3.1, + macOS-11-xcode-13.4.1, ubuntu-18.04-gcc-5-tbb, ] @@ -52,10 +52,10 @@ jobs: build_type: Debug python_version: "3" - - name: macOS-10.15-xcode-11.3.1 - os: macOS-10.15 + - name: macOS-11-xcode-13.4.1 + os: macOS-11 compiler: xcode - version: "11.3.1" + version: "13.4.1" - name: ubuntu-18.04-gcc-5-tbb os: ubuntu-18.04 @@ -103,7 +103,7 @@ jobs: echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else - sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app + sudo xcode-select -switch /Applications/Xcode.app echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi From 7edd021956e659f1b2ecba5a630682df1d7ea0d2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Jul 2022 14:00:03 -0400 Subject: [PATCH 47/61] fix copying of test files in Cmake --- python/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index cba206d11..0d63c6c54 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -104,7 +104,8 @@ copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" # Hack to get python test and util files copied every time they are modified file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py") foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) - configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY) + get_filename_component(test_file_name ${test_file} NAME) + configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY) endforeach() file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py") foreach(util_file ${GTSAM_PYTHON_UTIL_FILES}) From aef4ec81857692c507fe0db759e3303d5cc4e565 Mon Sep 17 00:00:00 2001 From: agilemapper <87449851+agilemapper@users.noreply.github.com> Date: Sat, 23 Jul 2022 13:04:42 +0200 Subject: [PATCH 48/61] replace addtogroup with ingroup for the SLAM group --- gtsam/groups.dox | 5 ++++- gtsam/navigation/CombinedImuFactor.h | 4 ++-- gtsam/navigation/ImuFactor.h | 6 +++--- gtsam/nonlinear/PriorFactor.h | 2 +- gtsam/sam/BearingRangeFactor.h | 2 +- gtsam/slam/AntiFactor.h | 2 +- gtsam/slam/BetweenFactor.h | 2 +- gtsam/slam/BoundingConstraint.h | 2 +- gtsam/slam/EssentialMatrixConstraint.h | 2 +- gtsam/slam/GeneralSFMFactor.h | 2 +- gtsam/slam/ProjectionFactor.h | 2 +- gtsam/slam/SmartProjectionPoseFactor.h | 4 ++-- gtsam/slam/SmartProjectionRigFactor.h | 4 ++-- gtsam/slam/StereoFactor.h | 2 +- gtsam/slam/TriangulationFactor.h | 2 +- gtsam_unstable/slam/BetweenFactorEM.h | 2 +- gtsam_unstable/slam/BiasedGPSFactor.h | 2 +- gtsam_unstable/slam/MultiProjectionFactor.h | 2 +- gtsam_unstable/slam/PoseBetweenFactor.h | 2 +- gtsam_unstable/slam/PosePriorFactor.h | 2 +- gtsam_unstable/slam/PoseToPointFactor.h | 2 +- gtsam_unstable/slam/ProjectionFactorPPP.h | 2 +- gtsam_unstable/slam/ProjectionFactorPPPC.h | 2 +- gtsam_unstable/slam/ProjectionFactorRollingShutter.h | 2 +- .../slam/SmartProjectionPoseFactorRollingShutter.h | 4 ++-- gtsam_unstable/slam/SmartRangeFactor.h | 2 +- gtsam_unstable/slam/SmartStereoProjectionFactorPP.h | 4 ++-- gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h | 4 ++-- gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h | 2 +- gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h | 2 +- 30 files changed, 41 insertions(+), 38 deletions(-) diff --git a/gtsam/groups.dox b/gtsam/groups.dox index 6031b33bb..6f7124c42 100644 --- a/gtsam/groups.dox +++ b/gtsam/groups.dox @@ -11,4 +11,7 @@ @} -*/ \ No newline at end of file +\defgroup SLAM Useful SLAM components +@{ @} + +*/ diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 213f5f223..54c5a7dbb 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -123,7 +123,7 @@ public: * it is received from the IMU) so as to avoid costly integration at time of * factor construction. * - * @addtogroup SLAM + * @ingroup SLAM */ class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType { @@ -252,7 +252,7 @@ public: * the correlation between the bias uncertainty and the preintegrated * measurements uncertainty. * - * @addtogroup SLAM + * @ingroup SLAM */ class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6 { diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 740827162..c3b398e22 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -66,7 +66,7 @@ typedef ManifoldPreintegration PreintegrationType; * as soon as it is received from the IMU) so as to avoid costly integration * at time of factor construction. * - * @addtogroup SLAM + * @ingroup SLAM */ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType { @@ -165,7 +165,7 @@ public: * (which are usually slowly varying quantities), which is up to the caller. * See also CombinedImuFactor for a class that does this for you. * - * @addtogroup SLAM + * @ingroup SLAM */ class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5 { @@ -256,7 +256,7 @@ public: /** * ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity. - * @addtogroup SLAM + * @ingroup SLAM */ class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3 { private: diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index a490162ac..c5124a3fe 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -24,7 +24,7 @@ namespace gtsam { /** * A class for a soft prior on any Value type - * @addtogroup SLAM + * @ingroup SLAM */ template class PriorFactor: public NoiseModelFactor1 { diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index 9874760c4..c83009651 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -27,7 +27,7 @@ namespace gtsam { /** * Binary factor for a bearing/range measurement - * @addtogroup SLAM + * @ingroup SLAM */ template ::result_type, diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index 3b559fe79..a0d6a1207 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -26,7 +26,7 @@ namespace gtsam { * A class for downdating an existing factor from a graph. The AntiFactor returns the same * linearized Hessian matrices of the original factor, but with the opposite sign. This effectively * cancels out any affects of the original factor during optimization." - * @addtogroup SLAM + * @ingroup SLAM */ class AntiFactor: public NonlinearFactor { diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index f80462847..fc1dc4481 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -34,7 +34,7 @@ namespace gtsam { /** * A class for a measurement predicted by "between(config[key1],config[key2])" * @tparam VALUE the Value type - * @addtogroup SLAM + * @ingroup SLAM */ template class BetweenFactor: public NoiseModelFactor2 { diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 3d5842fa2..07c765669 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -27,7 +27,7 @@ namespace gtsam { * greater/less than a fixed threshold. The function * will need to have its value function implemented to return * a scalar for comparison. - * @addtogroup SLAM + * @ingroup SLAM */ template struct BoundingConstraint1: public NoiseModelFactor1 { diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 6274be963..91487b522 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -25,7 +25,7 @@ namespace gtsam { /** * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement - * @addtogroup SLAM + * @ingroup SLAM */ class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2 { diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index bfc3a0f78..5cf8d5b75 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -54,7 +54,7 @@ namespace gtsam { /** * Non-linear factor for a constraint derived from a 2D measurement. * The calibration is unknown here compared to GenericProjectionFactor - * @addtogroup SLAM + * @ingroup SLAM */ template class GeneralSFMFactor: public NoiseModelFactor2 { diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 42dba8bd0..e01b9b0b8 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -33,7 +33,7 @@ namespace gtsam { * Non-linear factor for a constraint derived from a 2D measurement. * The calibration is known here. * The main building block for visual SLAM. - * @addtogroup SLAM + * @ingroup SLAM */ template diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index f4c0c73aa..f1403bdb1 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -24,7 +24,7 @@ namespace gtsam { /** * - * @addtogroup SLAM + * @ingroup SLAM * * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally @@ -39,7 +39,7 @@ namespace gtsam { * The factor only constrains poses (variable dimension is 6). * This factor requires that values contains the involved poses (Pose3). * If the calibration should be optimized, as well, use SmartProjectionFactor instead! - * @addtogroup SLAM + * @ingroup SLAM */ template class SmartProjectionPoseFactor diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 149c12928..40af55b2c 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -29,7 +29,7 @@ namespace gtsam { /** * - * @addtogroup SLAM + * @ingroup SLAM * * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating @@ -46,7 +46,7 @@ namespace gtsam { * calibration (i.e., are from the same camera), use SmartProjectionPoseFactor * instead! If the calibration should be optimized, as well, use * SmartProjectionFactor instead! - * @addtogroup SLAM + * @ingroup SLAM */ template class SmartProjectionRigFactor : public SmartProjectionFactor { diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index de084c762..b2fdbf7a4 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -25,7 +25,7 @@ namespace gtsam { /** * A Generic Stereo Factor - * @addtogroup SLAM + * @ingroup SLAM */ template class GenericStereoFactor: public NoiseModelFactor2 { diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index b6da02d55..405436b82 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -27,7 +27,7 @@ namespace gtsam { /** * Non-linear factor for a constraint derived from a 2D measurement. * The calibration and pose are assumed known. - * @addtogroup SLAM + * @ingroup SLAM */ template class TriangulationFactor: public NoiseModelFactor1 { diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 9c19bae8c..ffaa05c7d 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -28,7 +28,7 @@ namespace gtsam { /** * A class for a measurement predicted by "between(config[key1],config[key2])" * @tparam VALUE the Value type - * @addtogroup SLAM + * @ingroup SLAM */ template class BetweenFactorEM: public NonlinearFactor { diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index f2e9d3fa8..5359fac4e 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -25,7 +25,7 @@ namespace gtsam { /** * A class to model GPS measurements, including a bias term which models * common-mode errors and that can be partially corrected if other sensors are used - * @addtogroup SLAM + * @ingroup SLAM */ class BiasedGPSFactor: public NoiseModelFactor2 { diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index afc9e0b18..bd4f64fd7 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -29,7 +29,7 @@ namespace gtsam { /** * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. * i.e. the main building block for visual SLAM. - * @addtogroup SLAM + * @ingroup SLAM */ template class MultiProjectionFactor: public NoiseModelFactor { diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index a6c583418..a55f3a6e3 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -26,7 +26,7 @@ namespace gtsam { /** * A class for a measurement predicted by "between(config[key1],config[key2])" * @tparam POSE the Pose type - * @addtogroup SLAM + * @ingroup SLAM */ template class PoseBetweenFactor: public NoiseModelFactor2 { diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index f657fc443..6668a9b4b 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -23,7 +23,7 @@ namespace gtsam { /** * A class for a soft prior on any Value type - * @addtogroup SLAM + * @ingroup SLAM */ template class PosePriorFactor: public NoiseModelFactor1 { diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index b9b2ad5ce..071d13aec 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -18,7 +18,7 @@ namespace gtsam { /** * A class for a measurement between a pose and a point. - * @addtogroup SLAM + * @ingroup SLAM */ template class PoseToPointFactor : public NoiseModelFactor2 { diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 84adda707..41f79d614 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -28,7 +28,7 @@ namespace gtsam { /** * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. * i.e. the main building block for visual SLAM. - * @addtogroup SLAM + * @ingroup SLAM */ template class ProjectionFactorPPP: public NoiseModelFactor3 { diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 18ee13b9a..ec0a426f9 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -30,7 +30,7 @@ namespace gtsam { /** * Non-linear factor for a constraint derived from a 2D measurement. This factor * estimates the body pose, body-camera transform, 3D landmark, and calibration. - * @addtogroup SLAM + * @ingroup SLAM */ template class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 2aeaa4824..c349fd095 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -38,7 +38,7 @@ namespace gtsam { * define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose * interpolated between A and B by the alpha to project the corresponding * landmark to Point2. - * @addtogroup SLAM + * @ingroup SLAM */ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index ff84fcd16..b2f58f75f 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -25,7 +25,7 @@ namespace gtsam { /** * - * @addtogroup SLAM + * @ingroup SLAM * * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, @@ -39,7 +39,7 @@ namespace gtsam { * shutter model of the camera with given readout time. The factor requires that * values contain (for each pixel observation) two consecutive camera poses from * which the pixel observation pose can be interpolated. - * @addtogroup SLAM + * @ingroup SLAM */ template class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 14e97f6bc..8a2a1d3a8 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -24,7 +24,7 @@ namespace gtsam { /** * Smart factor for range SLAM - * @addtogroup SLAM + * @ingroup SLAM */ class SmartRangeFactor: public NoiseModelFactor { protected: diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index e20241a0e..e1bc01691 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -23,7 +23,7 @@ namespace gtsam { /** * - * @addtogroup SLAM + * @ingroup SLAM * * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, @@ -38,7 +38,7 @@ namespace gtsam { * calibration or the same calibration can be shared by multiple cameras. This * factor requires that values contain the involved poses and extrinsics (both * are Pose3 variables). - * @addtogroup SLAM + * @ingroup SLAM */ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index a46000a68..9e83b7736 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -26,7 +26,7 @@ namespace gtsam { /** * - * @addtogroup SLAM + * @ingroup SLAM * * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, @@ -41,7 +41,7 @@ namespace gtsam { * has its own calibration. * The factor only constrains poses (variable dimension is 6). * This factor requires that values contains the involved poses (Pose3). - * @addtogroup SLAM + * @ingroup SLAM */ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor : public SmartStereoProjectionFactor { diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 956c75999..80d2abf9b 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -29,7 +29,7 @@ namespace gtsam { /** * A class for a measurement predicted by "between(config[key1],config[key2])" * @tparam VALUE the Value type - * @addtogroup SLAM + * @ingroup SLAM */ template class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactor1 ? diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index 2b2edfae9..cce18cc44 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -31,7 +31,7 @@ namespace gtsam { /** * A class for a measurement predicted by "between(config[key1],config[key2])" * @tparam VALUE the Value type - * @addtogroup SLAM + * @ingroup SLAM */ template class TransformBtwRobotsUnaryFactorEM: public NonlinearFactor { From 27951b7b180f7cdfc3300fb9d8d163586fb9f305 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Jul 2022 22:17:48 -0400 Subject: [PATCH 49/61] use python 3.7 --- .github/scripts/python.sh | 2 +- .github/workflows/build-python.yml | 16 ++++++---------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 0855dbc21..b734f5919 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -59,7 +59,7 @@ PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin BUILD_PYBIND="ON" -sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt +$PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt mkdir $GITHUB_WORKSPACE/build cd $GITHUB_WORKSPACE/build diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f80b9362c..d391e571c 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -27,7 +27,7 @@ jobs: ] build_type: [Debug, Release] - python_version: [3] + python_version: [3.7] include: - name: ubuntu-18.04-gcc-5 os: ubuntu-18.04 @@ -44,14 +44,6 @@ jobs: compiler: clang version: "9" - # NOTE temporarily added this as it is a required check. - - name: ubuntu-18.04-clang-9 - os: ubuntu-18.04 - compiler: clang - version: "9" - build_type: Debug - python_version: "3" - - name: macOS-11-xcode-13.4.1 os: macOS-11 compiler: xcode @@ -65,7 +57,11 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 + - name: Set up Python + uses: actions/setup-python@v4 + with: + python-version: '3.7' - name: Install (Linux) if: runner.os == 'Linux' run: | From 4edcb41ad3680a9d4f6ee2f829c8ffd49b2bdba8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Jul 2022 22:19:05 -0400 Subject: [PATCH 50/61] remove redundant KeyVector definition --- gtsam/gtsam.i | 21 --------------------- 1 file changed, 21 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d4e959c3d..2671f0ef7 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -66,27 +66,6 @@ class KeySet { void serialize() const; }; -// Actually a vector -class KeyVector { - KeyVector(); - KeyVector(const gtsam::KeyVector& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t key) const; - - void serialize() const; -}; - // Actually a FastMap class KeyGroupMap { KeyGroupMap(); From 42bab8f3e70eee07a2ae591786f3b2a297b6784a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Jul 2022 22:19:29 -0400 Subject: [PATCH 51/61] use KeyVector for GNC inliers & outliers --- gtsam/nonlinear/GncParams.h | 12 ++++++++---- gtsam/nonlinear/nonlinear.i | 8 ++++---- python/gtsam/preamble/nonlinear.h | 2 -- python/gtsam/specializations/nonlinear.h | 2 +- 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index a7ccb88b7..08ae42336 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -72,8 +72,12 @@ class GTSAM_EXPORT GncParams { double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - std::vector knownInliers = std::vector(); ///< Slots in the factor graph corresponding to measurements that we know are inliers - std::vector knownOutliers = std::vector(); ///< Slots in the factor graph corresponding to measurements that we know are outliers + + // *NOTE* We use KeyVector for inliers and outliers since it is fast + wrapping + ///< Slots in the factor graph corresponding to measurements that we know are inliers + KeyVector knownInliers = KeyVector(); + ///< Slots in the factor graph corresponding to measurements that we know are outliers + KeyVector knownOutliers = KeyVector(); /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -114,7 +118,7 @@ class GTSAM_EXPORT GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures. * */ - void setKnownInliers(const std::vector& knownIn) { + void setKnownInliers(const KeyVector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); } @@ -125,7 +129,7 @@ class GTSAM_EXPORT GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * */ - void setKnownOutliers(const std::vector& knownOut) { + void setKnownOutliers(const KeyVector& knownOut) { for (size_t i = 0; i < knownOut.size(); i++){ knownOutliers.push_back(knownOut[i]); } diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index c79333236..4bb80a0d6 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -529,8 +529,8 @@ virtual class GncParams { double relativeCostTol; double weightsTol; Verbosity verbosity; - std::vector knownInliers; - std::vector knownOutliers; + gtsam::KeyVector knownInliers; + gtsam::KeyVector knownOutliers; void setLossType(const gtsam::GncLossType type); void setMaxIterations(const size_t maxIter); @@ -538,8 +538,8 @@ virtual class GncParams { void setRelativeCostTol(double value); void setWeightsTol(double value); void setVerbosityGNC(const gtsam::This::Verbosity value); - void setKnownInliers(const std::vector& knownIn); - void setKnownOutliers(const std::vector& knownOut); + void setKnownInliers(const gtsam::KeyVector& knownIn); + void setKnownOutliers(const gtsam::KeyVector& knownOut); void print(const string& str = "GncParams: ") const; enum Verbosity { diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index 56a07cfdd..d07a75f6f 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,5 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -#include diff --git a/python/gtsam/specializations/nonlinear.h b/python/gtsam/specializations/nonlinear.h index d46ccdc66..da8842eaf 100644 --- a/python/gtsam/specializations/nonlinear.h +++ b/python/gtsam/specializations/nonlinear.h @@ -9,4 +9,4 @@ * interface, but without the `` copying mechanism. Combined * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. - */ \ No newline at end of file + */ From 1d51c4e64692f0792685d4d50873ba00ef61d238 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 26 Jul 2022 07:54:05 -0400 Subject: [PATCH 52/61] Use new GncParams::IndexVector --- gtsam/nonlinear/GncParams.h | 12 +++++++----- tests/testGncOptimizer.cpp | 22 +++++++++++----------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 08ae42336..e069d5f3d 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -73,11 +73,13 @@ class GTSAM_EXPORT GncParams { double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - // *NOTE* We use KeyVector for inliers and outliers since it is fast + wrapping + //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is globally enabled. + /// Use IndexVector for inliers and outliers since it is fast + wrapping + using IndexVector = FastVector; ///< Slots in the factor graph corresponding to measurements that we know are inliers - KeyVector knownInliers = KeyVector(); + IndexVector knownInliers = IndexVector(); ///< Slots in the factor graph corresponding to measurements that we know are outliers - KeyVector knownOutliers = KeyVector(); + IndexVector knownOutliers = IndexVector(); /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -118,7 +120,7 @@ class GTSAM_EXPORT GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures. * */ - void setKnownInliers(const KeyVector& knownIn) { + void setKnownInliers(const IndexVector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); } @@ -129,7 +131,7 @@ class GTSAM_EXPORT GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * */ - void setKnownOutliers(const KeyVector& knownOut) { + void setKnownOutliers(const IndexVector& knownOut) { for (size_t i = 0; i < knownOut.size(); i++){ knownOutliers.push_back(knownOut[i]); } diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index c3335ce20..15d660114 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -763,7 +763,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // GNC // Note: in difficult instances, we set the odometry measurements to be // inliers, but this problem is simple enought to succeed even without that - // assumption std::vector knownInliers; + // assumption GncParams::IndexVector knownInliers; GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); @@ -784,12 +784,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers (check early stopping // when all measurements are known to be inliers or outliers) { - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -813,11 +813,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers { - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -841,7 +841,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // only known outliers { - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -916,11 +916,11 @@ TEST(GncOptimizer, setWeights) { // initialize weights and also set known inliers/outliers { GncParams gncParams; - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); gncParams.setKnownInliers(knownInliers); gncParams.setKnownOutliers(knownOutliers); From d92ce6ba9afe226370bfdecf95e4b04febc91466 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 26 Jul 2022 13:11:21 -0400 Subject: [PATCH 53/61] break up Python CI steps so reading logs is easier --- .github/scripts/python.sh | 85 +++++++++++++++++++----------- .github/workflows/build-python.yml | 13 ++--- 2 files changed, 60 insertions(+), 38 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 0855dbc21..662c4196e 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -43,46 +43,67 @@ if [ -z ${PYTHON_VERSION+x} ]; then exit 127 fi -PYTHON="python${PYTHON_VERSION}" +export PYTHON="python${PYTHON_VERSION}" -if [[ $(uname) == "Darwin" ]]; then +function install_dependencies() +{ + if [[ $(uname) == "Darwin" ]]; then brew install wget -else + else # Install a system package required by our library sudo apt-get install -y wget libicu-dev python3-pip python3-setuptools -fi + fi -PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin + export PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin -[ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb + [ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb + + $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt +} + +function build() +{ + mkdir $GITHUB_WORKSPACE/build + cd $GITHUB_WORKSPACE/build + + BUILD_PYBIND="ON" + + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \ + -DGTSAM_BUILD_TESTS=OFF \ + -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ + -DGTSAM_USE_QUATERNIONS=OFF \ + -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ + -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ + -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ + -DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \ + -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \ + -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \ + -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \ + -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \ + -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install -BUILD_PYBIND="ON" + # Set to 2 cores so that Actions does not error out during resource provisioning. + make -j2 install -sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt + cd $GITHUB_WORKSPACE/build/python + $PYTHON -m pip install --user . +} -mkdir $GITHUB_WORKSPACE/build -cd $GITHUB_WORKSPACE/build +function test() +{ + cd $GITHUB_WORKSPACE/python/gtsam/tests + $PYTHON -m unittest discover -v +} -cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \ - -DGTSAM_BUILD_TESTS=OFF \ - -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ - -DGTSAM_USE_QUATERNIONS=OFF \ - -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ - -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ - -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ - -DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \ - -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \ - -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \ - -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \ - -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \ - -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install - - -# Set to 2 cores so that Actions does not error out during resource provisioning. -make -j2 install - -cd $GITHUB_WORKSPACE/build/python -$PYTHON -m pip install --user . -cd $GITHUB_WORKSPACE/python/gtsam/tests -$PYTHON -m unittest discover -v +# select between build or test +case $1 in + -d) + install_dependencies + -b) + build + ;; + -t) + test + ;; +esac diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f80b9362c..1e1519da6 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -117,11 +117,12 @@ jobs: uses: pierotofy/set-swap-space@master with: swap-size-gb: 6 - - name: Build (Linux) - if: runner.os == 'Linux' + - name: Install Dependencies run: | - bash .github/scripts/python.sh - - name: Build (macOS) - if: runner.os == 'macOS' + bash .github/scripts/python.sh -d + - name: Build run: | - bash .github/scripts/python.sh + bash .github/scripts/python.sh -b + - name: Test + run: | + bash .github/scripts/python.sh -t From bb5b2be792d422cf19f3da5981bbb29bb67c5c4a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 26 Jul 2022 14:03:21 -0400 Subject: [PATCH 54/61] fix typo --- .github/scripts/python.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 662c4196e..718eee5ea 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -100,6 +100,7 @@ function test() case $1 in -d) install_dependencies + ;; -b) build ;; From a3aaaeb8a76f4fad1707ff834e3db0c06dbf3126 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Jul 2022 01:01:12 -0400 Subject: [PATCH 55/61] undo CI changes --- .github/workflows/build-python.yml | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 314e46545..5a2e554b1 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -27,7 +27,7 @@ jobs: ] build_type: [Debug, Release] - python_version: [3.7] + python_version: [3] include: - name: ubuntu-18.04-gcc-5 os: ubuntu-18.04 @@ -44,6 +44,14 @@ jobs: compiler: clang version: "9" + # NOTE temporarily added this as it is a required check. + - name: ubuntu-18.04-clang-9 + os: ubuntu-18.04 + compiler: clang + version: "9" + build_type: Debug + python_version: "3" + - name: macOS-11-xcode-13.4.1 os: macOS-11 compiler: xcode @@ -58,10 +66,6 @@ jobs: steps: - name: Checkout uses: actions/checkout@v3 - - name: Set up Python - uses: actions/setup-python@v4 - with: - python-version: '3.7' - name: Install (Linux) if: runner.os == 'Linux' run: | From 790403ca6989698f24f6ef2eada4cf0bfe1e3b36 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Jul 2022 10:53:19 -0400 Subject: [PATCH 56/61] revert checkout version --- .github/workflows/build-python.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 5a2e554b1..1e1519da6 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -65,7 +65,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v2 - name: Install (Linux) if: runner.os == 'Linux' run: | From 290759102f42e0231068bb4d662828e31a5ec75f Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Mon, 1 Aug 2022 07:32:33 +1000 Subject: [PATCH 57/61] Fix exports on Windows --- gtsam/nonlinear/GncOptimizer.h | 2 +- gtsam/nonlinear/GncParams.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index eb1c0233f..065278573 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -42,7 +42,7 @@ static double Chi2inv(const double alpha, const size_t dofs) { /* ************************************************************************* */ template -class GTSAM_EXPORT GncOptimizer { +class GncOptimizer { public: /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. typedef typename GncParameters::OptimizerType BaseOptimizer; diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index e069d5f3d..10ac80663 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -39,7 +39,7 @@ enum GncLossType { }; template -class GTSAM_EXPORT GncParams { +class GncParams { public: /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. typedef typename BaseOptimizerParameters::OptimizerType OptimizerType; From 4e6ec772948dcac540df3ff7cbe1a7cf20e1d919 Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Mon, 1 Aug 2022 07:36:36 +1000 Subject: [PATCH 58/61] Link to interface library in case system metis is used --- gtsam_unstable/partition/tests/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/partition/tests/CMakeLists.txt b/gtsam_unstable/partition/tests/CMakeLists.txt index 9e74d9996..d89a1fe98 100644 --- a/gtsam_unstable/partition/tests/CMakeLists.txt +++ b/gtsam_unstable/partition/tests/CMakeLists.txt @@ -1,2 +1,2 @@ set(ignore_test "testNestedDissection.cpp") -gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam") +gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam-if") From 8a68e061cd1b43bf60b5544280ac3d8dec7961d1 Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Mon, 1 Aug 2022 07:48:03 +1000 Subject: [PATCH 59/61] Delete FindBoost.cmake --- cmake/FindBoost.cmake | 2347 ----------------------------------------- 1 file changed, 2347 deletions(-) delete mode 100644 cmake/FindBoost.cmake diff --git a/cmake/FindBoost.cmake b/cmake/FindBoost.cmake deleted file mode 100644 index 5f7cb5ddc..000000000 --- a/cmake/FindBoost.cmake +++ /dev/null @@ -1,2347 +0,0 @@ -# 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. - -# Patch for GTSAM: -#include(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake) -include(FindPackageHandleStandardArgs) - -# 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() - -# 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() -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 - 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() - 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 28b740eb8da7048c27012546038074ad21b623ce Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 1 Aug 2022 15:06:15 -0400 Subject: [PATCH 60/61] fix matlab compilation --- gtsam/basis/basis.i | 2 +- gtsam/geometry/geometry.i | 8 ++++---- gtsam/nonlinear/nonlinear.i | 2 +- matlab/CMakeLists.txt | 1 - 4 files changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index a6c9d87ee..0cd87ba65 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -137,7 +137,7 @@ class FitBasis { static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( const std::map& sequence, const gtsam::noiseModel::Base* model, size_t N); - This::Parameters parameters() const; + gtsam::This::Parameters parameters() const; }; } // namespace gtsam diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 4ebff6c03..1b1169d2f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1126,10 +1126,10 @@ class TriangulationResult { Status status; TriangulationResult(const gtsam::Point3& p); const gtsam::Point3& get() const; - static TriangulationResult Degenerate(); - static TriangulationResult Outlier(); - static TriangulationResult FarPoint(); - static TriangulationResult BehindCamera(); + static gtsam::TriangulationResult Degenerate(); + static gtsam::TriangulationResult Outlier(); + static gtsam::TriangulationResult FarPoint(); + static gtsam::TriangulationResult BehindCamera(); bool valid() const; bool degenerate() const; bool outlier() const; diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 4bb80a0d6..55966ee84 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -701,7 +701,7 @@ class ISAM2Result { /** Getters and Setters for all properties */ size_t getVariablesRelinearized() const; size_t getVariablesReeliminated() const; - FactorIndices getNewFactorsIndices() const; + gtsam::FactorIndices getNewFactorsIndices() const; size_t getCliques() const; double getErrorBefore() const; double getErrorAfter() const; diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index b83bf1f0f..a77db06a4 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -99,7 +99,6 @@ endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) # Record the root dir for gtsam - needed during external builds, e.g., ROS set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR}) -message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") # Tests message(STATUS "Installing Matlab Toolbox") install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig") From 9ec762b76dcd1d956e5acf4d397e756a3aeff636 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 1 Aug 2022 15:06:36 -0400 Subject: [PATCH 61/61] print boost version --- cmake/HandlePrintConfiguration.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake index 43ee5b57b..04d27c27f 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -33,6 +33,7 @@ print_build_options_for_target(gtsam) print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") print_config("Use System Metis" "${GTSAM_USE_SYSTEM_METIS}") +print_config("Using Boost version" "${Boost_VERSION}") if(GTSAM_USE_TBB) print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})")