diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index c78b5a3bf..fa7559972 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -16,87 +16,43 @@ #pragma once -#include +#include #include #include namespace gtsam { - /** - * Binary factor for a bearing measurement - * @addtogroup SLAM - */ - template - class BearingFactor: public NoiseModelFactor2 { - private: +/** + * Binary factor for a bearing measurement + * @addtogroup SAM + */ +template +class BearingFactor : public ExpressionFactor { + private: + typedef BearingFactor This; + typedef ExpressionFactor Base; - typedef POSE Pose; - typedef ROTATION Rot; - typedef POINT Point; + /** concept check by type */ + GTSAM_CONCEPT_TESTABLE_TYPE(Measured) + GTSAM_CONCEPT_POSE_TYPE(Pose) - typedef BearingFactor This; - typedef NoiseModelFactor2 Base; + public: + /** primary constructor */ + BearingFactor(Key poseKey, Key pointKey, const Measured& measured, + const SharedNoiseModel& model) + : Base(model, measured, + Expression(Expression(poseKey), &Pose::bearing, + Expression(pointKey))) {} - Rot measured_; /** measurement */ + virtual ~BearingFactor() {} - /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(Rot) - GTSAM_CONCEPT_POSE_TYPE(Pose) + /** print contents */ + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "BearingFactor, bearing = "; + this->measurement_.print(); + Base::print("", keyFormatter); + } +}; // BearingFactor - public: - - /** default constructor for serialization/testing only */ - BearingFactor() {} - - /** primary constructor */ - BearingFactor(Key poseKey, Key pointKey, const Rot& measured, - const SharedNoiseModel& model) : - Base(model, poseKey, pointKey), measured_(measured) { - } - - virtual ~BearingFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** h(x)-z -> between(z,h(x)) for Rot2 manifold */ - Vector evaluateError(const Pose& pose, const Point& point, - boost::optional H1, boost::optional H2) const { - Rot hx = pose.bearing(point, H1, H2); - return Rot::Logmap(measured_.between(hx)); - } - - /** return the measured */ - const Rot& measured() const { - return measured_; - } - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); - } - - /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "BearingFactor, bearing = "; - measured_.print(); - Base::print("", keyFormatter); - } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - } - - }; // BearingFactor - -} // namespace gtsam +} // namespace gtsam