Apply easy constructor

gtsam/hybrid/tests/Switching.h

@@ -161,13 +161,8 @@ struct Switching {
     for (size_t k = 0; k < K - 1; k++) {
       KeyVector keys = {X(k), X(k + 1)};
       auto motion_models = motionModels(k, between_sigma);
-      std::vector<NonlinearFactorValuePair> components;
-      for (auto &&f : motion_models) {
-        components.push_back(
-            {std::dynamic_pointer_cast<NonlinearFactor>(f), 0.0});
-      }
       nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(keys, modes[k],
-                                                                 components);
+                                                                 motion_models);
     }
 
     // Add measurement factors
@@ -191,8 +186,8 @@ struct Switching {
   }
 
   // Create motion models for a given time step
-  static std::vector<MotionModel::shared_ptr> motionModels(size_t k,
+  static std::vector<NonlinearFactor::shared_ptr> motionModels(
-                                                           double sigma = 1.0) {
+      size_t k, double sigma = 1.0) {
     auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
     auto still =
              std::make_shared<MotionModel>(X(k), X(k + 1), 0.0, noise_model),

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -391,8 +391,7 @@ TEST(HybridBayesNet, Sampling) {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
   nfg.emplace_shared<HybridNonlinearFactor>(
       KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2),
-      std::vector<NonlinearFactorValuePair>{{zero_motion, 0.0},
+      std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
-                                            {one_motion, 0.0}});
 
   DiscreteKey mode(M(0), 2);
   nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -435,8 +435,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
   const auto one_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
-  std::vector<NonlinearFactorValuePair> components = {{zero_motion, 0.0},
+  std::vector<NonlinearFactor::shared_ptr> components = {zero_motion,
-                                                      {one_motion, 0.0}};
+                                                         one_motion};
   nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, m,
                                             components);
 
@@ -566,10 +566,8 @@ std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
       [](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair {
         return {gf, 0.0};
       });
-  auto updated_gmf = std::make_shared<HybridGaussianFactor>(
+  return std::make_shared<HybridGaussianFactor>(
       gmf->continuousKeys(), gmf->discreteKeys(), updated_pairs);
-
-  return updated_gmf;
 }
 
 /****************************************************************************/
@@ -591,8 +589,7 @@ TEST(HybridEstimation, ModeSelection) {
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_loose)),
        model1 = std::make_shared<MotionModel>(
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
-  std::vector<NonlinearFactorValuePair> components = {{model0, 0.0},
+  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
-                                                      {model1, 0.0}};
 
   KeyVector keys = {X(0), X(1)};
   DiscreteKey modes(M(0), 2);
@@ -688,8 +685,7 @@ TEST(HybridEstimation, ModeSelection2) {
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_loose)),
        model1 = std::make_shared<BetweenFactor<Vector3>>(
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
-  std::vector<NonlinearFactorValuePair> components = {{model0, 0.0},
+  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
-                                                      {model1, 0.0}};
 
   KeyVector keys = {X(0), X(1)};
   DiscreteKey modes(M(0), 2);

gtsam/hybrid/tests/testHybridGaussianISAM.cpp

@@ -416,12 +416,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
   Pose2 odometry(1.0, 0.0, 0.0);
   KeyVector contKeys = {W(0), W(1)};
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
-  auto still = std::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 0),
+  std::vector<NonlinearFactor::shared_ptr> components;
-                                                   noise_model),
+  components.emplace_back(
-       moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
+      new PlanarMotionModel(W(0), W(1), odometry, noise_model));  // moving
-                                                    noise_model);
+  components.emplace_back(
-  std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
+      new PlanarMotionModel(W(0), W(1), Pose2(0, 0, 0), noise_model));  // still
-      {moving, 0.0}, {still, 0.0}};
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(1), 2), components);
 
@@ -456,11 +455,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
   /*************** Run Round 3 ***************/
   // Add odometry factor with discrete modes.
   contKeys = {W(1), W(2)};
