diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 3e5a4f1d1..0462d7998 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -84,12 +84,12 @@ void HybridGaussianISAM::updateInternal( // Add the removed top and the new factors HybridGaussianFactorGraph factors; - factors += bn; - factors += newFactors; + factors.push_back(bn); + factors.push_back(newFactors); // Add the orphaned subtrees for (const sharedClique& orphan : *orphans) { - factors += std::make_shared>(orphan); + factors.push_back(std::make_shared>(orphan)); } const VariableIndex index(factors); diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 953025220..6f2898bf1 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -94,9 +94,9 @@ struct HybridConstructorTraversalData { symbolicFactors.reserve(node->factors.size() + data.childSymbolicFactors.size()); // Add ETree node factors - symbolicFactors += node->factors; + symbolicFactors.push_back(node->factors); // Add symbolic factors passed up from children - symbolicFactors += data.childSymbolicFactors; + symbolicFactors.push_back(data.childSymbolicFactors); Ordering keyAsOrdering; keyAsOrdering.push_back(node->key); diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 64cca5546..2d8f917dc 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -381,13 +381,13 @@ namespace gtsam { gttoc(Full_root_factoring); gttic(Variable_joint); - p_BC1C2 += p_B; - p_BC1C2 += *p_C1_B; - p_BC1C2 += *p_C2_B; + p_BC1C2.push_back(p_B); + p_BC1C2.push_back(*p_C1_B); + p_BC1C2.push_back(*p_C2_B); if(C1 != B) - p_BC1C2 += C1->conditional(); + p_BC1C2.push_back(C1->conditional()); if(C2 != B) - p_BC1C2 += C2->conditional(); + p_BC1C2.push_back(C2->conditional()); gttoc(Variable_joint); } else @@ -395,8 +395,8 @@ namespace gtsam { // The nodes have no common ancestor, they're in different trees, so they're joint is just the // product of their marginals. gttic(Disjoint_marginals); - p_BC1C2 += C1->marginal2(function); - p_BC1C2 += C2->marginal2(function); + p_BC1C2.push_back(C1->marginal2(function)); + p_BC1C2.push_back(C2->marginal2(function)); gttoc(Disjoint_marginals); } diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 62ed0b90f..a91fa4f78 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -123,7 +123,7 @@ namespace gtsam { gttoc(BayesTreeCliqueBase_shortcut); FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) gttic(BayesTreeCliqueBase_shortcut); - p_Cp_B += parent->conditional_; // P(Fp|Sp) + p_Cp_B.push_back(parent->conditional_); // P(Fp|Sp) // Determine the variables we want to keepSet, S union B KeyVector keep = shortcut_indices(B, p_Cp_B); @@ -171,7 +171,7 @@ namespace gtsam { gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); // now add the parent conditional - p_Cp += parent->conditional_; // P(Fp|Sp) + p_Cp.push_back(parent->conditional_); // P(Fp|Sp) // The variables we want to keepSet are exactly the ones in S KeyVector indicesS(this->conditional()->beginParents(), @@ -198,7 +198,7 @@ namespace gtsam { // initialize with separator marginal P(S) FactorGraphType p_C = this->separatorMarginal(function); // add the conditional P(F|S) - p_C += std::shared_ptr(this->conditional_); + p_C.push_back(std::shared_ptr(this->conditional_)); return p_C; } diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index fe5c53e36..a6175f76b 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -185,8 +185,8 @@ struct EliminationData { // Gather factors FactorGraphType gatheredFactors; gatheredFactors.reserve(node->factors.size() + node->nrChildren()); - gatheredFactors += node->factors; - gatheredFactors += myData.childFactors; + gatheredFactors.push_back(node->factors); + gatheredFactors.push_back(myData.childFactors); // Check for Bayes tree orphan subtrees, and add them to our children // TODO(frank): should this really happen here? diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index b93c58c48..aaebf9385 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -36,12 +36,12 @@ void ISAM::updateInternal(const FactorGraphType& newFactors, // Add the removed top and the new factors FactorGraphType factors; - factors += bn; - factors += newFactors; + factors.push_back(bn); + factors.push_back(newFactors); // Add the orphaned subtrees for (const sharedClique& orphan : *orphans) - factors += std::make_shared >(orphan); + factors.push_back(std::make_shared >(orphan)); // Get an ordering where the new keys are eliminated last const VariableIndex index(factors); diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 0a4c88948..0f79c2a9a 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -77,9 +77,9 @@ struct ConstructorTraversalData { symbolicFactors.reserve( ETreeNode->factors.size() + myData.childSymbolicFactors.size()); // Add ETree node factors - symbolicFactors += ETreeNode->factors; + symbolicFactors.push_back(ETreeNode->factors); // Add symbolic factors passed up from children - symbolicFactors += myData.childSymbolicFactors; + symbolicFactors.push_back(myData.childSymbolicFactors); Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 52c6a296f..ded6bc5e3 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -54,7 +54,8 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const { // Create a factor graph GaussianFactorGraph factorGraph; - factorGraph += p, newFactor; + factorGraph.push_back(p); + factorGraph.push_back(newFactor); // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) return solve(factorGraph); @@ -66,7 +67,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x0, // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma + factorGraph.emplace_shared(0, I_, x0, P0); // |x-x0|^2_diagSigma return solve(factorGraph); } @@ -75,7 +76,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) + factorGraph.emplace_shared(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) return solve(factorGraph); } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 727a8befd..f4c61e659 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -294,8 +294,7 @@ void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams, gttic(orphans); // Add the orphaned subtrees for (const auto& orphan : *orphans) - factors += - std::make_shared >(orphan); + factors.emplace_shared >(orphan); gttoc(orphans); // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 57136214d..160423a64 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -213,7 +213,7 @@ NonlinearFactorGraph LinearContainerFactor::ConvertLinearGraph( result.reserve(linear_graph.size()); for (const auto& f : linear_graph) if (f) - result += std::make_shared(f, linearizationPoint); + result.emplace_shared(f, linearizationPoint); return result; } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index f8aaf4f39..6e57a2cf5 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -199,9 +199,9 @@ SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const for (const sharedFactor& factor: factors_) { if(factor) - *symbolic += SymbolicFactor(*factor); + symbolic->push_back(SymbolicFactor(*factor)); else - *symbolic += SymbolicFactorGraph::sharedFactor(); + symbolic->push_back(SymbolicFactorGraph::sharedFactor()); } return symbolic; @@ -265,11 +265,11 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li linearFG->reserve(size()); // linearize all factors - for(const sharedFactor& factor: factors_) { - if(factor) { - (*linearFG) += factor->linearize(linearizationPoint); + for (const sharedFactor& factor : factors_) { + if (factor) { + linearFG->push_back(factor->linearize(linearizationPoint)); } else - (*linearFG) += GaussianFactor::shared_ptr(); + linearFG->push_back(GaussianFactor::shared_ptr()); } #endif diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h index aa0a3daa3..be1afc972 100644 --- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -131,7 +131,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { const Key& key = key_dim.first; const size_t& dim = key_dim.second; const CachedModel* item = getCachedModel(dim); - damped += std::make_shared(key, item->A, item->b, item->model); + damped.emplace_shared(key, item->A, item->b, item->model); } return damped; } @@ -147,7 +147,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { const size_t dim = key_vector.second.size(); CachedModel* item = getCachedModel(dim); item->A.diagonal() = sqrtHessianDiagonal.at(key); // use diag(hessian) - damped += std::make_shared(key, item->A, item->b, item->model); + damped.emplace_shared(key, item->A, item->b, item->model); } catch (const std::out_of_range&) { continue; // Don't attempt any damping if no key found in diagonal } diff --git a/gtsam/symbolic/SymbolicFactor.cpp b/gtsam/symbolic/SymbolicFactor.cpp index e51b3cf29..1f26389e9 100644 --- a/gtsam/symbolic/SymbolicFactor.cpp +++ b/gtsam/symbolic/SymbolicFactor.cpp @@ -49,7 +49,7 @@ namespace gtsam { SymbolicFactor::eliminate(const Ordering& keys) const { SymbolicFactorGraph graph; - graph += *this; // TODO: Is there a way to avoid copying this factor? + graph.push_back(*this); // TODO: Is there a way to avoid copying this factor? return EliminateSymbolic(graph, keys); }