Removed remaining references to denseQR, some fixes in NonlinearOptimizer
							parent
							
								
									3b09594a3c
								
							
						
					
					
						commit
						acde4d99a5
					
				|  | @ -16,8 +16,8 @@ noinst_PROGRAMS += PlanarSLAMExample_easy			# Solves SLAM example from tutorial | |||
| noinst_PROGRAMS += PlanarSLAMSelfContained_advanced	# Solves SLAM example from tutorial with all typedefs in the script | ||||
| noinst_PROGRAMS += Pose2SLAMExample_easy 			# Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface | ||||
| noinst_PROGRAMS += Pose2SLAMExample_advanced		# Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface | ||||
| noinst_PROGRAMS += Pose2SLAMwSPCG_easy      		# Solves a simple Pose2 SLAM example with advanced SPCG solver | ||||
| noinst_PROGRAMS += Pose2SLAMwSPCG_advanced  		# Solves a simple Pose2 SLAM example with easy SPCG solver | ||||
| #noinst_PROGRAMS += Pose2SLAMwSPCG_easy      		# Solves a simple Pose2 SLAM example with advanced SPCG solver
 | ||||
| #noinst_PROGRAMS += Pose2SLAMwSPCG_advanced  		# Solves a simple Pose2 SLAM example with easy SPCG solver
 | ||||
| SUBDIRS = vSLAMexample # does not compile.... | ||||
| #----------------------------------------------------------------------------------------------------
 | ||||
| # rules to build local programs
 | ||||
|  |  | |||
|  | @ -66,8 +66,7 @@ int main(int argc, char** argv) { | |||
| 	Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial); | ||||
| 
 | ||||
| 	/* 4.2.2 set up solver and optimize */ | ||||
| 	Optimizer::shared_solver solver(new Optimizer::solver(ordering)); | ||||
| 	Optimizer optimizer(graph, initial, solver); | ||||
| 	Optimizer optimizer(graph, initial, ordering); | ||||
| 	Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT; | ||||
| 	Optimizer optimizer_result = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity); | ||||
| 
 | ||||
