From 790e3d515c16fcc7d45d28961278862b0cd93e16 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 6 Oct 2023 12:34:49 -0400 Subject: [PATCH] add templated at methods for FactorGraph so it can perform typecasting for us --- .../tests/testHybridGaussianFactorGraph.cpp | 6 +++--- gtsam/inference/FactorGraph.h | 15 +++++++++++++++ gtsam/linear/tests/testGaussianFactorGraph.cpp | 3 +-- gtsam/nonlinear/GncOptimizer.h | 15 ++++++--------- gtsam/sfm/tests/testShonanAveraging.cpp | 8 +++++--- gtsam/slam/tests/testDataset.cpp | 13 +++++-------- gtsam_unstable/discrete/tests/testLoopyBelief.cpp | 7 ++----- .../nonlinear/tests/testNonlinearClusterTree.cpp | 2 +- tests/testNonlinearFactor.cpp | 2 +- 9 files changed, 39 insertions(+), 32 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index df38c171e..b240e1626 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -42,8 +42,8 @@ #include #include #include -#include #include +#include #include "Switching.h" #include "TinyHybridExample.h" @@ -613,11 +613,11 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) { // Create expected decision tree with two factor graphs: // Get mixture factor: - auto mixture = std::dynamic_pointer_cast(fg.at(0)); + auto mixture = fg.at(0); CHECK(mixture); // Get prior factor: - const auto gf = std::dynamic_pointer_cast(fg.at(1)); + const auto gf = fg.at(1); CHECK(gf); using GF = GaussianFactor::shared_ptr; const GF prior = gf->asGaussian(); diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 327fca49a..b6046d0bb 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -310,6 +310,21 @@ class FactorGraph { */ sharedFactor& at(size_t i) { return factors_.at(i); } + /** Get a specific factor by index and typecast to factor type F + * (this checks array bounds and may throw + * an exception, as opposed to operator[] which does not). + */ + template + std::shared_ptr at(size_t i) { + return std::dynamic_pointer_cast(factors_.at(i)); + } + + /// Const version of templated `at` method. + template + const std::shared_ptr at(size_t i) const { + return std::dynamic_pointer_cast(factors_.at(i)); + } + /** Get a specific factor by index (this does not check array bounds, as * opposed to at() which does). */ diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 2ba00abc1..03222bb3f 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -391,8 +391,7 @@ TEST(GaussianFactorGraph, clone) { EXPECT(assert_equal(init_graph, actCloned)); // Same as the original version // Apply an in-place change to init_graph and compare - JacobianFactor::shared_ptr jacFactor0 = - std::dynamic_pointer_cast(init_graph.at(0)); + JacobianFactor::shared_ptr jacFactor0 = init_graph.at(0); CHECK(jacFactor0); jacFactor0->getA(jacFactor0->begin()) *= 7.; EXPECT(assert_inequal(init_graph, exp_graph)); diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index d59eb4371..b646d009e 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -65,10 +65,9 @@ class GncOptimizer { nfg_.resize(graph.size()); for (size_t i = 0; i < graph.size(); i++) { if (graph[i]) { - NoiseModelFactor::shared_ptr factor = std::dynamic_pointer_cast< - NoiseModelFactor>(graph[i]); - auto robust = std::dynamic_pointer_cast< - noiseModel::Robust>(factor->noiseModel()); + NoiseModelFactor::shared_ptr factor = graph.at(i); + auto robust = + std::dynamic_pointer_cast(factor->noiseModel()); // if the factor has a robust loss, we remove the robust loss nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor; } @@ -401,11 +400,9 @@ class GncOptimizer { newGraph.resize(nfg_.size()); for (size_t i = 0; i < nfg_.size(); i++) { if (nfg_[i]) { - auto factor = std::dynamic_pointer_cast< - NoiseModelFactor>(nfg_[i]); - auto noiseModel = - std::dynamic_pointer_cast( - factor->noiseModel()); + auto factor = nfg_.at(i); + auto noiseModel = std::dynamic_pointer_cast( + factor->noiseModel()); if (noiseModel) { Matrix newInfo = weights[i] * noiseModel->information(); auto newNoiseModel = noiseModel::Gaussian::Information(newInfo); diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index dd4759daa..dfa725ab6 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -372,9 +372,11 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) { // test that each factor is actually robust for (size_t i=0; i<=4; i++) { // note: last is the Gauge factor and is not robust - const auto &robust = std::dynamic_pointer_cast( - std::dynamic_pointer_cast(graph[i])->noiseModel()); - EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber) + const auto &robust = std::dynamic_pointer_cast( + graph.at(i)->noiseModel()); + // we expect the factors to be use a robust noise model + // (in particular, Huber) + EXPECT(robust); } // test result diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 8591a3932..13104174f 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -97,8 +97,7 @@ TEST(dataSet, load2D) { auto model = noiseModel::Unit::Create(3); BetweenFactor expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381), model); - BetweenFactor::shared_ptr actual = - std::dynamic_pointer_cast>(graph->at(0)); + BetweenFactor::shared_ptr actual = graph->at>(0); EXPECT(assert_equal(expected, *actual)); // Check binary measurements, Pose2 @@ -113,9 +112,8 @@ TEST(dataSet, load2D) { // // Check factor parsing const auto actualFactors = parseFactors(filename); for (size_t i : {0, 1, 2, 3, 4, 5}) { - EXPECT(assert_equal( - *std::dynamic_pointer_cast>(graph->at(i)), - *actualFactors[i], 1e-5)); + EXPECT(assert_equal(*graph->at>(i), *actualFactors[i], + 1e-5)); } // Check pose parsing @@ -194,9 +192,8 @@ TEST(dataSet, readG2o3D) { // Check factor parsing const auto actualFactors = parseFactors(g2oFile); for (size_t i : {0, 1, 2, 3, 4, 5}) { - EXPECT(assert_equal( - *std::dynamic_pointer_cast>(expectedGraph[i]), - *actualFactors[i], 1e-5)); + EXPECT(assert_equal(*expectedGraph.at>(i), + *actualFactors[i], 1e-5)); } // Check pose parsing diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index 7c769fd78..39d9d743b 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -183,13 +183,10 @@ class LoopyBelief { // accumulate unary factors if (graph.at(factorIndex)->size() == 1) { if (!prodOfUnaries) - prodOfUnaries = std::dynamic_pointer_cast( - graph.at(factorIndex)); + prodOfUnaries = graph.at(factorIndex); else prodOfUnaries = std::make_shared( - *prodOfUnaries * - (*std::dynamic_pointer_cast( - graph.at(factorIndex)))); + *prodOfUnaries * (*graph.at(factorIndex))); } } diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp index 38cfd7348..bbb461abb 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -87,7 +87,7 @@ TEST(NonlinearClusterTree, Clusters) { Ordering ordering; ordering.push_back(x1); const auto [bn, fg] = gfg->eliminatePartialSequential(ordering); - auto expectedFactor = std::dynamic_pointer_cast(fg->at(0)); + auto expectedFactor = fg->at(0); if (!expectedFactor) throw std::runtime_error("Expected HessianFactor"); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 06ae3366c..cca78b80e 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -323,7 +323,7 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel ) // create actual NonlinearFactorGraph actual; SharedNoiseModel noise2 = noiseModel::Isotropic::Sigma(2,sigma2); - actual.push_back( std::dynamic_pointer_cast(nfg[0])->cloneWithNewNoiseModel(noise2) ); + actual.push_back(nfg.at(0)->cloneWithNewNoiseModel(noise2)); // check it's all good CHECK(assert_equal(expected, actual));