-  still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
+  components.clear();
-                                              noise_model);
+  components.emplace_back(
-  moving =
+      new PlanarMotionModel(W(1), W(2), odometry, noise_model));  // moving
-      std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
+  components.emplace_back(
-  components = {{moving, 0.0}, {still, 0.0}};
+      new PlanarMotionModel(W(1), W(2), Pose2(0, 0, 0), noise_model));  // still
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(2), 2), components);
 
@@ -498,11 +497,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
   /*************** Run Round 4 ***************/
   // Add odometry factor with discrete modes.
   contKeys = {W(2), W(3)};
-  still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
+  components.clear();
-                                              noise_model);
+  components.emplace_back(
-  moving =
+      new PlanarMotionModel(W(2), W(3), odometry, noise_model));  // moving
-      std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
+  components.emplace_back(
-  components = {{moving, 0.0}, {still, 0.0}};
+      new PlanarMotionModel(W(2), W(3), Pose2(0, 0, 0), noise_model));  // still
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(3), 2), components);
 

gtsam/hybrid/tests/testHybridNonlinearFactor.cpp

@@ -43,24 +43,39 @@ TEST(HybridNonlinearFactor, Constructor) {
   HybridNonlinearFactor::iterator it = factor.begin();
   CHECK(it == factor.end());
 }
+/* ************************************************************************* */
+namespace test_constructor {
+DiscreteKey m1(1, 2);
+double between0 = 0.0;
+double between1 = 1.0;
+
+Vector1 sigmas = Vector1(1.0);
+auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
+
+auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
+auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
+}  // namespace test_constructor
+
+/* ************************************************************************* */
+// Test simple to complex constructors...
+TEST(HybridGaussianFactor, ConstructorVariants) {
+  using namespace test_constructor;
+  HybridNonlinearFactor fromFactors({X(1), X(2)}, m1, {f0, f1});
+
+  std::vector<NonlinearFactorValuePair> pairs{{f0, 0.0}, {f1, 0.0}};
+  HybridNonlinearFactor fromPairs({X(1), X(2)}, m1, pairs);
+  assert_equal(fromFactors, fromPairs);
+
+  HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs);
+  HybridNonlinearFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree);
+  assert_equal(fromDecisionTree, fromPairs);
+}
 
 /* ************************************************************************* */
 // Test .print() output.
 TEST(HybridNonlinearFactor, Printing) {
-  DiscreteKey m1(1, 2);
+  using namespace test_constructor;
-  double between0 = 0.0;
+  HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, {f0, f1});
-  double between1 = 1.0;
-
-  Vector1 sigmas = Vector1(1.0);
-  auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
-
-  auto f0 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
-  auto f1 =
-      std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
-  std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
-
-  HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, factors);
 
   std::string expected =
       R"(Hybrid [x1 x2; 1]
@@ -86,9 +101,7 @@ static HybridNonlinearFactor getHybridNonlinearFactor() {
       std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
   auto f1 =
       std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
-  std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
+  return HybridNonlinearFactor({X(1), X(2)}, m1, {f0, f1});
-
-  return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors);
 }
 
 /* ************************************************************************* */

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -115,35 +115,40 @@ TEST(HybridNonlinearFactorGraph, Resize) {
   EXPECT_LONGS_EQUAL(fg.size(), 0);
 }
 
