Loading...
Searching...
No Matches
PriorFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <string>
namespace gtsam {
template<class VALUE>
class PriorFactor: public NoiseModelFactorN<VALUE> {
public:
typedef VALUE T;
private:
typedef NoiseModelFactorN<VALUE> Base;
VALUE prior_;
GTSAM_CONCEPT_TESTABLE_TYPE(T)
public:
typedef typename boost::shared_ptr<PriorFactor<VALUE> > shared_ptr;
typedef PriorFactor<VALUE> This;
PriorFactor() {}
~PriorFactor() override {}
PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) :
Base(model, key), prior_(prior) {
}
PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) :
Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) {
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
void print(const std::string& s,
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
traits<T>::Print(prior_, " prior mean: ");
if (this->noiseModel_)
this->noiseModel_->print(" noise model: ");
else
std::cout << "no noise model" << std::endl;
}
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
const This* e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
}
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z)
return -traits<T>::Local(x, prior_);
}
const VALUE & prior() const { return prior_; }
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_);
}
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
template<class VALUE>
struct traits<PriorFactor<VALUE> > : public Testable<PriorFactor<VALUE> > {};
}
Testable.h
Concept check for values that can be used in unit tests.
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition LossFunctions.cpp:27
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactorN< VALUE >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN< VALUE >::key
Key key() const
Definition NonlinearFactor.h:518
gtsam::PriorFactor
A class for a soft prior on any Value type.
Definition PriorFactor.h:30
gtsam::PriorFactor::This
PriorFactor< VALUE > This
Typedef to this class.
Definition PriorFactor.h:50
gtsam::PriorFactor::PriorFactor
PriorFactor()
default constructor - only use for serialization
Definition PriorFactor.h:53
gtsam::PriorFactor::PriorFactor
PriorFactor(Key key, const VALUE &prior, const Matrix &covariance)
Convenience constructor that takes a full covariance argument.
Definition PriorFactor.h:63
gtsam::PriorFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition PriorFactor.h:75
gtsam::PriorFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Definition PriorFactor.h:86
gtsam::PriorFactor::shared_ptr
boost::shared_ptr< PriorFactor< VALUE > > shared_ptr
The measurement.
Definition PriorFactor.h:47
gtsam::PriorFactor::evaluateError
Vector evaluateError(const T &x, boost::optional< Matrix & > H=boost::none) const override
implement functions needed to derive from Factor
Definition PriorFactor.h:94
gtsam::PriorFactor::PriorFactor
PriorFactor(Key key, const VALUE &prior, const SharedNoiseModel &model=nullptr)
Constructor.
Definition PriorFactor.h:58
gtsam::PriorFactor::access
friend class boost::serialization::access
Serialization function.
Definition PriorFactor.h:105
gtsam::PriorFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition PriorFactor.h:68