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

linearize returns object instead of pointer

release/4.3a0
Varun Agrawal 2022-05-30 17:30:25 -04:00
parent 7e18277849
commit 119679a366
1 changed files with 9 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/hybrid/NonlinearHybridFactorGraph.h
View File

@ -150,13 +150,11 @@ class GTSAM_EXPORT NonlinearHybridFactorGraph : public HybridFactorGraph {
* @param continuousValues: Dictionary of continuous values. * @param continuousValues: Dictionary of continuous values.
* @return GaussianHybridFactorGraph::shared_ptr * @return GaussianHybridFactorGraph::shared_ptr
*/ */
GaussianHybridFactorGraph::shared_ptr linearize( GaussianHybridFactorGraph linearize(const Values& continuousValues) const {
const Values& continuousValues) const {
// create an empty linear FG // create an empty linear FG
GaussianHybridFactorGraph::shared_ptr linearFG = GaussianHybridFactorGraph linearFG;
boost::make_shared<GaussianHybridFactorGraph>();
linearFG->reserve(size()); linearFG.reserve(size());
// linearize all hybrid factors // linearize all hybrid factors
for (auto&& factor : factors_) { for (auto&& factor : factors_) {
@ -168,9 +166,9 @@ class GTSAM_EXPORT NonlinearHybridFactorGraph : public HybridFactorGraph {
if (factor->isHybrid()) { if (factor->isHybrid()) {
// Check if it is a nonlinear mixture factor // Check if it is a nonlinear mixture factor
if (auto nlmf = boost::dynamic_pointer_cast<MixtureFactor>(factor)) { if (auto nlmf = boost::dynamic_pointer_cast<MixtureFactor>(factor)) {
linearFG->push_back(nlmf->linearize(continuousValues)); linearFG.push_back(nlmf->linearize(continuousValues));
} else { } else {
linearFG->push_back(factor); linearFG.push_back(factor);
} }
// Now check if the factor is a continuous only factor. // Now check if the factor is a continuous only factor.
@ -183,18 +181,18 @@ class GTSAM_EXPORT NonlinearHybridFactorGraph : public HybridFactorGraph {
boost::dynamic_pointer_cast<NonlinearFactor>(nlhf->inner())) { boost::dynamic_pointer_cast<NonlinearFactor>(nlhf->inner())) {
auto hgf = boost::make_shared<HybridGaussianFactor>( auto hgf = boost::make_shared<HybridGaussianFactor>(
nlf->linearize(continuousValues)); nlf->linearize(continuousValues));
linearFG->push_back(hgf); linearFG.push_back(hgf);
} else { } else {
linearFG->push_back(factor); linearFG.push_back(factor);
} }
// Finally if nothing else, we are discrete-only which doesn't need // Finally if nothing else, we are discrete-only which doesn't need
// lineariztion. // lineariztion.
} else { } else {
linearFG->push_back(factor); linearFG.push_back(factor);
} }
} else { } else {
linearFG->push_back(GaussianFactor::shared_ptr()); linearFG.push_back(GaussianFactor::shared_ptr());
} }
} }
return linearFG; return linearFG;