diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 4b47e133e..e0066f21a 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -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 NonlinearEquality: public NonlinearFactor1 { + template + class NonlinearEquality: public NonlinearFactor1 { public: typedef typename KEY::Value T; @@ -69,7 +69,7 @@ namespace gtsam { */ bool (*compare_)(const T& a, const T& b); - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; /** default constructor - only for serialization */ NonlinearEquality() {} @@ -101,14 +101,14 @@ namespace gtsam { } /** Check if two factors are equal */ - bool equals(const NonlinearEquality& f, double tol = 1e-9) const { + bool equals(const NonlinearEquality& 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 NonlinearEquality1 : public NonlinearFactor1 { + template + class NonlinearEquality1 : public NonlinearFactor1 { public: typedef typename KEY::Value X; protected: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; /** default constructor to allow for serialization */ NonlinearEquality1() {} @@ -181,7 +181,7 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > 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 NonlinearEquality2 : public NonlinearFactor2 { + template + class NonlinearEquality2 : public NonlinearFactor2 { public: typedef typename KEY::Value X; protected: - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; GTSAM_CONCEPT_MANIFOLD_TYPE(X); @@ -235,7 +235,7 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0) : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} diff --git a/gtsam/nonlinear/NonlinearOptimization-inl.h b/gtsam/nonlinear/NonlinearOptimization-inl.h index 36d3cccd0..97da0c367 100644 --- a/gtsam/nonlinear/NonlinearOptimization-inl.h +++ b/gtsam/nonlinear/NonlinearOptimization-inl.h @@ -31,17 +31,17 @@ namespace gtsam { /** * The Elimination solver */ - template - T optimizeSequential(const G& graph, const T& initialEstimate, + template + 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 Optimizer; + typedef NonlinearOptimizer Optimizer; Optimizer optimizer(boost::make_shared(graph), - boost::make_shared(initialEstimate), ordering, + boost::make_shared(initialEstimate), ordering, boost::make_shared(parameters)); // Now optimize using either LM or GN methods. @@ -54,17 +54,17 @@ namespace gtsam { /** * The multifrontal solver */ - template - T optimizeMultiFrontal(const G& graph, const T& initialEstimate, + template + 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 Optimizer; + typedef NonlinearOptimizer Optimizer; Optimizer optimizer(boost::make_shared(graph), - boost::make_shared(initialEstimate), ordering, + boost::make_shared(initialEstimate), ordering, boost::make_shared(parameters)); // now optimize using either LM or GN methods @@ -77,20 +77,20 @@ namespace gtsam { /** * The sparse preconditioned conjugate gradient solver */ - template - T optimizeSPCG(const G& graph, const T& initialEstimate, + template + 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 Solver; + typedef SubgraphSolver Solver; typedef boost::shared_ptr shared_Solver; - typedef NonlinearOptimizer SPCGOptimizer; + typedef NonlinearOptimizer SPCGOptimizer; shared_Solver solver = boost::make_shared( graph, initialEstimate, IterativeOptimizationParameters()); SPCGOptimizer optimizer( boost::make_shared(graph), - boost::make_shared(initialEstimate), + boost::make_shared(initialEstimate), solver->ordering(), solver, boost::make_shared(parameters)); @@ -105,19 +105,19 @@ namespace gtsam { /** * optimization that returns the values */ - template - T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters, + template + DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate, const NonlinearOptimizationParameters& parameters, const LinearSolver& solver, const NonlinearOptimizationMethod& nonlinear_method) { switch (solver) { case SEQUENTIAL: - return optimizeSequential(graph, initialEstimate, parameters, + return optimizeSequential(graph, initialEstimate, parameters, nonlinear_method == LM); case MULTIFRONTAL: - return optimizeMultiFrontal(graph, initialEstimate, parameters, + return optimizeMultiFrontal(graph, initialEstimate, parameters, nonlinear_method == LM); case SPCG: -// return optimizeSPCG(graph, initialEstimate, parameters, +// return optimizeSPCG(graph, initialEstimate, parameters, // nonlinear_method == LM); throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint"); } diff --git a/gtsam/nonlinear/NonlinearOptimization.h b/gtsam/nonlinear/NonlinearOptimization.h index a477d4fd5..86ab7a3c8 100644 --- a/gtsam/nonlinear/NonlinearOptimization.h +++ b/gtsam/nonlinear/NonlinearOptimization.h @@ -42,8 +42,8 @@ namespace gtsam { /** * optimization that returns the values */ - template - T optimize(const G& graph, const T& initialEstimate, + template + DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(), const LinearSolver& solver = MULTIFRONTAL, const NonlinearOptimizationMethod& nonlinear_method = LM); diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index 02ead8b0d..2e7a83f76 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -30,8 +30,8 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - template - NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, + template + NonlinearOptimizer::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 - NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, + template + NonlinearOptimizer::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 - NonlinearOptimizer NonlinearOptimizer::iterate() const { + template + NonlinearOptimizer NonlinearOptimizer::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 - NonlinearOptimizer NonlinearOptimizer::gaussNewton() const { + template + NonlinearOptimizer NonlinearOptimizer::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 - NonlinearOptimizer NonlinearOptimizer::try_lambda(const L& linearSystem) { + template + NonlinearOptimizer NonlinearOptimizer::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 - NonlinearOptimizer NonlinearOptimizer::iterateLM() { + template + NonlinearOptimizer NonlinearOptimizer::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 - NonlinearOptimizer NonlinearOptimizer::levenbergMarquardt() { + template + NonlinearOptimizer NonlinearOptimizer::levenbergMarquardt() { // Initialize bool converged = false; @@ -299,20 +299,20 @@ namespace gtsam { } /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::iterateDogLeg() { + template + NonlinearOptimizer NonlinearOptimizer::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 - NonlinearOptimizer NonlinearOptimizer::dogLeg() { + template + NonlinearOptimizer NonlinearOptimizer::dogLeg() { static W writer(error_); // check if we're already close enough diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 4073df424..e87b44a5d 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -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 implements - * linearize: G * T -> L - * solve : L -> T + * Concept NonLinearSolver 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. * 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 in your cpp file */ -template +template class NonlinearOptimizer { public: // For performance reasons in recursion, we store values in a shared_ptr - typedef boost::shared_ptr shared_values; ///Prevent memory leaks in Values + typedef boost::shared_ptr shared_values; ///Prevent memory leaks in Values typedef boost::shared_ptr shared_graph; /// Prevent memory leaks in Graph typedef boost::shared_ptr shared_linear; /// Not sure typedef boost::shared_ptr shared_ordering; ///ordering parameters @@ -73,7 +73,7 @@ public: private: - typedef NonlinearOptimizer This; + typedef NonlinearOptimizer This; typedef boost::shared_ptr > shared_dimensions; /// keep a reference to const version of the graph @@ -167,7 +167,7 @@ public: /** * Copy constructor */ - NonlinearOptimizer(const NonlinearOptimizer &optimizer) : + NonlinearOptimizer(const NonlinearOptimizer &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(graph), - boost::make_shared(values), + boost::make_shared(values), boost::make_shared(parameters)); } static shared_values optimizeLM(const G& graph, - const T& values, + const DynamicValues& values, Parameters::verbosityLevel verbosity) { return optimizeLM(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(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(graph), - boost::make_shared(values), + boost::make_shared(values), boost::make_shared(parameters)); } static shared_values optimizeDogLeg(const G& graph, - const T& values, + const DynamicValues& values, Parameters::verbosityLevel verbosity) { return optimizeDogLeg(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(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(graph), - boost::make_shared(values), + boost::make_shared(values), boost::make_shared(parameters)); } }; diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index d17dd135e..eaaf3d4e9 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -25,16 +25,16 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template - class BearingFactor: public NonlinearFactor2 { + template + class BearingFactor: public NonlinearFactor2 { private: typedef typename POSEKEY::Value Pose; typedef typename POSEKEY::Value::Rotation Rot; typedef typename POINTKEY::Value Point; - typedef BearingFactor This; - typedef NonlinearFactor2 Base; + typedef BearingFactor This; + typedef NonlinearFactor2 Base; Rot z_; /** measurement */ @@ -68,7 +68,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol); } diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 833708b5d..ef71f8f13 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -27,16 +27,16 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template - class BearingRangeFactor: public NonlinearFactor2 { + template + class BearingRangeFactor: public NonlinearFactor2 { private: typedef typename POSEKEY::Value Pose; typedef typename POSEKEY::Value::Rotation Rot; typedef typename POINTKEY::Value Point; - typedef BearingRangeFactor This; - typedef NonlinearFactor2 Base; + typedef BearingRangeFactor This; + typedef NonlinearFactor2 Base; // the measurement Rot bearing_; @@ -68,7 +68,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && fabs(this->range_ - e->range_) < tol && diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index dee18ebc7..09d54d524 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -29,13 +29,13 @@ namespace gtsam { * KEY1::Value is the Lie Group type * T is the measurement type, by default the same */ - template - class BetweenFactor: public NonlinearFactor2 { + template + class BetweenFactor: public NonlinearFactor2 { private: - typedef BetweenFactor This; - typedef NonlinearFactor2 Base; + typedef BetweenFactor This; + typedef NonlinearFactor2 Base; T measured_; /** The measurement */ @@ -71,7 +71,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); } diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index e8f59cb2d..274c0d83d 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -28,11 +28,11 @@ namespace gtsam { * will need to have its value function implemented to return * a scalar for comparison. */ -template -struct BoundingConstraint1: public NonlinearFactor1 { +template +struct BoundingConstraint1: public NonlinearFactor1 { typedef typename KEY::Value X; - typedef NonlinearFactor1 Base; - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor1 Base; + typedef boost::shared_ptr > shared_ptr; double threshold_; bool isGreaterThan_; /// flag for greater/less than @@ -57,7 +57,7 @@ struct BoundingConstraint1: public NonlinearFactor1 { 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 -struct BoundingConstraint2: public NonlinearFactor2 { +template +struct BoundingConstraint2: public NonlinearFactor2 { typedef typename KEY1::Value X1; typedef typename KEY2::Value X2; - typedef NonlinearFactor2 Base; - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor2 Base; + typedef boost::shared_ptr > shared_ptr; double threshold_; bool isGreaterThan_; /// flag for greater/less than @@ -125,7 +125,7 @@ struct BoundingConstraint2: public NonlinearFactor2 { boost::optional 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_; diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 2d7b631cc..fa33a9bc7 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -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 PriorFactor: public NonlinearFactor1 { + template + class PriorFactor: public NonlinearFactor1 { public: typedef typename KEY::Value T; private: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; T prior_; /** The measurement */ @@ -69,9 +69,8 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const PriorFactor *e = dynamic_cast*> (&expected); + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const PriorFactor *e = dynamic_cast*> (&expected); return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b5322ee47..712125588 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -30,7 +30,7 @@ using namespace gtsam; #define LINESIZE 81920 typedef boost::shared_ptr sharedPose2Graph; -typedef boost::shared_ptr sharedPose2Values; +typedef boost::shared_ptr sharedPose2Values; namespace gtsam { @@ -81,7 +81,7 @@ pair 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 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 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 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 > factor_, graph) { + BOOST_FOREACH(boost::shared_ptr factor_, graph) { boost::shared_ptr factor = boost::dynamic_pointer_cast(factor_); if (!factor) continue; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index d808dfa24..2b339db18 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -41,16 +41,16 @@ std::pair > * @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 > load2D( +std::pair, boost::shared_ptr > load2D( std::pair > dataset, int maxID = 0, bool addNoise=false, bool smart=true); -std::pair, boost::shared_ptr > load2D( +std::pair, boost::shared_ptr > load2D( const std::string& filename, boost::optional model = boost::optional(), 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); /** diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index 86dd3ea16..090607d72 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -22,16 +22,16 @@ // Use pose2SLAM namespace for specific SLAM instance namespace gtsam { - template class NonlinearOptimizer; + template class NonlinearOptimizer; 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; } diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 45017a81b..714bcce51 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -37,9 +37,6 @@ namespace gtsam { /// Keys with Pose2 and symbol 'x' typedef TypedSymbol Key; - /// Values with type 'Key' - typedef Values 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 Prior; + typedef PriorFactor Prior; /// A factor to put constraints between two factors. - typedef BetweenFactor Constraint; + typedef BetweenFactor Constraint; /// A hard constraint would enforce that the given key would have the input value in the results. - typedef NonlinearEquality HardConstraint; + typedef NonlinearEquality HardConstraint; /// Graph - struct Graph: public NonlinearFactorGraph { + struct Graph: public NonlinearFactorGraph { /// Adds a factor between keys of the same type - typedef BetweenFactor Constraint; + typedef BetweenFactor Constraint; /// A simple typedef from Pose2 to Pose typedef Pose2 Pose; @@ -78,17 +75,16 @@ namespace gtsam { }; /// The sequential optimizer - typedef NonlinearOptimizer OptimizerSequential; + typedef NonlinearOptimizer OptimizerSequential; /// The multifrontal optimizer - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer 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 diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index cae60bb80..4cb23f76a 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -32,44 +32,46 @@ namespace gtsam { // Simulated2D robots have no orientation, just a position typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - typedef Values PoseValues; - typedef Values PointValues; /** * Custom Values class that holds poses and points */ - class Values: public TupleValues2 { + class Values: public DynamicValues { + size_t nrPoses_; + size_t nrPoints_; public: - typedef TupleValues2 Base; ///< base class + typedef DynamicValues Base; ///< base class typedef boost::shared_ptr 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 GenericPrior: public NonlinearFactor1 { + template + class GenericPrior: public NonlinearFactor1 { public: - typedef NonlinearFactor1 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor1 Base; ///< base class + typedef boost::shared_ptr > 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(model, key), z_(z) { + NonlinearFactor1(model, key), z_(z) { } /// Return error and optional derivative @@ -150,11 +152,11 @@ namespace gtsam { /** * Binary factor simulating "odometry" between two Vectors */ - template - class GenericOdometry: public NonlinearFactor2 { + template + class GenericOdometry: public NonlinearFactor2 { public: - typedef NonlinearFactor2 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor2 Base; ///< base class + typedef boost::shared_ptr > 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(model, i1, i2), z_(z) { + NonlinearFactor2(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 GenericMeasurement: public NonlinearFactor2 { + template + class GenericMeasurement: public NonlinearFactor2 { public: - typedef NonlinearFactor2 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor2 Base; ///< base class + typedef boost::shared_ptr > 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(model, i, j), z_(z) { + NonlinearFactor2(model, i, j), z_(z) { } /// Evaluate error and optionally return derivatives @@ -229,13 +231,13 @@ namespace gtsam { }; /** Typedefs for regular use */ - typedef GenericPrior Prior; - typedef GenericOdometry Odometry; - typedef GenericMeasurement Measurement; + typedef GenericPrior Prior; + typedef GenericOdometry Odometry; + typedef GenericMeasurement Measurement; // Specialization of a graph for this example domain // TODO: add functions to add factor types - class Graph : public NonlinearFactorGraph { + class Graph : public NonlinearFactorGraph { public: Graph() {} }; diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index b9cf150ef..f44c92118 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -76,15 +76,15 @@ namespace gtsam { boost::none, boost::optional H2 = boost::none); /// Unary factor encoding a soft prior on a vector - template - struct GenericPosePrior: public NonlinearFactor1 { + template + struct GenericPosePrior: public NonlinearFactor1 { Pose2 z_; ///< measurement /// Create generic pose prior GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, const Key& key) : - NonlinearFactor1(model, key), z_(z) { + NonlinearFactor1(model, key), z_(z) { } /// Evaluate error and optionally derivative @@ -98,8 +98,8 @@ namespace gtsam { /** * Binary factor simulating "odometry" between two Vectors */ - template - struct GenericOdometry: public NonlinearFactor2 { + template + struct GenericOdometry: public NonlinearFactor2 { 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(model, i1, i2), z_(z) { + NonlinearFactor2(model, i1, i2), z_(z) { } /// Evaluate error and optionally derivative @@ -119,10 +119,10 @@ namespace gtsam { }; - typedef GenericOdometry Odometry; + typedef GenericOdometry Odometry; /// Graph specialization for syntactic sugar use with matlab - class Graph : public NonlinearFactorGraph { + class Graph : public NonlinearFactorGraph { public: Graph() {} // TODO: add functions to add factors diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h index 32f05e967..e995308f8 100644 --- a/gtsam/slam/simulated3D.h +++ b/gtsam/slam/simulated3D.h @@ -40,10 +40,6 @@ namespace simulated3D { typedef gtsam::TypedSymbol PoseKey; typedef gtsam::TypedSymbol PointKey; -typedef Values PoseValues; -typedef Values PointValues; -typedef TupleValues2 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 { +struct PointPrior3D: public NonlinearFactor1 { Point3 z_; ///< The prior pose value for the variable attached to this factor @@ -78,7 +74,7 @@ struct PointPrior3D: public NonlinearFactor1 { */ PointPrior3D(const Point3& z, const SharedNoiseModel& model, const PoseKey& j) : - NonlinearFactor1 (model, j), z_(z) { + NonlinearFactor1 (model, j), z_(z) { } /** @@ -97,8 +93,7 @@ struct PointPrior3D: public NonlinearFactor1 { /** * Models a linear 3D measurement between 3D points */ -struct Simulated3DMeasurement: public NonlinearFactor2 { +struct Simulated3DMeasurement: public NonlinearFactor2 { 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 ( + NonlinearFactor2 ( model, j1, j2), z_(z) { } diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 40d3e98ff..948616ea6 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -39,7 +39,7 @@ using namespace std; namespace gtsam { namespace example { - typedef boost::shared_ptr > shared; + typedef boost::shared_ptr 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 { + struct UnaryFactor: public gtsam::NonlinearFactor1 { Point2 z_; UnaryFactor(const Point2& z, const SharedNoiseModel& model, const simulated2D::PoseKey& key) : - gtsam::NonlinearFactor1(model, key), z_(z) { + gtsam::NonlinearFactor1(model, key), z_(z) { } Vector evaluateError(const Point2& x, boost::optional A = @@ -423,7 +422,7 @@ namespace example { boost::tuple, Ordering, VectorValues> planarGraph(size_t N) { // create empty graph - NonlinearFactorGraph 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))); diff --git a/gtsam/slam/smallExample.h b/gtsam/slam/smallExample.h index 1889ee1b5..2380d86f2 100644 --- a/gtsam/slam/smallExample.h +++ b/gtsam/slam/smallExample.h @@ -29,7 +29,7 @@ namespace gtsam { namespace example { typedef simulated2D::Values Values; - typedef NonlinearFactorGraph Graph; + typedef NonlinearFactorGraph Graph; /** * Create small example for non-linear factor graph