|  |  | |||
|  | @ -16,7 +16,7 @@ GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<Gaussia | |||
|     junctionTree_(new GaussianJunctionTree(factorGraph)) {} | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| typename BayesTree<GaussianConditional>::sharedClique GaussianMultifrontalSolver::eliminate() const { | ||||
| BayesTree<GaussianConditional>::sharedClique GaussianMultifrontalSolver::eliminate() const { | ||||
|   return junctionTree_->eliminate(); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -56,7 +56,7 @@ public: | |||
|    * Eliminate the factor graph sequentially.  Uses a column elimination tree | ||||
|    * to recursively eliminate. | ||||
|    */ | ||||
|   typename BayesTree<GaussianConditional>::sharedClique eliminate() const; | ||||
|   BayesTree<GaussianConditional>::sharedClique eliminate() const; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Compute the least-squares solution of the GaussianFactorGraph.  This | ||||
|  |  | |||
|  | @ -31,7 +31,6 @@ | |||
| 
 | ||||
| #include <gtsam/linear/NoiseModel.h> | ||||
| #include <gtsam/linear/SharedDiagonal.h> | ||||
| #include <gtsam/base/DenseQRUtil.h> | ||||
| 
 | ||||
| namespace ublas = boost::numeric::ublas; | ||||
| typedef ublas::matrix_column<Matrix> column; | ||||
|  | @ -220,23 +219,28 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<std::vector<long>&> firs | |||
| 
 | ||||
| // General QR, see also special version in Constrained
 | ||||
| SharedDiagonal Gaussian::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<long>& firstZeroRows) const { | ||||
| 
 | ||||
|   // get size(A) and maxRank
 | ||||
|   // TODO: really no rank problems ?
 | ||||
|   size_t m = Ab.size1(), n = Ab.size2()-1; | ||||
|   size_t maxRank = min(m,n); | ||||
| 
 | ||||
|   // pre-whiten everything (cheaply if possible)
 | ||||
|   WhitenInPlace(Ab); | ||||
| 
 | ||||
|   // Perform in-place Householder
 | ||||
| #ifdef GT_USE_LAPACK | ||||
|   householder_denseqr_colmajor(Ab, &firstZeroRows[0]); | ||||
| #else | ||||
|   householder(Ab, maxRank); | ||||
| #endif | ||||
| 
 | ||||
|   return Unit::Create(maxRank); | ||||
| 	Matrix Abresult(Ab); | ||||
| 	gtsam::print(Abresult, "Abresult before = "); | ||||
| 	SharedDiagonal result = QR(Abresult, firstZeroRows); | ||||
| 	gtsam::print(Abresult, "Abresult after = "); | ||||
| 	Ab = Abresult; | ||||
| 	return result; | ||||
| //  // get size(A) and maxRank
 | ||||
| //  // TODO: really no rank problems ?
 | ||||
| //  size_t m = Ab.size1(), n = Ab.size2()-1;
 | ||||
| //  size_t maxRank = min(m,n);
 | ||||
| //
 | ||||
| //  // pre-whiten everything (cheaply if possible)
 | ||||
| //  WhitenInPlace(Ab);
 | ||||
| //
 | ||||
| //  // Perform in-place Householder
 | ||||
| //#ifdef GT_USE_LAPACK
 | ||||
| //  householder_denseqr_colmajor(Ab, &firstZeroRows[0]);
 | ||||
| //#else
 | ||||
| //  householder(Ab, maxRank);
 | ||||
| //#endif
 | ||||
| //
 | ||||
| //  return Unit::Create(maxRank);
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -400,10 +404,14 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<long>&> f | |||
| } | ||||
| 
 | ||||
| SharedDiagonal Constrained::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<long>& firstZeroRows) const { | ||||
|   Matrix AbRowWise(Ab); | ||||
|   SharedDiagonal result = this->QR(AbRowWise, firstZeroRows); | ||||
|   Ab = AbRowWise; | ||||
|   return result; | ||||
| //  Matrix AbRowWise(Ab);
 | ||||
| //  SharedDiagonal result = this->QR(AbRowWise, firstZeroRows);
 | ||||
| //  Ab = AbRowWise;
 | ||||
| //  return result;
 | ||||
| 	Matrix Abresult(Ab); | ||||
| 	SharedDiagonal result = QR(Abresult, firstZeroRows); | ||||
| 	Ab = Abresult; | ||||
| 	return result; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -119,7 +119,6 @@ namespace gtsam { | |||
|     /** Add a delta config to current config and returns a new config */ | ||||
|     LieValues expmap(const VectorValues& delta, const Ordering& ordering) const; | ||||
| 
 | ||||
| 
 | ||||
|     /** Get a delta config about a linearization point c0 (*this) */ | ||||
|     VectorValues logmap(const LieValues& cp, const Ordering& ordering) const; | ||||
| 
 | ||||
|  |  | |||
|  | @ -19,7 +19,8 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/linear/Factorization.h> | ||||
| #include <gtsam/linear/GaussianSequentialSolver.h> | ||||
| #include <gtsam/linear/GaussianMultifrontalSolver.h> | ||||
| #include <gtsam/linear/SubgraphSolver-inl.h> | ||||
| #include <gtsam/nonlinear/NonlinearOptimizer-inl.h> | ||||
| #include <gtsam/nonlinear/NonlinearOptimization.h> | ||||
|  | @ -38,10 +39,9 @@ namespace gtsam { | |||
| 	  Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); | ||||
| 
 | ||||
| 		// initial optimization state is the same in both cases tested
 | ||||
| 	  typedef NonlinearOptimizer<G, T> Optimizer; | ||||
| 	  typename Optimizer::shared_solver solver(new Factorization<G, T>(ordering)); | ||||
| 	  typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianSequentialSolver> Optimizer; | ||||
| 	  Optimizer optimizer(boost::make_shared<const G>(graph), | ||||
| 	  		boost::make_shared<const T>(initialEstimate), solver); | ||||
| 	  		boost::make_shared<const T>(initialEstimate), ordering); | ||||
| 
 | ||||
| 		// Levenberg-Marquardt
 | ||||
| 	  Optimizer result = optimizer.levenbergMarquardt(parameters); | ||||
|  | @ -53,28 +53,39 @@ namespace gtsam { | |||
| 	 */ | ||||
| 	template<class G, class T> | ||||
| 	T	optimizeMultiFrontal(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters) { | ||||
| 		throw runtime_error("optimizeMultiFrontal: not implemented"); | ||||
| 	} | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * The multifrontal solver | ||||
| 	 */ | ||||
| 	template<class G, class T> | ||||
| 	T	optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) { | ||||
| 		// Use a variable ordering from COLAMD
 | ||||
| 	  Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); | ||||
| 
 | ||||
| 		// initial optimization state is the same in both cases tested
 | ||||
| 		typedef NonlinearOptimizer<G, T, SubgraphPreconditioner, SubgraphSolver<G,T> > SPCGOptimizer; | ||||
| 		typename SPCGOptimizer::shared_solver solver(new SubgraphSolver<G,T>(graph, initialEstimate)); | ||||
| 		SPCGOptimizer optimizer( | ||||
| 				boost::make_shared<const G>(graph), | ||||
| 				boost::make_shared<const T>(initialEstimate), | ||||
| 				solver); | ||||
| 	  typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer; | ||||
| 	  Optimizer optimizer(boost::make_shared<const G>(graph), | ||||
| 	  		boost::make_shared<const T>(initialEstimate), ordering); | ||||
| 
 | ||||
| 		// Levenberg-Marquardt
 | ||||
| 		SPCGOptimizer result = optimizer.levenbergMarquardt(parameters); | ||||
| 	  Optimizer result = optimizer.levenbergMarquardt(parameters); | ||||
| 		return *result.config(); | ||||
| 	} | ||||
| 
 | ||||
