diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index a4218593b..3a3bf720b 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -27,8 +27,7 @@ void HybridNonlinearFactorGraph::add( } /* ************************************************************************* */ -void HybridNonlinearFactorGraph::add( - boost::shared_ptr factor) { +void HybridNonlinearFactorGraph::add(boost::shared_ptr factor) { FactorGraph::add(boost::make_shared(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(); - 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(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(nlhf->inner())) { auto hgf = boost::make_shared( 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; diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 7a19c7755..afa410318 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/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 <> diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index ecd90e234..8bcb26c92 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -166,7 +166,7 @@ struct Switching { linearizationPoint.insert(X(k), static_cast(k)); } - linearizedFactorGraph = nonlinearFactorGraph.linearize(linearizationPoint); + linearizedFactorGraph = *nonlinearFactorGraph.linearize(linearizationPoint); } // Create motion models for a given time step diff --git a/gtsam/hybrid/tests/testHybridIncremental.cpp b/gtsam/hybrid/tests/testHybridIncremental.cpp index 4449aba0b..8e16d02b9 100644 --- a/gtsam/hybrid/tests/testHybridIncremental.cpp +++ b/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! diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 6b689b4e3..9e93eaba3 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -60,7 +60,7 @@ TEST(HybridFactorGraph, GaussianFactorGraph) { Values linearizationPoint; linearizationPoint.insert(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(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;