diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h new file mode 100644 index 000000000..8d8c67d5c --- /dev/null +++ b/gtsam/nonlinear/PriorFactor.h @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PriorFactor.h + * @author Frank Dellaert + **/ +#pragma once + +#include +#include + +#include + +namespace gtsam { + + /** + * A class for a soft prior on any Value type + * @addtogroup SLAM + */ + template + class PriorFactor: public NoiseModelFactor1 { + + public: + typedef VALUE T; + + private: + + typedef NoiseModelFactor1 Base; + + VALUE prior_; /** The measurement */ + + /** concept check by type */ + GTSAM_CONCEPT_TESTABLE_TYPE(T) + + public: + + /// shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr > shared_ptr; + + /// Typedef to this class + typedef PriorFactor This; + + /** default constructor - only use for serialization */ + PriorFactor() {} + + virtual ~PriorFactor() {} + + /** Constructor */ + PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) : + Base(model, key), prior_(prior) { + } + + /** Convenience constructor that takes a full covariance argument */ + PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) : + Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) { + } + + /// @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))); } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; + traits::Print(prior_, " prior mean: "); + if (this->noiseModel_) + this->noiseModel_->print(" noise model: "); + else + std::cout << "no noise model" << std::endl; + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This* e = dynamic_cast (&expected); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const T& x, boost::optional H = boost::none) const { + if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); + // manifold equivalent of z-x -> Local(x,z) + // TODO(ASL) Add Jacobians. + return -traits::Local(x, prior_); + } + + const VALUE & prior() const { return prior_; } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(prior_); + } + + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + }; + +} /// namespace gtsam diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 8d8c67d5c..45bf2e4ed 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -15,106 +15,6 @@ **/ #pragma once -#include -#include - -#include - -namespace gtsam { - - /** - * A class for a soft prior on any Value type - * @addtogroup SLAM - */ - template - class PriorFactor: public NoiseModelFactor1 { - - public: - typedef VALUE T; - - private: - - typedef NoiseModelFactor1 Base; - - VALUE prior_; /** The measurement */ - - /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(T) - - public: - - /// shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr > shared_ptr; - - /// Typedef to this class - typedef PriorFactor This; - - /** default constructor - only use for serialization */ - PriorFactor() {} - - virtual ~PriorFactor() {} - - /** Constructor */ - PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) : - Base(model, key), prior_(prior) { - } - - /** Convenience constructor that takes a full covariance argument */ - PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) : - Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) { - } - - /// @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))); } - - /** implement functions needed for Testable */ - - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { - std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - traits::Print(prior_, " prior mean: "); - if (this->noiseModel_) - this->noiseModel_->print(" noise model: "); - else - std::cout << "no noise model" << std::endl; - } - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This* e = dynamic_cast (&expected); - return e != nullptr && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); - } - - /** implement functions needed to derive from Factor */ - - /** vector of errors */ - Vector evaluateError(const T& x, boost::optional H = boost::none) const { - if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); - // manifold equivalent of z-x -> Local(x,z) - // TODO(ASL) Add Jacobians. - return -traits::Local(x, prior_); - } - - const VALUE & prior() const { return prior_; } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(prior_); - } - - // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) - }; - -} /// namespace gtsam +// Note: PriorFactor has been moved to gtsam/nonlinear. This file has been +// left here for backwards compatibility. +#include \ No newline at end of file