| //	/**
 | ||||
| //	 * The multifrontal solver
 | ||||
| //	 */
 | ||||
| //	template<class G, class T>
 | ||||
| //	T	optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
 | ||||
| //
 | ||||
| //		// initial optimization state is the same in both cases tested
 | ||||
| //		typedef NonlinearOptimizer<G, T, SubgraphPreconditioner, SubgraphSolver<G,T> > SPCGOptimizer;
 | ||||
| //		typename SPCGOptimizer::shared_solver solver(new SubgraphSolver<G,T>(graph, initialEstimate));
 | ||||
| //		SPCGOptimizer optimizer(
 | ||||
| //				boost::make_shared<const G>(graph),
 | ||||
| //				boost::make_shared<const T>(initialEstimate),
 | ||||
| //				solver);
 | ||||
| //
 | ||||
| //		// Levenberg-Marquardt
 | ||||
| //		SPCGOptimizer result = optimizer.levenbergMarquardt(parameters);
 | ||||
| //		return *result.config();
 | ||||
| //	}
 | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * optimization that returns the values | ||||
| 	 */ | ||||
|  |  | |||
|  | @ -74,14 +74,14 @@ namespace gtsam { | |||
| 	/* ************************************************************************* */ | ||||
| 	template<class G, class C, class L, class S, class W> | ||||
| 	NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph, | ||||
| 			shared_values config, shared_solver solver, double lambda) : | ||||
| 		graph_(graph), config_(config), lambda_(lambda), solver_(solver) { | ||||
| 			shared_values config, shared_ordering ordering, double lambda) : | ||||
| 		graph_(graph), config_(config), ordering_(ordering), lambda_(lambda) { | ||||
| 		if (!graph) throw std::invalid_argument( | ||||
| 				"NonlinearOptimizer constructor: graph = NULL"); | ||||
| 		if (!config) throw std::invalid_argument( | ||||
| 				"NonlinearOptimizer constructor: config = NULL"); | ||||
| 		if (!solver) throw std::invalid_argument( | ||||
| 				"NonlinearOptimizer constructor: solver = NULL"); | ||||
| 		if (!ordering) throw std::invalid_argument( | ||||
| 				"NonlinearOptimizer constructor: ordering = NULL"); | ||||
| 		error_ = graph->error(*config); | ||||
| 	} | ||||
| 
 | ||||
|  | @ -90,9 +90,9 @@ namespace gtsam { | |||
| 	/* ************************************************************************* */ | ||||
| 	template<class G, class C, class L, class S, class W> | ||||
| 	VectorValues NonlinearOptimizer<G, C, L, S, W>::linearizeAndOptimizeForDelta() const { | ||||
| 		boost::shared_ptr<L> linearized = solver_->linearize(*graph_, *config_); | ||||
| 		NonlinearOptimizer prepared(graph_, config_, solver_->prepareLinear(*linearized), error_, lambda_); | ||||
| 		return prepared.solver_->optimize(*linearized); | ||||
| 		boost::shared_ptr<L> linearized = graph_->linearize(*config_, *ordering_); | ||||
| 		NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_); | ||||
| 		return *S(*linearized).optimize(); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
|  | @ -109,13 +109,13 @@ namespace gtsam { | |||
| 			delta.print("delta"); | ||||
| 
 | ||||
| 		// take old config and update it
 | ||||
| 		shared_values newValues(new C(solver_->expmap(*config_, delta))); | ||||
| 		shared_values newValues(new C(config_->expmap(delta, *ordering_))); | ||||
| 
 | ||||
| 		// maybe show output
 | ||||
| 		if (verbosity >= Parameters::CONFIG) | ||||
| 			newValues->print("newValues"); | ||||
| 
 | ||||
| 		NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, newValues, solver_, lambda_); | ||||
| 		NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, newValues, ordering_, lambda_); | ||||
| 
 | ||||
