12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

return shared pointer for HybridNonlinearFactorGraph::linearize

release/4.3a0
Varun Agrawal 2022-09-12 19:55:38 -04:00
parent 8b5b42b6e9
commit 8c10a8089e
5 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -27,8 +27,7 @@ void HybridNonlinearFactorGraph::add(
}
/* ************************************************************************* */
void HybridNonlinearFactorGraph::add(
boost::shared_ptr<DiscreteFactor> factor) {
void HybridNonlinearFactorGraph::add(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;

3
gtsam/hybrid/HybridNonlinearFactorGraph.h
View File

@ -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 <>

2
gtsam/hybrid/tests/Switching.h
View File

@ -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

8
gtsam/hybrid/tests/testHybridIncremental.cpp
View File

@ -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!

8
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -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;