add templated at methods for FactorGraph so it can perform typecasting for us

gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp

@@ -42,8 +42,8 @@
 #include <functional>
 #include <iostream>
 #include <iterator>
-#include <vector>
 #include <numeric>
+#include <vector>
 
 #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<GaussianMixtureFactor>(fg.at(0));
+  auto mixture = fg.at<GaussianMixtureFactor>(0);
   CHECK(mixture);
 
   // Get prior factor:
-  const auto gf = std::dynamic_pointer_cast<HybridConditional>(fg.at(1));
+  const auto gf = fg.at<HybridConditional>(1);
   CHECK(gf);
   using GF = GaussianFactor::shared_ptr;
   const GF prior = gf->asGaussian();

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 <typename F>
+  std::shared_ptr<F> at(size_t i) {
+    return std::dynamic_pointer_cast<F>(factors_.at(i));
+  }
+
+  /// Const version of templated `at` method.
+  template <typename F>
+  const std::shared_ptr<F> at(size_t i) const {
+    return std::dynamic_pointer_cast<F>(factors_.at(i));
+  }
+
   /** Get a specific factor by index (this does not check array bounds, as
    * opposed to at() which does).
    */

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<JacobianFactor>(init_graph.at(0));
+  JacobianFactor::shared_ptr jacFactor0 = init_graph.at<JacobianFactor>(0);
   CHECK(jacFactor0);
   jacFactor0->getA(jacFactor0->begin()) *= 7.;
   EXPECT(assert_inequal(init_graph, exp_graph));

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<NoiseModelFactor>(i);
+        auto robust =
+            std::dynamic_pointer_cast<noiseModel::Robust>(factor->noiseModel());
         // if the factor has a robust loss, we remove the robust loss
         nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor;
       }
@@ -401,10 +400,8 @@ 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<noiseModel::Gaussian>(
+        auto factor = nfg_.at<NoiseModelFactor>(i);
+        auto noiseModel = std::dynamic_pointer_cast<noiseModel::Gaussian>(
             factor->noiseModel());
         if (noiseModel) {
           Matrix newInfo = weights[i] * noiseModel->information();

gtsam/sfm/tests/testShonanAveraging.cpp

@@ -373,8 +373,10 @@ 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<noiseModel::Robust>(
-			  std::dynamic_pointer_cast<NoiseModelFactor>(graph[i])->noiseModel());
-	  EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber)
+        graph.at<NoiseModelFactor>(i)->noiseModel());
+    // we expect the factors to be use a robust noise model
+    // (in particular, Huber)
+    EXPECT(robust);
   }
 
   // test result

gtsam/slam/tests/testDataset.cpp

@@ -97,8 +97,7 @@ TEST(dataSet, load2D) {
   auto model = noiseModel::Unit::Create(3);
   BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381),
                                 model);
-  BetweenFactor<Pose2>::shared_ptr actual =
-      std::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(0));
+  BetweenFactor<Pose2>::shared_ptr actual = graph->at<BetweenFactor<Pose2>>(0);
   EXPECT(assert_equal(expected, *actual));
 
   // Check binary measurements, Pose2
@@ -113,9 +112,8 @@ TEST(dataSet, load2D) {
   // // Check factor parsing
   const auto actualFactors = parseFactors<Pose2>(filename);
   for (size_t i : {0, 1, 2, 3, 4, 5}) {
-    EXPECT(assert_equal(
-        *std::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
-        *actualFactors[i], 1e-5));
+    EXPECT(assert_equal(*graph->at<BetweenFactor<Pose2>>(i), *actualFactors[i],
+                        1e-5));
   }
 
   // Check pose parsing
@@ -194,8 +192,7 @@ TEST(dataSet, readG2o3D) {
   // Check factor parsing
   const auto actualFactors = parseFactors<Pose3>(g2oFile);
   for (size_t i : {0, 1, 2, 3, 4, 5}) {
-    EXPECT(assert_equal(
-        *std::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
+    EXPECT(assert_equal(*expectedGraph.at<BetweenFactor<Pose3>>(i),
                         *actualFactors[i], 1e-5));
   }
 

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<DecisionTreeFactor>(
-                graph.at(factorIndex));
+            prodOfUnaries = graph.at<DecisionTreeFactor>(factorIndex);
           else
             prodOfUnaries = std::make_shared<DecisionTreeFactor>(
-                *prodOfUnaries *
-                (*std::dynamic_pointer_cast<DecisionTreeFactor>(
-                    graph.at(factorIndex))));
+                *prodOfUnaries * (*graph.at<DecisionTreeFactor>(factorIndex)));
         }
       }
 

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<HessianFactor>(fg->at(0));
+  auto expectedFactor = fg->at<HessianFactor>(0);
   if (!expectedFactor)
     throw std::runtime_error("Expected HessianFactor");
 

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<NoiseModelFactor>(nfg[0])->cloneWithNewNoiseModel(noise2) );
+  actual.push_back(nfg.at<NoiseModelFactor>(0)->cloneWithNewNoiseModel(noise2));
 
   // check it's all good
   CHECK(assert_equal(expected, actual));