return shared pointer for HybridNonlinearFactorGraph::linearize

gtsam/hybrid/HybridNonlinearFactorGraph.cpp

@@ -27,8 +27,7 @@ void HybridNonlinearFactorGraph::add(
 }
 
 /* ************************************************************************* */
-void HybridNonlinearFactorGraph::add(
+void HybridNonlinearFactorGraph::add(boost::shared_ptr<DiscreteFactor> factor) {
-    boost::shared_ptr<DiscreteFactor> factor) {
   FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
 }
 
@@ -49,12 +48,12 @@ void HybridNonlinearFactorGraph::print(const std::string& s,
 }
 
 /* ************************************************************************* */
-HybridGaussianFactorGraph HybridNonlinearFactorGraph::linearize(
+HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
     const Values& continuousValues) const {
   // create an empty linear FG
-  HybridGaussianFactorGraph linearFG;
+  auto linearFG = boost::make_shared<HybridGaussianFactorGraph>();
 
-  linearFG.reserve(size());
+  linearFG->reserve(size());
 
   // linearize all hybrid factors
   for (auto&& factor : factors_) {
@@ -66,9 +65,9 @@ HybridGaussianFactorGraph HybridNonlinearFactorGraph::linearize(
       if (factor->isHybrid()) {
         // Check if it is a nonlinear mixture factor
         if (auto nlmf = boost::dynamic_pointer_cast<MixtureFactor>(factor)) {
-          linearFG.push_back(nlmf->linearize(continuousValues));
+          linearFG->push_back(nlmf->linearize(continuousValues));
         } else {
-          linearFG.push_back(factor);
+          linearFG->push_back(factor);
         }
 
         // Now check if the factor is a continuous only factor.
@@ -80,18 +79,18 @@ HybridGaussianFactorGraph HybridNonlinearFactorGraph::linearize(
                 boost::dynamic_pointer_cast<NonlinearFactor>(nlhf->inner())) {
           auto hgf = boost::make_shared<HybridGaussianFactor>(
               nlf->linearize(continuousValues));
-          linearFG.push_back(hgf);
+          linearFG->push_back(hgf);
         } else {
-          linearFG.push_back(factor);
+          linearFG->push_back(factor);
         }
         // Finally if nothing else, we are discrete-only which doesn't need
         // lineariztion.
       } else {
-        linearFG.push_back(factor);
+        linearFG->push_back(factor);
       }
 
     } else {
-      linearFG.push_back(GaussianFactor::shared_ptr());
+      linearFG->push_back(GaussianFactor::shared_ptr());
     }
   }
   return linearFG;

gtsam/hybrid/HybridNonlinearFactorGraph.h

@@ -127,7 +127,8 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
    * @param continuousValues: Dictionary of continuous values.
    * @return HybridGaussianFactorGraph::shared_ptr
    */
-  HybridGaussianFactorGraph linearize(const Values& continuousValues) const;
+  HybridGaussianFactorGraph::shared_ptr linearize(
+      const Values& continuousValues) const;
 };
 
 template <>

gtsam/hybrid/tests/Switching.h

@@ -166,7 +166,7 @@ struct Switching {
       linearizationPoint.insert<double>(X(k), static_cast<double>(k));
     }
 
-    linearizedFactorGraph = nonlinearFactorGraph.linearize(linearizationPoint);
+    linearizedFactorGraph = *nonlinearFactorGraph.linearize(linearizationPoint);
   }
 
   // Create motion models for a given time step

gtsam/hybrid/tests/testHybridIncremental.cpp

@@ -399,7 +399,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
   initial.insert(Z(0), Pose2(0.0, 2.0, 0.0));
   initial.insert(W(0), Pose2(0.0, 3.0, 0.0));
 
-  HybridGaussianFactorGraph gfg = fg.linearize(initial);
+  HybridGaussianFactorGraph gfg = *fg.linearize(initial);
   fg = HybridNonlinearFactorGraph();
 
   HybridGaussianISAM inc;
@@ -444,7 +444,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
   // The leg link did not move so we set the expected pose accordingly.
   initial.insert(W(1), Pose2(0.0, 3.0, 0.0));
 
-  gfg = fg.linearize(initial);
+  gfg = *fg.linearize(initial);
   fg = HybridNonlinearFactorGraph();
 
   // Update without pruning
@@ -483,7 +483,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
   initial.insert(Z(2), Pose2(2.0, 2.0, 0.0));
   initial.insert(W(2), Pose2(0.0, 3.0, 0.0));
 
-  gfg = fg.linearize(initial);
+  gfg = *fg.linearize(initial);
   fg = HybridNonlinearFactorGraph();
 
   // Now we prune!
@@ -526,7 +526,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
   initial.insert(Z(3), Pose2(3.0, 2.0, 0.0));
   initial.insert(W(3), Pose2(0.0, 3.0, 0.0));
 
-  gfg = fg.linearize(initial);
+  gfg = *fg.linearize(initial);
   fg = HybridNonlinearFactorGraph();
 
   // Keep pruning!

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -60,7 +60,7 @@ TEST(HybridFactorGraph, GaussianFactorGraph) {
   Values linearizationPoint;
   linearizationPoint.insert<double>(X(0), 0);
 
-  HybridGaussianFactorGraph ghfg = fg.linearize(linearizationPoint);
+  HybridGaussianFactorGraph ghfg = *fg.linearize(linearizationPoint);
 
   // Add a factor to the GaussianFactorGraph
   ghfg.add(JacobianFactor(X(0), I_1x1, Vector1(5)));
@@ -139,7 +139,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
   linearizationPoint.insert<double>(X(1), 1);
 
   // Generate `HybridGaussianFactorGraph` by linearizing
-  HybridGaussianFactorGraph gfg = nhfg.linearize(linearizationPoint);
+  HybridGaussianFactorGraph gfg = *nhfg.linearize(linearizationPoint);
 
   EXPECT_LONGS_EQUAL(gfg.size(), 3);
 
@@ -250,7 +250,7 @@ TEST(HybridFactorGraph, Linearization) {
 
   // Linearize here:
   HybridGaussianFactorGraph actualLinearized =
-      self.nonlinearFactorGraph.linearize(self.linearizationPoint);
+      *self.nonlinearFactorGraph.linearize(self.linearizationPoint);
 
   EXPECT_LONGS_EQUAL(7, actualLinearized.size());
 }
@@ -718,7 +718,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
   ordering += X(0);
   ordering += X(1);
 
-  HybridGaussianFactorGraph linearized = fg.linearize(initialEstimate);
+  HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate);
   gtsam::HybridBayesNet::shared_ptr hybridBayesNet;
   gtsam::HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;