From 5b5bbfdfff048d5f50545eab1c0972d2317cf6b0 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Sun, 29 Jan 2012 21:12:58 +0000 Subject: [PATCH] testSimmulated2D passes. Too many warnings in boost from clang! --- gtsam/nonlinear/DoglegOptimizerImpl.h | 2 +- gtsam/nonlinear/Makefile.am | 4 +- ...orGraph-inl.h => NonlinearFactorGraph.cpp} | 9 ++-- gtsam/nonlinear/NonlinearFactorGraph.h | 1 - .../{Values-inl.h => ValuesOld-inl.h} | 48 +++++++++---------- gtsam/nonlinear/{Values.h => ValuesOld.h} | 32 ++++++------- gtsam/slam/PartialPriorFactor.h | 8 ++-- gtsam/slam/ProjectionFactor.h | 10 ++-- gtsam/slam/RangeFactor.h | 10 ++-- gtsam/slam/StereoFactor.h | 6 +-- gtsam/slam/dataset.cpp | 10 ++-- gtsam/slam/planarSLAM.cpp | 4 +- gtsam/slam/planarSLAM.h | 46 +++++++----------- gtsam/slam/pose2SLAM.h | 1 - gtsam/slam/pose3SLAM.cpp | 6 +-- gtsam/slam/pose3SLAM.h | 16 +++---- gtsam/slam/simulated2D.h | 1 - gtsam/slam/simulated2DOriented.h | 14 +++--- gtsam/slam/simulated3D.h | 1 - gtsam/slam/visualSLAM.h | 28 +++++------ tests/testNonlinearFactor.cpp | 8 ++-- 21 files changed, 120 insertions(+), 145 deletions(-) rename gtsam/nonlinear/{NonlinearFactorGraph-inl.h => NonlinearFactorGraph.cpp} (98%) rename gtsam/nonlinear/{Values-inl.h => ValuesOld-inl.h} (81%) rename gtsam/nonlinear/{Values.h => ValuesOld.h} (90%) diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index daab0c66e..5923baa18 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -7,7 +7,7 @@ #include #include // To get optimize(BayesTree) -#include +//#include #include namespace gtsam { diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 40a96ba5f..f641ab9e7 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -22,10 +22,10 @@ check_PROGRAMS += tests/testValues tests/testDynamicValues tests/testKey tests/t # Nonlinear nonlinear headers += Key.h -headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h +headers += NonlinearFactorGraph.h headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h NonlinearOptimizationParameters.h headers += NonlinearFactor.h -sources += NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp +sources += NonlinearFactorGraph.cpp NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp headers += DoglegOptimizer.h DoglegOptimizer-inl.h # Nonlinear iSAM(2) diff --git a/gtsam/nonlinear/NonlinearFactorGraph-inl.h b/gtsam/nonlinear/NonlinearFactorGraph.cpp similarity index 98% rename from gtsam/nonlinear/NonlinearFactorGraph-inl.h rename to gtsam/nonlinear/NonlinearFactorGraph.cpp index 445dbf20e..880cb2fb9 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph-inl.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -10,19 +10,18 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearFactorGraph-inl.h + * @file NonlinearFactorGraph.cpp * @brief Factor Graph Consisting of non-linear factors * @author Frank Dellaert * @author Carlos Nieto * @author Christian Potthast */ -#pragma once - +#include +#include #include #include -#include -#include +#include using namespace std; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 64b035bc0..96beadf3d 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -100,4 +100,3 @@ namespace gtsam { } // namespace -#include diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/ValuesOld-inl.h similarity index 81% rename from gtsam/nonlinear/Values-inl.h rename to gtsam/nonlinear/ValuesOld-inl.h index 28f08bfb5..5f53e32e8 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/ValuesOld-inl.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file Values.cpp + * @file ValuesOld.cpp * @author Richard Roberts */ @@ -34,8 +34,8 @@ namespace gtsam { /* ************************************************************************* */ template - void Values::print(const string &s) const { - cout << "Values " << s << ", size " << values_.size() << "\n"; + void ValuesOld::print(const string &s) const { + cout << "ValuesOld " << s << ", size " << values_.size() << "\n"; BOOST_FOREACH(const KeyValuePair& v, values_) { gtsam::print(v.second, (string)(v.first)); } @@ -43,7 +43,7 @@ namespace gtsam { /* ************************************************************************* */ template - bool Values::equals(const Values& expected, double tol) const { + bool ValuesOld::equals(const ValuesOld& expected, double tol) const { if (values_.size() != expected.values_.size()) return false; BOOST_FOREACH(const KeyValuePair& v, values_) { if (!expected.exists(v.first)) return false; @@ -55,7 +55,7 @@ namespace gtsam { /* ************************************************************************* */ template - const typename J::Value& Values::at(const J& j) const { + const typename J::Value& ValuesOld::at(const J& j) const { const_iterator it = values_.find(j); if (it == values_.end()) throw KeyDoesNotExist("retrieve", j); else return it->second; @@ -63,7 +63,7 @@ namespace gtsam { /* ************************************************************************* */ template - size_t Values::dim() const { + size_t ValuesOld::dim() const { size_t n = 0; BOOST_FOREACH(const KeyValuePair& value, values_) n += value.second.dim(); @@ -72,27 +72,27 @@ namespace gtsam { /* ************************************************************************* */ template - VectorValues Values::zero(const Ordering& ordering) const { + VectorValues ValuesOld::zero(const Ordering& ordering) const { return VectorValues::Zero(this->dims(ordering)); } /* ************************************************************************* */ template - void Values::insert(const J& name, const typename J::Value& val) { + void ValuesOld::insert(const J& name, const typename J::Value& val) { if(!values_.insert(make_pair(name, val)).second) throw KeyAlreadyExists(name, val); } /* ************************************************************************* */ template - void Values::insert(const Values& cfg) { + void ValuesOld::insert(const ValuesOld& cfg) { BOOST_FOREACH(const KeyValuePair& v, cfg.values_) insert(v.first, v.second); } /* ************************************************************************* */ template - void Values::update(const Values& cfg) { + void ValuesOld::update(const ValuesOld& cfg) { BOOST_FOREACH(const KeyValuePair& v, values_) { boost::optional t = cfg.exists_(v.first); if (t) values_[v.first] = *t; @@ -101,13 +101,13 @@ namespace gtsam { /* ************************************************************************* */ template - void Values::update(const J& j, const typename J::Value& val) { + void ValuesOld::update(const J& j, const typename J::Value& val) { values_[j] = val; } /* ************************************************************************* */ template - std::list Values::keys() const { + std::list ValuesOld::keys() const { std::list ret; BOOST_FOREACH(const KeyValuePair& v, values_) ret.push_back(v.first); @@ -116,14 +116,14 @@ namespace gtsam { /* ************************************************************************* */ template - void Values::erase(const J& j) { + void ValuesOld::erase(const J& j) { size_t dim; // unused erase(j, dim); } /* ************************************************************************* */ template - void Values::erase(const J& j, size_t& dim) { + void ValuesOld::erase(const J& j, size_t& dim) { iterator it = values_.find(j); if (it == values_.end()) throw KeyDoesNotExist("erase", j); @@ -134,8 +134,8 @@ namespace gtsam { /* ************************************************************************* */ // todo: insert for every element is inefficient template - Values Values::retract(const VectorValues& delta, const Ordering& ordering) const { - Values newValues; + ValuesOld ValuesOld::retract(const VectorValues& delta, const Ordering& ordering) const { + ValuesOld newValues; typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, this->values_) { const J& j = value.first; @@ -151,7 +151,7 @@ namespace gtsam { /* ************************************************************************* */ template - std::vector Values::dims(const Ordering& ordering) const { + std::vector ValuesOld::dims(const Ordering& ordering) const { _ValuesDimensionCollector dimCollector(ordering); this->apply(dimCollector); return dimCollector.dimensions; @@ -159,7 +159,7 @@ namespace gtsam { /* ************************************************************************* */ template - Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const { + Ordering::shared_ptr ValuesOld::orderingArbitrary(Index firstVar) const { // Generate an initial key ordering in iterator order _ValuesKeyOrderer keyOrderer(firstVar); this->apply(keyOrderer); @@ -169,12 +169,12 @@ namespace gtsam { // /* ************************************************************************* */ // // todo: insert for every element is inefficient // template -// Values Values::retract(const Vector& delta) const { +// ValuesOld ValuesOld::retract(const Vector& delta) const { // if(delta.size() != dim()) { -// cout << "Values::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; +// cout << "ValuesOld::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; // throw invalid_argument("Delta vector length does not match config dimensionality."); // } -// Values newValues; +// ValuesOld newValues; // int delta_offset = 0; // typedef pair KeyValue; // BOOST_FOREACH(const KeyValue& value, this->values_) { @@ -191,7 +191,7 @@ namespace gtsam { // todo: insert for every element is inefficient // todo: currently only logmaps elements in both configs template - inline VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const { + inline VectorValues ValuesOld::localCoordinates(const ValuesOld& cp, const Ordering& ordering) const { VectorValues delta(this->dims(ordering)); localCoordinates(cp, ordering, delta); return delta; @@ -199,7 +199,7 @@ namespace gtsam { /* ************************************************************************* */ template - void Values::localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const { + void ValuesOld::localCoordinates(const ValuesOld& cp, const Ordering& ordering, VectorValues& delta) const { typedef pair KeyValue; BOOST_FOREACH(const KeyValue& value, cp) { assert(this->exists(value.first)); @@ -213,7 +213,7 @@ namespace gtsam { if(message_.empty()) message_ = "Attempting to " + std::string(operation_) + " the key \"" + - (std::string)key_ + "\", which does not exist in the Values."; + (std::string)key_ + "\", which does not exist in the ValuesOld."; return message_.c_str(); } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/ValuesOld.h similarity index 90% rename from gtsam/nonlinear/Values.h rename to gtsam/nonlinear/ValuesOld.h index a4bc57c0c..88595a5dd 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/ValuesOld.h @@ -10,14 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file Values.h + * @file ValuesOld.h * @author Richard Roberts * * @brief A templated config for Manifold-group elements * * Detailed story: * A values structure is a map from keys to values. It is used to specify the value of a bunch - * of variables in a factor graph. A Values is a values structure which can hold variables that + * of variables in a factor graph. A ValuesOld is a values structure which can hold variables that * are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. */ @@ -52,7 +52,7 @@ namespace gtsam { * labels (example: Pose2, Point2, etc) */ template - class Values { + class ValuesOld { public: @@ -77,12 +77,12 @@ namespace gtsam { public: - Values() {} - Values(const Values& config) : + ValuesOld() {} + ValuesOld(const ValuesOld& config) : values_(config.values_) {} template - Values(const Values& other) {} // do nothing when initializing with wrong type - virtual ~Values() {} + ValuesOld(const ValuesOld& other) {} // do nothing when initializing with wrong type + virtual ~ValuesOld() {} /// @name Testable /// @{ @@ -91,7 +91,7 @@ namespace gtsam { void print(const std::string &s="") const; /** Test whether configs are identical in keys and values */ - bool equals(const Values& expected, double tol=1e-9) const; + bool equals(const ValuesOld& expected, double tol=1e-9) const; /// @} @@ -132,13 +132,13 @@ namespace gtsam { size_t dim() const; /** Add a delta config to current config and returns a new config */ - Values retract(const VectorValues& delta, const Ordering& ordering) const; + ValuesOld retract(const VectorValues& delta, const Ordering& ordering) const; /** Get a delta config about a linearization point c0 (*this) */ - VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const; + VectorValues localCoordinates(const ValuesOld& cp, const Ordering& ordering) const; /** Get a delta config about a linearization point c0 (*this) */ - void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const; + void localCoordinates(const ValuesOld& cp, const Ordering& ordering, VectorValues& delta) const; /// @} @@ -148,10 +148,10 @@ namespace gtsam { void insert(const J& j, const Value& val); /** Add a set of variables, throws KeyAlreadyExists if a key is already present */ - void insert(const Values& cfg); + void insert(const ValuesOld& cfg); /** update the current available values without adding new ones */ - void update(const Values& cfg); + void update(const ValuesOld& cfg); /** single element change of existing element */ void update(const J& j, const Value& val); @@ -172,7 +172,7 @@ namespace gtsam { std::list keys() const; /** Replace all keys and variables */ - Values& operator=(const Values& rhs) { + ValuesOld& operator=(const ValuesOld& rhs) { values_ = rhs.values_; return (*this); } @@ -185,7 +185,7 @@ namespace gtsam { /** * Apply a class with an application operator() to a const_iterator over * every pair. The operator must be able to handle such an - * iterator for every type in the Values, (i.e. through templating). + * iterator for every type in the ValuesOld, (i.e. through templating). */ template void apply(A& operation) { @@ -297,5 +297,5 @@ public: } // \namespace gtsam -#include +#include diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index eb1c4614c..4d66620dd 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -38,16 +38,16 @@ namespace gtsam { * For practical use, it would be good to subclass this factor and have the class type * construct the mask. */ - template - class PartialPriorFactor: public NonlinearFactor1 { + template + class PartialPriorFactor: public NonlinearFactor1 { public: typedef typename KEY::Value T; protected: - typedef NonlinearFactor1 Base; - typedef PartialPriorFactor This; + typedef NonlinearFactor1 Base; + typedef PartialPriorFactor This; Vector prior_; ///< measurement on logmap parameters, in compressed form std::vector mask_; ///< indices of values to constrain in compressed prior vector diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 73d18a7e6..dcf425aaa 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -29,8 +29,8 @@ namespace gtsam { * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. * i.e. the main building block for visual SLAM. */ - template - class GenericProjectionFactor: public NonlinearFactor2 { + template + class GenericProjectionFactor: public NonlinearFactor2 { protected: // Keep a copy of measurement and calibration for I/O @@ -40,10 +40,10 @@ namespace gtsam { public: /// shorthand for base class type - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; /// Default constructor GenericProjectionFactor() : @@ -73,7 +73,7 @@ namespace gtsam { } /// equals - bool equals(const GenericProjectionFactor& p + bool equals(const GenericProjectionFactor& p , double tol = 1e-9) const { return Base::equals(p, tol) && this->z_.equals(p.z_, tol) && this->K_->equals(*p.K_, tol); diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 93cef86f1..b762c5690 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -25,14 +25,14 @@ namespace gtsam { /** * Binary factor for a range measurement */ - template - class RangeFactor: public NonlinearFactor2 { + template + class RangeFactor: public NonlinearFactor2 { private: double z_; /** measurement */ - typedef RangeFactor This; - typedef NonlinearFactor2 Base; + typedef RangeFactor This; + typedef NonlinearFactor2 Base; typedef typename POSEKEY::Value Pose; typedef typename POINTKEY::Value Point; @@ -64,7 +64,7 @@ namespace gtsam { } /** equals specialized to this factor */ - 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->z_ - e->z_) < tol; } diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 729b9fff1..7f1e8077d 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -22,8 +22,8 @@ namespace gtsam { -template -class GenericStereoFactor: public NonlinearFactor2 { +template +class GenericStereoFactor: public NonlinearFactor2 { private: // Keep a copy of measurement and calibration for I/O @@ -33,7 +33,7 @@ private: public: // shorthand for base class type - typedef NonlinearFactor2 Base; ///< typedef for base class + typedef NonlinearFactor2 Base; ///< typedef for base class typedef boost::shared_ptr shared_ptr; ///< typedef for shared pointer to this object typedef typename KEY1::Value CamPose; ///< typedef for Pose Lie Value type diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 712125588..d94749b7f 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -156,10 +156,10 @@ void save2D(const Pose2Graph& graph, const DynamicValues& config, const SharedDi fstream stream(filename.c_str(), fstream::out); // save poses - pose2SLAM::Key key; - Pose2 pose; - BOOST_FOREACH(boost::tie(key, pose), config) - stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; + for (DynamicValues::const_iterator it = config.begin(); it != config.end(); ++it) { + Pose2 pose = config.at(it->first); + stream << "VERTEX2 " << it->first.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; + } // save edges Matrix R = model->R(); @@ -168,7 +168,7 @@ void save2D(const Pose2Graph& graph, const DynamicValues& config, const SharedDi boost::shared_ptr factor = boost::dynamic_pointer_cast(factor_); if (!factor) continue; - pose = factor->measured().inverse(); + Pose2 pose = factor->measured().inverse(); stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index c90e56be9..d4f48e15d 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -25,8 +25,8 @@ namespace gtsam { namespace planarSLAM { - Graph::Graph(const NonlinearFactorGraph& graph) : - NonlinearFactorGraph(graph) {} + Graph::Graph(const NonlinearFactorGraph& graph) : + NonlinearFactorGraph(graph) {} void Graph::addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model) { diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 5776e989c..15f9ff61f 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -22,7 +22,6 @@ #include #include #include -#include #include #include #include @@ -41,32 +40,21 @@ namespace gtsam { /// Typedef for a PointKey with Point2 data and 'l' symbol typedef TypedSymbol PointKey; - /// Typedef for Values structure with PoseKey type - typedef Values PoseValues; - - /// Typedef for Values structure with PointKey type - typedef Values PointValues; - /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys - struct Values: public TupleValues2 { + struct Values: public DynamicValues { /// Default constructor Values() {} /// Copy constructor - Values(const TupleValues2& values) : - TupleValues2(values) { - } - - /// Copy constructor - Values(const TupleValues2::Base& values) : - TupleValues2(values) { + Values(const DynamicValues& values) : + DynamicValues(values) { } /// From sub-values - Values(const PoseValues& poses, const PointValues& points) : - TupleValues2(poses, points) { - } +// Values(const DynamicValues& poses, const DynamicValues& points) : +// DynamicValues(poses, points) { +// } // Convenience for MATLAB wrapper, which does not allow for identically named methods @@ -88,26 +76,26 @@ namespace gtsam { */ /// A hard constraint for PoseKeys to enforce particular values - typedef NonlinearEquality Constraint; + typedef NonlinearEquality Constraint; /// A prior factor to bias the value of a PoseKey - typedef PriorFactor Prior; + typedef PriorFactor Prior; /// A factor between two PoseKeys set with a Pose2 - typedef BetweenFactor Odometry; + typedef BetweenFactor Odometry; /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) - typedef BearingFactor Bearing; + typedef BearingFactor Bearing; /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) - typedef RangeFactor Range; + typedef RangeFactor Range; /// A factor between a PoseKey and a PointKey to express difference in rotation and location - typedef BearingRangeFactor BearingRange; + typedef BearingRangeFactor BearingRange; /// Creates a NonlinearFactorGraph with the Values type - struct Graph: public NonlinearFactorGraph { + struct Graph: public NonlinearFactorGraph { /// Default constructor for a NonlinearFactorGraph Graph(){} /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph); + Graph(const NonlinearFactorGraph& graph); /// Biases the value of PoseKey key with Pose2 p given a noise model void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel); @@ -132,15 +120,15 @@ namespace gtsam { const Rot2& bearing, double range, const SharedNoiseModel& model); /// Optimize - Values optimize(const Values& initialEstimate) { - typedef NonlinearOptimizer Optimizer; + Values optimize(const DynamicValues& initialEstimate) { + typedef NonlinearOptimizer Optimizer; return *Optimizer::optimizeLM(*this, initialEstimate, NonlinearOptimizationParameters::LAMBDA); } }; /// Optimizer - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; } // planarSLAM diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 714bcce51..88110c40e 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -19,7 +19,6 @@ #include #include -#include #include #include #include diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 102652813..29eb14ba9 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -25,8 +25,8 @@ namespace gtsam { namespace pose3SLAM { /* ************************************************************************* */ - Values circle(size_t n, double radius) { - Values x; + DynamicValues circle(size_t n, double radius) { + DynamicValues x; double theta = 0, dtheta = 2 * M_PI / n; // We use aerospace/navlab convention, X forward, Y right, Z down // First pose will be at (R,0,0) @@ -39,7 +39,7 @@ namespace gtsam { Point3 gti(radius*cos(theta), radius*sin(theta), 0); Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down ! Pose3 gTi(gR0 * _0Ri, gti); - x.insert(i, gTi); + x.insert(Key(i), gTi); } return x; } diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index f4516e647..544dcb5f0 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -18,7 +18,6 @@ #pragma once #include -#include #include #include #include @@ -33,8 +32,6 @@ namespace gtsam { /// Creates a Key with data Pose3 and symbol 'x' typedef TypedSymbol Key; - /// Creates a Values structure with type 'Key' - typedef Values Values; /** * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) @@ -42,17 +39,17 @@ namespace gtsam { * @param R radius of circle * @return circle of n 3D poses */ - Values circle(size_t n, double R); + DynamicValues circle(size_t n, double R); /// A prior factor on Key with Pose3 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 void addPrior(const Key& i, const Pose3& p, @@ -67,14 +64,13 @@ namespace gtsam { }; /// Optimizer - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; } // pose3SLAM /** * Backwards compatibility */ - typedef pose3SLAM::Values Pose3Values; ///< Typedef for Values class for backwards compatibility typedef pose3SLAM::Prior Pose3Prior; ///< Typedef for Prior class for backwards compatibility typedef pose3SLAM::Constraint Pose3Factor; ///< Typedef for Constraint class for backwards compatibility typedef pose3SLAM::Graph Pose3Graph; ///< Typedef for Graph class for backwards compatibility diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index 4cb23f76a..291bde25e 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -19,7 +19,6 @@ #pragma once #include -#include #include #include diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index f44c92118..d54924c12 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -19,7 +19,6 @@ #pragma once #include -#include #include #include @@ -32,25 +31,26 @@ namespace gtsam { // The types that take an oriented pose2 rather than point2 typedef TypedSymbol PointKey; typedef TypedSymbol PoseKey; - typedef Values PoseValues; - typedef Values PointValues; /// Specialized Values structure with syntactic sugar for /// compatibility with matlab - class Values: public TupleValues2 { + class Values: public DynamicValues { + int nrPoses_, nrPoints_; public: - Values() {} + Values() : nrPoses_(0), nrPoints_(0) {} void insertPose(const PoseKey& i, const Pose2& p) { insert(i, p); + nrPoses_++; } void insertPoint(const PointKey& j, const Point2& p) { insert(j, p); + nrPoints_++; } - int nrPoses() const { return this->first_.size(); } - int nrPoints() const { return this->second_.size(); } + int nrPoses() const { return nrPoses_; } + int nrPoints() const { return nrPoints_; } Pose2 pose(const PoseKey& i) const { return (*this)[i]; } Point2 point(const PointKey& j) const { return (*this)[j]; } diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h index e995308f8..4a24bcaa6 100644 --- a/gtsam/slam/simulated3D.h +++ b/gtsam/slam/simulated3D.h @@ -24,7 +24,6 @@ #include #include #include -#include // \namespace diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 16e424344..958337677 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -22,7 +22,6 @@ #include #include #include -#include #include #include #include @@ -39,25 +38,22 @@ namespace gtsam { */ typedef TypedSymbol PoseKey; ///< The key type used for poses typedef TypedSymbol PointKey; ///< The key type used for points - typedef Values PoseValues; ///< Values used for poses - typedef Values PointValues; ///< Values used for points - typedef TupleValues2 Values; ///< Values data structure - typedef boost::shared_ptr shared_values; ///< shared pointer to values data structure + typedef boost::shared_ptr shared_values; ///< shared pointer to values data structure - typedef NonlinearEquality PoseConstraint; ///< put a hard constraint on a pose - typedef NonlinearEquality PointConstraint; ///< put a hard constraint on a point - typedef PriorFactor PosePrior; ///< put a soft prior on a Pose - typedef PriorFactor PointPrior; ///< put a soft prior on a point - typedef RangeFactor RangeFactor; ///< put a factor on the range from a pose to a point + typedef NonlinearEquality PoseConstraint; ///< put a hard constraint on a pose + typedef NonlinearEquality PointConstraint; ///< put a hard constraint on a point + typedef PriorFactor PosePrior; ///< put a soft prior on a Pose + typedef PriorFactor PointPrior; ///< put a soft prior on a point + typedef RangeFactor RangeFactor; ///< put a factor on the range from a pose to a point /// monocular and stereo camera typedefs for general use - typedef GenericProjectionFactor ProjectionFactor; - typedef GenericStereoFactor StereoFactor; + typedef GenericProjectionFactor ProjectionFactor; + typedef GenericStereoFactor StereoFactor; /** * Non-linear factor graph for vanilla visual SLAM */ - class Graph: public NonlinearFactorGraph { + class Graph: public NonlinearFactorGraph { public: /// shared pointer to this type of graph @@ -69,12 +65,12 @@ namespace gtsam { /// print out graph void print(const std::string& s = "") const { - NonlinearFactorGraph::print(s); + NonlinearFactorGraph::print(s); } /// check if two graphs are equal bool equals(const Graph& p, double tol = 1e-9) const { - return NonlinearFactorGraph::equals(p, tol); + return NonlinearFactorGraph::equals(p, tol); } /** @@ -140,6 +136,6 @@ namespace gtsam { }; // Graph /// typedef for Optimizer. The current default will use the multi-frontal solver - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; } } // namespaces diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 095f562a4..0d5402607 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -93,7 +93,7 @@ TEST( NonlinearFactor, NonlinearFactor ) CHECK(assert_equal(0.1*ones(2),actual_e)); // error = 0.5 * [1 1] * [1;1] = 1 - double expected = 1.0; + double expected = 1.0; // calculate the error from the factor "f1" double actual = factor->error(cfg); @@ -179,14 +179,14 @@ TEST( NonlinearFactor, size ) { // create a non linear factor graph Graph fg = createNonlinearFactorGraph(); - + // create a values structure for the non linear factor graph example::Values cfg = createNoisyValues(); - + // get some factors from the graph Graph::sharedFactor factor1 = fg[0], factor2 = fg[1], factor3 = fg[2]; - + CHECK(factor1->size() == 1); CHECK(factor2->size() == 2); CHECK(factor3->size() == 2);