12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

VariableIndex is now constructed at the creation of a NonlinearOptimizer and passed in at solver creation.

release/4.3a0
Alex Cunningham 2011-02-10 16:51:56 +00:00
parent 7407843214
commit 9f4661544f
3 changed files with 47 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/linear/SubgraphSolver.h
View File

@ -62,6 +62,11 @@ namespace gtsam {
throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported");
}
SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr<VariableIndex>& structure) {
std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl;
throw std::runtime_error("SubgraphSolver: gaussian factor graph and variable index initialization not supported");
}
SubgraphSolver(const SubgraphSolver& solver) :
IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_){}

20
gtsam/nonlinear/NonlinearOptimizer-inl.h
View File

@ -36,7 +36,9 @@ namespace gtsam {
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
shared_values values, shared_ordering ordering, shared_parameters parameters) :
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
parameters_(parameters), iterations_(0), dimensions_(new vector<size_t>(values->dims(*ordering))) {
parameters_(parameters), iterations_(0),
dimensions_(new vector<size_t>(values->dims(*ordering))),
structure_(new VariableIndex(*graph->symbolic(*values, *ordering))) {
if (!graph) throw std::invalid_argument(
"NonlinearOptimizer constructor: graph = NULL");
if (!values) throw std::invalid_argument(
@ -46,6 +48,7 @@ namespace gtsam {
}
/* ************************************************************************* */
// FIXME: remove this constructor
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
shared_values values, shared_ordering ordering,
@ -70,10 +73,14 @@ namespace gtsam {
Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
// FIXME: allow for passing variable index through createSolver()
// FIXME: get rid of spcg solver
if (spcg_solver_) spcg_solver_->replaceFactors(linearize());
shared_solver solver = (spcg_solver_) ? spcg_solver_ : createSolver();
shared_solver solver;
if (spcg_solver_) { // special case for SPCG
spcg_solver_->replaceFactors(linearize());
solver = spcg_solver_;
} else { // normal case
solver = createSolver();
}
VectorValues delta = *solver->optimize();
@ -159,10 +166,9 @@ namespace gtsam {
if (verbosity >= Parameters::DAMPED) damped->print("damped");
// solve
// FIXME: incorporate variable index
// FIXME: remove spcg specific code
// FIXME: remove spcg specific code
if (spcg_solver_) spcg_solver_->replaceFactors(damped);
shared_solver solver = (spcg_solver_) ? spcg_solver_ : shared_solver(new S(*damped));
shared_solver solver = (spcg_solver_) ? spcg_solver_ : shared_solver(new S(damped, structure_));
VectorValues delta = *solver->optimize();
if (verbosity >= Parameters::TRYDELTA) delta.print("delta");

42
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -75,6 +75,7 @@ public:
typedef boost::shared_ptr<GS> shared_solver;
typedef NonlinearOptimizationParameters Parameters;
typedef boost::shared_ptr<const Parameters> shared_parameters ;
typedef boost::shared_ptr<VariableIndex> shared_structure; // TODO: make this const
private:
@ -104,6 +105,11 @@ private:
// The dimensions of each linearized variable
const shared_dimensions dimensions_;
// storage of structural components that don't change between iterations
// used at creation of solvers in each iteration
// TODO: make this structure component specific to solver type
const shared_structure structure_;
// solver used only for SPCG
// FIXME: remove this!
shared_solver spcg_solver_;
@ -111,29 +117,38 @@ private:
/**
* Constructor that does not do any computation
*/
NonlinearOptimizer(shared_graph graph, shared_values values, const double error, shared_ordering ordering,
shared_parameters parameters, shared_dimensions dimensions, size_t iterations): graph_(graph), values_(values),
error_(error), ordering_(ordering), parameters_(parameters), iterations_(iterations), dimensions_(dimensions) {}
NonlinearOptimizer(shared_graph graph, shared_values values, const double error,
shared_ordering ordering, shared_parameters parameters, shared_dimensions dimensions,
size_t iterations, shared_structure structure)
: graph_(graph), values_(values), error_(error), ordering_(ordering),
parameters_(parameters), iterations_(iterations), dimensions_(dimensions),
structure_(structure) {}
/** constructors to replace specific components */
This newValues_(shared_values newValues) const {
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, parameters_, dimensions_, iterations_); }
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues),
ordering_, parameters_, dimensions_, iterations_, structure_); }
This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const {
return NonlinearOptimizer(graph_, newValues, newError, ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); }
return NonlinearOptimizer(graph_, newValues, newError, ordering_,
parameters_->newLambda_(newLambda), dimensions_, iterations_, structure_); }
This newIterations_(int iterations) const {
return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters_, dimensions_, iterations); }
return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters_, dimensions_,
iterations, structure_); }
This newLambda_(double newLambda) const {
return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); }
return NonlinearOptimizer(graph_, values_, error_, ordering_,
parameters_->newLambda_(newLambda), dimensions_, iterations_, structure_); }
This newValuesLambda_(shared_values newValues, double newLambda) const {
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); }
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues),
ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_, structure_); }
This newParameters_(shared_parameters parameters) const {
return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters, dimensions_, iterations_); }
return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters, dimensions_,
iterations_, structure_); }
public:
@ -161,7 +176,7 @@ public:
NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_),
ordering_(optimizer.ordering_), parameters_(optimizer.parameters_),
iterations_(0), dimensions_(optimizer.dimensions_) {}
iterations_(0), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {}
// access to member variables
@ -186,6 +201,9 @@ public:
/** Return the parameters */
shared_parameters parameters() const { return parameters_; }
/** Return the structure variable (variable index) */
shared_structure structure() const { return structure_; }
/**
* Return a linearized graph at the current graph/values/ordering
*/
@ -198,9 +216,7 @@ public:
* NOTE: this will actually solve a linear system
*/
shared_solver createSolver() const {
shared_linear linearGraph = linearize();
shared_solver solver(new GS(*linearGraph));
return solver;
return shared_solver(new GS(linearize(), structure_));
}
/**