[inprogress] switching to DynamicValues
Duy-Nguyen Ta
parent
ee0b7f8a74
commit
d3d5ee3b39
|
|
@ -45,8 +45,8 @@ namespace gtsam {
|
|||
* - 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
|
||||
*/
|
||||
template<class VALUES, class KEY>
|
||||
class NonlinearEquality: public NonlinearFactor1<VALUES, KEY> {
|
||||
template<class KEY>
|
||||
class NonlinearEquality: public NonlinearFactor1<KEY> {
|
||||
|
||||
public:
|
||||
typedef typename KEY::Value T;
|
||||
|
|
@ -69,7 +69,7 @@ namespace gtsam {
|
|||
*/
|
||||
bool (*compare_)(const T& a, const T& b);
|
||||
|
||||
typedef NonlinearFactor1<VALUES, KEY> Base;
|
||||
typedef NonlinearFactor1<KEY> Base;
|
||||
|
||||
/** default constructor - only for serialization */
|
||||
NonlinearEquality() {}
|
||||
|
|
@ -101,14 +101,14 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** 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;
|
||||
return feasible_.equals(f.feasible_, tol) &&
|
||||
fabs(error_gain_ - f.error_gain_) < tol;
|
||||
}
|
||||
|
||||
/** actual error function calculation */
|
||||
virtual double error(const VALUES& c) const {
|
||||
virtual double error(const DynamicValues& c) const {
|
||||
const T& xj = c[this->key_];
|
||||
Vector e = this->unwhitenedError(c);
|
||||
if (allow_error_ || !compare_(xj, feasible_)) {
|
||||
|
|
@ -135,7 +135,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
// 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_];
|
||||
Matrix A;
|
||||
Vector b = evaluateError(xj, A);
|
||||
|
|
@ -162,14 +162,14 @@ namespace gtsam {
|
|||
/**
|
||||
* Simple unary equality constraint - fixes a value for a variable
|
||||
*/
|
||||
template<class VALUES, class KEY>
|
||||
class NonlinearEquality1 : public NonlinearFactor1<VALUES, KEY> {
|
||||
template<class KEY>
|
||||
class NonlinearEquality1 : public NonlinearFactor1<KEY> {
|
||||
|
||||
public:
|
||||
typedef typename KEY::Value X;
|
||||
|
||||
protected:
|
||||
typedef NonlinearFactor1<VALUES, KEY> Base;
|
||||
typedef NonlinearFactor1<KEY> Base;
|
||||
|
||||
/** default constructor to allow for serialization */
|
||||
NonlinearEquality1() {}
|
||||
|
|
@ -181,7 +181,7 @@ namespace gtsam {
|
|||
|
||||
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)
|
||||
: 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
|
||||
* be the same.
|
||||
*/
|
||||
template<class VALUES, class KEY>
|
||||
class NonlinearEquality2 : public NonlinearFactor2<VALUES, KEY, KEY> {
|
||||
template<class KEY>
|
||||
class NonlinearEquality2 : public NonlinearFactor2<KEY, KEY> {
|
||||
public:
|
||||
typedef typename KEY::Value X;
|
||||
|
||||
protected:
|
||||
typedef NonlinearFactor2<VALUES, KEY, KEY> Base;
|
||||
typedef NonlinearFactor2<KEY, KEY> Base;
|
||||
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
|
||||
|
||||
|
|
@ -235,7 +235,7 @@ namespace gtsam {
|
|||
|
||||
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)
|
||||
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
|
||||
|
|
|
|||
|
|
@ -31,17 +31,17 @@ namespace gtsam {
|
|||
/**
|
||||
* The Elimination solver
|
||||
*/
|
||||
template<class G, class T>
|
||||
T optimizeSequential(const G& graph, const T& initialEstimate,
|
||||
template<class G>
|
||||
DynamicValues optimizeSequential(const G& graph, const DynamicValues& initialEstimate,
|
||||
const NonlinearOptimizationParameters& parameters, bool useLM) {
|
||||
|
||||
// Use a variable ordering from COLAMD
|
||||
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
|
||||
|
||||
// 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),
|
||||
boost::make_shared<const T>(initialEstimate), ordering,
|
||||
boost::make_shared<const DynamicValues>(initialEstimate), ordering,
|
||||
boost::make_shared<NonlinearOptimizationParameters>(parameters));
|
||||
|
||||
// Now optimize using either LM or GN methods.
|
||||
|
|
@ -54,17 +54,17 @@ namespace gtsam {
|
|||
/**
|
||||
* The multifrontal solver
|
||||
*/
|
||||
template<class G, class T>
|
||||
T optimizeMultiFrontal(const G& graph, const T& initialEstimate,
|
||||
template<class G>
|
||||
DynamicValues optimizeMultiFrontal(const G& graph, const DynamicValues& initialEstimate,
|
||||
const NonlinearOptimizationParameters& parameters, bool useLM) {
|
||||
|
||||
// Use a variable ordering from COLAMD
|
||||
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
|
||||
|
||||
// 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),
|
||||
boost::make_shared<const T>(initialEstimate), ordering,
|
||||
boost::make_shared<const DynamicValues>(initialEstimate), ordering,
|
||||
boost::make_shared<NonlinearOptimizationParameters>(parameters));
|
||||
|
||||
// now optimize using either LM or GN methods
|
||||
|
|
@ -77,20 +77,20 @@ namespace gtsam {
|
|||
/**
|
||||
* The sparse preconditioned conjugate gradient solver
|
||||
*/
|
||||
template<class G, class T>
|
||||
T optimizeSPCG(const G& graph, const T& initialEstimate,
|
||||
template<class G>
|
||||
DynamicValues optimizeSPCG(const G& graph, const DynamicValues& initialEstimate,
|
||||
const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
|
||||
bool useLM = true) {
|
||||
|
||||
// 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 NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> SPCGOptimizer;
|
||||
typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, Solver> SPCGOptimizer;
|
||||
shared_Solver solver = boost::make_shared<Solver>(
|
||||
graph, initialEstimate, IterativeOptimizationParameters());
|
||||
SPCGOptimizer optimizer(
|
||||
boost::make_shared<const G>(graph),
|
||||
boost::make_shared<const T>(initialEstimate),
|
||||
boost::make_shared<const DynamicValues>(initialEstimate),
|
||||
solver->ordering(),
|
||||
solver,
|
||||
boost::make_shared<NonlinearOptimizationParameters>(parameters));
|
||||
|
|
@ -105,19 +105,19 @@ namespace gtsam {
|
|||
/**
|
||||
* optimization that returns the values
|
||||
*/
|
||||
template<class G, class T>
|
||||
T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters,
|
||||
template<class G>
|
||||
DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate, const NonlinearOptimizationParameters& parameters,
|
||||
const LinearSolver& solver,
|
||||
const NonlinearOptimizationMethod& nonlinear_method) {
|
||||
switch (solver) {
|
||||
case SEQUENTIAL:
|
||||
return optimizeSequential<G,T>(graph, initialEstimate, parameters,
|
||||
return optimizeSequential<G,DynamicValues>(graph, initialEstimate, parameters,
|
||||
nonlinear_method == LM);
|
||||
case MULTIFRONTAL:
|
||||
return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters,
|
||||
return optimizeMultiFrontal<G,DynamicValues>(graph, initialEstimate, parameters,
|
||||
nonlinear_method == LM);
|
||||
case SPCG:
|
||||
// return optimizeSPCG<G,T>(graph, initialEstimate, parameters,
|
||||
// return optimizeSPCG<G,DynamicValues>(graph, initialEstimate, parameters,
|
||||
// nonlinear_method == LM);
|
||||
throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -42,8 +42,8 @@ namespace gtsam {
|
|||
/**
|
||||
* optimization that returns the values
|
||||
*/
|
||||
template<class G, class T>
|
||||
T optimize(const G& graph, const T& initialEstimate,
|
||||
template<class G>
|
||||
DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate,
|
||||
const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
|
||||
const LinearSolver& solver = MULTIFRONTAL,
|
||||
const NonlinearOptimizationMethod& nonlinear_method = LM);
|
||||
|
|
|
|||
|
|
@ -30,8 +30,8 @@ using namespace std;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
|
||||
template<class G, class L, class S, class W>
|
||||
NonlinearOptimizer<G, 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),
|
||||
|
|
@ -47,8 +47,8 @@ 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,
|
||||
template<class G, class L, class S, class W>
|
||||
NonlinearOptimizer<G, L, S, W>::NonlinearOptimizer(shared_graph graph,
|
||||
shared_values values, shared_ordering ordering,
|
||||
shared_solver spcg_solver, shared_parameters parameters) :
|
||||
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
|
||||
|
|
@ -66,8 +66,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
// One iteration of Gauss Newton
|
||||
/* ************************************************************************* */
|
||||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate() const {
|
||||
template<class G, class L, class S, class W>
|
||||
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterate() const {
|
||||
|
||||
Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
|
||||
|
||||
|
|
@ -86,7 +86,7 @@ namespace gtsam {
|
|||
if (verbosity >= Parameters::DELTA) delta.print("delta");
|
||||
|
||||
// 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
|
||||
if (verbosity >= Parameters::VALUES) newValues->print("newValues");
|
||||
|
|
@ -99,8 +99,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::gaussNewton() const {
|
||||
template<class G, class L, class S, class W>
|
||||
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::gaussNewton() const {
|
||||
static W writer(error_);
|
||||
|
||||
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.
|
||||
// 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$
|
||||
template<class G, class T, class L, class S, class W>
|
||||
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::try_lambda(const L& linearSystem) {
|
||||
template<class G, class L, class S, class W>
|
||||
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::try_lambda(const L& linearSystem) {
|
||||
|
||||
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
|
||||
const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ;
|
||||
|
|
@ -178,7 +178,7 @@ namespace gtsam {
|
|||
if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
|
||||
|
||||
// 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
|
||||
double error = graph_->error(*newValues);
|
||||
|
|
@ -228,8 +228,8 @@ namespace gtsam {
|
|||
// One iteration of Levenberg Marquardt
|
||||
// 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$
|
||||
template<class G, class T, class L, class S, class W>
|
||||
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateLM() {
|
||||
template<class G, class L, class S, class W>
|
||||
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateLM() {
|
||||
|
||||
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
|
||||
const double lambda = parameters_->lambda_ ;
|
||||
|
|
@ -253,8 +253,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
// 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$
|
||||
template<class G, class T, class L, class S, class W>
|
||||
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::levenbergMarquardt() {
|
||||
template<class G, class L, class S, class W>
|
||||
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::levenbergMarquardt() {
|
||||
|
||||
// Initialize
|
||||
bool converged = false;
|
||||
|
|
@ -299,20 +299,20 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class G, class T, class L, class S, class W>
|
||||
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateDogLeg() {
|
||||
template<class G, class L, class S, class W>
|
||||
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateDogLeg() {
|
||||
|
||||
S solver(*graph_->linearize(*values_, *ordering_));
|
||||
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
|
||||
parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(),
|
||||
*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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class G, class T, class L, class S, class W>
|
||||
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::dogLeg() {
|
||||
template<class G, class L, class S, class W>
|
||||
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::dogLeg() {
|
||||
static W writer(error_);
|
||||
|
||||
// check if we're already close enough
|
||||
|
|
|
|||
|
|
@ -37,19 +37,19 @@ public:
|
|||
* and then one of the optimization routines is called. These iterate
|
||||
* 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$
|
||||
*
|
||||
* 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$
|
||||
* Concept NonLinearSolver<G,T,L> implements
|
||||
* linearize: G * T -> L
|
||||
* solve : L -> T
|
||||
* Concept NonLinearSolver<G,DynamicValues,L> implements
|
||||
* linearize: G * DynamicValues -> L
|
||||
* solve : L -> DynamicValues
|
||||
*
|
||||
* 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>.
|
||||
* 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
|
||||
|
|
@ -57,12 +57,12 @@ public:
|
|||
*
|
||||
* 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 {
|
||||
public:
|
||||
|
||||
// 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<L> shared_linear; /// Not sure
|
||||
typedef boost::shared_ptr<const Ordering> shared_ordering; ///ordering parameters
|
||||
|
|
@ -73,7 +73,7 @@ public:
|
|||
|
||||
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;
|
||||
|
||||
/// keep a reference to const version of the graph
|
||||
|
|
@ -167,7 +167,7 @@ public:
|
|||
/**
|
||||
* 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_),
|
||||
ordering_(optimizer.ordering_), parameters_(optimizer.parameters_),
|
||||
iterations_(optimizer.iterations_), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {}
|
||||
|
|
@ -222,7 +222,7 @@ public:
|
|||
|
||||
/**
|
||||
* 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 {
|
||||
return *createSolver()->optimize();
|
||||
|
|
@ -309,18 +309,18 @@ public:
|
|||
* Static interface to LM optimization (no shared_ptr arguments) - see above
|
||||
*/
|
||||
static shared_values optimizeLM(const G& graph,
|
||||
const T& values,
|
||||
const DynamicValues& values,
|
||||
const Parameters parameters = Parameters()) {
|
||||
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));
|
||||
}
|
||||
|
||||
static shared_values optimizeLM(const G& graph,
|
||||
const T& values,
|
||||
const DynamicValues& values,
|
||||
Parameters::verbosityLevel verbosity) {
|
||||
return optimizeLM(boost::make_shared<const G>(graph),
|
||||
boost::make_shared<const T>(values),
|
||||
boost::make_shared<const DynamicValues>(values),
|
||||
verbosity);
|
||||
}
|
||||
|
||||
|
|
@ -360,18 +360,18 @@ public:
|
|||
* Static interface to Dogleg optimization (no shared_ptr arguments) - see above
|
||||
*/
|
||||
static shared_values optimizeDogLeg(const G& graph,
|
||||
const T& values,
|
||||
const DynamicValues& values,
|
||||
const Parameters parameters = Parameters()) {
|
||||
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));
|
||||
}
|
||||
|
||||
static shared_values optimizeDogLeg(const G& graph,
|
||||
const T& values,
|
||||
const DynamicValues& values,
|
||||
Parameters::verbosityLevel verbosity) {
|
||||
return optimizeDogLeg(boost::make_shared<const G>(graph),
|
||||
boost::make_shared<const T>(values),
|
||||
boost::make_shared<const DynamicValues>(values),
|
||||
verbosity);
|
||||
}
|
||||
|
||||
|
|
@ -398,9 +398,9 @@ public:
|
|||
/**
|
||||
* 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),
|
||||
boost::make_shared<const T>(values),
|
||||
boost::make_shared<const DynamicValues>(values),
|
||||
boost::make_shared<Parameters>(parameters));
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -25,16 +25,16 @@ namespace gtsam {
|
|||
/**
|
||||
* Binary factor for a bearing measurement
|
||||
*/
|
||||
template<class VALUES, class POSEKEY, class POINTKEY>
|
||||
class BearingFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
|
||||
template<class POSEKEY, class POINTKEY>
|
||||
class BearingFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
|
||||
private:
|
||||
|
||||
typedef typename POSEKEY::Value Pose;
|
||||
typedef typename POSEKEY::Value::Rotation Rot;
|
||||
typedef typename POINTKEY::Value Point;
|
||||
|
||||
typedef BearingFactor<VALUES, POSEKEY, POINTKEY> This;
|
||||
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
|
||||
typedef BearingFactor<POSEKEY, POINTKEY> This;
|
||||
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
|
||||
|
||||
Rot z_; /** measurement */
|
||||
|
||||
|
|
@ -68,7 +68,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** 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);
|
||||
return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -27,16 +27,16 @@ namespace gtsam {
|
|||
/**
|
||||
* Binary factor for a bearing measurement
|
||||
*/
|
||||
template<class VALUES, class POSEKEY, class POINTKEY>
|
||||
class BearingRangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
|
||||
template<class POSEKEY, class POINTKEY>
|
||||
class BearingRangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
|
||||
private:
|
||||
|
||||
typedef typename POSEKEY::Value Pose;
|
||||
typedef typename POSEKEY::Value::Rotation Rot;
|
||||
typedef typename POINTKEY::Value Point;
|
||||
|
||||
typedef BearingRangeFactor<VALUES, POSEKEY, POINTKEY> This;
|
||||
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
|
||||
typedef BearingRangeFactor<POSEKEY, POINTKEY> This;
|
||||
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
|
||||
|
||||
// the measurement
|
||||
Rot bearing_;
|
||||
|
|
@ -68,7 +68,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** 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);
|
||||
return e != NULL && Base::equals(*e, tol) &&
|
||||
fabs(this->range_ - e->range_) < tol &&
|
||||
|
|
|
|||
|
|
@ -29,13 +29,13 @@ namespace gtsam {
|
|||
* KEY1::Value is the Lie Group type
|
||||
* T is the measurement type, by default the same
|
||||
*/
|
||||
template<class VALUES, class KEY1, class T = typename KEY1::Value>
|
||||
class BetweenFactor: public NonlinearFactor2<VALUES, KEY1, KEY1> {
|
||||
template<class KEY1, class T = typename KEY1::Value>
|
||||
class BetweenFactor: public NonlinearFactor2<KEY1, KEY1> {
|
||||
|
||||
private:
|
||||
|
||||
typedef BetweenFactor<VALUES, KEY1, T> This;
|
||||
typedef NonlinearFactor2<VALUES, KEY1, KEY1> Base;
|
||||
typedef BetweenFactor<KEY1, T> This;
|
||||
typedef NonlinearFactor2<KEY1, KEY1> Base;
|
||||
|
||||
T measured_; /** The measurement */
|
||||
|
||||
|
|
@ -71,7 +71,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** 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);
|
||||
return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -28,11 +28,11 @@ namespace gtsam {
|
|||
* will need to have its value function implemented to return
|
||||
* a scalar for comparison.
|
||||
*/
|
||||
template<class VALUES, class KEY>
|
||||
struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> {
|
||||
template<class KEY>
|
||||
struct BoundingConstraint1: public NonlinearFactor1<KEY> {
|
||||
typedef typename KEY::Value X;
|
||||
typedef NonlinearFactor1<VALUES, KEY> Base;
|
||||
typedef boost::shared_ptr<BoundingConstraint1<VALUES, KEY> > shared_ptr;
|
||||
typedef NonlinearFactor1<KEY> Base;
|
||||
typedef boost::shared_ptr<BoundingConstraint1<KEY> > shared_ptr;
|
||||
|
||||
double threshold_;
|
||||
bool isGreaterThan_; /// flag for greater/less than
|
||||
|
|
@ -57,7 +57,7 @@ struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> {
|
|||
boost::none) const = 0;
|
||||
|
||||
/** 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
|
||||
double x = value(c[this->key_]);
|
||||
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
|
||||
|
|
@ -95,13 +95,13 @@ private:
|
|||
* Binary scalar inequality constraint, with a similar value() function
|
||||
* to implement for specific systems
|
||||
*/
|
||||
template<class VALUES, class KEY1, class KEY2>
|
||||
struct BoundingConstraint2: public NonlinearFactor2<VALUES, KEY1, KEY2> {
|
||||
template<class KEY1, class KEY2>
|
||||
struct BoundingConstraint2: public NonlinearFactor2<KEY1, KEY2> {
|
||||
typedef typename KEY1::Value X1;
|
||||
typedef typename KEY2::Value X2;
|
||||
|
||||
typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;
|
||||
typedef boost::shared_ptr<BoundingConstraint2<VALUES, KEY1, KEY2> > shared_ptr;
|
||||
typedef NonlinearFactor2<KEY1, KEY2> Base;
|
||||
typedef boost::shared_ptr<BoundingConstraint2<KEY1, KEY2> > shared_ptr;
|
||||
|
||||
double threshold_;
|
||||
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;
|
||||
|
||||
/** 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
|
||||
double x = value(c[this->key1_], c[this->key2_]);
|
||||
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
|
||||
|
|
|
|||
|
|
@ -28,15 +28,15 @@ namespace gtsam {
|
|||
* The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
|
||||
* a simple type like int will not work
|
||||
*/
|
||||
template<class VALUES, class KEY>
|
||||
class PriorFactor: public NonlinearFactor1<VALUES, KEY> {
|
||||
template<class KEY>
|
||||
class PriorFactor: public NonlinearFactor1<KEY> {
|
||||
|
||||
public:
|
||||
typedef typename KEY::Value T;
|
||||
|
||||
private:
|
||||
|
||||
typedef NonlinearFactor1<VALUES, KEY> Base;
|
||||
typedef NonlinearFactor1<KEY> Base;
|
||||
|
||||
T prior_; /** The measurement */
|
||||
|
||||
|
|
@ -69,9 +69,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
|
||||
const PriorFactor<VALUES, KEY> *e = dynamic_cast<const PriorFactor<
|
||||
VALUES, KEY>*> (&expected);
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||
const PriorFactor<KEY> *e = dynamic_cast<const PriorFactor<KEY>*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ using namespace gtsam;
|
|||
#define LINESIZE 81920
|
||||
|
||||
typedef boost::shared_ptr<Pose2Graph> sharedPose2Graph;
|
||||
typedef boost::shared_ptr<Pose2Values> sharedPose2Values;
|
||||
typedef boost::shared_ptr<DynamicValues> sharedPose2Values;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -81,7 +81,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
|
|||
exit(-1);
|
||||
}
|
||||
|
||||
sharedPose2Values poses(new Pose2Values);
|
||||
sharedPose2Values poses(new DynamicValues);
|
||||
sharedPose2Graph graph(new Pose2Graph);
|
||||
|
||||
string tag;
|
||||
|
|
@ -96,7 +96,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
|
|||
is >> id >> x >> y >> yaw;
|
||||
// optional filter
|
||||
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');
|
||||
}
|
||||
|
|
@ -133,8 +133,8 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
|
|||
l1Xl2 = l1Xl2.retract((*model)->sample());
|
||||
|
||||
// Insert vertices if pure odometry file
|
||||
if (!poses->exists(id1)) poses->insert(id1, Pose2());
|
||||
if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2);
|
||||
if (!poses->exists(pose2SLAM::Key(id1))) poses->insert(pose2SLAM::Key(id1), Pose2());
|
||||
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));
|
||||
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) {
|
||||
typedef Pose2Values::Key Key;
|
||||
// typedef DynamicValues::Key Key;
|
||||
|
||||
fstream stream(filename.c_str(), fstream::out);
|
||||
|
||||
// save poses
|
||||
Pose2Values::Key key;
|
||||
pose2SLAM::Key key;
|
||||
Pose2 pose;
|
||||
BOOST_FOREACH(boost::tie(key, pose), config)
|
||||
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
|
||||
Matrix R = model->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_);
|
||||
if (!factor) continue;
|
||||
|
||||
|
|
|
|||
|
|
@ -41,16 +41,16 @@ std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
|
|||
* @param maxID, if non-zero cut out vertices >= maxID
|
||||
* @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,
|
||||
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,
|
||||
boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
|
||||
int maxID = 0, bool addNoise=false, bool smart=true);
|
||||
|
||||
/** 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);
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -22,16 +22,16 @@
|
|||
// Use pose2SLAM namespace for specific SLAM instance
|
||||
namespace gtsam {
|
||||
|
||||
template class NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values, GaussianFactorGraph, GaussianSequentialSolver>;
|
||||
template class NonlinearOptimizer<pose2SLAM::Graph, GaussianFactorGraph, GaussianSequentialSolver>;
|
||||
|
||||
namespace pose2SLAM {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values circle(size_t n, double R) {
|
||||
Values x;
|
||||
DynamicValues circle(size_t n, double R) {
|
||||
DynamicValues x;
|
||||
double theta = 0, dtheta = 2 * M_PI / n;
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -37,9 +37,6 @@ namespace gtsam {
|
|||
/// Keys with Pose2 and symbol 'x'
|
||||
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)
|
||||
* @param n number of poses
|
||||
|
|
@ -47,22 +44,22 @@ namespace gtsam {
|
|||
* @param c character to use for keys
|
||||
* @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.
|
||||
typedef PriorFactor<Values, Key> Prior;
|
||||
typedef PriorFactor<Key> Prior;
|
||||
|
||||
/// 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.
|
||||
typedef NonlinearEquality<Values, Key> HardConstraint;
|
||||
typedef NonlinearEquality<Key> HardConstraint;
|
||||
|
||||
/// Graph
|
||||
struct Graph: public NonlinearFactorGraph<Values> {
|
||||
struct Graph: public NonlinearFactorGraph {
|
||||
|
||||
/// 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
|
||||
typedef Pose2 Pose;
|
||||
|
|
@ -78,17 +75,16 @@ namespace gtsam {
|
|||
};
|
||||
|
||||
/// The sequential optimizer
|
||||
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
|
||||
typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
|
||||
|
||||
/// The multifrontal optimizer
|
||||
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
|
||||
typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
|
||||
|
||||
} // pose2SLAM
|
||||
|
||||
/**
|
||||
* 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::Constraint Pose2Factor; ///< Typedef for Constraint class for backwards compatibility
|
||||
typedef pose2SLAM::Graph Pose2Graph; ///< Typedef for Graph class for backwards compatibility
|
||||
|
|
|
|||
|
|
@ -32,44 +32,46 @@ namespace gtsam {
|
|||
// Simulated2D robots have no orientation, just a position
|
||||
typedef TypedSymbol<Point2, 'x'> PoseKey;
|
||||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
typedef Values<PoseKey> PoseValues;
|
||||
typedef Values<PointKey> PointValues;
|
||||
|
||||
/**
|
||||
* 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:
|
||||
typedef TupleValues2<PoseValues, PointValues> Base; ///< base class
|
||||
typedef DynamicValues Base; ///< base class
|
||||
typedef boost::shared_ptr<Point2> sharedPoint; ///< shortcut to shared Point type
|
||||
|
||||
/// Constructor
|
||||
Values() {
|
||||
Values() : nrPoses_(0), nrPoints_(0) {
|
||||
}
|
||||
|
||||
/// Copy constructor
|
||||
Values(const Base& base) :
|
||||
Base(base) {
|
||||
Base(base), nrPoses_(0), nrPoints_(0) {
|
||||
}
|
||||
|
||||
/// Insert a pose
|
||||
void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
|
||||
insert(i, p);
|
||||
nrPoses_++;
|
||||
}
|
||||
|
||||
/// Insert a point
|
||||
void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
|
||||
insert(j, p);
|
||||
nrPoints_++;
|
||||
}
|
||||
|
||||
/// Number of poses
|
||||
int nrPoses() const {
|
||||
return this->first_.size();
|
||||
return nrPoses_;
|
||||
}
|
||||
|
||||
/// Number of points
|
||||
int nrPoints() const {
|
||||
return this->second_.size();
|
||||
return nrPoints_;
|
||||
}
|
||||
|
||||
/// Return pose i
|
||||
|
|
@ -112,18 +114,18 @@ namespace gtsam {
|
|||
/**
|
||||
* Unary factor encoding a soft prior on a vector
|
||||
*/
|
||||
template<class VALUES = Values, class KEY = PoseKey>
|
||||
class GenericPrior: public NonlinearFactor1<VALUES, KEY> {
|
||||
template<class KEY = PoseKey>
|
||||
class GenericPrior: public NonlinearFactor1<KEY> {
|
||||
public:
|
||||
typedef NonlinearFactor1<VALUES, KEY> Base; ///< base class
|
||||
typedef boost::shared_ptr<GenericPrior<VALUES, KEY> > shared_ptr;
|
||||
typedef NonlinearFactor1<KEY> Base; ///< base class
|
||||
typedef boost::shared_ptr<GenericPrior<KEY> > shared_ptr;
|
||||
typedef typename KEY::Value Pose; ///< shortcut to Pose type
|
||||
|
||||
Pose z_; ///< prior mean
|
||||
|
||||
/// Create generic prior
|
||||
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
|
||||
|
|
@ -150,11 +152,11 @@ namespace gtsam {
|
|||
/**
|
||||
* Binary factor simulating "odometry" between two Vectors
|
||||
*/
|
||||
template<class VALUES = Values, class KEY = PoseKey>
|
||||
class GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
|
||||
template<class KEY = PoseKey>
|
||||
class GenericOdometry: public NonlinearFactor2<KEY, KEY> {
|
||||
public:
|
||||
typedef NonlinearFactor2<VALUES, KEY, KEY> Base; ///< base class
|
||||
typedef boost::shared_ptr<GenericOdometry<VALUES, KEY> > shared_ptr;
|
||||
typedef NonlinearFactor2<KEY, KEY> Base; ///< base class
|
||||
typedef boost::shared_ptr<GenericOdometry<KEY> > shared_ptr;
|
||||
typedef typename KEY::Value Pose; ///< shortcut to Pose type
|
||||
|
||||
Pose z_; ///< odometry measurement
|
||||
|
|
@ -162,7 +164,7 @@ namespace gtsam {
|
|||
/// Create odometry
|
||||
GenericOdometry(const Pose& z, const SharedNoiseModel& model,
|
||||
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
|
||||
|
|
@ -190,11 +192,11 @@ namespace gtsam {
|
|||
/**
|
||||
* Binary factor simulating "measurement" between two Vectors
|
||||
*/
|
||||
template<class VALUES = Values, class XKEY = PoseKey, class LKEY = PointKey>
|
||||
class GenericMeasurement: public NonlinearFactor2<VALUES, XKEY, LKEY> {
|
||||
template<class XKEY = PoseKey, class LKEY = PointKey>
|
||||
class GenericMeasurement: public NonlinearFactor2<XKEY, LKEY> {
|
||||
public:
|
||||
typedef NonlinearFactor2<VALUES, XKEY, LKEY> Base; ///< base class
|
||||
typedef boost::shared_ptr<GenericMeasurement<VALUES, XKEY, LKEY> > shared_ptr;
|
||||
typedef NonlinearFactor2<XKEY, LKEY> Base; ///< base class
|
||||
typedef boost::shared_ptr<GenericMeasurement<XKEY, LKEY> > shared_ptr;
|
||||
typedef typename XKEY::Value Pose; ///< shortcut to Pose type
|
||||
typedef typename LKEY::Value Point; ///< shortcut to Point type
|
||||
|
||||
|
|
@ -203,7 +205,7 @@ namespace gtsam {
|
|||
/// Create measurement factor
|
||||
GenericMeasurement(const Point& z, const SharedNoiseModel& model,
|
||||
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
|
||||
|
|
@ -229,13 +231,13 @@ namespace gtsam {
|
|||
};
|
||||
|
||||
/** Typedefs for regular use */
|
||||
typedef GenericPrior<Values, PoseKey> Prior;
|
||||
typedef GenericOdometry<Values, PoseKey> Odometry;
|
||||
typedef GenericMeasurement<Values, PoseKey, PointKey> Measurement;
|
||||
typedef GenericPrior<PoseKey> Prior;
|
||||
typedef GenericOdometry<PoseKey> Odometry;
|
||||
typedef GenericMeasurement<PoseKey, PointKey> Measurement;
|
||||
|
||||
// Specialization of a graph for this example domain
|
||||
// TODO: add functions to add factor types
|
||||
class Graph : public NonlinearFactorGraph<Values> {
|
||||
class Graph : public NonlinearFactorGraph {
|
||||
public:
|
||||
Graph() {}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -76,15 +76,15 @@ namespace gtsam {
|
|||
boost::none, boost::optional<Matrix&> H2 = boost::none);
|
||||
|
||||
/// Unary factor encoding a soft prior on a vector
|
||||
template<class VALUES = Values, class Key = PoseKey>
|
||||
struct GenericPosePrior: public NonlinearFactor1<VALUES, Key> {
|
||||
template<class Key = PoseKey>
|
||||
struct GenericPosePrior: public NonlinearFactor1<Key> {
|
||||
|
||||
Pose2 z_; ///< measurement
|
||||
|
||||
/// Create generic pose prior
|
||||
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model,
|
||||
const Key& key) :
|
||||
NonlinearFactor1<VALUES, Key>(model, key), z_(z) {
|
||||
NonlinearFactor1<Key>(model, key), z_(z) {
|
||||
}
|
||||
|
||||
/// Evaluate error and optionally derivative
|
||||
|
|
@ -98,8 +98,8 @@ namespace gtsam {
|
|||
/**
|
||||
* Binary factor simulating "odometry" between two Vectors
|
||||
*/
|
||||
template<class VALUES = Values, class KEY = PoseKey>
|
||||
struct GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
|
||||
template<class KEY = PoseKey>
|
||||
struct GenericOdometry: public NonlinearFactor2<KEY, KEY> {
|
||||
Pose2 z_; ///< Between measurement for odometry factor
|
||||
|
||||
/**
|
||||
|
|
@ -107,7 +107,7 @@ namespace gtsam {
|
|||
*/
|
||||
GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
|
||||
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
|
||||
|
|
@ -119,10 +119,10 @@ namespace gtsam {
|
|||
|
||||
};
|
||||
|
||||
typedef GenericOdometry<Values, PoseKey> Odometry;
|
||||
typedef GenericOdometry<PoseKey> Odometry;
|
||||
|
||||
/// Graph specialization for syntactic sugar use with matlab
|
||||
class Graph : public NonlinearFactorGraph<Values> {
|
||||
class Graph : public NonlinearFactorGraph {
|
||||
public:
|
||||
Graph() {}
|
||||
// TODO: add functions to add factors
|
||||
|
|
|
|||
|
|
@ -40,10 +40,6 @@ namespace simulated3D {
|
|||
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
|
||||
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
|
||||
|
||||
typedef Values<PoseKey> PoseValues;
|
||||
typedef Values<PointKey> PointValues;
|
||||
typedef TupleValues2<PoseValues, PointValues> Values;
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
|
||||
struct PointPrior3D: public NonlinearFactor1<PoseKey> {
|
||||
|
||||
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,
|
||||
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
|
||||
*/
|
||||
struct Simulated3DMeasurement: public NonlinearFactor2<Values,
|
||||
PoseKey, PointKey> {
|
||||
struct Simulated3DMeasurement: public NonlinearFactor2<PoseKey, PointKey> {
|
||||
|
||||
Point3 z_; ///< Linear displacement between a pose and landmark
|
||||
|
||||
|
|
@ -111,7 +106,7 @@ PoseKey, PointKey> {
|
|||
*/
|
||||
Simulated3DMeasurement(const Point3& z,
|
||||
const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
|
||||
NonlinearFactor2<Values, PoseKey, PointKey> (
|
||||
NonlinearFactor2<PoseKey, PointKey> (
|
||||
model, j1, j2), z_(z) {
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ using namespace std;
|
|||
namespace gtsam {
|
||||
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 sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
|
||||
|
|
@ -195,14 +195,13 @@ namespace example {
|
|||
0.0, cos(v.y()));
|
||||
}
|
||||
|
||||
struct UnaryFactor: public gtsam::NonlinearFactor1<Values,
|
||||
simulated2D::PoseKey> {
|
||||
struct UnaryFactor: public gtsam::NonlinearFactor1<simulated2D::PoseKey> {
|
||||
|
||||
Point2 z_;
|
||||
|
||||
UnaryFactor(const Point2& z, const SharedNoiseModel& model,
|
||||
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 =
|
||||
|
|
@ -423,7 +422,7 @@ namespace example {
|
|||
boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N) {
|
||||
|
||||
// create empty graph
|
||||
NonlinearFactorGraph<Values> nlfg;
|
||||
NonlinearFactorGraph nlfg;
|
||||
|
||||
// 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)));
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ namespace gtsam {
|
|||
namespace example {
|
||||
|
||||
typedef simulated2D::Values Values;
|
||||
typedef NonlinearFactorGraph<Values> Graph;
|
||||
typedef NonlinearFactorGraph Graph;
|
||||
|
||||
/**
|
||||
* Create small example for non-linear factor graph
|
||||
|
|
|
|||
Loading…
Reference in New Issue