25#define BETWEENFACTOR_VISIBILITY
29#define BETWEENFACTOR_VISIBILITY GTSAM_EXPORT
60 typedef typename boost::shared_ptr<BetweenFactor>
shared_ptr;
79 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
80 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
81 gtsam::NonlinearFactor::shared_ptr(
new This(*
this))); }
88 const std::string& s =
"",
89 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
90 std::cout << s <<
"BetweenFactor("
91 << keyFormatter(this->key1()) <<
","
92 << keyFormatter(this->key2()) <<
")\n";
94 this->noiseModel_->print(
" noise model: ");
99 const This *e =
dynamic_cast<const This*
> (&expected);
108 Vector
evaluateError(
const T& p1,
const T& p2, boost::optional<Matrix&> H1 =
109 boost::none, boost::optional<Matrix&> H2 = boost::none)
const override {
112#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
114 Vector rval =
traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0);
115 if (H1) *H1 = Hlocal * (*H1);
116 if (H2) *H2 = Hlocal * (*H2);
137 template<
class ARCHIVE>
138 void serialize(ARCHIVE & ar,
const unsigned int ) {
140 ar & boost::serialization::make_nvp(
"NoiseModelFactor2",
141 boost::serialization::base_object<Base>(*
this));
142 ar & BOOST_SERIALIZATION_NVP(measured_);
146 enum { NeedsToAlign = (
sizeof(VALUE) % 16) == 0 };
152 template<
class VALUE>
160 template<
class VALUE>
163 typedef boost::shared_ptr<BetweenConstraint<VALUE> > shared_ptr;
175 template<
class ARCHIVE>
176 void serialize(ARCHIVE & ar,
const unsigned int ) {
177 ar & boost::serialization::make_nvp(
"BetweenFactor",
183 template<
class VALUE>
Concept check for values that can be used in unit tests.
Base class and basic functions for Lie types.
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
Lie Group Concept.
Definition Lie.h:260
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
Nonlinear factor base class.
Definition NonlinearFactor.h:42
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
A class for a measurement predicted by "between(config[key1],config[key2])".
Definition BetweenFactor.h:40
BetweenFactor()
default constructor - only use for serialization
Definition BetweenFactor.h:66
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition BetweenFactor.h:98
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition BetweenFactor.h:79
boost::shared_ptr< BetweenFactor > shared_ptr
The measurement.
Definition BetweenFactor.h:60
const VALUE & measured() const
return the measurement
Definition BetweenFactor.h:128
BetweenFactor(Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model=nullptr)
Constructor.
Definition BetweenFactor.h:69
friend class boost::serialization::access
Serialization function.
Definition BetweenFactor.h:136
Vector evaluateError(const T &p1, const T &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
evaluate error, returns vector of errors size of tangent space
Definition BetweenFactor.h:108
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition BetweenFactor.h:87
Binary between constraint - forces between to a given value This constraint requires the underlying t...
Definition BetweenFactor.h:161
BetweenConstraint(const VALUE &measured, Key key1, Key key2, double mu=1000.0)
Syntactic sugar for constrained version.
Definition BetweenFactor.h:166
friend class boost::serialization::access
Serialization function.
Definition BetweenFactor.h:174