Qualified more namespaces after removing using namespace from headers
							parent
							
								
									d188ed2e1a
								
							
						
					
					
						commit
						1913640d2c
					
				|  | @ -101,7 +101,7 @@ namespace gtsam { | |||
| 	typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors, | ||||
|     const FastMap<KEY, std::vector<KEY> >& variableSlots) { | ||||
| 
 | ||||
| 		typedef const pair<const KEY, std::vector<KEY> > KeySlotPair; | ||||
| 		typedef const std::pair<const KEY, std::vector<KEY> > KeySlotPair; | ||||
|     // Local functional for getting keys out of key-value pairs
 | ||||
|     struct Local { static KEY FirstOf(const KeySlotPair& pr) { return pr.first; } }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -67,8 +67,7 @@ namespace gtsam { | |||
| 	IndexFactor::shared_ptr CombineSymbolic( | ||||
| 			const FactorGraph<IndexFactor>& factors, const FastMap<Index, | ||||
| 					std::vector<Index> >& variableSlots) { | ||||
| 		IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors, | ||||
| 				variableSlots)); | ||||
| 		IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors, variableSlots)); | ||||
| //		combined->assertInvariants();
 | ||||
| 		return combined; | ||||
| 	} | ||||
|  |  | |||
|  | @ -70,7 +70,7 @@ SDGraph<KEY> toBoostGraph(const G& graph) { | |||
| 
 | ||||
| 	for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) { | ||||
| 		if ((*itFactor)->keys().size() > 2) | ||||
| 			throw(invalid_argument("toBoostGraph: only support factors with at most two keys")); | ||||
| 			throw(std::invalid_argument("toBoostGraph: only support factors with at most two keys")); | ||||
| 
 | ||||
| 		if ((*itFactor)->keys().size() == 1) | ||||
| 			continue; | ||||
|  | @ -131,7 +131,7 @@ predecessorMap2Graph(const PredecessorMap<KEY>& p_map) { | |||
| 	} | ||||
| 
 | ||||
| 	if (!foundRoot) | ||||
| 		throw invalid_argument("predecessorMap2Graph: invalid predecessor map!"); | ||||
| 		throw std::invalid_argument("predecessorMap2Graph: invalid predecessor map!"); | ||||
| 	else | ||||
| 	  return boost::tuple<G, V, std::map<KEY, V> >(g, root, key2vertex); | ||||
| } | ||||
|  | @ -171,7 +171,7 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY> | |||
| 
 | ||||
| 	PoseGraph g; | ||||
| 	PoseVertex root; | ||||
| 	map<KEY, PoseVertex> key2vertex; | ||||
| 	std::map<KEY, PoseVertex> key2vertex; | ||||
| 	boost::tie(g, root, key2vertex) = | ||||
| 			predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree); | ||||
| 
 | ||||
