From 39286f667214a4f7fd88c224943bfb850d5e2a32 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 19 Dec 2021 10:41:07 -0500 Subject: [PATCH 01/64] 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/64] 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/64] 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/64] 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/64] 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/64] 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 fa743ae0ac861897bd8d63f7e7815314ec48d3c4 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 14 May 2022 00:18:04 -0700 Subject: [PATCH 07/64] intialize values that may have between factors --- gtsam/sfm/TranslationRecovery.cpp | 14 ++++++++++---- gtsam/sfm/TranslationRecovery.h | 6 ++++++ 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 810fe7de9..8f1108806 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -134,6 +134,7 @@ void TranslationRecovery::addPrior( Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, std::mt19937 *rng, const Values &initialValues) const { uniform_real_distribution randomVal(-1, 1); // Create a lambda expression that checks whether value exists and randomly @@ -155,14 +156,19 @@ Values TranslationRecovery::initializeRandomly( insert(edge.key1()); insert(edge.key2()); } + for (auto edge : betweenTranslations) { + insert(edge.key1()); + insert(edge.key2()); + } return initial; } Values TranslationRecovery::initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, const Values &initialValues) const { - return initializeRandomly(relativeTranslations, &kRandomNumberGenerator, - initialValues); + return initializeRandomly(relativeTranslations, betweenTranslations, + &kRandomNumberGenerator, initialValues); } Values TranslationRecovery::run( @@ -187,8 +193,8 @@ Values TranslationRecovery::run( &graph); // Uses initial values from params if provided. - Values initial = - initializeRandomly(nonzeroRelativeTranslations, initialValues); + Values initial = initializeRandomly( + nonzeroRelativeTranslations, nonzeroBetweenTranslations, initialValues); // If there are no valid edges, but zero-distance edges exist, initialize one // of the nodes in a connected component of zero-distance edges. diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 7863f5133..6ccc39ddb 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -112,12 +112,15 @@ class TranslationRecovery { * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param rng random number generator * @param intialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, std::mt19937 *rng, const Values &initialValues = Values()) const; /** @@ -125,11 +128,14 @@ class TranslationRecovery { * * @param relativeTranslations unit translation directions between * translations to be estimated + * @param betweenTranslations relative translations (with scale) between 2 + * points in world coordinate frame known a priori. * @param initialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( const std::vector> &relativeTranslations, + const std::vector> &betweenTranslations, const Values &initialValues = Values()) const; /** From 10001f4cea1927d3f67f84770c41faa6d5e1b581 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 25 May 2022 17:52:49 -0400 Subject: [PATCH 08/64] use This for templated type in geometry.i --- gtsam/geometry/geometry.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 8e3c93224..8079e2c2a 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -873,7 +873,7 @@ template class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); - PinholeCamera(const gtsam::PinholeCamera other); + PinholeCamera(const This other); PinholeCamera(const gtsam::Pose3& pose); PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, @@ -942,7 +942,7 @@ template class PinholePose { // Standard Constructors and Named Constructors PinholePose(); - PinholePose(const gtsam::PinholePose other); + PinholePose(const This other); PinholePose(const gtsam::Pose3& pose); PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K); static This Level(const gtsam::Pose2& pose, double height); From f95b449904d55b56f82f4befe962f8cd01eff485 Mon Sep 17 00:00:00 2001 From: "lilong.huang" Date: Thu, 26 May 2022 09:21:29 +0800 Subject: [PATCH 09/64] fix typo issue in comment --- gtsam/geometry/Rot3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 8f8088a74..08213b065 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -292,8 +292,8 @@ pair RQ(const Matrix3& A, OptionalJacobian<3, 9> H) { (*H)(1, 8) = yHb22 * cx; // Next, calculate the derivate of z. We have - // c20 = a10 * cy + a11 * sx * sy + a12 * cx * sy - // c22 = a11 * cx - a12 * sx + // c10 = a10 * cy + a11 * sx * sy + a12 * cx * sy + // c11 = a11 * cx - a12 * sx const auto c10Hx = (A(1, 1) * cx - A(1, 2) * sx) * sy; const auto c10Hy = A(1, 2) * cx * cy + A(1, 1) * cy * sx - A(1, 0) * sy; Vector9 c10HA = c10Hx * H->row(0) + c10Hy * H->row(1); From f443cf30e0c2517da1da26a99ec13460297c7ccf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 May 2022 18:32:19 -0400 Subject: [PATCH 10/64] add docs for HybridFactor --- gtsam/hybrid/HybridFactor.h | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index da103da43..244fba4cc 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -34,6 +34,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, /** * Base class for hybrid probabilistic factors + * * Examples: * - HybridGaussianFactor * - HybridDiscreteFactor @@ -64,13 +65,29 @@ class GTSAM_EXPORT HybridFactor : public Factor { /** Default constructor creates empty factor */ HybridFactor() = default; + /** + * @brief Construct hybrid factor from continuous keys. + * + * @param keys Vector of continuous keys. + */ explicit HybridFactor(const KeyVector &keys); + /** + * @brief Construct hybrid factor from discrete keys. + * + * @param keys Vector of discrete keys. + */ + explicit HybridFactor(const DiscreteKeys &discreteKeys); + + /** + * @brief Construct a new Hybrid Factor object. + * + * @param continuousKeys Vector of keys for continuous variables. + * @param discreteKeys Vector of keys for discrete variables. + */ HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); - explicit HybridFactor(const DiscreteKeys &discreteKeys); - /// Virtual destructor virtual ~HybridFactor() = default; From 841e6b01df76685ca8357305906731d32e2963b4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 May 2022 18:03:52 -0400 Subject: [PATCH 11/64] improved equality checks --- gtsam/hybrid/GaussianMixtureConditional.cpp | 3 ++- gtsam/hybrid/GaussianMixtureFactor.cpp | 3 ++- gtsam/hybrid/GaussianMixtureFactor.h | 10 ++++++---- gtsam/hybrid/HybridConditional.cpp | 3 ++- gtsam/hybrid/HybridDiscreteFactor.cpp | 4 +++- gtsam/hybrid/HybridFactor.cpp | 5 ++++- gtsam/hybrid/HybridGaussianFactor.cpp | 15 ++++++++++----- gtsam/hybrid/HybridGaussianFactor.h | 3 ++- 8 files changed, 31 insertions(+), 15 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixtureConditional.cpp index 726af6d5f..81d1244c2 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.cpp +++ b/gtsam/hybrid/GaussianMixtureConditional.cpp @@ -79,7 +79,8 @@ GaussianMixtureConditional::asGaussianFactorGraphTree() const { /* *******************************************************************************/ bool GaussianMixtureConditional::equals(const HybridFactor &lf, double tol) const { - return BaseFactor::equals(lf, tol); + const This *e = dynamic_cast(&lf); + return e != nullptr && BaseFactor::equals(*e, tol); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 589e5c660..a81cf341d 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -34,7 +34,8 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys, /* *******************************************************************************/ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { - return Base::equals(lf, tol); + const This *e = dynamic_cast(&lf); + return e != nullptr && Base::equals(*e, tol); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index b2fbe4aef..b3569183b 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -11,7 +11,7 @@ /** * @file GaussianMixtureFactor.h - * @brief A factor that is a function of discrete and continuous variables. + * @brief A set of GaussianFactors, indexed by a set of discrete keys. * @author Fan Jiang * @author Varun Agrawal * @author Frank Dellaert @@ -32,9 +32,10 @@ class GaussianFactorGraph; using GaussianFactorVector = std::vector; /** - * @brief A linear factor that is a function of both discrete and continuous - * variables, i.e. P(X, M | Z) where X is the set of continuous variables, M is - * the set of discrete variables and Z is the measurement set. + * @brief Implementation of a discrete conditional mixture factor. + * Implements a joint discrete-continuous factor where the discrete variable + * serves to "select" a mixture component corresponding to a GaussianFactor type + * of measurement. * * Represents the underlying Gaussian Mixture as a Decision Tree, where the set * of discrete variables indexes to the continuous gaussian distribution. @@ -52,6 +53,7 @@ class GaussianMixtureFactor : public HybridFactor { using Factors = DecisionTree; private: + /// Decision tree of Gaussian factors indexed by discrete keys. Factors factors_; /** diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 7d1b72067..e3932308b 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -101,7 +101,8 @@ void HybridConditional::print(const std::string &s, /* ************************************************************************ */ bool HybridConditional::equals(const HybridFactor &other, double tol) const { - return BaseFactor::equals(other, tol); + const This *e = dynamic_cast(&other); + return e != nullptr && BaseFactor::equals(*e, tol); } } // namespace gtsam diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 989127a28..2bdcdee8c 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -38,7 +38,9 @@ HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) /* ************************************************************************ */ bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { - return Base::equals(lf, tol); + const This *e = dynamic_cast(&lf); + // TODO(Varun) How to compare inner_ when they are abstract types? + return e != nullptr && Base::equals(*e, tol); } /* ************************************************************************ */ diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 815ea0415..127c9761c 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -70,7 +70,10 @@ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) /* ************************************************************************ */ bool HybridFactor::equals(const HybridFactor &lf, double tol) const { - return Base::equals(lf, tol); + const This *e = dynamic_cast(&lf); + return e != nullptr && Base::equals(*e, tol) && + isDiscrete_ == e->isDiscrete_ && isContinuous_ == e->isContinuous_ && + isHybrid_ == e->isHybrid_ && nrContinuous_ == e->nrContinuous_; } /* ************************************************************************ */ diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 721cb4cc7..59d20fb79 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -21,18 +21,23 @@ namespace gtsam { +/* ************************************************************************* */ HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) - : Base(other->keys()) { - inner_ = other; -} + : Base(other->keys()), inner_(other) {} +/* ************************************************************************* */ HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()), inner_(boost::make_shared(std::move(jf))) {} -bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { - return false; +/* ************************************************************************* */ +bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const { + const This *e = dynamic_cast(&other); + // TODO(Varun) How to compare inner_ when they are abstract types? + return e != nullptr && Base::equals(*e, tol); } + +/* ************************************************************************* */ void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 8d457e778..6fa83b726 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -25,7 +25,8 @@ namespace gtsam { /** * A HybridGaussianFactor is a layer over GaussianFactor so that we do not have - * a diamond inheritance. + * a diamond inheritance i.e. an extra factor type that inherits from both + * HybridFactor and GaussianFactor. */ class HybridGaussianFactor : public HybridFactor { private: From 7c7b5dd03096c1e1516a964563b0b840a2b77a45 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Jun 2022 23:52:21 -0400 Subject: [PATCH 12/64] Rename files so that everything is HybridX --- gtsam/hybrid/HybridBayesTree.cpp | 5 +- gtsam/hybrid/HybridBayesTree.h | 9 +-- gtsam/hybrid/HybridConditional.h | 4 +- gtsam/hybrid/HybridEliminationTree.cpp | 6 +- gtsam/hybrid/HybridEliminationTree.h | 10 +-- ...raph.cpp => HybridGaussianFactorGraph.cpp} | 24 +++---- ...torGraph.h => HybridGaussianFactorGraph.h} | 33 ++++----- ...{HybridISAM.cpp => HybridGaussianISAM.cpp} | 22 +++--- .../{HybridISAM.h => HybridGaussianISAM.h} | 18 ++--- ...ree.cpp => HybridGaussianJunctionTree.cpp} | 70 +++++++++---------- ...ionTree.h => HybridGaussianJunctionTree.h} | 14 ++-- gtsam/hybrid/hybrid.i | 16 ++--- gtsam/hybrid/tests/Switching.h | 8 +-- .../tests/testGaussianHybridFactorGraph.cpp | 70 ++++++++----------- gtsam/inference/inference.i | 14 ++-- python/gtsam/tests/test_HybridFactorGraph.py | 8 +-- 16 files changed, 161 insertions(+), 170 deletions(-) rename gtsam/hybrid/{GaussianHybridFactorGraph.cpp => HybridGaussianFactorGraph.cpp} (94%) rename gtsam/hybrid/{GaussianHybridFactorGraph.h => HybridGaussianFactorGraph.h} (77%) rename gtsam/hybrid/{HybridISAM.cpp => HybridGaussianISAM.cpp} (84%) rename gtsam/hybrid/{HybridISAM.h => HybridGaussianISAM.h} (76%) rename gtsam/hybrid/{HybridJunctionTree.cpp => HybridGaussianJunctionTree.cpp} (73%) rename gtsam/hybrid/{HybridJunctionTree.h => HybridGaussianJunctionTree.h} (84%) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 400cac5e7..ff07f1817 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -11,7 +11,8 @@ /** * @file HybridBayesTree.cpp - * @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree + * @brief Hybrid Bayes Tree, the result of eliminating a + * HybridGaussianJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ @@ -26,7 +27,7 @@ namespace gtsam { // Instantiate base class template class BayesTreeCliqueBase; + HybridGaussianFactorGraph>; template class BayesTree; /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 610181458..28d9ef809 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -11,15 +11,16 @@ /** * @file HybridBayesTree.h - * @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree + * @brief Hybrid Bayes Tree, the result of eliminating a + * HybridGaussianJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ #pragma once -#include #include +#include #include #include #include @@ -38,10 +39,10 @@ class VectorValues; */ class GTSAM_EXPORT HybridBayesTreeClique : public BayesTreeCliqueBase { + HybridGaussianFactorGraph> { public: typedef HybridBayesTreeClique This; - typedef BayesTreeCliqueBase + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 9592b0c69..887b49f12 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,9 +18,9 @@ #pragma once #include -#include #include #include +#include #include #include #include @@ -34,7 +34,7 @@ namespace gtsam { -class GaussianHybridFactorGraph; +class HybridGaussianFactorGraph; /** * Hybrid Conditional Density diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp index 148685327..c2df2dd60 100644 --- a/gtsam/hybrid/HybridEliminationTree.cpp +++ b/gtsam/hybrid/HybridEliminationTree.cpp @@ -21,17 +21,17 @@ namespace gtsam { // Instantiate base class -template class EliminationTree; +template class EliminationTree; /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const GaussianHybridFactorGraph& factorGraph, + const HybridGaussianFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order) : Base(factorGraph, structure, order) {} /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const GaussianHybridFactorGraph& factorGraph, const Ordering& order) + const HybridGaussianFactorGraph& factorGraph, const Ordering& order) : Base(factorGraph, order) {} /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 04bd9cd35..77a84fea8 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -17,8 +17,8 @@ #pragma once -#include #include +#include #include namespace gtsam { @@ -27,12 +27,12 @@ namespace gtsam { * Elimination Tree type for Hybrid */ class GTSAM_EXPORT HybridEliminationTree - : public EliminationTree { + : public EliminationTree { private: friend class ::EliminationTreeTester; public: - typedef EliminationTree + typedef EliminationTree Base; ///< Base class typedef HybridEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class @@ -49,7 +49,7 @@ class GTSAM_EXPORT HybridEliminationTree * named constructor instead. * @return The elimination tree */ - HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph, + HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order); /** Build the elimination tree of a factor graph. Note that this has to @@ -57,7 +57,7 @@ class GTSAM_EXPORT HybridEliminationTree * this precomputed, use the other constructor instead. * @param factorGraph The factor graph for which to build the elimination tree */ - HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph, + HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph, const Ordering& order); /// @} diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp similarity index 94% rename from gtsam/hybrid/GaussianHybridFactorGraph.cpp rename to gtsam/hybrid/HybridGaussianFactorGraph.cpp index 3354d5b4d..0ac2c2656 100644 --- a/gtsam/hybrid/GaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianHybridFactorGraph.cpp + * @file HybridGaussianFactorGraph.cpp * @brief Hybrid factor graph that uses type erasure * @author Fan Jiang * @author Varun Agrawal @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include @@ -31,7 +30,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -53,7 +53,7 @@ namespace gtsam { -template class EliminateableFactorGraph; +template class EliminateableFactorGraph; /* ************************************************************************ */ static GaussianMixtureFactor::Sum &addGaussian( @@ -78,7 +78,7 @@ static GaussianMixtureFactor::Sum &addGaussian( /* ************************************************************************ */ std::pair -continuousElimination(const GaussianHybridFactorGraph &factors, +continuousElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { GaussianFactorGraph gfg; for (auto &fp : factors) { @@ -103,7 +103,7 @@ continuousElimination(const GaussianHybridFactorGraph &factors, /* ************************************************************************ */ std::pair -discreteElimination(const GaussianHybridFactorGraph &factors, +discreteElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { DiscreteFactorGraph dfg; for (auto &fp : factors) { @@ -129,7 +129,7 @@ discreteElimination(const GaussianHybridFactorGraph &factors, /* ************************************************************************ */ std::pair -hybridElimination(const GaussianHybridFactorGraph &factors, +hybridElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys, const KeySet &continuousSeparator, const std::set &discreteSeparatorSet) { @@ -236,7 +236,7 @@ hybridElimination(const GaussianHybridFactorGraph &factors, } /* ************************************************************************ */ std::pair // -EliminateHybrid(const GaussianHybridFactorGraph &factors, +EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { // NOTE: Because we are in the Conditional Gaussian regime there are only // a few cases: @@ -345,22 +345,22 @@ EliminateHybrid(const GaussianHybridFactorGraph &factors, } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(JacobianFactor &&factor) { +void HybridGaussianFactorGraph::add(JacobianFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr factor) { +void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(DecisionTreeFactor &&factor) { +void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { +void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h similarity index 77% rename from gtsam/hybrid/GaussianHybridFactorGraph.h rename to gtsam/hybrid/HybridGaussianFactorGraph.h index c8e0718fc..6944af510 100644 --- a/gtsam/hybrid/GaussianHybridFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianHybridFactorGraph.h + * @file HybridGaussianFactorGraph.h * @brief Linearized Hybrid factor graph that uses type erasure * @author Fan Jiang * @date Mar 11, 2022 @@ -26,36 +26,37 @@ namespace gtsam { // Forward declarations -class GaussianHybridFactorGraph; +class HybridGaussianFactorGraph; class HybridConditional; class HybridBayesNet; class HybridEliminationTree; class HybridBayesTree; -class HybridJunctionTree; +class HybridGaussianJunctionTree; class DecisionTreeFactor; class JacobianFactor; -/** Main elimination function for GaussianHybridFactorGraph */ +/** Main elimination function for HybridGaussianFactorGraph */ GTSAM_EXPORT std::pair, HybridFactor::shared_ptr> -EliminateHybrid(const GaussianHybridFactorGraph& factors, const Ordering& keys); +EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys); /* ************************************************************************* */ template <> -struct EliminationTraits { +struct EliminationTraits { typedef HybridFactor FactorType; ///< Type of factors in factor graph - typedef GaussianHybridFactorGraph + typedef HybridGaussianFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. - ///< GaussianHybridFactorGraph) + ///< HybridGaussianFactorGraph) typedef HybridConditional ConditionalType; ///< Type of conditionals from elimination typedef HybridBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination typedef HybridEliminationTree - EliminationTreeType; ///< Type of elimination tree - typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree - typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree + EliminationTreeType; ///< Type of elimination tree + typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree + typedef HybridGaussianJunctionTree + JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function static std::pair, boost::shared_ptr > @@ -70,12 +71,12 @@ struct EliminationTraits { * This is the linearized version of a hybrid factor graph. * Everything inside needs to be hybrid factor or hybrid conditional. */ -class GaussianHybridFactorGraph +class HybridGaussianFactorGraph : public FactorGraph, - public EliminateableFactorGraph { + public EliminateableFactorGraph { public: using Base = FactorGraph; - using This = GaussianHybridFactorGraph; ///< this class + using This = HybridGaussianFactorGraph; ///< this class using BaseEliminateable = EliminateableFactorGraph; ///< for elimination using shared_ptr = boost::shared_ptr; ///< shared_ptr to This @@ -86,7 +87,7 @@ class GaussianHybridFactorGraph /// @name Constructors /// @{ - GaussianHybridFactorGraph() = default; + HybridGaussianFactorGraph() = default; /** * Implicit copy/downcast constructor to override explicit template container @@ -94,7 +95,7 @@ class GaussianHybridFactorGraph * `cachedSeparatorMarginal_.reset(*separatorMarginal)` * */ template - GaussianHybridFactorGraph(const FactorGraph& graph) + HybridGaussianFactorGraph(const FactorGraph& graph) : Base(graph) {} /// @} diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp similarity index 84% rename from gtsam/hybrid/HybridISAM.cpp rename to gtsam/hybrid/HybridGaussianISAM.cpp index 476149126..7783a88dd 100644 --- a/gtsam/hybrid/HybridISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -10,16 +10,16 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridISAM.h + * @file HybridGaussianISAM.h * @date March 31, 2022 * @author Fan Jiang * @author Frank Dellaert * @author Richard Roberts */ -#include #include -#include +#include +#include #include #include @@ -31,15 +31,17 @@ namespace gtsam { // template class ISAM; /* ************************************************************************* */ -HybridISAM::HybridISAM() {} +HybridGaussianISAM::HybridGaussianISAM() {} /* ************************************************************************* */ -HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {} +HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree) + : Base(bayesTree) {} /* ************************************************************************* */ -void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors, - HybridBayesTree::Cliques* orphans, - const HybridBayesTree::Eliminate& function) { +void HybridGaussianISAM::updateInternal( + const HybridGaussianFactorGraph& newFactors, + HybridBayesTree::Cliques* orphans, + const HybridBayesTree::Eliminate& function) { // Remove the contaminated part of the Bayes tree BayesNetType bn; const KeySet newFactorKeys = newFactors.keys(); @@ -90,8 +92,8 @@ void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors, } /* ************************************************************************* */ -void HybridISAM::update(const GaussianHybridFactorGraph& newFactors, - const HybridBayesTree::Eliminate& function) { +void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors, + const HybridBayesTree::Eliminate& function) { Cliques orphans; this->updateInternal(newFactors, &orphans, function); } diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridGaussianISAM.h similarity index 76% rename from gtsam/hybrid/HybridISAM.h rename to gtsam/hybrid/HybridGaussianISAM.h index fae7efafd..d5b6271da 100644 --- a/gtsam/hybrid/HybridISAM.h +++ b/gtsam/hybrid/HybridGaussianISAM.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridISAM.h + * @file HybridGaussianISAM.h * @date March 31, 2022 * @author Fan Jiang * @author Frank Dellaert @@ -20,33 +20,33 @@ #pragma once #include -#include #include +#include #include namespace gtsam { -class GTSAM_EXPORT HybridISAM : public ISAM { +class GTSAM_EXPORT HybridGaussianISAM : public ISAM { public: typedef ISAM Base; - typedef HybridISAM This; + typedef HybridGaussianISAM This; typedef boost::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ /** Create an empty Bayes Tree */ - HybridISAM(); + HybridGaussianISAM(); /** Copy constructor */ - HybridISAM(const HybridBayesTree& bayesTree); + HybridGaussianISAM(const HybridBayesTree& bayesTree); /// @} private: /// Internal method that performs the ISAM update. void updateInternal( - const GaussianHybridFactorGraph& newFactors, + const HybridGaussianFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); @@ -58,13 +58,13 @@ class GTSAM_EXPORT HybridISAM : public ISAM { * @param newFactors Factor graph of new factors to add and eliminate. * @param function Elimination function. */ - void update(const GaussianHybridFactorGraph& newFactors, + void update(const HybridGaussianFactorGraph& newFactors, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); }; /// traits template <> -struct traits : public Testable {}; +struct traits : public Testable {}; } // namespace gtsam diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridGaussianJunctionTree.cpp similarity index 73% rename from gtsam/hybrid/HybridJunctionTree.cpp rename to gtsam/hybrid/HybridGaussianJunctionTree.cpp index 8fa3aa033..8ceb7c87b 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridGaussianJunctionTree.cpp @@ -10,14 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridJunctionTree.cpp + * @file HybridGaussianJunctionTree.cpp * @date Mar 11, 2022 * @author Fan Jiang */ -#include #include -#include +#include +#include #include #include @@ -27,19 +27,19 @@ namespace gtsam { // Instantiate base classes template class EliminatableClusterTree; -template class JunctionTree; + HybridGaussianFactorGraph>; +template class JunctionTree; struct HybridConstructorTraversalData { typedef - typename JunctionTree::Node + typename JunctionTree::Node Node; typedef typename JunctionTree::sharedNode sharedNode; + HybridGaussianFactorGraph>::sharedNode sharedNode; HybridConstructorTraversalData* const parentData; - sharedNode myJTNode; + sharedNode junctionTreeNode; FastVector childSymbolicConditionals; FastVector childSymbolicFactors; KeySet discreteKeys; @@ -57,24 +57,24 @@ struct HybridConstructorTraversalData { // On the pre-order pass, before children have been visited, we just set up // a traversal data structure with its own JT node, and create a child // pointer in its parent. - HybridConstructorTraversalData myData = + HybridConstructorTraversalData data = HybridConstructorTraversalData(&parentData); - myData.myJTNode = boost::make_shared(node->key, node->factors); - parentData.myJTNode->addChild(myData.myJTNode); + data.junctionTreeNode = boost::make_shared(node->key, node->factors); + parentData.junctionTreeNode->addChild(data.junctionTreeNode); for (HybridFactor::shared_ptr& f : node->factors) { for (auto& k : f->discreteKeys()) { - myData.discreteKeys.insert(k.first); + data.discreteKeys.insert(k.first); } } - return myData; + return data; } // Post-order visitor function static void ConstructorTraversalVisitorPostAlg2( const boost::shared_ptr& ETreeNode, - const HybridConstructorTraversalData& myData) { + const HybridConstructorTraversalData& data) { // In this post-order visitor, we combine the symbolic elimination results // from the elimination tree children and symbolically eliminate the current // elimination tree node. We then check whether each of our elimination @@ -87,50 +87,50 @@ struct HybridConstructorTraversalData { // Do symbolic elimination for this node SymbolicFactors symbolicFactors; symbolicFactors.reserve(ETreeNode->factors.size() + - myData.childSymbolicFactors.size()); + data.childSymbolicFactors.size()); // Add ETree node factors symbolicFactors += ETreeNode->factors; // Add symbolic factors passed up from children - symbolicFactors += myData.childSymbolicFactors; + symbolicFactors += data.childSymbolicFactors; Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); - SymbolicConditional::shared_ptr myConditional; - SymbolicFactor::shared_ptr mySeparatorFactor; - boost::tie(myConditional, mySeparatorFactor) = + SymbolicConditional::shared_ptr conditional; + SymbolicFactor::shared_ptr separatorFactor; + boost::tie(conditional, separatorFactor) = internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); // Store symbolic elimination results in the parent - myData.parentData->childSymbolicConditionals.push_back(myConditional); - myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); - myData.parentData->discreteKeys.merge(myData.discreteKeys); + data.parentData->childSymbolicConditionals.push_back(conditional); + data.parentData->childSymbolicFactors.push_back(separatorFactor); + data.parentData->discreteKeys.merge(data.discreteKeys); - sharedNode node = myData.myJTNode; + sharedNode node = data.junctionTreeNode; const FastVector& childConditionals = - myData.childSymbolicConditionals; - node->problemSize_ = (int)(myConditional->size() * symbolicFactors.size()); + data.childSymbolicConditionals; + node->problemSize_ = (int)(conditional->size() * symbolicFactors.size()); // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. - const size_t myNrParents = myConditional->nrParents(); + const size_t nrParents = conditional->nrParents(); const size_t nrChildren = node->nrChildren(); assert(childConditionals.size() == nrChildren); // decide which children to merge, as index into children - std::vector nrFrontals = node->nrFrontalsOfChildren(); + std::vector nrChildrenFrontals = node->nrFrontalsOfChildren(); std::vector merge(nrChildren, false); - size_t myNrFrontals = 1; + size_t nrFrontals = 1; for (size_t i = 0; i < nrChildren; i++) { // Check if we should merge the i^th child - if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + if (nrParents + nrFrontals == childConditionals[i]->nrParents()) { const bool myType = - myData.discreteKeys.exists(myConditional->frontals()[0]); + data.discreteKeys.exists(conditional->frontals()[0]); const bool theirType = - myData.discreteKeys.exists(childConditionals[i]->frontals()[0]); + data.discreteKeys.exists(childConditionals[i]->frontals()[0]); if (myType == theirType) { // Increment number of frontal variables - myNrFrontals += nrFrontals[i]; + nrFrontals += nrChildrenFrontals[i]; merge[i] = true; } } @@ -142,7 +142,7 @@ struct HybridConstructorTraversalData { }; /* ************************************************************************* */ -HybridJunctionTree::HybridJunctionTree( +HybridGaussianJunctionTree::HybridGaussianJunctionTree( const HybridEliminationTree& eliminationTree) { gttic(JunctionTree_FromEliminationTree); // Here we rely on the BayesNet having been produced by this elimination tree, @@ -156,7 +156,7 @@ HybridJunctionTree::HybridJunctionTree( // as we go. Gather the created junction tree roots in a dummy Node. typedef HybridConstructorTraversalData Data; Data rootData(0); - rootData.myJTNode = + rootData.junctionTreeNode = boost::make_shared(); // Make a dummy node to gather // the junction tree roots treeTraversal::DepthFirstForest(eliminationTree, rootData, @@ -164,7 +164,7 @@ HybridJunctionTree::HybridJunctionTree( Data::ConstructorTraversalVisitorPostAlg2); // Assign roots from the dummy node - this->addChildrenAsRoots(rootData.myJTNode); + this->addChildrenAsRoots(rootData.junctionTreeNode); // Transfer remaining factors from elimination tree Base::remainingFactors_ = eliminationTree.remainingFactors(); diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridGaussianJunctionTree.h similarity index 84% rename from gtsam/hybrid/HybridJunctionTree.h rename to gtsam/hybrid/HybridGaussianJunctionTree.h index ce9b818e6..314e7daa6 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridGaussianJunctionTree.h @@ -10,15 +10,15 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridJunctionTree.h + * @file HybridGaussianJunctionTree.h * @date Mar 11, 2022 * @author Fan Jiang */ #pragma once -#include #include +#include #include namespace gtsam { @@ -48,12 +48,12 @@ class HybridEliminationTree; * \addtogroup Multifrontal * \nosubgrouping */ -class GTSAM_EXPORT HybridJunctionTree - : public JunctionTree { +class GTSAM_EXPORT HybridGaussianJunctionTree + : public JunctionTree { public: - typedef JunctionTree + typedef JunctionTree Base; ///< Base class - typedef HybridJunctionTree This; ///< This class + typedef HybridGaussianJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class /** @@ -65,7 +65,7 @@ class GTSAM_EXPORT HybridJunctionTree * named constructor instead. * @return The elimination tree */ - HybridJunctionTree(const HybridEliminationTree& eliminationTree); + HybridGaussianJunctionTree(const HybridEliminationTree& eliminationTree); }; } // namespace gtsam diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 612f3abc5..94eaa88b2 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -98,15 +98,15 @@ class HybridBayesNet { const gtsam::DotWriter& writer = gtsam::DotWriter()) const; }; -#include -class GaussianHybridFactorGraph { - GaussianHybridFactorGraph(); - GaussianHybridFactorGraph(const gtsam::HybridBayesNet& bayesNet); +#include +class HybridGaussianFactorGraph { + HybridGaussianFactorGraph(); + HybridGaussianFactorGraph(const gtsam::HybridBayesNet& bayesNet); // Building the graph void push_back(const gtsam::HybridFactor* factor); void push_back(const gtsam::HybridConditional* conditional); - void push_back(const gtsam::GaussianHybridFactorGraph& graph); + void push_back(const gtsam::HybridGaussianFactorGraph& graph); void push_back(const gtsam::HybridBayesNet& bayesNet); void push_back(const gtsam::HybridBayesTree& bayesTree); void push_back(const gtsam::GaussianMixtureFactor* gmm); @@ -120,13 +120,13 @@ class GaussianHybridFactorGraph { const gtsam::HybridFactor* at(size_t i) const; void print(string s = "") const; - bool equals(const gtsam::GaussianHybridFactorGraph& fg, double tol = 1e-9) const; + bool equals(const gtsam::HybridGaussianFactorGraph& fg, double tol = 1e-9) const; gtsam::HybridBayesNet* eliminateSequential(); gtsam::HybridBayesNet* eliminateSequential( gtsam::Ordering::OrderingType type); gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - pair + pair eliminatePartialSequential(const gtsam::Ordering& ordering); gtsam::HybridBayesTree* eliminateMultifrontal(); @@ -134,7 +134,7 @@ class GaussianHybridFactorGraph { gtsam::Ordering::OrderingType type); gtsam::HybridBayesTree* eliminateMultifrontal( const gtsam::Ordering& ordering); - pair + pair eliminatePartialMultifrontal(const gtsam::Ordering& ordering); string dot( diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 77d8182c8..c081b8e87 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include #include @@ -29,10 +29,10 @@ using gtsam::symbol_shorthand::C; using gtsam::symbol_shorthand::X; namespace gtsam { -inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain( +inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( size_t n, std::function keyFunc = X, std::function dKeyFunc = C) { - GaussianHybridFactorGraph hfg; + HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1)); @@ -51,7 +51,7 @@ inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain( } } - return boost::make_shared(std::move(hfg)); + return boost::make_shared(std::move(hfg)); } inline std::pair> makeBinaryOrdering( diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index 853353278..8ff959d74 100644 --- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file testGaussianHybridFactorGraph.cpp + * @file testHybridGaussianFactorGraph.cpp * @date Mar 11, 2022 * @author Fan Jiang */ @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -29,7 +28,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -57,25 +57,11 @@ using gtsam::symbol_shorthand::D; using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::Y; -#ifdef HYBRID_DEBUG -#define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED - -#include // ::signal, ::raise - -#include - -void my_signal_handler(int signum) { - ::signal(signum, SIG_DFL); - std::cout << boost::stacktrace::stacktrace(); - ::raise(SIGABRT); -} -#endif - /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, creation) { +TEST(HybridGaussianFactorGraph, creation) { HybridConditional test; - GaussianHybridFactorGraph hfg; + HybridGaussianFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -91,8 +77,8 @@ TEST(GaussianHybridFactorGraph, creation) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminate) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminate) { + HybridGaussianFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -102,8 +88,8 @@ TEST(GaussianHybridFactorGraph, eliminate) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateMultifrontal) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateMultifrontal) { + HybridGaussianFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -119,8 +105,8 @@ TEST(GaussianHybridFactorGraph, eliminateMultifrontal) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -143,8 +129,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -171,8 +157,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -204,8 +190,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { + HybridGaussianFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -240,8 +226,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) { * This test is about how to assemble the Bayes Tree roots after we do partial * elimination */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { + HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); @@ -290,7 +276,7 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); GTSAM_PRINT(*hbt); @@ -309,7 +295,7 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(GaussianHybridFactorGraph, Switching) { +TEST(HybridGaussianFactorGraph, Switching) { auto N = 12; auto hfg = makeSwitchingChain(N); @@ -381,7 +367,7 @@ TEST(GaussianHybridFactorGraph, Switching) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -417,7 +403,7 @@ TEST(GaussianHybridFactorGraph, Switching) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(GaussianHybridFactorGraph, SwitchingISAM) { +TEST(HybridGaussianFactorGraph, SwitchingISAM) { auto N = 11; auto hfg = makeSwitchingChain(N); @@ -473,7 +459,7 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -499,10 +485,10 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) { } auto new_fg = makeSwitchingChain(12); - auto isam = HybridISAM(*hbt); + auto isam = HybridGaussianISAM(*hbt); { - GaussianHybridFactorGraph factorGraph; + HybridGaussianFactorGraph factorGraph; factorGraph.push_back(new_fg->at(new_fg->size() - 2)); factorGraph.push_back(new_fg->at(new_fg->size() - 1)); isam.update(factorGraph); @@ -512,7 +498,7 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, SwitchingTwoVar) { +TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { const int N = 7; auto hfg = makeSwitchingChain(N, X); hfg->push_back(*makeSwitchingChain(N, Y, D)); @@ -582,7 +568,7 @@ TEST(GaussianHybridFactorGraph, SwitchingTwoVar) { } { HybridBayesNet::shared_ptr hbn; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbn, remaining) = hfg->eliminatePartialSequential(ordering_partial); diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index e8d918a1d..fbdd70fdf 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -9,7 +9,7 @@ namespace gtsam { #include #include #include -#include +#include #include @@ -107,36 +107,36 @@ class Ordering { template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering ColamdConstrainedLast( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering ColamdConstrainedFirst( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Natural(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Metis(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType, const FACTOR_GRAPH& graph); diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 895c9e14e..07a8178e6 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -20,8 +20,8 @@ from gtsam.symbol_shorthand import C, X from gtsam.utils.test_case import GtsamTestCase -class TestGaussianHybridFactorGraph(GtsamTestCase): - """Unit tests for GaussianHybridFactorGraph.""" +class TestHybridGaussianFactorGraph(GtsamTestCase): + """Unit tests for HybridGaussianFactorGraph.""" def test_create(self): """Test contruction of hybrid factor graph.""" @@ -36,13 +36,13 @@ class TestGaussianHybridFactorGraph(GtsamTestCase): gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2]) - hfg = gtsam.GaussianHybridFactorGraph() + hfg = gtsam.HybridGaussianFactorGraph() hfg.add(jf1) hfg.add(jf2) hfg.push_back(gmf) hbn = hfg.eliminateSequential( - gtsam.Ordering.ColamdConstrainedLastGaussianHybridFactorGraph( + gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( hfg, [C(0)])) # print("hbn = ", hbn) From 31ab1a32f3b93c9c7d2e97d95e9bd1c47013ef73 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 00:03:31 -0400 Subject: [PATCH 13/64] update description of GaussianMixtureConditional --- gtsam/hybrid/GaussianMixtureConditional.h | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h index d12fa09d7..a743c00f3 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixtureConditional.h @@ -27,13 +27,19 @@ namespace gtsam { /** - * @brief A conditional of gaussian mixtures indexed by discrete variables. + * @brief A conditional of gaussian mixtures indexed by discrete variables, as + * part of a Bayes Network. * * Represents the conditional density P(X | M, Z) where X is a continuous random - * variable, M is the discrete variable and Z is the set of measurements. + * variable, M is the selection of discrete variables corresponding to a subset + * of the Gaussian variables and Z is parent of this node + * + * The negative log-probability is given by \f$ \sum_{m=1}^M \pi_m \frac{1}{2} + * |Rx - (d - Sy - Tz - ...)|^2 \f$, where \f$ \pi_m \f$ is the mixing + * coefficient. * */ -class GaussianMixtureConditional +class GTSAM_EXPORT GaussianMixtureConditional : public HybridFactor, public Conditional { public: From d2029f3d0339175fbf8b1e4fe0734e9b8e5a37bb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 00:09:38 -0400 Subject: [PATCH 14/64] Add more information on conditionals requirement for GaussianMixture --- gtsam/hybrid/GaussianMixtureConditional.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h index a743c00f3..ab0e2ce43 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixtureConditional.h @@ -68,13 +68,17 @@ class GTSAM_EXPORT GaussianMixtureConditional /// Defaut constructor, mainly for serialization. GaussianMixtureConditional() = default; + /** - * @brief Construct a new GaussianMixtureConditional object + * @brief Construct a new GaussianMixtureConditional object. * * @param continuousFrontals the continuous frontals. * @param continuousParents the continuous parents. * @param discreteParents the discrete parents. Will be placed last. - * @param conditionals a decision tree of GaussianConditionals. + * @param conditionals a decision tree of GaussianConditionals. The number of + * conditionals should be C^(number of discrete parents), where C is the + * cardinality of the DiscreteKeys in discreteParents, since the + * discreteParents will be used as the labels in the decision tree. */ GaussianMixtureConditional(const KeyVector &continuousFrontals, const KeyVector &continuousParents, From c8bf9d350ca9ace6de6218607dabc9357b7340a8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 00:13:02 -0400 Subject: [PATCH 15/64] rename GaussianMixtureConditional to GaussianMixture --- ...ureConditional.cpp => GaussianMixture.cpp} | 28 +++++++++---------- ...MixtureConditional.h => GaussianMixture.h} | 20 ++++++------- gtsam/hybrid/HybridConditional.cpp | 2 +- gtsam/hybrid/HybridConditional.h | 14 +++++----- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 8 +++--- gtsam/hybrid/hybrid.i | 8 +++--- .../tests/testGaussianHybridFactorGraph.cpp | 6 ++-- 7 files changed, 43 insertions(+), 43 deletions(-) rename gtsam/hybrid/{GaussianMixtureConditional.cpp => GaussianMixture.cpp} (80%) rename gtsam/hybrid/{GaussianMixtureConditional.h => GaussianMixture.h} (87%) diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixture.cpp similarity index 80% rename from gtsam/hybrid/GaussianMixtureConditional.cpp rename to gtsam/hybrid/GaussianMixture.cpp index 726af6d5f..1663236f0 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixtureConditional.cpp + * @file GaussianMixture.cpp * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -20,41 +20,41 @@ #include #include -#include +#include #include #include namespace gtsam { -GaussianMixtureConditional::GaussianMixtureConditional( +GaussianMixture::GaussianMixture( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, - const GaussianMixtureConditional::Conditionals &conditionals) + const GaussianMixture::Conditionals &conditionals) : BaseFactor(CollectKeys(continuousFrontals, continuousParents), discreteParents), BaseConditional(continuousFrontals.size()), conditionals_(conditionals) {} /* *******************************************************************************/ -const GaussianMixtureConditional::Conditionals & -GaussianMixtureConditional::conditionals() { +const GaussianMixture::Conditionals & +GaussianMixture::conditionals() { return conditionals_; } /* *******************************************************************************/ -GaussianMixtureConditional GaussianMixtureConditional::FromConditionals( +GaussianMixture GaussianMixture::FromConditionals( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionalsList) { Conditionals dt(discreteParents, conditionalsList); - return GaussianMixtureConditional(continuousFrontals, continuousParents, + return GaussianMixture(continuousFrontals, continuousParents, discreteParents, dt); } /* *******************************************************************************/ -GaussianMixtureConditional::Sum GaussianMixtureConditional::add( - const GaussianMixtureConditional::Sum &sum) const { +GaussianMixture::Sum GaussianMixture::add( + const GaussianMixture::Sum &sum) const { using Y = GaussianFactorGraph; auto add = [](const Y &graph1, const Y &graph2) { auto result = graph1; @@ -66,8 +66,8 @@ GaussianMixtureConditional::Sum GaussianMixtureConditional::add( } /* *******************************************************************************/ -GaussianMixtureConditional::Sum -GaussianMixtureConditional::asGaussianFactorGraphTree() const { +GaussianMixture::Sum +GaussianMixture::asGaussianFactorGraphTree() const { auto lambda = [](const GaussianFactor::shared_ptr &factor) { GaussianFactorGraph result; result.push_back(factor); @@ -77,13 +77,13 @@ GaussianMixtureConditional::asGaussianFactorGraphTree() const { } /* *******************************************************************************/ -bool GaussianMixtureConditional::equals(const HybridFactor &lf, +bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { return BaseFactor::equals(lf, tol); } /* *******************************************************************************/ -void GaussianMixtureConditional::print(const std::string &s, +void GaussianMixture::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s; if (isContinuous()) std::cout << "Continuous "; diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixture.h similarity index 87% rename from gtsam/hybrid/GaussianMixtureConditional.h rename to gtsam/hybrid/GaussianMixture.h index ab0e2ce43..756e7b77b 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixtureConditional.h + * @file GaussianMixture.h * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -39,14 +39,14 @@ namespace gtsam { * coefficient. * */ -class GTSAM_EXPORT GaussianMixtureConditional +class GTSAM_EXPORT GaussianMixture : public HybridFactor, - public Conditional { + public Conditional { public: - using This = GaussianMixtureConditional; - using shared_ptr = boost::shared_ptr; + using This = GaussianMixture; + using shared_ptr = boost::shared_ptr; using BaseFactor = HybridFactor; - using BaseConditional = Conditional; + using BaseConditional = Conditional; /// Alias for DecisionTree of GaussianFactorGraphs using Sum = DecisionTree; @@ -67,10 +67,10 @@ class GTSAM_EXPORT GaussianMixtureConditional /// @{ /// Defaut constructor, mainly for serialization. - GaussianMixtureConditional() = default; + GaussianMixture() = default; /** - * @brief Construct a new GaussianMixtureConditional object. + * @brief Construct a new GaussianMixture object. * * @param continuousFrontals the continuous frontals. * @param continuousParents the continuous parents. @@ -80,7 +80,7 @@ class GTSAM_EXPORT GaussianMixtureConditional * cardinality of the DiscreteKeys in discreteParents, since the * discreteParents will be used as the labels in the decision tree. */ - GaussianMixtureConditional(const KeyVector &continuousFrontals, + GaussianMixture(const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const Conditionals &conditionals); @@ -107,7 +107,7 @@ class GTSAM_EXPORT GaussianMixtureConditional /* print utility */ void print( - const std::string &s = "GaussianMixtureConditional\n", + const std::string &s = "GaussianMixture\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; /// @} diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 7d1b72067..8f95caf34 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -54,7 +54,7 @@ HybridConditional::HybridConditional( /* ************************************************************************ */ HybridConditional::HybridConditional( - boost::shared_ptr gaussianMixture) + boost::shared_ptr gaussianMixture) : BaseFactor(KeyVector(gaussianMixture->keys().begin(), gaussianMixture->keys().begin() + gaussianMixture->nrContinuous()), diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 887b49f12..3ba5da393 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include #include @@ -42,7 +42,7 @@ class HybridGaussianFactorGraph; * As a type-erased variant of: * - DiscreteConditional * - GaussianConditional - * - GaussianMixtureConditional + * - GaussianMixture * * The reason why this is important is that `Conditional` is a CRTP class. * CRTP is static polymorphism such that all CRTP classes, while bearing the @@ -128,16 +128,16 @@ class GTSAM_EXPORT HybridConditional * HybridConditional. */ HybridConditional( - boost::shared_ptr gaussianMixture); + boost::shared_ptr gaussianMixture); /** - * @brief Return HybridConditional as a GaussianMixtureConditional + * @brief Return HybridConditional as a GaussianMixture * - * @return GaussianMixtureConditional::shared_ptr + * @return GaussianMixture::shared_ptr */ - GaussianMixtureConditional::shared_ptr asMixture() { + GaussianMixture::shared_ptr asMixture() { if (!isHybrid()) throw std::invalid_argument("Not a mixture"); - return boost::static_pointer_cast(inner_); + return boost::static_pointer_cast(inner_); } /** diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 0ac2c2656..f4f09701f 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include @@ -207,8 +207,8 @@ hybridElimination(const HybridGaussianFactorGraph &factors, const GaussianMixtureFactor::Factors &separatorFactors = pair.second; - // Create the GaussianMixtureConditional from the conditionals - auto conditional = boost::make_shared( + // Create the GaussianMixture from the conditionals + auto conditional = boost::make_shared( frontalKeys, keysOfSeparator, discreteSeparator, pair.first); // If there are no more continuous parents, then we should create here a @@ -262,7 +262,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, // Because of all these reasons, we carefully consider how to // implement the hybrid factors so that we do not get poor performance. - // The first thing is how to represent the GaussianMixtureConditional. + // The first thing is how to represent the GaussianMixture. // A very possible scenario is that the incoming factors will have different // levels of discrete keys. For example, imagine we are going to eliminate the // fragment: $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 94eaa88b2..bbe1e2400 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -38,16 +38,16 @@ class GaussianMixtureFactor : gtsam::HybridFactor { gtsam::DefaultKeyFormatter) const; }; -#include -class GaussianMixtureConditional : gtsam::HybridFactor { - static GaussianMixtureConditional FromConditionals( +#include +class GaussianMixture : gtsam::HybridFactor { + static GaussianMixture FromConditionals( const gtsam::KeyVector& continuousFrontals, const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, const std::vector& conditionalsList); - void print(string s = "GaussianMixtureConditional\n", + void print(string s = "GaussianMixture\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; }; diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index 8ff959d74..552bb18f5 100644 --- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -65,9 +65,9 @@ TEST(HybridGaussianFactorGraph, creation) { hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); - GaussianMixtureConditional clgc( + GaussianMixture clgc( {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), - GaussianMixtureConditional::Conditionals( + GaussianMixture::Conditionals( C(0), boost::make_shared(X(0), Z_3x1, I_3x3, X(1), I_3x3), From 932e65c7a2d418c711a67019fb8763da298dd97f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 00:39:52 -0400 Subject: [PATCH 16/64] Add GTSAM_EXPORT and Testable traits --- gtsam/hybrid/GaussianMixture.h | 10 +++++++--- gtsam/hybrid/GaussianMixtureFactor.h | 7 ++++++- gtsam/hybrid/HybridDiscreteFactor.h | 7 ++++++- gtsam/hybrid/HybridGaussianFactor.h | 7 ++++++- gtsam/hybrid/HybridGaussianFactorGraph.h | 2 +- 5 files changed, 26 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 756e7b77b..f805c76c6 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -81,9 +81,9 @@ class GTSAM_EXPORT GaussianMixture * discreteParents will be used as the labels in the decision tree. */ GaussianMixture(const KeyVector &continuousFrontals, - const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, - const Conditionals &conditionals); + const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const Conditionals &conditionals); /** * @brief Make a Gaussian Mixture from a list of Gaussian conditionals @@ -125,4 +125,8 @@ class GTSAM_EXPORT GaussianMixture Sum add(const Sum &sum) const; }; +// traits +template <> +struct traits : public Testable {}; + } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index b3569183b..21770f836 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -41,7 +41,7 @@ using GaussianFactorVector = std::vector; * of discrete variables indexes to the continuous gaussian distribution. * */ -class GaussianMixtureFactor : public HybridFactor { +class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { public: using Base = HybridFactor; using This = GaussianMixtureFactor; @@ -113,4 +113,9 @@ class GaussianMixtureFactor : public HybridFactor { Sum add(const Sum &sum) const; }; +// traits +template <> +struct traits : public Testable { +}; + } // namespace gtsam diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index 572ddfbcd..9cbea8170 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -29,7 +29,7 @@ namespace gtsam { * us to hide the implementation of DiscreteFactor and thus avoid diamond * inheritance. */ -class HybridDiscreteFactor : public HybridFactor { +class GTSAM_EXPORT HybridDiscreteFactor : public HybridFactor { private: DiscreteFactor::shared_ptr inner_; @@ -61,4 +61,9 @@ class HybridDiscreteFactor : public HybridFactor { /// Return pointer to the internal discrete factor DiscreteFactor::shared_ptr inner() const { return inner_; } }; + +// traits +template <> +struct traits : public Testable {}; + } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 6fa83b726..2a92c717c 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -28,7 +28,7 @@ namespace gtsam { * a diamond inheritance i.e. an extra factor type that inherits from both * HybridFactor and GaussianFactor. */ -class HybridGaussianFactor : public HybridFactor { +class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { private: GaussianFactor::shared_ptr inner_; @@ -59,4 +59,9 @@ class HybridGaussianFactor : public HybridFactor { GaussianFactor::shared_ptr inner() const { return inner_; } }; + +// traits +template <> +struct traits : public Testable {}; + } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 6944af510..e7dc3dfca 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -71,7 +71,7 @@ struct EliminationTraits { * This is the linearized version of a hybrid factor graph. * Everything inside needs to be hybrid factor or hybrid conditional. */ -class HybridGaussianFactorGraph +class GTSAM_EXPORT HybridGaussianFactorGraph : public FactorGraph, public EliminateableFactorGraph { public: From 2afa6781f80044d2841a4de436e42a5df8129c3b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 09:20:43 -0400 Subject: [PATCH 17/64] fix python test --- python/gtsam/tests/test_HybridFactorGraph.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 07a8178e6..781cfd924 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -49,7 +49,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): self.assertEqual(hbn.size(), 2) mixture = hbn.at(0).inner() - self.assertIsInstance(mixture, gtsam.GaussianMixtureConditional) + self.assertIsInstance(mixture, gtsam.GaussianMixture) self.assertEqual(len(mixture.keys()), 2) discrete_conditional = hbn.at(hbn.size() - 1).inner() From 3e10514846c8d32000ab6d51de296ce5722ad8f5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 22:45:12 -0400 Subject: [PATCH 18/64] if checks for dynamic_cast --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 42 +++++++++------------- 1 file changed, 16 insertions(+), 26 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index f4f09701f..99202ba6a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -82,23 +82,19 @@ continuousElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { GaussianFactorGraph gfg; for (auto &fp : factors) { - auto ptr = boost::dynamic_pointer_cast(fp); - if (ptr) { + if (auto ptr = boost::dynamic_pointer_cast(fp)) { gfg.push_back(ptr->inner()); + } else if (auto p = + boost::static_pointer_cast(fp)->inner()) { + gfg.push_back(boost::static_pointer_cast(p)); } else { - auto p = boost::static_pointer_cast(fp)->inner(); - if (p) { - gfg.push_back(boost::static_pointer_cast(p)); - } else { - // It is an orphan wrapped conditional - } + // It is an orphan wrapped conditional } } auto result = EliminatePreferCholesky(gfg, frontalKeys); - return std::make_pair( - boost::make_shared(result.first), - boost::make_shared(result.second)); + return {boost::make_shared(result.first), + boost::make_shared(result.second)}; } /* ************************************************************************ */ @@ -107,24 +103,20 @@ discreteElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { DiscreteFactorGraph dfg; for (auto &fp : factors) { - auto ptr = boost::dynamic_pointer_cast(fp); - if (ptr) { + if (auto ptr = boost::dynamic_pointer_cast(fp)) { dfg.push_back(ptr->inner()); + } else if (auto p = + boost::static_pointer_cast(fp)->inner()) { + dfg.push_back(boost::static_pointer_cast(p)); } else { - auto p = boost::static_pointer_cast(fp)->inner(); - if (p) { - dfg.push_back(boost::static_pointer_cast(p)); - } else { - // It is an orphan wrapper - } + // It is an orphan wrapper } } auto result = EliminateDiscrete(dfg, frontalKeys); - return std::make_pair( - boost::make_shared(result.first), - boost::make_shared(result.second)); + return {boost::make_shared(result.first), + boost::make_shared(result.second)}; } /* ************************************************************************ */ @@ -146,13 +138,11 @@ hybridElimination(const HybridGaussianFactorGraph &factors, for (auto &f : factors) { if (f->isHybrid()) { - auto cgmf = boost::dynamic_pointer_cast(f); - if (cgmf) { + if (auto cgmf = boost::dynamic_pointer_cast(f)) { sum = cgmf->add(sum); } - auto gm = boost::dynamic_pointer_cast(f); - if (gm) { + if (auto gm = boost::dynamic_pointer_cast(f)) { sum = gm->asMixture()->add(sum); } From dd877479fa7a3fc124435408afcf186ccc46293e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 22:47:45 -0400 Subject: [PATCH 19/64] separate out summing of frontals into separate function --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 83 ++++++++++++---------- 1 file changed, 46 insertions(+), 37 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 99202ba6a..3fa6e3e6b 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -76,6 +76,51 @@ static GaussianMixtureFactor::Sum &addGaussian( return sum; } +/* ************************************************************************ */ +GaussianMixtureFactor::Sum sumFrontals( + const HybridGaussianFactorGraph &factors) { + // sum out frontals, this is the factor on the separator + gttic(sum); + + GaussianMixtureFactor::Sum sum; + std::vector deferredFactors; + + for (auto &f : factors) { + if (f->isHybrid()) { + if (auto cgmf = boost::dynamic_pointer_cast(f)) { + sum = cgmf->add(sum); + } + + if (auto gm = boost::dynamic_pointer_cast(f)) { + sum = gm->asMixture()->add(sum); + } + + } else if (f->isContinuous()) { + deferredFactors.push_back( + boost::dynamic_pointer_cast(f)->inner()); + } else { + // We need to handle the case where the object is actually an + // BayesTreeOrphanWrapper! + auto orphan = boost::dynamic_pointer_cast< + BayesTreeOrphanWrapper>(f); + if (!orphan) { + auto &fr = *f; + throw std::invalid_argument( + std::string("factor is discrete in continuous elimination") + + typeid(fr).name()); + } + } + } + + for (auto &f : deferredFactors) { + sum = addGaussian(sum, f); + } + + gttoc(sum); + + return sum; +} + /* ************************************************************************ */ std::pair continuousElimination(const HybridGaussianFactorGraph &factors, @@ -131,43 +176,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, discreteSeparatorSet.end()); // sum out frontals, this is the factor on the separator - gttic(sum); - - GaussianMixtureFactor::Sum sum; - std::vector deferredFactors; - - for (auto &f : factors) { - if (f->isHybrid()) { - if (auto cgmf = boost::dynamic_pointer_cast(f)) { - sum = cgmf->add(sum); - } - - if (auto gm = boost::dynamic_pointer_cast(f)) { - sum = gm->asMixture()->add(sum); - } - - } else if (f->isContinuous()) { - deferredFactors.push_back( - boost::dynamic_pointer_cast(f)->inner()); - } else { - // We need to handle the case where the object is actually an - // BayesTreeOrphanWrapper! - auto orphan = boost::dynamic_pointer_cast< - BayesTreeOrphanWrapper>(f); - if (!orphan) { - auto &fr = *f; - throw std::invalid_argument( - std::string("factor is discrete in continuous elimination") + - typeid(fr).name()); - } - } - } - - for (auto &f : deferredFactors) { - sum = addGaussian(sum, f); - } - - gttoc(sum); + GaussianMixtureFactor::Sum sum = sumFrontals(factors); using EliminationPair = GaussianFactorGraph::EliminationResult; From 92176db645bed9374cf3a6fda677fbc8b59d31de Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 22:50:44 -0400 Subject: [PATCH 20/64] add comments --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 3fa6e3e6b..c1251309c 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -183,6 +183,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, KeyVector keysOfEliminated; // Not the ordering KeyVector keysOfSeparator; // TODO(frank): Is this just (keys - ordering)? + // This is the elimination method on the leaf nodes auto eliminate = [&](const GaussianFactorGraph &graph) -> GaussianFactorGraph::EliminationResult { if (graph.empty()) { @@ -200,8 +201,10 @@ hybridElimination(const HybridGaussianFactorGraph &factors, return result; }; + // Perform elimination! DecisionTree eliminationResults(sum, eliminate); + // Separate out decision tree into conditionals and remaining factors. auto pair = unzip(eliminationResults); const GaussianMixtureFactor::Factors &separatorFactors = pair.second; From b47cd9d97bec48b0c4598a61dcda16b407d11c0f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 22:55:46 -0400 Subject: [PATCH 21/64] update GaussianMixture docstring --- gtsam/hybrid/GaussianMixture.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index f805c76c6..e85506715 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -34,9 +34,10 @@ namespace gtsam { * variable, M is the selection of discrete variables corresponding to a subset * of the Gaussian variables and Z is parent of this node * - * The negative log-probability is given by \f$ \sum_{m=1}^M \pi_m \frac{1}{2} - * |Rx - (d - Sy - Tz - ...)|^2 \f$, where \f$ \pi_m \f$ is the mixing - * coefficient. + * The probability P(x|y,z,...) is proportional to + * \f$ \sum_i k_i \exp - \frac{1}{2} |R_i x - (d_i - S_i y - T_i z - ...)|^2 \f$ + * where i indexes the components and k_i is a component-wise normalization + * constant. * */ class GTSAM_EXPORT GaussianMixture From eeecb27f14aca16f38c46821f5a5f2f16a512ef9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 3 Jun 2022 13:29:05 -0400 Subject: [PATCH 22/64] rename back to HybridJunctionTree --- gtsam/hybrid/HybridBayesTree.cpp | 2 +- gtsam/hybrid/HybridBayesTree.h | 2 +- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 +- gtsam/hybrid/HybridGaussianFactorGraph.h | 4 ++-- ...ridGaussianJunctionTree.cpp => HybridJunctionTree.cpp} | 6 +++--- ...{HybridGaussianJunctionTree.h => HybridJunctionTree.h} | 8 ++++---- 6 files changed, 12 insertions(+), 12 deletions(-) rename gtsam/hybrid/{HybridGaussianJunctionTree.cpp => HybridJunctionTree.cpp} (97%) rename gtsam/hybrid/{HybridGaussianJunctionTree.h => HybridJunctionTree.h} (91%) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index ff07f1817..d65270f91 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -12,7 +12,7 @@ /** * @file HybridBayesTree.cpp * @brief Hybrid Bayes Tree, the result of eliminating a - * HybridGaussianJunctionTree + * HybridJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 28d9ef809..0b89ca8c4 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -12,7 +12,7 @@ /** * @file HybridBayesTree.h * @brief Hybrid Bayes Tree, the result of eliminating a - * HybridGaussianJunctionTree + * HybridJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index c1251309c..88730cae9 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index e7dc3dfca..0188aa652 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -31,7 +31,7 @@ class HybridConditional; class HybridBayesNet; class HybridEliminationTree; class HybridBayesTree; -class HybridGaussianJunctionTree; +class HybridJunctionTree; class DecisionTreeFactor; class JacobianFactor; @@ -55,7 +55,7 @@ struct EliminationTraits { typedef HybridEliminationTree EliminationTreeType; ///< Type of elimination tree typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree - typedef HybridGaussianJunctionTree + typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function static std::pair, diff --git a/gtsam/hybrid/HybridGaussianJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp similarity index 97% rename from gtsam/hybrid/HybridGaussianJunctionTree.cpp rename to gtsam/hybrid/HybridJunctionTree.cpp index 8ceb7c87b..7725742cf 100644 --- a/gtsam/hybrid/HybridGaussianJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -10,14 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridGaussianJunctionTree.cpp + * @file HybridJunctionTree.cpp * @date Mar 11, 2022 * @author Fan Jiang */ #include #include -#include +#include #include #include @@ -142,7 +142,7 @@ struct HybridConstructorTraversalData { }; /* ************************************************************************* */ -HybridGaussianJunctionTree::HybridGaussianJunctionTree( +HybridJunctionTree::HybridJunctionTree( const HybridEliminationTree& eliminationTree) { gttic(JunctionTree_FromEliminationTree); // Here we rely on the BayesNet having been produced by this elimination tree, diff --git a/gtsam/hybrid/HybridGaussianJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h similarity index 91% rename from gtsam/hybrid/HybridGaussianJunctionTree.h rename to gtsam/hybrid/HybridJunctionTree.h index 314e7daa6..cad1e15a1 100644 --- a/gtsam/hybrid/HybridGaussianJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridGaussianJunctionTree.h + * @file HybridJunctionTree.h * @date Mar 11, 2022 * @author Fan Jiang */ @@ -48,12 +48,12 @@ class HybridEliminationTree; * \addtogroup Multifrontal * \nosubgrouping */ -class GTSAM_EXPORT HybridGaussianJunctionTree +class GTSAM_EXPORT HybridJunctionTree : public JunctionTree { public: typedef JunctionTree Base; ///< Base class - typedef HybridGaussianJunctionTree This; ///< This class + typedef HybridJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class /** @@ -65,7 +65,7 @@ class GTSAM_EXPORT HybridGaussianJunctionTree * named constructor instead. * @return The elimination tree */ - HybridGaussianJunctionTree(const HybridEliminationTree& eliminationTree); + HybridJunctionTree(const HybridEliminationTree& eliminationTree); }; } // namespace gtsam From 8482cee10b66f8bcc7c3674235c6fbbcb07373e0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 3 Jun 2022 13:53:25 -0400 Subject: [PATCH 23/64] improvements made during the old hybrid scheme implementation --- gtsam/discrete/DecisionTree-inl.h | 9 +++------ gtsam/discrete/DecisionTree.h | 2 +- gtsam/discrete/DiscreteFactorGraph.h | 4 ++++ gtsam/inference/FactorGraph.h | 2 +- gtsam/inference/Ordering.cpp | 11 +++++++++++ gtsam/inference/Ordering.h | 18 +++++++++++++++++- gtsam/inference/tests/testOrdering.cpp | 26 ++++++++++++++++++++++++++ gtsam/linear/GaussianBayesNet.h | 2 +- gtsam/linear/GaussianConditional.cpp | 4 ++-- gtsam/linear/GaussianConditional.h | 1 + 10 files changed, 67 insertions(+), 12 deletions(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 99f29b8e5..f72743206 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -39,10 +39,10 @@ #include #include -using boost::assign::operator+=; - namespace gtsam { + using boost::assign::operator+=; + /****************************************************************************/ // Node /****************************************************************************/ @@ -291,10 +291,7 @@ namespace gtsam { } os << "\"" << this->id() << "\" -> \"" << branch->id() << "\""; - if (B == 2) { - if (i == 0) os << " [style=dashed]"; - if (i > 1) os << " [style=bold]"; - } + if (B == 2 && i == 0) os << " [style=dashed]"; os << std::endl; branch->dot(os, labelFormatter, valueFormatter, showZero); } diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 1f45d320b..7449ae995 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -220,7 +220,7 @@ namespace gtsam { /// @{ /// Make virtual - virtual ~DecisionTree() {} + virtual ~DecisionTree() = default; /// Check if tree is empty. bool empty() const { return !root_; } diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index f962b1802..bfa57e7db 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -209,6 +209,10 @@ class GTSAM_EXPORT DiscreteFactorGraph /// @} }; // \ DiscreteFactorGraph +std::pair // +EliminateForMPE(const DiscreteFactorGraph& factors, + const Ordering& frontalKeys); + /// traits template <> struct traits : public Testable {}; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index afea63da8..101134c83 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -361,7 +361,7 @@ class FactorGraph { * less than the original, factors at the end will be removed. If the new * size is larger than the original, null factors will be appended. */ - void resize(size_t size) { factors_.resize(size); } + virtual void resize(size_t size) { factors_.resize(size); } /** delete factor without re-arranging indexes by inserting a nullptr pointer */ diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 2ac2c0dde..7b7ff1403 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -283,6 +283,17 @@ void Ordering::print(const std::string& str, cout.flush(); } +/* ************************************************************************* */ +Ordering::This& Ordering::operator+=(KeyVector& keys) { + this->insert(this->end(), keys.begin(), keys.end()); + return *this; +} + +/* ************************************************************************* */ +bool Ordering::contains(const Key& key) const { + return std::find(this->begin(), this->end(), key) != this->end(); +} + /* ************************************************************************* */ bool Ordering::equals(const Ordering& other, double tol) const { return (*this) == other; diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index a2d165792..b366b3b3a 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -70,7 +70,23 @@ public: boost::assign_detail::call_push_back(*this))(key); } - /// Invert (not reverse) the ordering - returns a map from key to order position + /** + * @brief Append new keys to the ordering as `ordering += keys`. + * + * @param key + * @return The ordering variable with appended keys. + */ + This& operator+=(KeyVector& keys); + + /// Check if key exists in ordering. + bool contains(const Key& key) const; + + /** + * @brief Invert (not reverse) the ordering - returns a map from key to order + * position. + * + * @return FastMap + */ FastMap invert() const; /// @name Fill-reducing Orderings @{ diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 6fdca0d89..1afa1cfde 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -199,6 +199,32 @@ TEST(Ordering, csr_format_3) { EXPECT(adjExpected == adjAcutal); } +/* ************************************************************************* */ +TEST(Ordering, AppendVector) { + using symbol_shorthand::X; + Ordering actual; + KeyVector keys = {X(0), X(1), X(2)}; + actual += keys; + + Ordering expected; + expected += X(0); + expected += X(1); + expected += X(2); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(Ordering, Contains) { + using symbol_shorthand::X; + Ordering ordering; + ordering += X(0); + ordering += X(1); + ordering += X(2); + + EXPECT(ordering.contains(X(1))); + EXPECT(!ordering.contains(X(4))); +} + /* ************************************************************************* */ #ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, csr_format_4) { diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 940ffd882..e4f2950ed 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -45,7 +45,7 @@ namespace gtsam { /// @name Standard Constructors /// @{ - /** Construct empty factor graph */ + /** Construct empty bayes net */ GaussianBayesNet() {} /** Construct from iterator over conditionals */ diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 6199f91a7..951577641 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -15,10 +15,10 @@ * @author Christian Potthast, Frank Dellaert */ -#include #include -#include #include +#include +#include #include #ifdef __GNUC__ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index b2b616dab..11fe1f318 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include // for std::mt19937_64 From 2396bca22f4c3345e6a76f74a71ee0073a68cf4b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 7 Jun 2022 10:07:53 -0400 Subject: [PATCH 24/64] Add GaussianMixture tests --- gtsam/hybrid/GaussianMixture.cpp | 27 ++++++- gtsam/hybrid/GaussianMixture.h | 12 +++ gtsam/hybrid/tests/testGaussianMixture.cpp | 92 ++++++++++++++++++++++ 3 files changed, 128 insertions(+), 3 deletions(-) create mode 100644 gtsam/hybrid/tests/testGaussianMixture.cpp diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 000057518..3d4eb79fa 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -19,7 +19,7 @@ */ #include -#include +#include #include #include #include @@ -77,8 +77,29 @@ GaussianMixture::asGaussianFactorGraphTree() const { } /* *******************************************************************************/ -bool GaussianMixture::equals(const HybridFactor &lf, - double tol) const { +size_t GaussianMixture::nrComponents() const { + size_t total = 0; + conditionals_.visit([&total](const GaussianFactor::shared_ptr &node) { + if (node) total += 1; + }); + return total; +} + +/* *******************************************************************************/ +GaussianConditional::shared_ptr GaussianMixture::operator()( + const DiscreteValues &discreteVals) const { + auto &ptr = conditionals_(discreteVals); + if (!ptr) return nullptr; + auto conditional = boost::dynamic_pointer_cast(ptr); + if (conditional) + return conditional; + else + throw std::logic_error( + "A GaussianMixture unexpectedly contained a non-conditional"); +} + +/* *******************************************************************************/ +bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { const This *e = dynamic_cast(&lf); return e != nullptr && BaseFactor::equals(*e, tol); } diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index e85506715..fc1eb0f06 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -19,7 +19,9 @@ #pragma once +#include #include +#include #include #include #include @@ -99,6 +101,16 @@ class GTSAM_EXPORT GaussianMixture const DiscreteKeys &discreteParents, const std::vector &conditionals); + /// @} + /// @name Standard API + /// @{ + + GaussianConditional::shared_ptr operator()( + const DiscreteValues &discreteVals) const; + + /// Returns the total number of continuous components + size_t nrComponents() const; + /// @} /// @name Testable /// @{ diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp new file mode 100644 index 000000000..8a91a86cc --- /dev/null +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGaussianMixture.cpp + * @brief Unit tests for GaussianMixture class + * @author Varun Agrawal + * @author Fan Jiang + * @author Frank Dellaert + * @date December 2021 + */ + +#include +#include +#include +#include + +#include + +// Include for test suite +#include + +using namespace std; +using namespace gtsam; +using noiseModel::Isotropic; +using symbol_shorthand::M; +using symbol_shorthand::X; + +/* ************************************************************************* */ +TEST(GaussianConditional, Equals) { + // create a conditional gaussian node + Matrix S1(2, 2); + S1(0, 0) = 1; + S1(1, 0) = 2; + S1(0, 1) = 3; + S1(1, 1) = 4; + + Matrix S2(2, 2); + S2(0, 0) = 6; + S2(1, 0) = 0.2; + S2(0, 1) = 8; + S2(1, 1) = 0.4; + + Matrix R1(2, 2); + R1(0, 0) = 0.1; + R1(1, 0) = 0.3; + R1(0, 1) = 0.0; + R1(1, 1) = 0.34; + + Matrix R2(2, 2); + R2(0, 0) = 0.1; + R2(1, 0) = 0.3; + R2(0, 1) = 0.0; + R2(1, 1) = 0.34; + + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34)); + + Vector2 d1(0.2, 0.5), d2(0.5, 0.2); + + auto conditional0 = boost::make_shared(X(1), d1, R1, + X(2), S1, model), + conditional1 = boost::make_shared(X(1), d2, R2, + X(2), S2, model); + + // Create decision tree + DiscreteKey m1(1, 2); + GaussianMixture::Conditionals conditionals( + {m1}, + vector{conditional0, conditional1}); + GaussianMixture mixtureFactor({X(1)}, {X(2)}, {m1}, conditionals); + + // Let's check that this worked: + DiscreteValues mode; + mode[m1.first] = 1; + auto actual = mixtureFactor(mode); + EXPECT(actual == conditional1); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ \ No newline at end of file From fc939b08e045d443a31858db107f676b693a3948 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 7 Jun 2022 10:48:08 -0400 Subject: [PATCH 25/64] GaussianMixtureFactor tests --- gtsam/hybrid/GaussianMixtureFactor.cpp | 7 +- gtsam/hybrid/GaussianMixtureFactor.h | 19 +++ gtsam/hybrid/HybridFactor.cpp | 23 ++- gtsam/hybrid/HybridFactor.h | 3 + .../tests/testGaussianMixtureFactor.cpp | 159 ++++++++++++++++++ 5 files changed, 205 insertions(+), 6 deletions(-) create mode 100644 gtsam/hybrid/tests/testGaussianMixtureFactor.cpp diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index a81cf341d..8f832d8ea 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -51,16 +51,19 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactors( void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); + std::cout << "]{\n"; factors_.print( - "mixture = ", [&](Key k) { return formatter(k); }, + "", [&](Key k) { return formatter(k); }, [&](const GaussianFactor::shared_ptr &gf) -> std::string { RedirectCout rd; - if (!gf->empty()) + std::cout << ":\n"; + if (gf) gf->print("", formatter); else return {"nullptr"}; return rd.str(); }); + std::cout << "}" << std::endl; } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 21770f836..6c90ee6a7 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -84,6 +84,19 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { const DiscreteKeys &discreteKeys, const Factors &factors); + /** + * @brief Construct a new GaussianMixtureFactor object using a vector of + * GaussianFactor shared pointers. + * + * @param keys Vector of keys for continuous factors. + * @param discreteKeys Vector of discrete keys. + * @param factors Vector of gaussian factor shared pointers. + */ + GaussianMixtureFactor(const KeyVector &keys, const DiscreteKeys &discreteKeys, + const std::vector &factors) + : GaussianMixtureFactor(keys, discreteKeys, + Factors(discreteKeys, factors)) {} + static This FromFactors( const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const std::vector &factors); @@ -111,6 +124,12 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * @return Sum */ Sum add(const Sum &sum) const; + + /// Add MixtureFactor to a Sum, syntactic sugar. + friend Sum &operator+=(Sum &sum, const GaussianMixtureFactor &factor) { + sum = factor.add(sum); + return sum; + } }; // traits diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 127c9761c..8df2d524f 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -50,7 +50,10 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, /* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &keys) - : Base(keys), isContinuous_(true), nrContinuous_(keys.size()) {} + : Base(keys), + isContinuous_(true), + nrContinuous_(keys.size()), + continuousKeys_(keys) {} /* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &continuousKeys, @@ -60,13 +63,15 @@ HybridFactor::HybridFactor(const KeyVector &continuousKeys, isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)), isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)), nrContinuous_(continuousKeys.size()), - discreteKeys_(discreteKeys) {} + discreteKeys_(discreteKeys), + continuousKeys_(continuousKeys) {} /* ************************************************************************ */ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), isDiscrete_(true), - discreteKeys_(discreteKeys) {} + discreteKeys_(discreteKeys), + continuousKeys_({}) {} /* ************************************************************************ */ bool HybridFactor::equals(const HybridFactor &lf, double tol) const { @@ -83,7 +88,17 @@ void HybridFactor::print(const std::string &s, if (isContinuous_) std::cout << "Continuous "; if (isDiscrete_) std::cout << "Discrete "; if (isHybrid_) std::cout << "Hybrid "; - this->printKeys("", formatter); + for (size_t c=0; c +#include +#include +#include +#include +#include + +// Include for test suite +#include + +using namespace std; +using namespace gtsam; +using noiseModel::Isotropic; +using symbol_shorthand::M; +using symbol_shorthand::X; + +/* ************************************************************************* */ +// Check iterators of empty mixture. +TEST(GaussianMixtureFactor, Constructor) { + GaussianMixtureFactor factor; + GaussianMixtureFactor::const_iterator const_it = factor.begin(); + CHECK(const_it == factor.end()); + GaussianMixtureFactor::iterator it = factor.begin(); + CHECK(it == factor.end()); +} + +/* ************************************************************************* */ +// "Add" two mixture factors together. +TEST(GaussianMixtureFactor, Sum) { + DiscreteKey m1(1, 2), m2(2, 3); + + auto A1 = Matrix::Zero(2, 1); + auto A2 = Matrix::Zero(2, 2); + auto A3 = Matrix::Zero(2, 3); + auto b = Matrix::Zero(2, 1); + Vector2 sigmas; + sigmas << 1, 2; + auto model = noiseModel::Diagonal::Sigmas(sigmas, true); + + auto f10 = boost::make_shared(X(1), A1, X(2), A2, b); + auto f11 = boost::make_shared(X(1), A1, X(2), A2, b); + auto f20 = boost::make_shared(X(1), A1, X(3), A3, b); + auto f21 = boost::make_shared(X(1), A1, X(3), A3, b); + auto f22 = boost::make_shared(X(1), A1, X(3), A3, b); + std::vector factorsA{f10, f11}; + std::vector factorsB{f20, f21, f22}; + + // TODO(Frank): why specify keys at all? And: keys in factor should be *all* + // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey? + // Design review! + GaussianMixtureFactor mixtureFactorA({X(1), X(2)}, {m1}, factorsA); + GaussianMixtureFactor mixtureFactorB({X(1), X(3)}, {m2}, factorsB); + + // Check that number of keys is 3 + EXPECT_LONGS_EQUAL(3, mixtureFactorA.keys().size()); + + // Check that number of discrete keys is 1 // TODO(Frank): should not exist? + EXPECT_LONGS_EQUAL(1, mixtureFactorA.discreteKeys().size()); + + // Create sum of two mixture factors: it will be a decision tree now on both + // discrete variables m1 and m2: + GaussianMixtureFactor::Sum sum; + sum += mixtureFactorA; + sum += mixtureFactorB; + + // Let's check that this worked: + Assignment mode; + mode[m1.first] = 1; + mode[m2.first] = 2; + auto actual = sum(mode); + EXPECT(actual.at(0) == f11); + EXPECT(actual.at(1) == f22); +} + +TEST(GaussianMixtureFactor, Printing) { + DiscreteKey m1(1, 2); + auto A1 = Matrix::Zero(2, 1); + auto A2 = Matrix::Zero(2, 2); + auto b = Matrix::Zero(2, 1); + auto f10 = boost::make_shared(X(1), A1, X(2), A2, b); + auto f11 = boost::make_shared(X(1), A1, X(2), A2, b); + std::vector factors{f10, f11}; + + GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); + + std::string expected = + R"(Hybrid x1 x2; 1 ]{ + Choice(1) + 0 Leaf : + A[x1] = [ + 0; + 0 +] + A[x2] = [ + 0, 0; + 0, 0 +] + b = [ 0 0 ] + No noise model + + 1 Leaf : + A[x1] = [ + 0; + 0 +] + A[x2] = [ + 0, 0; + 0, 0 +] + b = [ 0 0 ] + No noise model + +} +)"; + EXPECT(assert_print_equal(expected, mixtureFactor)); +} + +TEST_UNSAFE(GaussianMixtureFactor, GaussianMixture) { + KeyVector keys; + keys.push_back(X(0)); + keys.push_back(X(1)); + + DiscreteKeys dKeys; + dKeys.emplace_back(M(0), 2); + dKeys.emplace_back(M(1), 2); + + auto gaussians = boost::make_shared(); + GaussianMixture::Conditionals conditionals(gaussians); + GaussianMixture gm({}, keys, dKeys, conditionals); + + EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ \ No newline at end of file From 60e3321da35ec4d8536da91e528db7adbf1b4812 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 7 Jun 2022 14:11:49 -0400 Subject: [PATCH 26/64] some formatting and improved printing --- gtsam/hybrid/GaussianMixture.cpp | 14 ++++++-------- gtsam/hybrid/HybridDiscreteFactor.cpp | 2 +- gtsam/hybrid/HybridGaussianFactor.cpp | 2 +- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 3d4eb79fa..b04800d21 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -36,8 +36,7 @@ GaussianMixture::GaussianMixture( conditionals_(conditionals) {} /* *******************************************************************************/ -const GaussianMixture::Conditionals & -GaussianMixture::conditionals() { +const GaussianMixture::Conditionals &GaussianMixture::conditionals() { return conditionals_; } @@ -48,8 +47,8 @@ GaussianMixture GaussianMixture::FromConditionals( const std::vector &conditionalsList) { Conditionals dt(discreteParents, conditionalsList); - return GaussianMixture(continuousFrontals, continuousParents, - discreteParents, dt); + return GaussianMixture(continuousFrontals, continuousParents, discreteParents, + dt); } /* *******************************************************************************/ @@ -66,8 +65,7 @@ GaussianMixture::Sum GaussianMixture::add( } /* *******************************************************************************/ -GaussianMixture::Sum -GaussianMixture::asGaussianFactorGraphTree() const { +GaussianMixture::Sum GaussianMixture::asGaussianFactorGraphTree() const { auto lambda = [](const GaussianFactor::shared_ptr &factor) { GaussianFactorGraph result; result.push_back(factor); @@ -106,13 +104,13 @@ bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { /* *******************************************************************************/ void GaussianMixture::print(const std::string &s, - const KeyFormatter &formatter) const { + const KeyFormatter &formatter) const { std::cout << s; if (isContinuous()) std::cout << "Continuous "; if (isDiscrete()) std::cout << "Discrete "; if (isHybrid()) std::cout << "Hybrid "; BaseConditional::print("", formatter); - std::cout << "\nDiscrete Keys = "; + std::cout << " Discrete Keys = "; for (auto &dk : discreteKeys()) { std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 2bdcdee8c..0455e1e90 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -47,7 +47,7 @@ bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { void HybridDiscreteFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); - inner_->print("inner: ", formatter); + inner_->print("\n", formatter); }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 59d20fb79..4d7490e49 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -41,7 +41,7 @@ bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const { void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); - inner_->print("inner: ", formatter); + inner_->print("\n", formatter); }; } // namespace gtsam From 8b03eb5b2d3abb3b9f3784e570b114e776f553df Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 7 Jun 2022 17:57:26 -0400 Subject: [PATCH 27/64] move += operator inside namespace --- gtsam/discrete/DecisionTree-inl.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 99f29b8e5..ffb8d7c69 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -39,10 +39,10 @@ #include #include -using boost::assign::operator+=; - namespace gtsam { + using boost::assign::operator+=; + /****************************************************************************/ // Node /****************************************************************************/ From f911ccf176570cd93be5e1dea290283ec37ff3ff Mon Sep 17 00:00:00 2001 From: HViktorTsoi Date: Wed, 8 Jun 2022 16:30:10 +0800 Subject: [PATCH 28/64] Move Rot3::quaternion to the deprecated block --- gtsam/geometry/Rot3.cpp | 2 ++ gtsam/geometry/Rot3.h | 7 ++++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 08213b065..6db5e1919 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -228,6 +228,7 @@ double Rot3::yaw(OptionalJacobian<1, 3> H) const { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 Vector Rot3::quaternion() const { gtsam::Quaternion q = toQuaternion(); Vector v(4); @@ -237,6 +238,7 @@ Vector Rot3::quaternion() const { v(3) = q.z(); return v; } +#endif /* ************************************************************************* */ pair Rot3::axisAngle() const { diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 18bd88b52..01ca7778c 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -515,11 +515,16 @@ class GTSAM_EXPORT Rot3 : public LieGroup { */ gtsam::Quaternion toQuaternion() const; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** * Converts to a generic matrix to allow for use with matlab * In format: w x y z + * @deprecated: use Rot3::toQuaternion() instead. + * If still using this API, remind that in the returned Vector `V`, + * `V.x()` denotes the actual `qw`, `V.y()` denotes 'qx', `V.z()` denotes `qy`, and `V.w()` denotes 'qz'. */ - Vector quaternion() const; + Vector GTSAM_DEPRECATED quaternion() const; +#endif /** * @brief Spherical Linear intERPolation between *this and other From 27ddedfc630206a87c980c6b64f1b90dc7f26747 Mon Sep 17 00:00:00 2001 From: HViktorTsoi Date: Wed, 8 Jun 2022 16:42:50 +0800 Subject: [PATCH 29/64] Replace the usage of Rot3::quaternion to Rot3::toQuaternion in 'timeShonanAveraging.cpp' --- gtsam_unstable/timing/timeShonanAveraging.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/timing/timeShonanAveraging.cpp b/gtsam_unstable/timing/timeShonanAveraging.cpp index e932b6400..74dd04a78 100644 --- a/gtsam_unstable/timing/timeShonanAveraging.cpp +++ b/gtsam_unstable/timing/timeShonanAveraging.cpp @@ -79,9 +79,9 @@ void saveG2oResult(string name, const Values& values, std::map poses myfile << "VERTEX_SE3:QUAT" << " "; myfile << i << " "; myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " "; - Vector quaternion = Rot3(R).quaternion(); - myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1) - << " " << quaternion(0); + Quaternion quaternion = Rot3(R).toQuaternion(); + myfile << quaternion.x() << " " << quaternion.y() << " " << quaternion.z() + << " " << quaternion.w(); myfile << "\n"; } myfile.close(); From 5ac71d20d56c594e6ef118d01e47706a90e78c6e Mon Sep 17 00:00:00 2001 From: HViktorTsoi Date: Thu, 9 Jun 2022 17:56:35 +0800 Subject: [PATCH 30/64] Remove 'Rot3::quaternion()' API from 'geometry.i' --- gtsam/geometry/geometry.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 8079e2c2a..517558265 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -355,7 +355,7 @@ class Rot3 { double yaw() const; pair axisAngle() const; gtsam::Quaternion toQuaternion() const; - Vector quaternion() const; + // Vector quaternion() const; // @deprecated, see https://github.com/borglab/gtsam/pull/1219 gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; // enabling serialization functionality From f6b86fb17eab1788edc25630ab6f78508d0aaec6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Jun 2022 04:05:18 -0400 Subject: [PATCH 31/64] address review comments --- gtsam/hybrid/tests/testGaussianMixture.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 8a91a86cc..420e22315 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -35,7 +35,10 @@ using symbol_shorthand::M; using symbol_shorthand::X; /* ************************************************************************* */ -TEST(GaussianConditional, Equals) { +/* Check construction of GaussianMixture P(x1 | x2, m1) as well as accessing a + * specific mode i.e. P(x1 | x2, m1=1). + */ +TEST(GaussianMixture, Equals) { // create a conditional gaussian node Matrix S1(2, 2); S1(0, 0) = 1; @@ -89,4 +92,4 @@ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ \ 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 32/64] 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 5ec6127c0b5bebe03149068f276e53870a0aa8bb Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 18 Jun 2022 11:35:46 -0700 Subject: [PATCH 33/64] add unit test, update doc --- gtsam/sfm/TranslationRecovery.cpp | 1 + tests/testTranslationRecovery.cpp | 30 ++++++++++++++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 8f1108806..28c2b2e8a 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -156,6 +156,7 @@ Values TranslationRecovery::initializeRandomly( insert(edge.key1()); insert(edge.key2()); } + // There may be nodes in betweenTranslations that do not have a measurement. for (auto edge : betweenTranslations) { insert(edge.key1()); insert(edge.key2()); diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 5dd319d30..15f1caa1b 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -323,6 +323,36 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) { EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); } + +TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurements) { + // Checks that valid results are obtained when a between translation edge is + // provided with a node that does not have any other relative translations. + Values poses; + poses.insert(0, Pose3(Rot3(), Point3(0, 0, 0))); + poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0))); + poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0))); + poses.insert(4, Pose3(Rot3(), Point3(1, 2, 1))); + + auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 3}, {1, 3}}); + + std::vector> betweenTranslations; + betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), + noiseModel::Constrained::All(3, 1e2)); + // Node 4 only has this between translation prior, no relative translations. + betweenTranslations.emplace_back(0, 4, Point3(1, 2, 1)); + + TranslationRecovery algorithm; + auto result = + algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations); + + // Check result + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0), 1e-4)); + EXPECT(assert_equal(Point3(2, 0, 0), result.at(1), 1e-4)); + EXPECT(assert_equal(Point3(1, -1, 0), result.at(3), 1e-4)); + EXPECT(assert_equal(Point3(1, 2, 1), result.at(4), 1e-4)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 6fae6f42313552ca900b9d8af26b1f6698d889e8 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 18 Jun 2022 11:55:55 -0700 Subject: [PATCH 34/64] update docstring --- gtsam/sfm/TranslationRecovery.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 28c2b2e8a..c7ef2e526 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -119,16 +119,18 @@ void TranslationRecovery::addPrior( graph->emplace_shared>(edge->key1(), Point3(0, 0, 0), priorNoiseModel); - // Add between factors for optional relative translations. - for (auto edge : betweenTranslations) { - graph->emplace_shared>( - edge.key1(), edge.key2(), edge.measured(), edge.noiseModel()); - } - // Add a scale prior only if no other between factors were added. if (betweenTranslations.empty()) { graph->emplace_shared>( edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); + return; + } + + // Add between factors for optional relative translations. + for (auto prior_edge : betweenTranslations) { + graph->emplace_shared>( + prior_edge.key1(), prior_edge.key2(), prior_edge.measured(), + prior_edge.noiseModel()); } } From e18840baffd3e02fb3c05e76ee2586cf2ae83e77 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 18 Jun 2022 11:56:21 -0700 Subject: [PATCH 35/64] update doc in header --- gtsam/sfm/TranslationRecovery.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 6ccc39ddb..44a5ef43e 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -143,11 +143,15 @@ class TranslationRecovery { * * @param relativeTranslations the relative translations, in world coordinate * frames, vector of BinaryMeasurements of Unit3, where each key of a - * measurement is a point in 3D. + * measurement is a point in 3D. If a relative translation magnitude is zero, + * it is treated as a hard same-point constraint (the result of all nodes + * connected by a zero-magnitude edge will be the same). * @param scale scale for first relative translation which fixes gauge. * The scale is only used if betweenTranslations is empty. * @param betweenTranslations relative translations (with scale) between 2 - * points in world coordinate frame known a priori. + * points in world coordinate frame known a priori. Unlike + * relativeTranslations, zero-magnitude betweenTranslations are not treated as + * hard constraints. * @param initialValues intial values for optimization. Initializes randomly * if not provided. * @return Values From 0984fea3f506fa42675e92edbeda4ac9d4df3f5c Mon Sep 17 00:00:00 2001 From: vik748 Date: Mon, 20 Jun 2022 01:18:07 -0700 Subject: [PATCH 36/64] Address matplotlib deprecation warnings for fig.gca() and window.set_title(). --- python/gtsam/examples/Pose3ISAM2Example.py | 5 ++++- python/gtsam/examples/VisualISAM2Example.py | 5 ++++- python/gtsam/utils/plot.py | 19 ++++++++++++++----- 3 files changed, 22 insertions(+), 7 deletions(-) diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index cb71813c5..986bd5b9c 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -34,7 +34,10 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa # Plot the newly updated iSAM2 inference. fig = plt.figure(0) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plt.cla() i = 1 diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py index 4b480fab7..60d4fb376 100644 --- a/python/gtsam/examples/VisualISAM2Example.py +++ b/python/gtsam/examples/VisualISAM2Example.py @@ -33,7 +33,10 @@ def visual_ISAM2_plot(result): fignum = 0 fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plt.cla() # Plot points diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index a4d19f72b..880c436e8 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -333,7 +333,10 @@ def plot_point3( """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] plot_point3_on_axes(axes, point, linespec, P) axes.set_xlabel(axis_labels[0]) @@ -388,7 +391,7 @@ def plot_3d_points(fignum, fig = plt.figure(fignum) fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) + fig.canvas.manager.set_window_title(title.lower()) def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): @@ -490,7 +493,10 @@ def plot_trajectory( axis_labels (iterable[string]): List of axis labels to set. """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] axes.set_xlabel(axis_labels[0]) axes.set_ylabel(axis_labels[1]) @@ -522,7 +528,7 @@ def plot_trajectory( plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale) fig.suptitle(title) - fig.canvas.set_window_title(title.lower()) + fig.canvas.manager.set_window_title(title.lower()) def plot_incremental_trajectory(fignum: int, @@ -545,7 +551,10 @@ def plot_incremental_trajectory(fignum: int, Used to create animation effect. """ fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] poses = gtsam.utilities.allPose3s(values) keys = gtsam.KeyVector(poses.keys()) From 5ea8f2529fc91b82a414a770c027eac14fc3a205 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 20 Jun 2022 23:20:29 -0700 Subject: [PATCH 37/64] 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 38/64] 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 39/64] 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 40/64] 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 41/64] 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 42/64] 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 43/64] 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 44/64] 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 45/64] 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 46/64] 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 47/64] 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 48/64] 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 f5ec070f9fc87d52076846ff394b79b001f4bd13 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 7 Jul 2022 15:09:55 -0400 Subject: [PATCH 49/64] 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 50/64] 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 63e0446f0b04fe23e1224b19e9f819c0f9c9a249 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 10 Jul 2022 16:49:15 -0700 Subject: [PATCH 51/64] 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 52/64] 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 53/64] 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 54/64] 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 55/64] 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 56/64] 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 57/64] 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 58/64] 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 59/64] 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 b8fb26a0b03f096214c350b57d288d71104aedd7 Mon Sep 17 00:00:00 2001 From: Scott Date: Sat, 16 Jul 2022 15:18:45 -0700 Subject: [PATCH 60/64] 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 61/64] 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 07c33c46e526c63149f7fad9c8d755bf0fcaf686 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 20 Jul 2022 18:58:41 -0400 Subject: [PATCH 62/64] 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 31cfce0b8dfc284fb593c2e27f5c0b9c0f374ae7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Jul 2022 05:15:40 -0400 Subject: [PATCH 63/64] 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 64/64] 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})