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

[inprogress] switching to DynamicValues

release/4.3a0
Duy-Nguyen Ta 2012-01-28 20:47:43 +00:00
parent ee0b7f8a74
commit d3d5ee3b39
19 changed files with 176 additions and 185 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/nonlinear/NonlinearEquality.h
View File

@ -45,8 +45,8 @@ namespace gtsam {
* - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain * - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain
* - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error * - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error
*/ */
template<class VALUES, class KEY> template<class KEY>
class NonlinearEquality: public NonlinearFactor1<VALUES, KEY> { class NonlinearEquality: public NonlinearFactor1<KEY> {
public: public:
typedef typename KEY::Value T; typedef typename KEY::Value T;
@ -69,7 +69,7 @@ namespace gtsam {
*/ */
bool (*compare_)(const T& a, const T& b); bool (*compare_)(const T& a, const T& b);
typedef NonlinearFactor1<VALUES, KEY> Base; typedef NonlinearFactor1<KEY> Base;
/** default constructor - only for serialization */ /** default constructor - only for serialization */
NonlinearEquality() {} NonlinearEquality() {}
@ -101,14 +101,14 @@ namespace gtsam {
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearEquality<VALUES,KEY>& f, double tol = 1e-9) const { bool equals(const NonlinearEquality<KEY>& f, double tol = 1e-9) const {
if (!Base::equals(f)) return false; if (!Base::equals(f)) return false;
return feasible_.equals(f.feasible_, tol) && return feasible_.equals(f.feasible_, tol) &&
fabs(error_gain_ - f.error_gain_) < tol; fabs(error_gain_ - f.error_gain_) < tol;
} }
/** actual error function calculation */ /** actual error function calculation */
virtual double error(const VALUES& c) const { virtual double error(const DynamicValues& c) const {
const T& xj = c[this->key_]; const T& xj = c[this->key_];
Vector e = this->unwhitenedError(c); Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) { if (allow_error_ || !compare_(xj, feasible_)) {
@ -135,7 +135,7 @@ namespace gtsam {
} }
// Linearize is over-written, because base linearization tries to whiten // Linearize is over-written, because base linearization tries to whiten
virtual GaussianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { virtual GaussianFactor::shared_ptr linearize(const DynamicValues& x, const Ordering& ordering) const {
const T& xj = x[this->key_]; const T& xj = x[this->key_];
Matrix A; Matrix A;
Vector b = evaluateError(xj, A); Vector b = evaluateError(xj, A);
@ -162,14 +162,14 @@ namespace gtsam {
/** /**
* Simple unary equality constraint - fixes a value for a variable * Simple unary equality constraint - fixes a value for a variable
*/ */
template<class VALUES, class KEY> template<class KEY>
class NonlinearEquality1 : public NonlinearFactor1<VALUES, KEY> { class NonlinearEquality1 : public NonlinearFactor1<KEY> {
public: public:
typedef typename KEY::Value X; typedef typename KEY::Value X;
protected: protected:
typedef NonlinearFactor1<VALUES, KEY> Base; typedef NonlinearFactor1<KEY> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
NonlinearEquality1() {} NonlinearEquality1() {}
@ -181,7 +181,7 @@ namespace gtsam {
public: public:
typedef boost::shared_ptr<NonlinearEquality1<VALUES, KEY> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality1<KEY> > shared_ptr;
NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0) NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {} : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
@ -220,13 +220,13 @@ namespace gtsam {
* Simple binary equality constraint - this constraint forces two factors to * Simple binary equality constraint - this constraint forces two factors to
* be the same. * be the same.
*/ */
template<class VALUES, class KEY> template<class KEY>
class NonlinearEquality2 : public NonlinearFactor2<VALUES, KEY, KEY> { class NonlinearEquality2 : public NonlinearFactor2<KEY, KEY> {
public: public:
typedef typename KEY::Value X; typedef typename KEY::Value X;
protected: protected:
typedef NonlinearFactor2<VALUES, KEY, KEY> Base; typedef NonlinearFactor2<KEY, KEY> Base;
GTSAM_CONCEPT_MANIFOLD_TYPE(X); GTSAM_CONCEPT_MANIFOLD_TYPE(X);
@ -235,7 +235,7 @@ namespace gtsam {
public: public:
typedef boost::shared_ptr<NonlinearEquality2<VALUES, KEY> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality2<KEY> > shared_ptr;
NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0) NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}

36
gtsam/nonlinear/NonlinearOptimization-inl.h
View File

@ -31,17 +31,17 @@ namespace gtsam {
/** /**
* The Elimination solver * The Elimination solver
*/ */
template<class G, class T> template<class G>
T optimizeSequential(const G& graph, const T& initialEstimate, DynamicValues optimizeSequential(const G& graph, const DynamicValues& initialEstimate,
const NonlinearOptimizationParameters& parameters, bool useLM) { const NonlinearOptimizationParameters& parameters, bool useLM) {
// Use a variable ordering from COLAMD // Use a variable ordering from COLAMD
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
// Create an nonlinear Optimizer that uses a Sequential Solver // Create an nonlinear Optimizer that uses a Sequential Solver
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianSequentialSolver> Optimizer; typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph), Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate), ordering, boost::make_shared<const DynamicValues>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters)); boost::make_shared<NonlinearOptimizationParameters>(parameters));
// Now optimize using either LM or GN methods. // Now optimize using either LM or GN methods.
@ -54,17 +54,17 @@ namespace gtsam {
/** /**
* The multifrontal solver * The multifrontal solver
*/ */
template<class G, class T> template<class G>
T optimizeMultiFrontal(const G& graph, const T& initialEstimate, DynamicValues optimizeMultiFrontal(const G& graph, const DynamicValues& initialEstimate,
const NonlinearOptimizationParameters& parameters, bool useLM) { const NonlinearOptimizationParameters& parameters, bool useLM) {
// Use a variable ordering from COLAMD // Use a variable ordering from COLAMD
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
// Create an nonlinear Optimizer that uses a Multifrontal Solver // Create an nonlinear Optimizer that uses a Multifrontal Solver
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer; typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph), Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate), ordering, boost::make_shared<const DynamicValues>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters)); boost::make_shared<NonlinearOptimizationParameters>(parameters));
// now optimize using either LM or GN methods // now optimize using either LM or GN methods
@ -77,20 +77,20 @@ namespace gtsam {
/** /**
* The sparse preconditioned conjugate gradient solver * The sparse preconditioned conjugate gradient solver
*/ */
template<class G, class T> template<class G>
T optimizeSPCG(const G& graph, const T& initialEstimate, DynamicValues optimizeSPCG(const G& graph, const DynamicValues& initialEstimate,
const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(), const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
bool useLM = true) { bool useLM = true) {
// initial optimization state is the same in both cases tested // initial optimization state is the same in both cases tested
typedef SubgraphSolver<G,GaussianFactorGraph,T> Solver; typedef SubgraphSolver<G,GaussianFactorGraph,DynamicValues> Solver;
typedef boost::shared_ptr<Solver> shared_Solver; typedef boost::shared_ptr<Solver> shared_Solver;
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> SPCGOptimizer; typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, Solver> SPCGOptimizer;
shared_Solver solver = boost::make_shared<Solver>( shared_Solver solver = boost::make_shared<Solver>(
graph, initialEstimate, IterativeOptimizationParameters()); graph, initialEstimate, IterativeOptimizationParameters());
SPCGOptimizer optimizer( SPCGOptimizer optimizer(
boost::make_shared<const G>(graph), boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate), boost::make_shared<const DynamicValues>(initialEstimate),
solver->ordering(), solver->ordering(),
solver, solver,
boost::make_shared<NonlinearOptimizationParameters>(parameters)); boost::make_shared<NonlinearOptimizationParameters>(parameters));
@ -105,19 +105,19 @@ namespace gtsam {
/** /**
* optimization that returns the values * optimization that returns the values
*/ */
template<class G, class T> template<class G>
T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters, DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate, const NonlinearOptimizationParameters& parameters,
const LinearSolver& solver, const LinearSolver& solver,
const NonlinearOptimizationMethod& nonlinear_method) { const NonlinearOptimizationMethod& nonlinear_method) {
switch (solver) { switch (solver) {
case SEQUENTIAL: case SEQUENTIAL:
return optimizeSequential<G,T>(graph, initialEstimate, parameters, return optimizeSequential<G,DynamicValues>(graph, initialEstimate, parameters,
nonlinear_method == LM); nonlinear_method == LM);
case MULTIFRONTAL: case MULTIFRONTAL:
return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters, return optimizeMultiFrontal<G,DynamicValues>(graph, initialEstimate, parameters,
nonlinear_method == LM); nonlinear_method == LM);
case SPCG: case SPCG:
// return optimizeSPCG<G,T>(graph, initialEstimate, parameters, // return optimizeSPCG<G,DynamicValues>(graph, initialEstimate, parameters,
// nonlinear_method == LM); // nonlinear_method == LM);
throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint"); throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
} }

4
gtsam/nonlinear/NonlinearOptimization.h
View File

@ -42,8 +42,8 @@ namespace gtsam {
/** /**
* optimization that returns the values * optimization that returns the values
*/ */
template<class G, class T> template<class G>
T optimize(const G& graph, const T& initialEstimate, DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate,
const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(), const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
const LinearSolver& solver = MULTIFRONTAL, const LinearSolver& solver = MULTIFRONTAL,
const NonlinearOptimizationMethod& nonlinear_method = LM); const NonlinearOptimizationMethod& nonlinear_method = LM);

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

@ -30,8 +30,8 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C, class L, class S, class W> template<class G, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph, NonlinearOptimizer<G, L, S, W>::NonlinearOptimizer(shared_graph graph,
shared_values values, shared_ordering ordering, shared_parameters parameters) : shared_values values, shared_ordering ordering, shared_parameters parameters) :
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
parameters_(parameters), iterations_(0), parameters_(parameters), iterations_(0),
@ -47,8 +47,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// FIXME: remove this constructor // FIXME: remove this constructor
template<class G, class C, class L, class S, class W> template<class G, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph, NonlinearOptimizer<G, L, S, W>::NonlinearOptimizer(shared_graph graph,
shared_values values, shared_ordering ordering, shared_values values, shared_ordering ordering,
shared_solver spcg_solver, shared_parameters parameters) : shared_solver spcg_solver, shared_parameters parameters) :
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
@ -66,8 +66,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// One iteration of Gauss Newton // One iteration of Gauss Newton
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C, class L, class S, class W> template<class G, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate() const { NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterate() const {
Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
@ -86,7 +86,7 @@ namespace gtsam {
if (verbosity >= Parameters::DELTA) delta.print("delta"); if (verbosity >= Parameters::DELTA) delta.print("delta");
// take old values and update it // take old values and update it
shared_values newValues(new C(values_->retract(delta, *ordering_))); shared_values newValues(new DynamicValues(values_->retract(delta, *ordering_)));
// maybe show output // maybe show output
if (verbosity >= Parameters::VALUES) newValues->print("newValues"); if (verbosity >= Parameters::VALUES) newValues->print("newValues");
@ -99,8 +99,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C, class L, class S, class W> template<class G, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::gaussNewton() const { NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::gaussNewton() const {
static W writer(error_); static W writer(error_);
if (error_ < parameters_->sumError_ ) { if (error_ < parameters_->sumError_ ) {
@ -130,8 +130,8 @@ namespace gtsam {
// TODO: in theory we can't infinitely recurse, but maybe we should put a max. // TODO: in theory we can't infinitely recurse, but maybe we should put a max.
// Reminder: the parameters are Graph type $G$, Values class type $T$, // Reminder: the parameters are Graph type $G$, Values class type $T$,
// linear system class $L$, the non linear solver type $S$, and the writer type $W$ // linear system class $L$, the non linear solver type $S$, and the writer type $W$
template<class G, class T, class L, class S, class W> template<class G, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::try_lambda(const L& linearSystem) { NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::try_lambda(const L& linearSystem) {
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ; const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ;
@ -178,7 +178,7 @@ namespace gtsam {
if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
// update values // update values
shared_values newValues(new T(values_->retract(delta, *ordering_))); shared_values newValues(new DynamicValues(values_->retract(delta, *ordering_)));
// create new optimization state with more adventurous lambda // create new optimization state with more adventurous lambda
double error = graph_->error(*newValues); double error = graph_->error(*newValues);
@ -228,8 +228,8 @@ namespace gtsam {
// One iteration of Levenberg Marquardt // One iteration of Levenberg Marquardt
// Reminder: the parameters are Graph type $G$, Values class type $T$, // Reminder: the parameters are Graph type $G$, Values class type $T$,
// linear system class $L$, the non linear solver type $S$, and the writer type $W$ // linear system class $L$, the non linear solver type $S$, and the writer type $W$
template<class G, class T, class L, class S, class W> template<class G, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateLM() { NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateLM() {
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
const double lambda = parameters_->lambda_ ; const double lambda = parameters_->lambda_ ;
@ -253,8 +253,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Reminder: the parameters are Graph type $G$, Values class type $T$, // Reminder: the parameters are Graph type $G$, Values class type $T$,
// linear system class $L$, the non linear solver type $S$, and the writer type $W$ // linear system class $L$, the non linear solver type $S$, and the writer type $W$
template<class G, class T, class L, class S, class W> template<class G, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::levenbergMarquardt() { NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::levenbergMarquardt() {
// Initialize // Initialize
bool converged = false; bool converged = false;
@ -299,20 +299,20 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class T, class L, class S, class W> template<class G, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateDogLeg() { NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateDogLeg() {
S solver(*graph_->linearize(*values_, *ordering_)); S solver(*graph_->linearize(*values_, *ordering_));
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate( DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(), parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(),
*graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR); *graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR);
shared_values newValues(new T(values_->retract(result.dx_d, *ordering_))); shared_values newValues(new DynamicValues(values_->retract(result.dx_d, *ordering_)));
return newValuesErrorLambda_(newValues, result.f_error, result.Delta); return newValuesErrorLambda_(newValues, result.f_error, result.Delta);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class T, class L, class S, class W> template<class G, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::dogLeg() { NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::dogLeg() {
static W writer(error_); static W writer(error_);
// check if we're already close enough // check if we're already close enough

42
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -37,19 +37,19 @@ public:
* and then one of the optimization routines is called. These iterate * and then one of the optimization routines is called. These iterate
* until convergence. All methods are functional and return a new state. * until convergence. All methods are functional and return a new state.
* *
* The class is parameterized by the Graph type $G$, Values class type $T$, * The class is parameterized by the Graph type $G$, Values class type $DynamicValues$,
* linear system class $L$, the non linear solver type $S$, and the writer type $W$ * linear system class $L$, the non linear solver type $S$, and the writer type $W$
* *
* The values class type $T$ is in order to be able to optimize over non-vector values structures. * The values class type $DynamicValues$ is in order to be able to optimize over non-vector values structures.
* *
* A nonlinear system solver $S$ * A nonlinear system solver $S$
* Concept NonLinearSolver<G,T,L> implements * Concept NonLinearSolver<G,DynamicValues,L> implements
* linearize: G * T -> L * linearize: G * DynamicValues -> L
* solve : L -> T * solve : L -> DynamicValues
* *
* The writer $W$ generates output to disk or the screen. * The writer $W$ generates output to disk or the screen.
* *
* For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values, * For example, in a 2D case, $G$ can be Pose2Graph, $DynamicValues$ can be Pose2Values,
* $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Values>. * $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Values>.
* The solver class has two main functions: linearize and optimize. The first one * The solver class has two main functions: linearize and optimize. The first one
* linearizes the nonlinear cost function around the current estimate, and the second * linearizes the nonlinear cost function around the current estimate, and the second
@ -57,12 +57,12 @@ public:
* *
* To use the optimizer in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file * To use the optimizer in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file
*/ */
template<class G, class T, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter> template<class G, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
class NonlinearOptimizer { class NonlinearOptimizer {
public: public:
// For performance reasons in recursion, we store values in a shared_ptr // For performance reasons in recursion, we store values in a shared_ptr
typedef boost::shared_ptr<const T> shared_values; ///Prevent memory leaks in Values typedef boost::shared_ptr<const DynamicValues> shared_values; ///Prevent memory leaks in Values
typedef boost::shared_ptr<const G> shared_graph; /// Prevent memory leaks in Graph typedef boost::shared_ptr<const G> shared_graph; /// Prevent memory leaks in Graph
typedef boost::shared_ptr<L> shared_linear; /// Not sure typedef boost::shared_ptr<L> shared_linear; /// Not sure
typedef boost::shared_ptr<const Ordering> shared_ordering; ///ordering parameters typedef boost::shared_ptr<const Ordering> shared_ordering; ///ordering parameters
@ -73,7 +73,7 @@ public:
private: private:
typedef NonlinearOptimizer<G, T, L, GS> This; typedef NonlinearOptimizer<G, L, GS> This;
typedef boost::shared_ptr<const std::vector<size_t> > shared_dimensions; typedef boost::shared_ptr<const std::vector<size_t> > shared_dimensions;
/// keep a reference to const version of the graph /// keep a reference to const version of the graph
@ -167,7 +167,7 @@ public:
/** /**
* Copy constructor * Copy constructor
*/ */
NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) : NonlinearOptimizer(const NonlinearOptimizer<G, L, GS> &optimizer) :
graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_), graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_),
ordering_(optimizer.ordering_), parameters_(optimizer.parameters_), ordering_(optimizer.ordering_), parameters_(optimizer.parameters_),
iterations_(optimizer.iterations_), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {} iterations_(optimizer.iterations_), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {}
@ -222,7 +222,7 @@ public:
/** /**
* linearize and optimize * linearize and optimize
* This returns an VectorValues, i.e., vectors in tangent space of T * This returns an VectorValues, i.e., vectors in tangent space of DynamicValues
*/ */
VectorValues linearizeAndOptimizeForDelta() const { VectorValues linearizeAndOptimizeForDelta() const {
return *createSolver()->optimize(); return *createSolver()->optimize();
@ -309,18 +309,18 @@ public:
* Static interface to LM optimization (no shared_ptr arguments) - see above * Static interface to LM optimization (no shared_ptr arguments) - see above
*/ */
static shared_values optimizeLM(const G& graph, static shared_values optimizeLM(const G& graph,
const T& values, const DynamicValues& values,
const Parameters parameters = Parameters()) { const Parameters parameters = Parameters()) {
return optimizeLM(boost::make_shared<const G>(graph), return optimizeLM(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values), boost::make_shared<const DynamicValues>(values),
boost::make_shared<Parameters>(parameters)); boost::make_shared<Parameters>(parameters));
} }
static shared_values optimizeLM(const G& graph, static shared_values optimizeLM(const G& graph,
const T& values, const DynamicValues& values,
Parameters::verbosityLevel verbosity) { Parameters::verbosityLevel verbosity) {
return optimizeLM(boost::make_shared<const G>(graph), return optimizeLM(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values), boost::make_shared<const DynamicValues>(values),
verbosity); verbosity);
} }
@ -360,18 +360,18 @@ public:
* Static interface to Dogleg optimization (no shared_ptr arguments) - see above * Static interface to Dogleg optimization (no shared_ptr arguments) - see above
*/ */
static shared_values optimizeDogLeg(const G& graph, static shared_values optimizeDogLeg(const G& graph,
const T& values, const DynamicValues& values,
const Parameters parameters = Parameters()) { const Parameters parameters = Parameters()) {
return optimizeDogLeg(boost::make_shared<const G>(graph), return optimizeDogLeg(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values), boost::make_shared<const DynamicValues>(values),
boost::make_shared<Parameters>(parameters)); boost::make_shared<Parameters>(parameters));
} }
static shared_values optimizeDogLeg(const G& graph, static shared_values optimizeDogLeg(const G& graph,
const T& values, const DynamicValues& values,
Parameters::verbosityLevel verbosity) { Parameters::verbosityLevel verbosity) {
return optimizeDogLeg(boost::make_shared<const G>(graph), return optimizeDogLeg(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values), boost::make_shared<const DynamicValues>(values),
verbosity); verbosity);
} }
@ -398,9 +398,9 @@ public:
/** /**
* Static interface to GN optimization (no shared_ptr arguments) - see above * Static interface to GN optimization (no shared_ptr arguments) - see above
*/ */
static shared_values optimizeGN(const G& graph, const T& values, const Parameters parameters = Parameters()) { static shared_values optimizeGN(const G& graph, const DynamicValues& values, const Parameters parameters = Parameters()) {
return optimizeGN(boost::make_shared<const G>(graph), return optimizeGN(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values), boost::make_shared<const DynamicValues>(values),
boost::make_shared<Parameters>(parameters)); boost::make_shared<Parameters>(parameters));
} }
}; };

10
gtsam/slam/BearingFactor.h
View File

@ -25,16 +25,16 @@ namespace gtsam {
/** /**
* Binary factor for a bearing measurement * Binary factor for a bearing measurement
*/ */
template<class VALUES, class POSEKEY, class POINTKEY> template<class POSEKEY, class POINTKEY>
class BearingFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> { class BearingFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
private: private:
typedef typename POSEKEY::Value Pose; typedef typename POSEKEY::Value Pose;
typedef typename POSEKEY::Value::Rotation Rot; typedef typename POSEKEY::Value::Rotation Rot;
typedef typename POINTKEY::Value Point; typedef typename POINTKEY::Value Point;
typedef BearingFactor<VALUES, POSEKEY, POINTKEY> This; typedef BearingFactor<POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base; typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
Rot z_; /** measurement */ Rot z_; /** measurement */
@ -68,7 +68,7 @@ namespace gtsam {
} }
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol); return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol);
} }

10
gtsam/slam/BearingRangeFactor.h
View File

@ -27,16 +27,16 @@ namespace gtsam {
/** /**
* Binary factor for a bearing measurement * Binary factor for a bearing measurement
*/ */
template<class VALUES, class POSEKEY, class POINTKEY> template<class POSEKEY, class POINTKEY>
class BearingRangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> { class BearingRangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
private: private:
typedef typename POSEKEY::Value Pose; typedef typename POSEKEY::Value Pose;
typedef typename POSEKEY::Value::Rotation Rot; typedef typename POSEKEY::Value::Rotation Rot;
typedef typename POINTKEY::Value Point; typedef typename POINTKEY::Value Point;
typedef BearingRangeFactor<VALUES, POSEKEY, POINTKEY> This; typedef BearingRangeFactor<POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base; typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
// the measurement // the measurement
Rot bearing_; Rot bearing_;
@ -68,7 +68,7 @@ namespace gtsam {
} }
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && return e != NULL && Base::equals(*e, tol) &&
fabs(this->range_ - e->range_) < tol && fabs(this->range_ - e->range_) < tol &&

10
gtsam/slam/BetweenFactor.h
View File

@ -29,13 +29,13 @@ namespace gtsam {
* KEY1::Value is the Lie Group type * KEY1::Value is the Lie Group type
* T is the measurement type, by default the same * T is the measurement type, by default the same
*/ */
template<class VALUES, class KEY1, class T = typename KEY1::Value> template<class KEY1, class T = typename KEY1::Value>
class BetweenFactor: public NonlinearFactor2<VALUES, KEY1, KEY1> { class BetweenFactor: public NonlinearFactor2<KEY1, KEY1> {
private: private:
typedef BetweenFactor<VALUES, KEY1, T> This; typedef BetweenFactor<KEY1, T> This;
typedef NonlinearFactor2<VALUES, KEY1, KEY1> Base; typedef NonlinearFactor2<KEY1, KEY1> Base;
T measured_; /** The measurement */ T measured_; /** The measurement */
@ -71,7 +71,7 @@ namespace gtsam {
} }
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
} }

20
gtsam/slam/BoundingConstraint.h
View File

@ -28,11 +28,11 @@ namespace gtsam {
* will need to have its value function implemented to return * will need to have its value function implemented to return
* a scalar for comparison. * a scalar for comparison.
*/ */
template<class VALUES, class KEY> template<class KEY>
struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> { struct BoundingConstraint1: public NonlinearFactor1<KEY> {
typedef typename KEY::Value X; typedef typename KEY::Value X;
typedef NonlinearFactor1<VALUES, KEY> Base; typedef NonlinearFactor1<KEY> Base;
typedef boost::shared_ptr<BoundingConstraint1<VALUES, KEY> > shared_ptr; typedef boost::shared_ptr<BoundingConstraint1<KEY> > shared_ptr;
double threshold_; double threshold_;
bool isGreaterThan_; /// flag for greater/less than bool isGreaterThan_; /// flag for greater/less than
@ -57,7 +57,7 @@ struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> {
boost::none) const = 0; boost::none) const = 0;
/** active when constraint *NOT* met */ /** active when constraint *NOT* met */
bool active(const VALUES& c) const { bool active(const DynamicValues& c) const {
// note: still active at equality to avoid zigzagging // note: still active at equality to avoid zigzagging
double x = value(c[this->key_]); double x = value(c[this->key_]);
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
@ -95,13 +95,13 @@ private:
* Binary scalar inequality constraint, with a similar value() function * Binary scalar inequality constraint, with a similar value() function
* to implement for specific systems * to implement for specific systems
*/ */
template<class VALUES, class KEY1, class KEY2> template<class KEY1, class KEY2>
struct BoundingConstraint2: public NonlinearFactor2<VALUES, KEY1, KEY2> { struct BoundingConstraint2: public NonlinearFactor2<KEY1, KEY2> {
typedef typename KEY1::Value X1; typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2; typedef typename KEY2::Value X2;
typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base; typedef NonlinearFactor2<KEY1, KEY2> Base;
typedef boost::shared_ptr<BoundingConstraint2<VALUES, KEY1, KEY2> > shared_ptr; typedef boost::shared_ptr<BoundingConstraint2<KEY1, KEY2> > shared_ptr;
double threshold_; double threshold_;
bool isGreaterThan_; /// flag for greater/less than bool isGreaterThan_; /// flag for greater/less than
@ -125,7 +125,7 @@ struct BoundingConstraint2: public NonlinearFactor2<VALUES, KEY1, KEY2> {
boost::optional<Matrix&> H2 = boost::none) const = 0; boost::optional<Matrix&> H2 = boost::none) const = 0;
/** active when constraint *NOT* met */ /** active when constraint *NOT* met */
bool active(const VALUES& c) const { bool active(const DynamicValues& c) const {
// note: still active at equality to avoid zigzagging // note: still active at equality to avoid zigzagging
double x = value(c[this->key1_], c[this->key2_]); double x = value(c[this->key1_], c[this->key2_]);
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;

11
gtsam/slam/PriorFactor.h
View File

@ -28,15 +28,15 @@ namespace gtsam {
* The Key type is not arbitrary: we need to cast to a Symbol at linearize, so * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
* a simple type like int will not work * a simple type like int will not work
*/ */
template<class VALUES, class KEY> template<class KEY>
class PriorFactor: public NonlinearFactor1<VALUES, KEY> { class PriorFactor: public NonlinearFactor1<KEY> {
public: public:
typedef typename KEY::Value T; typedef typename KEY::Value T;
private: private:
typedef NonlinearFactor1<VALUES, KEY> Base; typedef NonlinearFactor1<KEY> Base;
T prior_; /** The measurement */ T prior_; /** The measurement */
@ -69,9 +69,8 @@ namespace gtsam {
} }
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const PriorFactor<VALUES, KEY> *e = dynamic_cast<const PriorFactor< const PriorFactor<KEY> *e = dynamic_cast<const PriorFactor<KEY>*> (&expected);
VALUES, KEY>*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
} }

18
gtsam/slam/dataset.cpp
View File

@ -30,7 +30,7 @@ using namespace gtsam;
#define LINESIZE 81920 #define LINESIZE 81920
typedef boost::shared_ptr<Pose2Graph> sharedPose2Graph; typedef boost::shared_ptr<Pose2Graph> sharedPose2Graph;
typedef boost::shared_ptr<Pose2Values> sharedPose2Values; typedef boost::shared_ptr<DynamicValues> sharedPose2Values;
namespace gtsam { namespace gtsam {
@ -81,7 +81,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
exit(-1); exit(-1);
} }
sharedPose2Values poses(new Pose2Values); sharedPose2Values poses(new DynamicValues);
sharedPose2Graph graph(new Pose2Graph); sharedPose2Graph graph(new Pose2Graph);
string tag; string tag;
@ -96,7 +96,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
is >> id >> x >> y >> yaw; is >> id >> x >> y >> yaw;
// optional filter // optional filter
if (maxID && id >= maxID) continue; if (maxID && id >= maxID) continue;
poses->insert(id, Pose2(x, y, yaw)); poses->insert(pose2SLAM::Key(id), Pose2(x, y, yaw));
} }
is.ignore(LINESIZE, '\n'); is.ignore(LINESIZE, '\n');
} }
@ -133,8 +133,8 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
l1Xl2 = l1Xl2.retract((*model)->sample()); l1Xl2 = l1Xl2.retract((*model)->sample());
// Insert vertices if pure odometry file // Insert vertices if pure odometry file
if (!poses->exists(id1)) poses->insert(id1, Pose2()); if (!poses->exists(pose2SLAM::Key(id1))) poses->insert(pose2SLAM::Key(id1), Pose2());
if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2); if (!poses->exists(pose2SLAM::Key(id2))) poses->insert(pose2SLAM::Key(id2), poses->at(pose2SLAM::Key(id1)) * l1Xl2);
Pose2Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model)); Pose2Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model));
graph->push_back(factor); graph->push_back(factor);
@ -149,14 +149,14 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
} }
/* ************************************************************************* */ /* ************************************************************************* */
void save2D(const Pose2Graph& graph, const Pose2Values& config, const SharedDiagonal model, void save2D(const Pose2Graph& graph, const DynamicValues& config, const SharedDiagonal model,
const string& filename) { const string& filename) {
typedef Pose2Values::Key Key; // typedef DynamicValues::Key Key;
fstream stream(filename.c_str(), fstream::out); fstream stream(filename.c_str(), fstream::out);
// save poses // save poses
Pose2Values::Key key; pose2SLAM::Key key;
Pose2 pose; Pose2 pose;
BOOST_FOREACH(boost::tie(key, pose), config) BOOST_FOREACH(boost::tie(key, pose), config)
stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
@ -164,7 +164,7 @@ void save2D(const Pose2Graph& graph, const Pose2Values& config, const SharedDiag
// save edges // save edges
Matrix R = model->R(); Matrix R = model->R();
Matrix RR = trans(R)*R;//prod(trans(R),R); Matrix RR = trans(R)*R;//prod(trans(R),R);
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<Pose2Values> > factor_, graph) { BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_); boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_);
if (!factor) continue; if (!factor) continue;

6
gtsam/slam/dataset.h
View File

@ -41,16 +41,16 @@ std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
* @param maxID, if non-zero cut out vertices >= maxID * @param maxID, if non-zero cut out vertices >= maxID
* @param smart: try to reduce complexity of covariance to cheapest model * @param smart: try to reduce complexity of covariance to cheapest model
*/ */
std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Values> > load2D( std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<DynamicValues> > load2D(
std::pair<std::string, boost::optional<SharedDiagonal> > dataset, std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
int maxID = 0, bool addNoise=false, bool smart=true); int maxID = 0, bool addNoise=false, bool smart=true);
std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Values> > load2D( std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<DynamicValues> > load2D(
const std::string& filename, const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(), boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
int maxID = 0, bool addNoise=false, bool smart=true); int maxID = 0, bool addNoise=false, bool smart=true);
/** save 2d graph */ /** save 2d graph */
void save2D(const gtsam::Pose2Graph& graph, const gtsam::Pose2Values& config, const gtsam::SharedDiagonal model, void save2D(const gtsam::Pose2Graph& graph, const DynamicValues& config, const gtsam::SharedDiagonal model,
const std::string& filename); const std::string& filename);
/** /**

8
gtsam/slam/pose2SLAM.cpp
View File

@ -22,16 +22,16 @@
// Use pose2SLAM namespace for specific SLAM instance // Use pose2SLAM namespace for specific SLAM instance
namespace gtsam { namespace gtsam {
template class NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values, GaussianFactorGraph, GaussianSequentialSolver>; template class NonlinearOptimizer<pose2SLAM::Graph, GaussianFactorGraph, GaussianSequentialSolver>;
namespace pose2SLAM { namespace pose2SLAM {
/* ************************************************************************* */ /* ************************************************************************* */
Values circle(size_t n, double R) { DynamicValues circle(size_t n, double R) {
Values x; DynamicValues x;
double theta = 0, dtheta = 2 * M_PI / n; double theta = 0, dtheta = 2 * M_PI / n;
for (size_t i = 0; i < n; i++, theta += dtheta) for (size_t i = 0; i < n; i++, theta += dtheta)
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta)); x.insert(Key(i), Pose2(cos(theta), sin(theta), M_PI_2 + theta));
return x; return x;
} }

20
gtsam/slam/pose2SLAM.h
View File

@ -37,9 +37,6 @@ namespace gtsam {
/// Keys with Pose2 and symbol 'x' /// Keys with Pose2 and symbol 'x'
typedef TypedSymbol<Pose2, 'x'> Key; typedef TypedSymbol<Pose2, 'x'> Key;
/// Values with type 'Key'
typedef Values<Key> Values;
/** /**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses * @param n number of poses
@ -47,22 +44,22 @@ namespace gtsam {
* @param c character to use for keys * @param c character to use for keys
* @return circle of n 2D poses * @return circle of n 2D poses
*/ */
Values circle(size_t n, double R); DynamicValues circle(size_t n, double R);
/// A prior factor on Key with Pose2 data type. /// A prior factor on Key with Pose2 data type.
typedef PriorFactor<Values, Key> Prior; typedef PriorFactor<Key> Prior;
/// A factor to put constraints between two factors. /// A factor to put constraints between two factors.
typedef BetweenFactor<Values, Key> Constraint; typedef BetweenFactor<Key> Constraint;
/// A hard constraint would enforce that the given key would have the input value in the results. /// A hard constraint would enforce that the given key would have the input value in the results.
typedef NonlinearEquality<Values, Key> HardConstraint; typedef NonlinearEquality<Key> HardConstraint;
/// Graph /// Graph
struct Graph: public NonlinearFactorGraph<Values> { struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type /// Adds a factor between keys of the same type
typedef BetweenFactor<Values, Key> Constraint; typedef BetweenFactor<Key> Constraint;
/// A simple typedef from Pose2 to Pose /// A simple typedef from Pose2 to Pose
typedef Pose2 Pose; typedef Pose2 Pose;
@ -78,17 +75,16 @@ namespace gtsam {
}; };
/// The sequential optimizer /// The sequential optimizer
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential; typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
/// The multifrontal optimizer /// The multifrontal optimizer
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer; typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
} // pose2SLAM } // pose2SLAM
/** /**
* Backwards compatibility * Backwards compatibility
*/ */
typedef pose2SLAM::Values Pose2Values; ///< Typedef for Values class for backwards compatibility
typedef pose2SLAM::Prior Pose2Prior; ///< Typedef for Prior class for backwards compatibility typedef pose2SLAM::Prior Pose2Prior; ///< Typedef for Prior class for backwards compatibility
typedef pose2SLAM::Constraint Pose2Factor; ///< Typedef for Constraint class for backwards compatibility typedef pose2SLAM::Constraint Pose2Factor; ///< Typedef for Constraint class for backwards compatibility
typedef pose2SLAM::Graph Pose2Graph; ///< Typedef for Graph class for backwards compatibility typedef pose2SLAM::Graph Pose2Graph; ///< Typedef for Graph class for backwards compatibility

56
gtsam/slam/simulated2D.h
View File

@ -32,44 +32,46 @@ namespace gtsam {
// Simulated2D robots have no orientation, just a position // Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey; typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
/** /**
* Custom Values class that holds poses and points * Custom Values class that holds poses and points
*/ */
class Values: public TupleValues2<PoseValues, PointValues> { class Values: public DynamicValues {
size_t nrPoses_;
size_t nrPoints_;
public: public:
typedef TupleValues2<PoseValues, PointValues> Base; ///< base class typedef DynamicValues Base; ///< base class
typedef boost::shared_ptr<Point2> sharedPoint; ///< shortcut to shared Point type typedef boost::shared_ptr<Point2> sharedPoint; ///< shortcut to shared Point type
/// Constructor /// Constructor
Values() { Values() : nrPoses_(0), nrPoints_(0) {
} }
/// Copy constructor /// Copy constructor
Values(const Base& base) : Values(const Base& base) :
Base(base) { Base(base), nrPoses_(0), nrPoints_(0) {
} }
/// Insert a pose /// Insert a pose
void insertPose(const simulated2D::PoseKey& i, const Point2& p) { void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
insert(i, p); insert(i, p);
nrPoses_++;
} }
/// Insert a point /// Insert a point
void insertPoint(const simulated2D::PointKey& j, const Point2& p) { void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
insert(j, p); insert(j, p);
nrPoints_++;
} }
/// Number of poses /// Number of poses
int nrPoses() const { int nrPoses() const {
return this->first_.size(); return nrPoses_;
} }
/// Number of points /// Number of points
int nrPoints() const { int nrPoints() const {
return this->second_.size(); return nrPoints_;
} }
/// Return pose i /// Return pose i
@ -112,18 +114,18 @@ namespace gtsam {
/** /**
* Unary factor encoding a soft prior on a vector * Unary factor encoding a soft prior on a vector
*/ */
template<class VALUES = Values, class KEY = PoseKey> template<class KEY = PoseKey>
class GenericPrior: public NonlinearFactor1<VALUES, KEY> { class GenericPrior: public NonlinearFactor1<KEY> {
public: public:
typedef NonlinearFactor1<VALUES, KEY> Base; ///< base class typedef NonlinearFactor1<KEY> Base; ///< base class
typedef boost::shared_ptr<GenericPrior<VALUES, KEY> > shared_ptr; typedef boost::shared_ptr<GenericPrior<KEY> > shared_ptr;
typedef typename KEY::Value Pose; ///< shortcut to Pose type typedef typename KEY::Value Pose; ///< shortcut to Pose type
Pose z_; ///< prior mean Pose z_; ///< prior mean
/// Create generic prior /// Create generic prior
GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) : GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) :
NonlinearFactor1<VALUES, KEY>(model, key), z_(z) { NonlinearFactor1<KEY>(model, key), z_(z) {
} }
/// Return error and optional derivative /// Return error and optional derivative
@ -150,11 +152,11 @@ namespace gtsam {
/** /**
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class VALUES = Values, class KEY = PoseKey> template<class KEY = PoseKey>
class GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> { class GenericOdometry: public NonlinearFactor2<KEY, KEY> {
public: public:
typedef NonlinearFactor2<VALUES, KEY, KEY> Base; ///< base class typedef NonlinearFactor2<KEY, KEY> Base; ///< base class
typedef boost::shared_ptr<GenericOdometry<VALUES, KEY> > shared_ptr; typedef boost::shared_ptr<GenericOdometry<KEY> > shared_ptr;
typedef typename KEY::Value Pose; ///< shortcut to Pose type typedef typename KEY::Value Pose; ///< shortcut to Pose type
Pose z_; ///< odometry measurement Pose z_; ///< odometry measurement
@ -162,7 +164,7 @@ namespace gtsam {
/// Create odometry /// Create odometry
GenericOdometry(const Pose& z, const SharedNoiseModel& model, GenericOdometry(const Pose& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) : const KEY& i1, const KEY& i2) :
NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) { NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
} }
/// Evaluate error and optionally return derivatives /// Evaluate error and optionally return derivatives
@ -190,11 +192,11 @@ namespace gtsam {
/** /**
* Binary factor simulating "measurement" between two Vectors * Binary factor simulating "measurement" between two Vectors
*/ */
template<class VALUES = Values, class XKEY = PoseKey, class LKEY = PointKey> template<class XKEY = PoseKey, class LKEY = PointKey>
class GenericMeasurement: public NonlinearFactor2<VALUES, XKEY, LKEY> { class GenericMeasurement: public NonlinearFactor2<XKEY, LKEY> {
public: public:
typedef NonlinearFactor2<VALUES, XKEY, LKEY> Base; ///< base class typedef NonlinearFactor2<XKEY, LKEY> Base; ///< base class
typedef boost::shared_ptr<GenericMeasurement<VALUES, XKEY, LKEY> > shared_ptr; typedef boost::shared_ptr<GenericMeasurement<XKEY, LKEY> > shared_ptr;
typedef typename XKEY::Value Pose; ///< shortcut to Pose type typedef typename XKEY::Value Pose; ///< shortcut to Pose type
typedef typename LKEY::Value Point; ///< shortcut to Point type typedef typename LKEY::Value Point; ///< shortcut to Point type
@ -203,7 +205,7 @@ namespace gtsam {
/// Create measurement factor /// Create measurement factor
GenericMeasurement(const Point& z, const SharedNoiseModel& model, GenericMeasurement(const Point& z, const SharedNoiseModel& model,
const XKEY& i, const LKEY& j) : const XKEY& i, const LKEY& j) :
NonlinearFactor2<VALUES, XKEY, LKEY>(model, i, j), z_(z) { NonlinearFactor2<XKEY, LKEY>(model, i, j), z_(z) {
} }
/// Evaluate error and optionally return derivatives /// Evaluate error and optionally return derivatives
@ -229,13 +231,13 @@ namespace gtsam {
}; };
/** Typedefs for regular use */ /** Typedefs for regular use */
typedef GenericPrior<Values, PoseKey> Prior; typedef GenericPrior<PoseKey> Prior;
typedef GenericOdometry<Values, PoseKey> Odometry; typedef GenericOdometry<PoseKey> Odometry;
typedef GenericMeasurement<Values, PoseKey, PointKey> Measurement; typedef GenericMeasurement<PoseKey, PointKey> Measurement;
// Specialization of a graph for this example domain // Specialization of a graph for this example domain
// TODO: add functions to add factor types // TODO: add functions to add factor types
class Graph : public NonlinearFactorGraph<Values> { class Graph : public NonlinearFactorGraph {
public: public:
Graph() {} Graph() {}
}; };

16
gtsam/slam/simulated2DOriented.h
View File

@ -76,15 +76,15 @@ namespace gtsam {
boost::none, boost::optional<Matrix&> H2 = boost::none); boost::none, boost::optional<Matrix&> H2 = boost::none);
/// Unary factor encoding a soft prior on a vector /// Unary factor encoding a soft prior on a vector
template<class VALUES = Values, class Key = PoseKey> template<class Key = PoseKey>
struct GenericPosePrior: public NonlinearFactor1<VALUES, Key> { struct GenericPosePrior: public NonlinearFactor1<Key> {
Pose2 z_; ///< measurement Pose2 z_; ///< measurement
/// Create generic pose prior /// Create generic pose prior
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, GenericPosePrior(const Pose2& z, const SharedNoiseModel& model,
const Key& key) : const Key& key) :
NonlinearFactor1<VALUES, Key>(model, key), z_(z) { NonlinearFactor1<Key>(model, key), z_(z) {
} }
/// Evaluate error and optionally derivative /// Evaluate error and optionally derivative
@ -98,8 +98,8 @@ namespace gtsam {
/** /**
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class VALUES = Values, class KEY = PoseKey> template<class KEY = PoseKey>
struct GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> { struct GenericOdometry: public NonlinearFactor2<KEY, KEY> {
Pose2 z_; ///< Between measurement for odometry factor Pose2 z_; ///< Between measurement for odometry factor
/** /**
@ -107,7 +107,7 @@ namespace gtsam {
*/ */
GenericOdometry(const Pose2& z, const SharedNoiseModel& model, GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) : const KEY& i1, const KEY& i2) :
NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) { NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
} }
/// Evaluate error and optionally derivative /// Evaluate error and optionally derivative
@ -119,10 +119,10 @@ namespace gtsam {
}; };
typedef GenericOdometry<Values, PoseKey> Odometry; typedef GenericOdometry<PoseKey> Odometry;
/// Graph specialization for syntactic sugar use with matlab /// Graph specialization for syntactic sugar use with matlab
class Graph : public NonlinearFactorGraph<Values> { class Graph : public NonlinearFactorGraph {
public: public:
Graph() {} Graph() {}
// TODO: add functions to add factors // TODO: add functions to add factors

13
gtsam/slam/simulated3D.h
View File

@ -40,10 +40,6 @@ namespace simulated3D {
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey; typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey; typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values;
/** /**
* Prior on a single pose * Prior on a single pose
*/ */
@ -66,7 +62,7 @@ Point3 mea(const Point3& x, const Point3& l,
/** /**
* A prior factor on a single linear robot pose * A prior factor on a single linear robot pose
*/ */
struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> { struct PointPrior3D: public NonlinearFactor1<PoseKey> {
Point3 z_; ///< The prior pose value for the variable attached to this factor Point3 z_; ///< The prior pose value for the variable attached to this factor
@ -78,7 +74,7 @@ struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
*/ */
PointPrior3D(const Point3& z, PointPrior3D(const Point3& z,
const SharedNoiseModel& model, const PoseKey& j) : const SharedNoiseModel& model, const PoseKey& j) :
NonlinearFactor1<Values, PoseKey> (model, j), z_(z) { NonlinearFactor1<PoseKey> (model, j), z_(z) {
} }
/** /**
@ -97,8 +93,7 @@ struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
/** /**
* Models a linear 3D measurement between 3D points * Models a linear 3D measurement between 3D points
*/ */
struct Simulated3DMeasurement: public NonlinearFactor2<Values, struct Simulated3DMeasurement: public NonlinearFactor2<PoseKey, PointKey> {
PoseKey, PointKey> {
Point3 z_; ///< Linear displacement between a pose and landmark Point3 z_; ///< Linear displacement between a pose and landmark
@ -111,7 +106,7 @@ PoseKey, PointKey> {
*/ */
Simulated3DMeasurement(const Point3& z, Simulated3DMeasurement(const Point3& z,
const SharedNoiseModel& model, PoseKey& j1, PointKey j2) : const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
NonlinearFactor2<Values, PoseKey, PointKey> ( NonlinearFactor2<PoseKey, PointKey> (
model, j1, j2), z_(z) { model, j1, j2), z_(z) {
} }

9
gtsam/slam/smallExample.cpp
View File

@ -39,7 +39,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
namespace example { namespace example {
typedef boost::shared_ptr<NonlinearFactor<Values> > shared; typedef boost::shared_ptr<NonlinearFactor > shared;
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
@ -195,14 +195,13 @@ namespace example {
0.0, cos(v.y())); 0.0, cos(v.y()));
} }
struct UnaryFactor: public gtsam::NonlinearFactor1<Values, struct UnaryFactor: public gtsam::NonlinearFactor1<simulated2D::PoseKey> {
simulated2D::PoseKey> {
Point2 z_; Point2 z_;
UnaryFactor(const Point2& z, const SharedNoiseModel& model, UnaryFactor(const Point2& z, const SharedNoiseModel& model,
const simulated2D::PoseKey& key) : const simulated2D::PoseKey& key) :
gtsam::NonlinearFactor1<Values, simulated2D::PoseKey>(model, key), z_(z) { gtsam::NonlinearFactor1<simulated2D::PoseKey>(model, key), z_(z) {
} }
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = Vector evaluateError(const Point2& x, boost::optional<Matrix&> A =
@ -423,7 +422,7 @@ namespace example {
boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N) { boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N) {
// create empty graph // create empty graph
NonlinearFactorGraph<Values> nlfg; NonlinearFactorGraph nlfg;
// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1))); shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1)));

2
gtsam/slam/smallExample.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
namespace example { namespace example {
typedef simulated2D::Values Values; typedef simulated2D::Values Values;
typedef NonlinearFactorGraph<Values> Graph; typedef NonlinearFactorGraph Graph;
/** /**
* Create small example for non-linear factor graph * Create small example for non-linear factor graph