|  | @ -181,7 +181,7 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY> | |||
| 	BOOST_FOREACH(typename G::sharedFactor nl_factor, graph) { | ||||
| 
 | ||||
| 		if (nl_factor->keys().size() > 2) | ||||
| 			throw invalid_argument("composePoses: only support factors with at most two keys"); | ||||
| 			throw std::invalid_argument("composePoses: only support factors with at most two keys"); | ||||
| 
 | ||||
| 		// e.g. in pose2graph, nonlinear factor needs to be converted to pose2factor
 | ||||
| 		boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor); | ||||
|  | @ -196,7 +196,7 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY> | |||
| 		POSE l1Xl2 = factor->measured(); | ||||
| 		boost::tie(edge12, found1) = boost::edge(v1, v2, g); | ||||
| 		boost::tie(edge21, found2) = boost::edge(v2, v1, g); | ||||
| 		if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree"); | ||||
| 		if (found1 && found2) throw std::invalid_argument ("composePoses: invalid spanning tree"); | ||||
| 		if (!found1 && !found2) continue; | ||||
| 		if (found1) | ||||
| 			boost::put(boost::edge_weight, g, edge12, l1Xl2); | ||||
|  | @ -223,13 +223,13 @@ PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) { | |||
| 	SDGraph<KEY> g = gtsam::toBoostGraph<G, FACTOR2, KEY>(fg); | ||||
| 
 | ||||
| 	// find minimum spanning tree
 | ||||
| 	vector<typename SDGraph<KEY>::Vertex> p_map(boost::num_vertices(g)); | ||||
| 	std::vector<typename SDGraph<KEY>::Vertex> p_map(boost::num_vertices(g)); | ||||
| 	prim_minimum_spanning_tree(g, &p_map[0]); | ||||
| 
 | ||||
| 	// convert edge to string pairs
 | ||||
| 	PredecessorMap<KEY> tree; | ||||
| 	typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first; | ||||
| 	typename vector<typename SDGraph<KEY>::Vertex>::iterator vi; | ||||
| 	typename std::vector<typename SDGraph<KEY>::Vertex>::iterator vi; | ||||
| 	for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) { | ||||
| 		KEY key = boost::get(boost::vertex_name, g, *itVertex); | ||||
| 		KEY parent = boost::get(boost::vertex_name, g, *vi); | ||||
|  | @ -248,7 +248,7 @@ void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) { | |||
| 	BOOST_FOREACH(const F& factor, g) | ||||
| 	{ | ||||
| 		if (factor->keys().size() > 2) | ||||
| 			throw(invalid_argument("split: only support factors with at most two keys")); | ||||
| 			throw(std::invalid_argument("split: only support factors with at most two keys")); | ||||
| 
 | ||||
| 		if (factor->keys().size() == 1) { | ||||
| 			Ab1.push_back(factor); | ||||
|  |  | |||
|  | @ -176,7 +176,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( | |||
|     result.dx_d = ComputeDoglegPoint(Delta, dx_u, dx_n, verbose); | ||||
|     toc(3, "Dog leg point"); | ||||
| 
 | ||||
|     if(verbose) cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << endl; | ||||
|     if(verbose) std::cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << std::endl; | ||||
| 
 | ||||
|     tic(4, "retract"); | ||||
|     // Compute expmapped solution
 | ||||
|  | @ -193,8 +193,8 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( | |||
|     const double new_M_error = jfg.error(result.dx_d); | ||||
|     toc(6, "decrease in M"); | ||||
| 
 | ||||
|     if(verbose) cout << setprecision(15) << "f error: " << f_error << " -> " << result.f_error << endl; | ||||
|     if(verbose) cout << setprecision(15) << "M error: " << M_error << " -> " << new_M_error << endl; | ||||
|     if(verbose) std::cout << std::setprecision(15) << "f error: " << f_error << " -> " << result.f_error << std::endl; | ||||
|     if(verbose) std::cout << std::setprecision(15) << "M error: " << M_error << " -> " << new_M_error << std::endl; | ||||
| 
 | ||||
|     tic(7, "adjust Delta"); | ||||
|     // Compute gain ratio.  Here we take advantage of the invariant that the
 | ||||
|  | @ -203,7 +203,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( | |||
|         0.5 : | ||||
|         (f_error - result.f_error) / (M_error - new_M_error); | ||||
| 
 | ||||
|     if(verbose) cout << setprecision(15) << "rho = " << rho << endl; | ||||
|     if(verbose) std::cout << std::setprecision(15) << "rho = " << rho << std::endl; | ||||
| 
 | ||||
|     if(rho >= 0.75) { | ||||
|       // M agrees very well with f, so try to increase lambda
 | ||||
|  | @ -253,7 +253,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( | |||
|         stay = true; | ||||
|         lastAction = DECREASED_DELTA; | ||||
|       } else { | ||||
|         if(verbose) cout << "Warning:  Dog leg stopping because cannot decrease error with minimum Delta" << endl; | ||||
|         if(verbose) std::cout << "Warning:  Dog leg stopping because cannot decrease error with minimum Delta" << std::endl; | ||||
|         stay = false; | ||||
|       } | ||||
|     } | ||||
|  |  | |||
|  | @ -50,7 +50,7 @@ namespace gtsam { | |||
| 
 | ||||
| 			// init gamma and calculate threshold
 | ||||
| 			gamma = dot(g,g) ; | ||||
| 			threshold = ::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma); | ||||
| 			threshold = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma); | ||||
| 
 | ||||
| 			// Allocate and calculate A*d for first iteration
 | ||||
| 			if (gamma > parameters_.epsilon_abs()) Ad = Ab * d; | ||||
|  | @ -59,10 +59,10 @@ namespace gtsam { | |||
| 		/* ************************************************************************* */ | ||||
| 		// print
 | ||||
| 		void print(const V& x) { | ||||
| 			cout << "iteration = " << k << endl; | ||||
| 		  std::cout << "iteration = " << k << std::endl; | ||||
| 			gtsam::print(x,"x"); | ||||
| 			gtsam::print(g, "g"); | ||||
| 			cout << "dotg = " << gamma << endl; | ||||
| 			std::cout << "dotg = " << gamma << std::endl; | ||||
| 			gtsam::print(d, "d"); | ||||
| 			gtsam::print(Ad, "Ad"); | ||||
| 		} | ||||
|  | @ -92,8 +92,8 @@ namespace gtsam { | |||
| 			// check for convergence
 | ||||
| 			double new_gamma = dot(g, g); | ||||
| 			if (parameters_.verbosity() != IterativeOptimizationParameters::SILENT) | ||||
| 				cout << "iteration " << k << ": alpha = " << alpha | ||||
| 				     << ", dotg = " << new_gamma << endl; | ||||
| 			  std::cout << "iteration " << k << ": alpha = " << alpha | ||||
| 				     << ", dotg = " << new_gamma << std::endl; | ||||
| 			if (new_gamma < threshold) return true; | ||||
| 
 | ||||
| 			// calculate new search direction
 | ||||
|  | @ -126,14 +126,14 @@ namespace gtsam { | |||
| 
 | ||||
| 		CGState<S, V, E> state(Ab, x, parameters, steepest); | ||||
| 		if (parameters.verbosity() != IterativeOptimizationParameters::SILENT) | ||||
| 			cout << "CG: epsilon = " << parameters.epsilon() | ||||
| 		  std::cout << "CG: epsilon = " << parameters.epsilon() | ||||
| 				 << ", maxIterations = " << parameters.maxIterations() | ||||
| 				 << ", ||g0||^2 = " << state.gamma | ||||
| 				 << ", threshold = " << state.threshold << endl; | ||||
| 				 << ", threshold = " << state.threshold << std::endl; | ||||
| 
 | ||||
| 		if ( state.gamma < state.threshold ) { | ||||
| 			if (parameters.verbosity() != IterativeOptimizationParameters::SILENT) | ||||
| 				cout << "||g0||^2 < threshold, exiting immediately !" << endl; | ||||
| 			  std::cout << "||g0||^2 < threshold, exiting immediately !" << std::endl; | ||||
| 
 | ||||
| 			return x; | ||||
| 		} | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue