diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index 0a66eb01f..3cac7c7dc 100644 --- a/gtsam/base/DerivedValue.h +++ b/gtsam/base/DerivedValue.h @@ -83,6 +83,16 @@ public: return (static_cast(this))->operator=(derivedRhs); } + /// Conversion to the derived class + operator const DERIVED& () const { + return static_cast(*this); + } + + /// Conversion to the derived class + operator DERIVED& () { + return static_cast(*this); + } + protected: /// Assignment operator, protected because only the Value or DERIVED /// assignment operators should be used. diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index e01623601..a95044dee 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -47,11 +47,11 @@ namespace gtsam { * * \nosubgrouping */ - template - class NonlinearEquality: public NonlinearFactor1 { + template + class NonlinearEquality: public NonlinearFactor1 { public: - typedef typename KEY::Value T; + typedef VALUE T; private: @@ -64,6 +64,12 @@ namespace gtsam { // error gain in allow error case double error_gain_; + // typedef to this class + typedef NonlinearEquality This; + + // typedef to base class + typedef NonlinearFactor1 Base; + public: /** @@ -71,7 +77,6 @@ namespace gtsam { */ bool (*compare_)(const T& a, const T& b); - typedef NonlinearFactor1 Base; /** default constructor - only for serialization */ NonlinearEquality() {} @@ -84,7 +89,7 @@ namespace gtsam { /** * Constructor - forces exact evaluation */ - NonlinearEquality(const KEY& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : + NonlinearEquality(const Symbol& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), allow_error_(false), error_gain_(0.0), compare_(_compare) { @@ -93,7 +98,7 @@ namespace gtsam { /** * Constructor - allows inexact evaluation */ - NonlinearEquality(const KEY& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) : + NonlinearEquality(const Symbol& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) : Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), compare_(_compare) { @@ -103,17 +108,17 @@ namespace gtsam { /// @name Testable /// @{ - void print(const std::string& s = "") const { - std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n"; + virtual void print(const std::string& s = "") const { + std::cout << "Constraint: " << s << " on [" << (std::string)(this->key()) << "]\n"; gtsam::print(feasible_,"Feasible Point"); std::cout << "Variable Dimension: " << feasible_.dim() << std::endl; } /** Check if two factors are equal */ - 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; + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + const This* e = dynamic_cast(&f); + return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) && + fabs(error_gain_ - e->error_gain_) < tol; } /// @} @@ -122,7 +127,7 @@ namespace gtsam { /** actual error function calculation */ virtual double error(const Values& c) const { - const T& xj = c[this->key_]; + const T& xj = c.at(this->key()); Vector e = this->unwhitenedError(c); if (allow_error_ || !compare_(xj, feasible_)) { return error_gain_ * dot(e,e); @@ -132,7 +137,7 @@ namespace gtsam { } /** error function */ - inline Vector evaluateError(const T& xj, boost::optional H = boost::none) const { + Vector evaluateError(const T& xj, boost::optional H = boost::none) const { size_t nj = feasible_.dim(); if (allow_error_) { if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare @@ -142,18 +147,18 @@ namespace gtsam { return zero(nj); // set error to zero if equal } else { if (H) throw std::invalid_argument( - "Linearization point not feasible for " + (std::string)(this->key_) + "!"); + "Linearization point not feasible for " + (std::string)(this->key()) + "!"); return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal } } // Linearize is over-written, because base linearization tries to whiten virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const { - const T& xj = x[this->key_]; + const T& xj = x.at(this->key()); Matrix A; Vector b = evaluateError(xj, A); SharedDiagonal model = noiseModel::Constrained::All(b.size()); - return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model)); + return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model)); } /// @} @@ -177,14 +182,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; + typedef VALUE X; protected: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; /** default constructor to allow for serialization */ NonlinearEquality1() {} @@ -196,10 +201,10 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; ///TODO: comment - NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0) + NonlinearEquality1(const X& value, const Symbol& key1, double mu = 1000.0) : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {} virtual ~NonlinearEquality1() {} @@ -236,13 +241,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; + typedef VALUE X; protected: - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; GTSAM_CONCEPT_MANIFOLD_TYPE(X); @@ -251,10 +256,10 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; ///TODO: comment - NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0) + NonlinearEquality2(const Symbol& key1, const Symbol& key2, double mu = 1000.0) : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} virtual ~NonlinearEquality2() {} diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ba2198b1b..166ad6980 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -95,6 +95,11 @@ public: std::cout << s << ": NonlinearFactor\n"; } + /** Check if two factors are equal */ + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + return Base::equals(f); + } + /// @} /// @name Standard Interface /// @{ @@ -202,8 +207,9 @@ public: } /** Check if two factors are equal */ - virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const { - return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol); + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + const NoiseModelFactor* e = dynamic_cast(&f); + return e && Base::equals(f, tol) && noiseModel_->equals(*e->noiseModel_, tol); } /** get the dimension of the factor (number of rows on linearization) */ diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 76a1ce035..31d6c318a 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -23,7 +23,10 @@ */ #include +#include +#include +#include #include // Only so Eclipse finds class definition namespace gtsam { @@ -37,6 +40,30 @@ namespace gtsam { ValueCloneAllocator() {} }; + /* ************************************************************************* */ + class ValueAutomaticCasting { + const Symbol& key_; + const Value& value_; + public: + ValueAutomaticCasting(const Symbol& key, const Value& value) : key_(key), value_(value) {} + + template + operator const ValueType& () const { + // Check the type and throw exception if incorrect + if(typeid(ValueType) != typeid(value_)) + throw ValuesIncorrectType(key_, typeid(ValueType), typeid(value_)); + + // We have already checked the type, so do a "blind" static_cast, not dynamic_cast + return static_cast(value_); + } + }; + +// /* ************************************************************************* */ +// template +// ValueType& operator=(ValueType& lhs, const ValueAutomaticCasting& rhs) { +// lhs = rhs; +// } + /* ************************************************************************* */ template const ValueType& Values::at(const Symbol& j) const { @@ -55,6 +82,18 @@ namespace gtsam { return static_cast(*item->second); } + /* ************************************************************************* */ + inline ValueAutomaticCasting Values::at(const Symbol& j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); + + // Throw exception if it does not exist + if(item == values_.end()) + throw ValuesKeyDoesNotExist("retrieve", j); + + return ValueAutomaticCasting(item->first, *item->second); + } + /* ************************************************************************* */ template const typename TypedKey::Value& Values::at(const TypedKey& j) const { @@ -65,6 +104,11 @@ namespace gtsam { return at(symbol); } + /* ************************************************************************* */ + inline ValueAutomaticCasting Values::operator[](const Symbol& j) const { + return at(j); + } + /* ************************************************************************* */ template boost::optional Values::exists(const Symbol& j) const { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 0aa67c63d..f226007f1 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -44,6 +44,7 @@ namespace gtsam { // Forward declarations class ValueCloneAllocator; + class ValueAutomaticCasting; /** * A non-templated config holding any types of Manifold-group elements. A @@ -138,6 +139,15 @@ namespace gtsam { template const ValueType& at(const Symbol& j) const; + /** Retrieve a variable by key \c j. This non-templated version returns a + * special ValueAutomaticCasting object that may be assigned to the proper + * value. + * @param j Retrieve the value associated with this key + * @return A ValueAutomaticCasting object that may be assignmed to a Value + * of the proper type. + */ + ValueAutomaticCasting at(const Symbol& j) const; + /** Retrieve a variable using a special key (typically TypedSymbol), which * contains the type of the value associated with the key, and which must * be conversion constructible to a Symbol, e.g. @@ -153,6 +163,9 @@ namespace gtsam { const typename TypedKey::Value& operator[](const TypedKey& j) const { return at(j); } + /** operator[] syntax for at(const Symbol& j) */ + ValueAutomaticCasting operator[](const Symbol& j) const; + /** Check if a value exists with key \c j. See exists<>(const Symbol& j) * and exists(const TypedKey& j) for versions that return the value if it * exists. */ diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 460f32b0c..70d19061e 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -29,9 +29,9 @@ namespace gtsam { class BearingFactor: public NonlinearFactor2 { private: - typedef typename POSE Pose; + typedef POSE Pose; typedef typename Pose::Rotation Rot; - typedef typename POINT Point; + typedef POINT Point; typedef BearingFactor This; typedef NonlinearFactor2 Base; diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index a81896b94..4e442ad86 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -31,9 +31,9 @@ namespace gtsam { class BearingRangeFactor: public NonlinearFactor2 { private: - typedef typename POSE Pose; + typedef POSE Pose; typedef typename POSE::Rotation Rot; - typedef typename POINT Point; + typedef POINT Point; typedef BearingRangeFactor This; typedef NonlinearFactor2 Base; @@ -60,9 +60,9 @@ namespace gtsam { /** Print */ virtual void print(const std::string& s = "") const { std::cout << s << ": BearingRangeFactor(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << ")\n"; - bearing_.print(" measured bearing"); + << (std::string) this->key1() << "," + << (std::string) this->key2() << ")\n"; + measuredBearing_.print(" measured bearing"); std::cout << " measured range: " << measuredRange_ << std::endl; this->noiseModel_->print(" noise model"); } diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 0fd8f6846..313241eee 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -26,12 +26,15 @@ namespace gtsam { /** * A class for a measurement predicted by "between(config[key1],config[key2])" - * KEY1::Value is the Lie Group type - * T is the measurement type, by default the same + * @tparam VALUE the Value type */ template class BetweenFactor: public NonlinearFactor2 { + public: + + typedef VALUE T; + private: typedef BetweenFactor This; @@ -64,8 +67,8 @@ namespace gtsam { /** print */ virtual void print(const std::string& s) const { std::cout << s << "BetweenFactor(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << ")\n"; + << (std::string) this->key1() << "," + << (std::string) this->key2() << ")\n"; measured_.print(" measured"); this->noiseModel_->print(" noise model"); } @@ -79,7 +82,7 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const typename KEY1::Value& p1, const typename KEY1::Value& p2, + Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { T hx = p1.between(p2, H1, H2); // h(x) @@ -121,7 +124,7 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, const Symbol& key1, const Symbol& key2, double mu = 1000.0) : - BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {} + BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {} private: diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index c6d2e2ede..60092c15d 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -37,10 +37,10 @@ namespace gtsam { public: - typedef typename CAMERA Cam; ///< typedef for camera type - typedef GeneralSFMFactor This ; ///< typedef for this object - typedef NonlinearFactor2 Base; ///< typedef for the base class - typedef Point2 Measurement; ///< typedef for the measurement + typedef CAMERA Cam; ///< typedef for camera type + typedef GeneralSFMFactor This; ///< typedef for this object + typedef NonlinearFactor2 Base; ///< typedef for the base class + typedef Point2 Measurement; ///< typedef for the measurement // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -52,7 +52,8 @@ namespace gtsam { * @param i is basically the frame number * @param j is the index of the landmark */ - GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Landmark& landmarkKey) : Base(model, i, j), measured_(measured) {} + GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Symbol& landmarkKey) : + Base(model, cameraKey, cameraKey), measured_(measured) {} GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 @@ -66,14 +67,15 @@ namespace gtsam { */ void print(const std::string& s = "SFMFactor") const { Base::print(s); - z_.print(s + ".z"); + measured_.print(s + ".z"); } /** * equals */ - bool equals(const GeneralSFMFactor &p, double tol = 1e-9) const { - return Base::equals(p, tol) && this->measured_.equals(p.z_, tol) ; + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; } /** h(x)-z */ diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index a5ff16c56..43a33a9fe 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -60,7 +60,7 @@ namespace gtsam { /** print */ virtual void print(const std::string& s) const { - std::cout << s << "PriorFactor(" << (std::string) this->key_ << ")\n"; + std::cout << s << "PriorFactor(" << (std::string) this->key() << ")\n"; prior_.print(" prior"); this->noiseModel_->print(" noise model"); } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 646f2a003..f654e5d33 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -78,8 +78,8 @@ namespace gtsam { /// equals virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { - const This *e = dynamic_cast (&expected); - return e && Base::equals(p, tol) && this->measured_.equals(e->z_, tol) && this->K_->equals(*e->K_, tol); + const This *e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol); } /// Evaluate error h(x)-z and optionally derivatives @@ -92,8 +92,8 @@ namespace gtsam { } catch( CheiralityException& e) { if (H1) *H1 = zeros(2,6); if (H2) *H2 = zeros(2,3); - cout << e.what() << ": Landmark "<< this->key2_.index() << - " moved behind camera " << this->key1_.index() << endl; + cout << e.what() << ": Landmark "<< this->key2().index() << + " moved behind camera " << this->key1().index() << endl; return ones(2) * 2.0 * K_->fx(); } } diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 77c11d00d..43cc3da57 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -34,8 +34,8 @@ namespace gtsam { typedef RangeFactor This; typedef NonlinearFactor2 Base; - typedef typename POSE Pose; - typedef typename POINT Point; + typedef POSE Pose; + typedef POINT Point; // Concept requirements for this factor GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point) @@ -46,7 +46,7 @@ namespace gtsam { RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured, const SharedNoiseModel& model) : - Base(model, poseKey, PointKey), measured_(measured) { + Base(model, poseKey, pointKey), measured_(measured) { } virtual ~RangeFactor() {} @@ -65,7 +65,7 @@ namespace gtsam { /** equals specialized to this factor */ 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->measured_ - e->z_) < tol; + return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol; } /** print contents */ diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 7f1e8077d..76359d8c9 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -22,20 +22,20 @@ namespace gtsam { -template -class GenericStereoFactor: public NonlinearFactor2 { +template +class GenericStereoFactor: public NonlinearFactor2 { private: // Keep a copy of measurement and calibration for I/O - StereoPoint2 z_; ///< the measurement + StereoPoint2 measured_; ///< the measurement boost::shared_ptr K_; ///< shared pointer to calibration 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 + typedef POSE CamPose; ///< typedef for Pose Lie Value type /** * Default constructor @@ -44,18 +44,17 @@ public: /** * Constructor - * @param z is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair + * @param measured is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair * @param model is the noise model in on the measurement - * @param j_pose the pose index - * @param j_landmark the landmark index + * @param poseKey the pose variable key + * @param landmarkKey the landmark variable key * @param K the constant calibration */ - GenericStereoFactor(const StereoPoint2& z, const SharedNoiseModel& model, KEY1 j_pose, - KEY2 j_landmark, const shared_ptrKStereo& K) : - Base(model, j_pose, j_landmark), z_(z), K_(K) { + GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey, const shared_ptrKStereo& K) : + Base(model, poseKey, landmarkKey), measured_(measured), K_(K) { } - ~GenericStereoFactor() {} ///< desctructor + ~GenericStereoFactor() {} ///< destructor /** * print @@ -63,41 +62,27 @@ public: */ void print(const std::string& s) const { Base::print(s); - z_.print(s + ".z"); + measured_.print(s + ".z"); } /** * equals */ - bool equals(const GenericStereoFactor& f, double tol) const { - const GenericStereoFactor* p = dynamic_cast (&f); - if (p == NULL) - goto fail; - //if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail; - if (!z_.equals(p->z_, tol)) - goto fail; - return true; - - fail: print("actual"); - p->print("expected"); - return false; + virtual bool equals(const NonlinearFactor& f, double tol) const { + const GenericStereoFactor* p = dynamic_cast (&f); + return p && Base::equals(f) && measured_.equals(p->measured_, tol); } /** h(x)-z */ Vector evaluateError(const Pose3& pose, const Point3& point, boost::optional H1, boost::optional H2) const { StereoCamera stereoCam(pose, K_); - return (stereoCam.project(point, H1, H2) - z_).vector(); - } - - /// get the measurement z - StereoPoint2 z() { - return z_; + return (stereoCam.project(point, H1, H2) - measured_).vector(); } /** return the measured */ - inline const StereoPoint2 measured() const { - return z_; + const StereoPoint2& measured() const { + return measured_; } private: @@ -105,7 +90,7 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); } }; diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 73fee971e..3f0551863 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -133,9 +133,9 @@ pair load2D(const string& filename, // Insert vertices if pure odometry file if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2()); - if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at(pose2SLAM::PoseKey(id1)) * l1Xl2); + if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at(pose2SLAM::PoseKey(id1)) * l1Xl2); - pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model)); + pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(pose2SLAM::PoseKey(id1), pose2SLAM::PoseKey(id2), l1Xl2, *model)); graph->push_back(factor); } is.ignore(LINESIZE, '\n'); diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 3ff8fa99c..2b9a3db1d 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -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(Key(i), gTi); + x.insert(PoseKey(i), gTi); } return x; } diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index ca78bb334..bb9f143c5 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -57,14 +57,14 @@ namespace simulated2D { } /// Insert a pose - void insertPose(const simulated2D::PoseKey& i, const Point2& p) { - insert(i, p); + void insertPose(Index j, const Point2& p) { + insert(PoseKey(j), p); nrPoses_++; } /// Insert a point - void insertPoint(const simulated2D::PointKey& j, const Point2& p) { - insert(j, p); + void insertPoint(Index j, const Point2& p) { + insert(PointKey(j), p); nrPoints_++; } @@ -79,13 +79,13 @@ namespace simulated2D { } /// Return pose i - Point2 pose(const simulated2D::PoseKey& i) const { - return (*this)[i]; + Point2 pose(Index j) const { + return at(PoseKey(j)); } /// Return point j - Point2 point(const simulated2D::PointKey& j) const { - return (*this)[j]; + Point2 point(Index j) const { + return at(PointKey(j)); } }; @@ -156,7 +156,7 @@ namespace simulated2D { /** * Binary factor simulating "odometry" between two Vectors */ - template + template class GenericOdometry: public NonlinearFactor2 { public: typedef NonlinearFactor2 Base; ///< base class @@ -203,7 +203,7 @@ namespace simulated2D { typedef POSE Pose; ///< shortcut to Pose type typedef LANDMARK Landmark; ///< shortcut to Landmark type - Landmark z_; ///< Measurement + Landmark measured_; ///< Measurement /// Create measurement factor GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey) : diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index d0880b8f3..f38e9c34d 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -40,21 +40,21 @@ namespace simulated2DOriented { public: Values() : nrPoses_(0), nrPoints_(0) {} - void insertPose(const PoseKey& i, const Pose2& p) { - insert(i, p); + void insertPose(Index j, const Pose2& p) { + insert(PoseKey(j), p); nrPoses_++; } - void insertPoint(const PointKey& j, const Point2& p) { - insert(j, p); + void insertPoint(Index j, const Point2& p) { + insert(PointKey(j), p); nrPoints_++; } 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]; } + Pose2 pose(Index j) const { return at(PoseKey(j)); } + Point2 point(Index j) const { return at(PointKey(j)); } }; //TODO:: point prior is not implemented right now @@ -111,7 +111,7 @@ namespace simulated2DOriented { } /// Evaluate error and optionally derivative - Vector evaluateError(const Pose2& x1, const Pose2& x2, + Vector evaluateError(const VALUE& x1, const VALUE& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { return measured_.localCoordinates(odo(x1, x2, H1, H2)); diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h index 4a24bcaa6..40adcdbe2 100644 --- a/gtsam/slam/simulated3D.h +++ b/gtsam/slam/simulated3D.h @@ -36,8 +36,11 @@ namespace simulated3D { * the simulated2D domain. */ -typedef gtsam::TypedSymbol PoseKey; -typedef gtsam::TypedSymbol PointKey; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } + + /// Convenience function for constructing a pose key + inline Symbol PointKey(Index j) { return Symbol('l', j); } /** * Prior on a single pose @@ -61,19 +64,18 @@ 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 + Point3 measured_; ///< The prior pose value for the variable attached to this factor /** * Constructor for a prior factor - * @param z is the measured/prior position for the pose + * @param measured is the measured/prior position for the pose * @param model is the measurement model for the factor (Dimension: 3) - * @param j is the key for the pose + * @param key is the key for the pose */ - PointPrior3D(const Point3& z, - const SharedNoiseModel& model, const PoseKey& j) : - NonlinearFactor1 (model, j), z_(z) { + PointPrior3D(const Point3& measured, const SharedNoiseModel& model, const Symbol& key) : + NonlinearFactor1 (model, key), measured_(measured) { } /** @@ -85,29 +87,27 @@ struct PointPrior3D: public NonlinearFactor1 { */ Vector evaluateError(const Point3& x, boost::optional H = boost::none) { - return (prior(x, H) - z_).vector(); + return (prior(x, H) - measured_).vector(); } }; /** * 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 + Point3 measured_; ///< Linear displacement between a pose and landmark /** * Creates a measurement factor with a given measurement - * @param z is the measurement, a linear displacement between poses and landmarks + * @param measured is the measurement, a linear displacement between poses and landmarks * @param model is a measurement model for the factor (Dimension: 3) - * @param j1 is the pose key of the robot - * @param j2 is the point key for the landmark + * @param poseKey is the pose key of the robot + * @param pointKey is the point key for the landmark */ - Simulated3DMeasurement(const Point3& z, - const SharedNoiseModel& model, PoseKey& j1, PointKey j2) : - NonlinearFactor2 ( - model, j1, j2), z_(z) { - } + Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, + const Symbol& poseKey, const Symbol& pointKey) : + NonlinearFactor2(model, poseKey, pointKey), measured_(measured) {} /** * Error function with optional derivatives @@ -117,9 +117,9 @@ struct Simulated3DMeasurement: public NonlinearFactor2 { * @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3) * @return vector error between measurement and prediction (Dimension: 3) */ - Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional< - Matrix&> H1 = boost::none, boost::optional H2 = boost::none) { - return (mea(x1, x2, H1, H2) - z_).vector(); + Vector evaluateError(const Point3& x1, const Point3& x2, + boost::optional H1 = boost::none, boost::optional H2 = boost::none) { + return (mea(x1, x2, H1, H2) - measured_).vector(); } }; diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 948616ea6..8ee8cb09a 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -39,7 +39,10 @@ using namespace std; namespace gtsam { namespace example { - typedef boost::shared_ptr shared; + using simulated2D::PoseKey; + using simulated2D::PointKey; + + 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); @@ -57,22 +60,22 @@ namespace example { // prior on x1 Point2 mu; - shared f1(new simulated2D::Prior(mu, sigma0_1, 1)); + shared f1(new simulated2D::Prior(mu, sigma0_1, PoseKey(1))); nlfg->push_back(f1); // odometry between x1 and x2 Point2 z2(1.5, 0); - shared f2(new simulated2D::Odometry(z2, sigma0_1, 1, 2)); + shared f2(new simulated2D::Odometry(z2, sigma0_1, PoseKey(1), PoseKey(2))); nlfg->push_back(f2); // measurement between x1 and l1 Point2 z3(0, -1); - shared f3(new simulated2D::Measurement(z3, sigma0_2, 1, 1)); + shared f3(new simulated2D::Measurement(z3, sigma0_2, PoseKey(1), PointKey(1))); nlfg->push_back(f3); // measurement between x2 and l1 Point2 z4(-1.5, -1.); - shared f4(new simulated2D::Measurement(z4, sigma0_2, 2, 1)); + shared f4(new simulated2D::Measurement(z4, sigma0_2, PoseKey(2), PointKey(1))); nlfg->push_back(f4); return nlfg; @@ -86,9 +89,9 @@ namespace example { /* ************************************************************************* */ Values createValues() { Values c; - c.insert(simulated2D::PoseKey(1), Point2(0.0, 0.0)); - c.insert(simulated2D::PoseKey(2), Point2(1.5, 0.0)); - c.insert(simulated2D::PointKey(1), Point2(0.0, -1.0)); + c.insert(PoseKey(1), Point2(0.0, 0.0)); + c.insert(PoseKey(2), Point2(1.5, 0.0)); + c.insert(PointKey(1), Point2(0.0, -1.0)); return c; } @@ -104,9 +107,9 @@ namespace example { /* ************************************************************************* */ boost::shared_ptr sharedNoisyValues() { boost::shared_ptr c(new Values); - c->insert(simulated2D::PoseKey(1), Point2(0.1, 0.1)); - c->insert(simulated2D::PoseKey(2), Point2(1.4, 0.2)); - c->insert(simulated2D::PointKey(1), Point2(0.1, -1.1)); + c->insert(PoseKey(1), Point2(0.1, 0.1)); + c->insert(PoseKey(2), Point2(1.4, 0.2)); + c->insert(PointKey(1), Point2(0.1, -1.1)); return c; } @@ -195,17 +198,15 @@ 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) { + UnaryFactor(const Point2& z, const SharedNoiseModel& model, const Symbol& key) : + gtsam::NonlinearFactor1(model, key), z_(z) { } - Vector evaluateError(const Point2& x, boost::optional A = - boost::none) const { + Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { if (A) *A = H(x); return (h(x) - z_).vector(); } @@ -220,7 +221,7 @@ namespace example { Vector z = Vector_(2, 1.0, 0.0); double sigma = 0.1; boost::shared_ptr factor( - new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), 1)); + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), PoseKey(1))); fg->push_back(factor); return fg; } @@ -238,23 +239,23 @@ namespace example { // prior on x1 Point2 x1(1.0, 0.0); - shared prior(new simulated2D::Prior(x1, sigma1_0, 1)); + shared prior(new simulated2D::Prior(x1, sigma1_0, PoseKey(1))); nlfg.push_back(prior); poses.insert(simulated2D::PoseKey(1), x1); for (int t = 2; t <= T; t++) { // odometry between x_t and x_{t-1} Point2 odo(1.0, 0.0); - shared odometry(new simulated2D::Odometry(odo, sigma1_0, t - 1, t)); + shared odometry(new simulated2D::Odometry(odo, sigma1_0, PoseKey(t - 1), PoseKey(t))); nlfg.push_back(odometry); // measurement on x_t is like perfect GPS Point2 xt(t, 0); - shared measurement(new simulated2D::Prior(xt, sigma1_0, t)); + shared measurement(new simulated2D::Prior(xt, sigma1_0, PoseKey(t))); nlfg.push_back(measurement); // initial estimate - poses.insert(simulated2D::PoseKey(t), xt); + poses.insert(PoseKey(t), xt); } return make_pair(nlfg, poses); @@ -414,8 +415,8 @@ namespace example { /* ************************************************************************* */ // Create key for simulated planar graph - simulated2D::PoseKey key(int x, int y) { - return simulated2D::PoseKey(1000*x+y); + Symbol key(int x, int y) { + return PoseKey(1000*x+y); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 99be5d09a..183b0c600 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -27,6 +27,8 @@ using namespace std; using namespace gtsam; +using pose3SLAM::PoseKey; + /* ************************************************************************* */ TEST( AntiFactor, NegativeHessian) { @@ -40,17 +42,17 @@ TEST( AntiFactor, NegativeHessian) // Create a configuration corresponding to the ground truth boost::shared_ptr values(new Values()); - values->insert(pose3SLAM::Key(1), pose1); - values->insert(pose3SLAM::Key(2), pose2); + values->insert(PoseKey(1), pose1); + values->insert(PoseKey(2), pose2); // Define an elimination ordering Ordering::shared_ptr ordering(new Ordering()); - ordering->insert(pose3SLAM::Key(1), 0); - ordering->insert(pose3SLAM::Key(2), 1); + ordering->insert(PoseKey(1), 0); + ordering->insert(PoseKey(2), 1); // Create a "standard" factor - BetweenFactor::shared_ptr originalFactor(new BetweenFactor(1, 2, z, sigma)); + BetweenFactor::shared_ptr originalFactor(new BetweenFactor(PoseKey(1), PoseKey(2), z, sigma)); // Linearize it into a Jacobian Factor GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); @@ -101,8 +103,8 @@ TEST( AntiFactor, EquivalentBayesNet) // Create a configuration corresponding to the ground truth boost::shared_ptr values(new Values()); - values->insert(pose3SLAM::Key(1), pose1); - values->insert(pose3SLAM::Key(2), pose2); + values->insert(PoseKey(1), pose1); + values->insert(PoseKey(2), pose2); // Define an elimination ordering Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); @@ -115,7 +117,7 @@ TEST( AntiFactor, EquivalentBayesNet) VectorValues expectedDeltas = optimize(*expectedBayesNet); // Add an additional factor between Pose1 and Pose2 - pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(1, 2, z, sigma)); + pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(PoseKey(1), PoseKey(2), z, sigma)); graph->push_back(f1); // Add the corresponding AntiFactor between Pose1 and Pose2 diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index e887cac06..022a76960 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -28,25 +28,23 @@ using namespace std; using namespace gtsam; typedef PinholeCamera GeneralCamera; -typedef TypedSymbol CameraKey; -typedef TypedSymbol PointKey; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared(z, model, i, j)); + void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { + push_back(boost::make_shared(z, model, Symbol('x',i), Symbol('l',j))); } void addCameraConstraint(int j, const GeneralCamera& p) { - boost::shared_ptr factor(new CameraConstraint(j, p)); + boost::shared_ptr factor(new CameraConstraint(Symbol('x',j), p)); push_back(factor); } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(j, p)); + boost::shared_ptr factor(new Point3Constraint(Symbol('x',j), p)); push_back(factor); } @@ -76,7 +74,7 @@ TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); - const int cameraFrameNumber=1, landmarkNumber=1; + const Symbol cameraFrameNumber="x1", landmarkNumber="l1"; const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); @@ -90,17 +88,16 @@ TEST( GeneralSFMFactor, equals ) /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); - const int cameraFrameNumber=1, landmarkNumber=1; const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr - factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + factor(new Projection(z, sigma, "x1", "l1")); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); - values.insert(CameraKey(1), GeneralCamera(x1)); - Point3 l1; values.insert(PointKey(1), l1); + values.insert("x1", GeneralCamera(x1)); + Point3 l1; values.insert("l1", l1); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 5a626d095..7c1d395ba 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -28,26 +28,24 @@ using namespace std; using namespace gtsam; typedef PinholeCamera GeneralCamera; -typedef TypedSymbol CameraKey; -typedef TypedSymbol PointKey; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; /* ************************************************************************* */ class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared(z, model, i, j)); + void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { + push_back(boost::make_shared(z, model, Symbol('x',i), Symbol('l',j))); } void addCameraConstraint(int j, const GeneralCamera& p) { - boost::shared_ptr factor(new CameraConstraint(j, p)); + boost::shared_ptr factor(new CameraConstraint(Symbol('x', j), p)); push_back(factor); } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(j, p)); + boost::shared_ptr factor(new Point3Constraint(Symbol('l', j), p)); push_back(factor); } @@ -77,7 +75,7 @@ TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); - const int cameraFrameNumber=1, landmarkNumber=1; + const Symbol cameraFrameNumber="x1", landmarkNumber="l1"; const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); @@ -91,17 +89,16 @@ TEST( GeneralSFMFactor, equals ) /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); - const int cameraFrameNumber=1, landmarkNumber=1; const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr - factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + factor(new Projection(z, sigma, "x1", "l1")); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); - values.insert(CameraKey(1), GeneralCamera(x1)); - Point3 l1; values.insert(PointKey(1), l1); + values.insert("x1", GeneralCamera(x1)); + Point3 l1; values.insert("l1", l1); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } @@ -171,13 +168,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index 56ac71923..3daf5f4ab 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -37,7 +37,7 @@ SharedNoiseModel /* ************************************************************************* */ TEST( planarSLAM, PriorFactor_equals ) { - planarSLAM::Prior factor1(2, x1, I3), factor2(2, x2, I3); + planarSLAM::Prior factor1(PoseKey(2), x1, I3), factor2(PoseKey(2), x2, I3); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -48,7 +48,7 @@ TEST( planarSLAM, BearingFactor ) { // Create factor Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - planarSLAM::Bearing factor(2, 3, z, sigma); + planarSLAM::Bearing factor(PoseKey(2), PointKey(3), z, sigma); // create config planarSLAM::Values c; @@ -64,8 +64,8 @@ TEST( planarSLAM, BearingFactor ) TEST( planarSLAM, BearingFactor_equals ) { planarSLAM::Bearing - factor1(2, 3, Rot2::fromAngle(0.1), sigma), - factor2(2, 3, Rot2::fromAngle(2.3), sigma); + factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), sigma), + factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(2.3), sigma); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -76,7 +76,7 @@ TEST( planarSLAM, RangeFactor ) { // Create factor double z(sqrt(2) - 0.22); // h(x) - z = 0.22 - planarSLAM::Range factor(2, 3, z, sigma); + planarSLAM::Range factor(PoseKey(2), PointKey(3), z, sigma); // create config planarSLAM::Values c; @@ -91,7 +91,7 @@ TEST( planarSLAM, RangeFactor ) /* ************************************************************************* */ TEST( planarSLAM, RangeFactor_equals ) { - planarSLAM::Range factor1(2, 3, 1.2, sigma), factor2(2, 3, 7.2, sigma); + planarSLAM::Range factor1(PoseKey(2), PointKey(3), 1.2, sigma), factor2(PoseKey(2), PointKey(3), 7.2, sigma); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -103,7 +103,7 @@ TEST( planarSLAM, BearingRangeFactor ) // Create factor Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 double b(sqrt(2) - 0.22); // h(x) - z = 0.22 - planarSLAM::BearingRange factor(2, 3, r, b, sigma2); + planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2); // create config planarSLAM::Values c; @@ -119,8 +119,8 @@ TEST( planarSLAM, BearingRangeFactor ) TEST( planarSLAM, BearingRangeFactor_equals ) { planarSLAM::BearingRange - factor1(2, 3, Rot2::fromAngle(0.1), 7.3, sigma2), - factor2(2, 3, Rot2::fromAngle(3), 2.0, sigma2); + factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), 7.3, sigma2), + factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(3), 2.0, sigma2); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -129,7 +129,7 @@ TEST( planarSLAM, BearingRangeFactor_equals ) /* ************************************************************************* */ TEST( planarSLAM, PoseConstraint_equals ) { - planarSLAM::Constraint factor1(2, x2), factor2(2, x3); + planarSLAM::Constraint factor1(PoseKey(2), x2), factor2(PoseKey(2), x3); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 73fec2a29..29c28dbc1 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -34,6 +34,7 @@ using namespace boost::assign; using namespace std; typedef pose2SLAM::Odometry Pose2Factor; +using pose2SLAM::PoseKey; // common measurement covariance static double sx=0.5, sy=0.5,st=0.1; @@ -49,7 +50,7 @@ static noiseModel::Gaussian::shared_ptr covariance( // Test constraint, small displacement Vector f1(const Pose2& pose1, const Pose2& pose2) { Pose2 z(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, z, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance); return constraint.evaluateError(pose1, pose2); } @@ -57,7 +58,7 @@ TEST( Pose2SLAM, constraint1 ) { // create a factor between unknown poses p1 and p2 Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, pose2, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance); Matrix H1, H2; Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); @@ -72,7 +73,7 @@ TEST( Pose2SLAM, constraint1 ) // Test constraint, large displacement Vector f2(const Pose2& pose1, const Pose2& pose2) { Pose2 z(2,2,M_PI_2); - Pose2Factor constraint(1, 2, z, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance); return constraint.evaluateError(pose1, pose2); } @@ -80,7 +81,7 @@ TEST( Pose2SLAM, constraint2 ) { // create a factor between unknown poses p1 and p2 Pose2 pose1, pose2(2,2,M_PI_2); - Pose2Factor constraint(1, 2, pose2, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance); Matrix H1, H2; Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); @@ -110,7 +111,7 @@ TEST( Pose2SLAM, linearization ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); - Pose2Factor constraint(1,2,measured, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance); pose2SLAM::Graph graph; graph.addOdometry(1,2,measured, covariance); @@ -178,8 +179,8 @@ TEST(Pose2Graph, optimize) { TEST(Pose2Graph, optimizeThreePoses) { // Create a hexagon of poses - Values hexagon = pose2SLAM::circle(3,1.0); - Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)]; + pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0); + Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new pose2SLAM::Graph); @@ -190,10 +191,10 @@ TEST(Pose2Graph, optimizeThreePoses) { fg->addOdometry(2, 0, delta, covariance); // Create initial config - boost::shared_ptr initial(new Values()); - initial->insert(pose2SLAM::PoseKey(0), p0); - initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1))); + boost::shared_ptr initial(new pose2SLAM::Values()); + initial->insertPose(0, p0); + initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); // Choose an ordering shared_ptr ordering(new Ordering); @@ -207,7 +208,7 @@ TEST(Pose2Graph, optimizeThreePoses) { Values actual = *optimizer.values(); // Check with ground truth - CHECK(assert_equal(hexagon, actual)); + CHECK(assert_equal((const Values&)hexagon, actual)); } /* ************************************************************************* */ @@ -215,8 +216,8 @@ TEST(Pose2Graph, optimizeThreePoses) { TEST_UNSAFE(Pose2SLAM, optimizeCircle) { // Create a hexagon of poses - Values hexagon = pose2SLAM::circle(6,1.0); - Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)]; + pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0); + Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new pose2SLAM::Graph); @@ -230,13 +231,13 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { fg->addOdometry(5, 0, delta, covariance); // Create initial config - boost::shared_ptr initial(new Values()); - initial->insert(pose2SLAM::PoseKey(0), p0); - initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(pose2SLAM::PoseKey(3), hexagon[pose2SLAM::PoseKey(3)].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(pose2SLAM::PoseKey(4), hexagon[pose2SLAM::PoseKey(4)].retract(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(pose2SLAM::PoseKey(5), hexagon[pose2SLAM::PoseKey(5)].retract(Vector_(3,-0.1, 0.1,-0.1))); + boost::shared_ptr initial(new pose2SLAM::Values()); + initial->insertPose(0, p0); + initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); + initial->insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1))); + initial->insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1))); // Choose an ordering shared_ptr ordering(new Ordering); @@ -250,10 +251,10 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { Values actual = *optimizer.values(); // Check with ground truth - CHECK(assert_equal(hexagon, actual)); + CHECK(assert_equal((const Values&)hexagon, actual)); // Check loop closure - CHECK(assert_equal(delta,actual[pose2SLAM::PoseKey(5)].between(actual[pose2SLAM::PoseKey(0)]))); + CHECK(assert_equal(delta, actual.at(PoseKey(5)).between(actual.at(PoseKey(0))))); // Pose2SLAMOptimizer myOptimizer("3"); @@ -387,10 +388,10 @@ TEST( Pose2Prior, error ) // Choose a linearization point Pose2 p1(1, 0, 0); // robot at (1,0) pose2SLAM::Values x0; - x0.insert(pose2SLAM::PoseKey(1), p1); + x0.insert(PoseKey(1), p1); // Create factor - pose2SLAM::Prior factor(1, p1, sigmas); + pose2SLAM::Prior factor(PoseKey(1), p1, sigmas); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -417,7 +418,7 @@ TEST( Pose2Prior, error ) /* ************************************************************************* */ // common Pose2Prior for tests below static gtsam::Pose2 priorVal(2,2,M_PI_2); -static pose2SLAM::Prior priorFactor(1,priorVal, sigmas); +static pose2SLAM::Prior priorFactor(PoseKey(1), priorVal, sigmas); /* ************************************************************************* */ // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector @@ -431,7 +432,7 @@ TEST( Pose2Prior, linearize ) { // Choose a linearization point at ground truth pose2SLAM::Values x0; - x0.insert(pose2SLAM::PoseKey(1),priorVal); + x0.insertPose(1, priorVal); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -451,12 +452,12 @@ TEST( Pose2Factor, error ) Pose2 p1; // robot at origin Pose2 p2(1, 0, 0); // robot at (1,0) pose2SLAM::Values x0; - x0.insert(pose2SLAM::PoseKey(1), p1); - x0.insert(pose2SLAM::PoseKey(2), p2); + x0.insertPose(1, p1); + x0.insertPose(2, p2); // Create factor Pose2 z = p1.between(p2); - Pose2Factor factor(1, 2, z, covariance); + Pose2Factor factor(PoseKey(1), PoseKey(2), z, covariance); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -483,7 +484,7 @@ TEST( Pose2Factor, error ) /* ************************************************************************* */ // common Pose2Factor for tests below static Pose2 measured(2,2,M_PI_2); -static Pose2Factor factor(1,2,measured, covariance); +static Pose2Factor factor(PoseKey(1),PoseKey(2),measured, covariance); /* ************************************************************************* */ TEST( Pose2Factor, rhs ) diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 79cc9fe1e..a9b743145 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -50,7 +50,7 @@ TEST(Pose3Graph, optimizeCircle) { // Create a hexagon of poses double radius = 10; Values hexagon = pose3SLAM::circle(6,radius); - Pose3 gT0 = hexagon[Key(0)], gT1 = hexagon[Key(1)]; + Pose3 gT0 = hexagon[PoseKey(0)], gT1 = hexagon[PoseKey(1)]; // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose3Graph); @@ -67,12 +67,12 @@ TEST(Pose3Graph, optimizeCircle) { // Create initial config boost::shared_ptr initial(new Values()); - initial->insert(Key(0), gT0); - initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert(Key(3), hexagon[Key(3)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert(Key(4), hexagon[Key(4)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert(Key(5), hexagon[Key(5)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(PoseKey(0), gT0); + initial->insert(PoseKey(1), hexagon.at(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(PoseKey(2), hexagon.at(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(PoseKey(3), hexagon.at(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(PoseKey(4), hexagon.at(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(PoseKey(5), hexagon.at(PoseKey(5)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); // Choose an ordering and optimize shared_ptr ordering(new Ordering); @@ -87,17 +87,17 @@ TEST(Pose3Graph, optimizeCircle) { CHECK(assert_equal(hexagon, actual,1e-4)); // Check loop closure - CHECK(assert_equal(_0T1,actual[Key(5)].between(actual[Key(0)]),1e-5)); + CHECK(assert_equal(_0T1, actual.at(PoseKey(5)).between(actual.at(PoseKey(0))),1e-5)); } /* ************************************************************************* */ TEST(Pose3Graph, partial_prior_height) { - typedef PartialPriorFactor Partial; + typedef PartialPriorFactor Partial; // reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3) // height prior - single element interface - pose3SLAM::Key key(1); + Symbol key = PoseKey(1); double exp_height = 5.0; SharedDiagonal model = noiseModel::Unit::Create(1); Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height)); @@ -117,7 +117,7 @@ TEST(Pose3Graph, partial_prior_height) { EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); - EXPECT(assert_equal(expected, actual[key], tol)); + EXPECT(assert_equal(expected, actual.at(key), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 1d8616c30..f7f804fb9 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -49,7 +49,7 @@ TEST( ProjectionFactor, error ) Point2 z(323.,240.); int cameraFrameNumber=1, landmarkNumber=1; boost::shared_ptr - factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + factor(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); // For the following values structure, the factor predicts 320,240 Values config; @@ -98,10 +98,10 @@ TEST( ProjectionFactor, equals ) Vector z = Vector_(2,323.,240.); int cameraFrameNumber=1, landmarkNumber=1; boost::shared_ptr - factor1(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + factor1(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); boost::shared_ptr - factor2(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + factor2(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); CHECK(assert_equal(*factor1, *factor2)); } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 94a4d4060..fc026fb35 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -52,11 +52,11 @@ TEST( StereoFactor, singlePoint) boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); boost::shared_ptr graph(new visualSLAM::Graph()); - graph->add(visualSLAM::PoseConstraint(1,camera1)); + graph->add(visualSLAM::PoseConstraint(PoseKey(1),camera1)); StereoPoint2 z14(320,320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K)); + graph->add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K)); // Create a configuration corresponding to the ground truth boost::shared_ptr values(new Values()); diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index c1a9123d9..4ecbf2e43 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -34,22 +34,24 @@ namespace visualSLAM { using namespace gtsam; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } + + /// Convenience function for constructing a pose key + inline Symbol PointKey(Index j) { return Symbol('l', j); } + /** * Typedefs that make up the visualSLAM namespace. */ - typedef TypedSymbol PoseKey; ///< The key type used for poses - typedef TypedSymbol PointKey; ///< The key type used for points - 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 @@ -76,69 +78,69 @@ namespace visualSLAM { /** * Add a projection factor measurement (monocular) - * @param z the measurement + * @param measured the measurement * @param model the noise model for the measurement - * @param i index of camera - * @param j index of point + * @param poseKey variable key for the camera pose + * @param pointKey variable key for the landmark * @param K shared pointer to calibration object */ - void addMeasurement(const Point2& z, const SharedNoiseModel& model, - PoseKey i, PointKey j, const shared_ptrK& K) { - boost::shared_ptr factor(new ProjectionFactor(z, model, i, j, K)); + void addMeasurement(const Point2& measured, const SharedNoiseModel& model, + const Symbol& poseKey, const Symbol& pointKey, const shared_ptrK& K) { + boost::shared_ptr factor(new ProjectionFactor(measured, model, poseKey, pointKey, K)); push_back(factor); } /** * Add a constraint on a pose (for now, *must* be satisfied in any Values) - * @param j index of camera + * @param key variable key of the camera pose * @param p to which pose to constrain it to */ - void addPoseConstraint(int j, const Pose3& p = Pose3()) { - boost::shared_ptr factor(new PoseConstraint(j, p)); + void addPoseConstraint(const Symbol& key, const Pose3& p = Pose3()) { + boost::shared_ptr factor(new PoseConstraint(key, p)); push_back(factor); } /** * Add a constraint on a point (for now, *must* be satisfied in any Values) - * @param j index of landmark + * @param key variable key of the landmark * @param p point around which soft prior is defined */ - void addPointConstraint(int j, const Point3& p = Point3()) { - boost::shared_ptr factor(new PointConstraint(j, p)); + void addPointConstraint(const Symbol& key, const Point3& p = Point3()) { + boost::shared_ptr factor(new PointConstraint(key, p)); push_back(factor); } /** * Add a prior on a pose - * @param j index of camera + * @param key variable key of the camera pose * @param p around which soft prior is defined * @param model uncertainty model of this prior */ - void addPosePrior(int j, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { - boost::shared_ptr factor(new PosePrior(j, p, model)); + void addPosePrior(const Symbol& key, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { + boost::shared_ptr factor(new PosePrior(key, p, model)); push_back(factor); } /** * Add a prior on a landmark - * @param j index of landmark + * @param key variable key of the landmark * @param p to which point to constrain it to * @param model uncertainty model of this prior */ - void addPointPrior(int j, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { - boost::shared_ptr factor(new PointPrior(j, p, model)); + void addPointPrior(const Symbol& key, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { + boost::shared_ptr factor(new PointPrior(key, p, model)); push_back(factor); } /** * Add a range prior to a landmark - * @param i index of pose - * @param j index of landmark + * @param poseKey variable key of the camera pose + * @param pointKey variable key of the landmark * @param range approximate range to landmark * @param model uncertainty model of this prior */ - void addRangeFactor(PoseKey i, PointKey j, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { - push_back(boost::shared_ptr(new RangeFactor(i, j, range, model))); + void addRangeFactor(const Symbol& poseKey, const Symbol& pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { + push_back(boost::shared_ptr(new RangeFactor(poseKey, pointKey, range, model))); } }; // Graph