+/***************************************************************************/
+namespace test_motion {
+KeyVector contKeys = {X(0), X(1)};
+gtsam::DiscreteKey m1(M(1), 2);
+auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
+std::vector<NonlinearFactor::shared_ptr> components = {
+    std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
+    std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model)};
+}  // namespace test_motion
+
 /***************************************************************************
  * Test that the resize method works correctly for a
  * HybridGaussianFactorGraph.
  */
 TEST(HybridGaussianFactorGraph, Resize) {
-  HybridNonlinearFactorGraph nhfg;
+  using namespace test_motion;
+
+  HybridNonlinearFactorGraph hnfg;
   auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
       X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
-  nhfg.push_back(nonlinearFactor);
+  hnfg.push_back(nonlinearFactor);
   auto discreteFactor = std::make_shared<DecisionTreeFactor>();
-  nhfg.push_back(discreteFactor);
+  hnfg.push_back(discreteFactor);
 
-  KeyVector contKeys = {X(0), X(1)};
+  auto dcFactor =
-  auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
+      std::make_shared<HybridNonlinearFactor>(contKeys, m1, components);
-  auto still = std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
+  hnfg.push_back(dcFactor);
-       moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
-
-  std::vector<std::pair<MotionModel::shared_ptr, double>> components = {
-      {still, 0.0}, {moving, 0.0}};
-  auto dcFactor = std::make_shared<HybridNonlinearFactor>(
-      contKeys, gtsam::DiscreteKey(M(1), 2), components);
-  nhfg.push_back(dcFactor);
 
   Values linearizationPoint;
   linearizationPoint.insert<double>(X(0), 0);
   linearizationPoint.insert<double>(X(1), 1);
 
   // Generate `HybridGaussianFactorGraph` by linearizing
-  HybridGaussianFactorGraph gfg = *nhfg.linearize(linearizationPoint);
+  HybridGaussianFactorGraph gfg = *hnfg.linearize(linearizationPoint);
 
   EXPECT_LONGS_EQUAL(gfg.size(), 3);
 
@@ -156,26 +161,19 @@ TEST(HybridGaussianFactorGraph, Resize) {
  * continuous keys provided do not match the keys in the factors.
  */
 TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
+  using namespace test_motion;
+
   auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
       X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
   auto discreteFactor = std::make_shared<DecisionTreeFactor>();
 
-  auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
-  auto still = std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
-       moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
-
-  std::vector<std::pair<MotionModel::shared_ptr, double>> components = {
-      {still, 0.0}, {moving, 0.0}};
-
   // Check for exception when number of continuous keys are under-specified.
-  KeyVector contKeys = {X(0)};
+  THROWS_EXCEPTION(
-  THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
+      std::make_shared<HybridNonlinearFactor>(KeyVector{X(0)}, m1, components));
-      contKeys, gtsam::DiscreteKey(M(1), 2), components));
 
   // Check for exception when number of continuous keys are too many.
-  contKeys = {X(0), X(1), X(2)};
   THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
-      contKeys, gtsam::DiscreteKey(M(1), 2), components));
+      KeyVector{X(0), X(1), X(2)}, m1, components));
 }
 
 /*****************************************************************************
@@ -832,12 +830,10 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) {
   Pose2 odometry(2.0, 0.0, 0.0);
   KeyVector contKeys = {X(0), X(1)};
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
-  auto still = std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
+  std::vector<NonlinearFactor::shared_ptr> motion_models = {
-                                                   noise_model),
+      std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
-       moving = std::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
+                                          noise_model),
-                                                    noise_model);
+      std::make_shared<PlanarMotionModel>(X(0), X(1), odometry, noise_model)};
-  std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> motion_models =
-      {{still, 0.0}, {moving, 0.0}};
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(1), 2), motion_models);
 

gtsam/hybrid/tests/testHybridNonlinearISAM.cpp

@@ -439,8 +439,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
                                                    noise_model),
        moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
                                                     noise_model);
-  std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
+  std::vector<NonlinearFactor::shared_ptr> components{moving, still};
-      {moving, 0.0}, {still, 0.0}};
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(1), 2), components);
 
@@ -479,7 +478,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
-  components = {{moving, 0.0}, {still, 0.0}};
+  components = {moving, still};
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(2), 2), components);
 
@@ -521,7 +520,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
-  components = {{moving, 0.0}, {still, 0.0}};
+  components = {moving, still};
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(3), 2), components);