Cleaned up ++ stragglers
							parent
							
								
									a9dd644015
								
							
						
					
					
						commit
						a38d76bcef
					
				|  | @ -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<BayesTreeOrphanWrapper<Node>>(orphan); | ||||
|     factors.push_back(std::make_shared<BayesTreeOrphanWrapper<Node>>(orphan)); | ||||
|   } | ||||
| 
 | ||||
|   const VariableIndex index(factors); | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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); | ||||
|     } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<FactorType>(this->conditional_); | ||||
|     p_C.push_back(std::shared_ptr<FactorType>(this->conditional_)); | ||||
|     return p_C; | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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?
 | ||||
|  |  | |||
|  | @ -36,12 +36,12 @@ void ISAM<BAYESTREE>::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<BayesTreeOrphanWrapper<Clique> >(orphan); | ||||
|     factors.push_back(std::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan)); | ||||
| 
 | ||||
|   // Get an ordering where the new keys are eliminated last
 | ||||
|   const VariableIndex index(factors); | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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<JacobianFactor>(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<HessianFactor>(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
 | ||||
|   return solve(factorGraph); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<BayesTreeOrphanWrapper<ISAM2::Clique> >(orphan); | ||||
|     factors.emplace_shared<BayesTreeOrphanWrapper<ISAM2::Clique> >(orphan); | ||||
|   gttoc(orphans); | ||||
| 
 | ||||
|   // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm
 | ||||
|  |  | |||
|  | @ -213,7 +213,7 @@ NonlinearFactorGraph LinearContainerFactor::ConvertLinearGraph( | |||
|   result.reserve(linear_graph.size()); | ||||
|   for (const auto& f : linear_graph) | ||||
|     if (f) | ||||
|       result += std::make_shared<LinearContainerFactor>(f, linearizationPoint); | ||||
|       result.emplace_shared<LinearContainerFactor>(f, linearizationPoint); | ||||
|   return result; | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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<JacobianFactor>(key, item->A, item->b, item->model); | ||||
|       damped.emplace_shared<JacobianFactor>(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<JacobianFactor>(key, item->A, item->b, item->model); | ||||
|         damped.emplace_shared<JacobianFactor>(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
 | ||||
|       } | ||||
|  |  | |||
|  | @ -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); | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue