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