| 		if (verbosity >= Parameters::ERROR) | ||||
| 			cout << "error: " << newOptimizer.error_ << endl; | ||||
|  | @ -177,17 +177,17 @@ namespace gtsam { | |||
| 			damped.print("damped"); | ||||
| 
 | ||||
| 		// solve
 | ||||
| 		VectorValues delta = solver_->optimize(damped); | ||||
| 		VectorValues delta = *S(damped).optimize(); | ||||
| 		if (verbosity >= Parameters::TRYDELTA) | ||||
| 			delta.print("delta"); | ||||
| 
 | ||||
| 		// update config
 | ||||
| 		shared_values newValues(new C(solver_->expmap(*config_, delta))); // TODO: updateValues
 | ||||
| 		shared_values newValues(new C(config_->expmap(delta, *ordering_))); // TODO: updateValues
 | ||||
| //		if (verbosity >= TRYCONFIG)
 | ||||
| //			newValues->print("config");
 | ||||
| 
 | ||||
| 		// create new optimization state with more adventurous lambda
 | ||||
| 		NonlinearOptimizer next(graph_, newValues, solver_, lambda_ / factor); | ||||
| 		NonlinearOptimizer next(graph_, newValues, ordering_, lambda_ / factor); | ||||
| 		if (verbosity >= Parameters::TRYLAMBDA) cout << "next error = " << next.error_ << endl; | ||||
| 
 | ||||
| 		if(lambdaMode >= Parameters::CAUTIOUS) { | ||||
|  | @ -199,7 +199,7 @@ namespace gtsam { | |||
| 			// If we're cautious, see if the current lambda is better
 | ||||
| 			// todo:  include stopping criterion here?
 | ||||
| 			if(lambdaMode == Parameters::CAUTIOUS) { | ||||
| 				NonlinearOptimizer sameLambda(graph_, newValues, solver_, lambda_); | ||||
| 				NonlinearOptimizer sameLambda(graph_, newValues, ordering_, lambda_); | ||||
| 				if(sameLambda.error_ <= next.error_) | ||||
| 					return sameLambda; | ||||
| 			} | ||||
|  | @ -212,7 +212,7 @@ namespace gtsam { | |||
| 
 | ||||
| 			// A more adventerous lambda was worse.  If we're cautious, try the same lambda.
 | ||||
| 			if(lambdaMode == Parameters::CAUTIOUS) { | ||||
| 				NonlinearOptimizer sameLambda(graph_, newValues, solver_, lambda_); | ||||
| 				NonlinearOptimizer sameLambda(graph_, newValues, ordering_, lambda_); | ||||
| 				if(sameLambda.error_ <= error_) | ||||
| 					return sameLambda; | ||||
| 			} | ||||
|  | @ -223,9 +223,9 @@ namespace gtsam { | |||
| 
 | ||||
| 			// TODO: can we avoid copying the config ?
 | ||||
| 			if(lambdaMode >= Parameters::BOUNDED && lambda_ >= 1.0e5) { | ||||
| 				return NonlinearOptimizer(graph_, newValues, solver_, lambda_);; | ||||
| 				return NonlinearOptimizer(graph_, newValues, ordering_, lambda_);; | ||||
| 			} else { | ||||
| 				NonlinearOptimizer cautious(graph_, config_, solver_, lambda_ * factor); | ||||
| 				NonlinearOptimizer cautious(graph_, config_, ordering_, lambda_ * factor); | ||||
| 				return cautious.try_lambda(linear, verbosity, factor, lambdaMode); | ||||
| 			} | ||||
| 
 | ||||
|  | @ -248,8 +248,8 @@ namespace gtsam { | |||
| 			cout << "lambda = " << lambda_ << endl; | ||||
| 
 | ||||
| 		// linearize all factors once
 | ||||
| 		boost::shared_ptr<L> linear = solver_->linearize(*graph_, *config_); | ||||
| 		NonlinearOptimizer prepared(graph_, config_, solver_->prepareLinear(*linear), error_, lambda_); | ||||
| 		boost::shared_ptr<L> linear = graph_->linearize(*config_, *ordering_); | ||||
| 		NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_); | ||||
| 		if (verbosity >= Parameters::LINEAR) | ||||
| 			linear->print("linear"); | ||||
| 
 | ||||
|  |  | |||
|  | @ -63,15 +63,16 @@ namespace gtsam { | |||
| 	 * | ||||
| 	 * | ||||
| 	 */ | ||||
| 	template<class G, class T, class L = GaussianFactorGraph, class S = Factorization<G, T>, class W = NullOptimizerWriter> | ||||
| 	template<class G, class T, class L = GaussianFactorGraph, class GS = GaussianSequentialSolver, class W = NullOptimizerWriter> | ||||
| 	class NonlinearOptimizer { | ||||
| 	public: | ||||
| 
 | ||||
| 		// For performance reasons in recursion, we store configs in a shared_ptr
 | ||||
| 		typedef boost::shared_ptr<const T> shared_values; | ||||
| 		typedef boost::shared_ptr<const G> shared_graph; | ||||
| 		typedef boost::shared_ptr<const S> shared_solver; | ||||
| 		typedef const S solver; | ||||
| 		typedef boost::shared_ptr<Ordering> shared_ordering; | ||||
| 		//typedef boost::shared_ptr<const GS> shared_solver;
 | ||||
| 		//typedef const GS solver;
 | ||||
| 		typedef NonlinearOptimizationParameters Parameters; | ||||
| 
 | ||||
| 	private: | ||||
|  | @ -85,13 +86,14 @@ namespace gtsam { | |||
| 		const shared_values config_; | ||||
| 		double error_; // TODO FD: no more const because in constructor I need to set it after checking :-(
 | ||||
| 
 | ||||
| 		const shared_ordering ordering_; | ||||
| 		// the linear system solver
 | ||||
| 		//const shared_solver solver_;
 | ||||
| 
 | ||||
| 		// keep current lambda for use within LM only
 | ||||
| 		// TODO: red flag, should we have an LM class ?
 | ||||
| 		const double lambda_; | ||||
| 
 | ||||
| 		// the linear system solver
 | ||||
| 		const shared_solver solver_; | ||||
| 
 | ||||
| 		// Recursively try to do tempered Gauss-Newton steps until we succeed
 | ||||
| 		NonlinearOptimizer try_lambda(const L& linear, | ||||
| 				Parameters::verbosityLevel verbosity, double factor, Parameters::LambdaMode lambdaMode) const; | ||||
|  | @ -101,22 +103,22 @@ namespace gtsam { | |||
| 		/**
 | ||||
| 		 * Constructor that evaluates new error | ||||
| 		 */ | ||||
| 		NonlinearOptimizer(shared_graph graph, shared_values config, shared_solver solver, | ||||
| 		NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering, | ||||
| 				const double lambda = 1e-5); | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Constructor that does not do any computation | ||||
| 		 */ | ||||
| 		NonlinearOptimizer(shared_graph graph, shared_values config, shared_solver solver, | ||||
| 		NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering, | ||||
| 				const double error, const double lambda): graph_(graph), config_(config), | ||||
| 			  error_(error), lambda_(lambda), solver_(solver) {} | ||||
| 			  error_(error), ordering_(ordering), lambda_(lambda) {} | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Copy constructor | ||||
| 		 */ | ||||
| 		NonlinearOptimizer(const NonlinearOptimizer<G, T, L, S> &optimizer) : | ||||
| 		NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) : | ||||
| 		  graph_(optimizer.graph_), config_(optimizer.config_), | ||||
| 		  error_(optimizer.error_), lambda_(optimizer.lambda_), solver_(optimizer.solver_) {} | ||||
| 		  error_(optimizer.error_), ordering_(optimizer.ordering_), lambda_(optimizer.lambda_) {} | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Return current error | ||||
|  | @ -205,8 +207,8 @@ namespace gtsam { | |||
| 			double relativeThreshold = 1e-5, absoluteThreshold = 1e-5; | ||||
| 
 | ||||
| 			// initial optimization state is the same in both cases tested
 | ||||
| 			shared_solver solver(new S(ordering)); | ||||
| 			NonlinearOptimizer optimizer(graph, config, solver); | ||||
| 			GS solver(*graph->linearize(*config, *ordering)); | ||||
| 			NonlinearOptimizer optimizer(graph, config, ordering); | ||||
| 
 | ||||
| 			// Levenberg-Marquardt
 | ||||
| 			NonlinearOptimizer result = optimizer.levenbergMarquardt(relativeThreshold, | ||||
|  | @ -236,8 +238,8 @@ namespace gtsam { | |||
| 			double relativeThreshold = 1e-5, absoluteThreshold = 1e-5; | ||||
| 
 | ||||
| 			// initial optimization state is the same in both cases tested
 | ||||
| 			shared_solver solver(new S(ordering)); | ||||
| 			NonlinearOptimizer optimizer(graph, config, solver); | ||||
| 			GS solver(*graph->linearize(*config, *ordering)); | ||||
| 			NonlinearOptimizer optimizer(graph, config, ordering); | ||||
| 
 | ||||
| 			// Gauss-Newton
 | ||||
| 			NonlinearOptimizer result = optimizer.gaussNewton(relativeThreshold, | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue