Loading...
Searching...
No Matches
BetweenFactor.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 <ostream>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#ifdef _WIN32
#define BETWEENFACTOR_VISIBILITY
#else
// This will trigger a LNKxxxx on MSVC, so disable for MSVC build
// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM-EXPORT.md
#define BETWEENFACTOR_VISIBILITY GTSAM_EXPORT
#endif
namespace gtsam {
template<class VALUE>
class BetweenFactor: public NoiseModelFactorN<VALUE, VALUE> {
// Check that VALUE type is a testable Lie group
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
BOOST_CONCEPT_ASSERT((IsLieGroup<VALUE>));
public:
typedef VALUE T;
private:
typedef BetweenFactor<VALUE> This;
typedef NoiseModelFactorN<VALUE, VALUE> Base;
VALUE measured_;
public:
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
BetweenFactor() {}
BetweenFactor(Key key1, Key key2, const VALUE& measured,
const SharedNoiseModel& model = nullptr) :
Base(model, key1, key2), measured_(measured) {
}
~BetweenFactor() override {}
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 << "BetweenFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
traits<T>::Print(measured_, " measured: ");
this->noiseModel_->print(" noise model: ");
}
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(this->measured_, e->measured_, tol);
}
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
typename traits<T>::ChartJacobian::Jacobian Hlocal;
Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0);
if (H1) *H1 = Hlocal * (*H1);
if (H2) *H2 = Hlocal * (*H2);
return rval;
#else
return traits<T>::Local(measured_, hx);
#endif
}
const VALUE& measured() const {
return measured_;
}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
}; // \class BetweenFactor
template<class VALUE>
struct traits<BetweenFactor<VALUE> > : public Testable<BetweenFactor<VALUE> > {};
template<class VALUE>
class BetweenConstraint : public BetweenFactor<VALUE> {
public:
typedef boost::shared_ptr<BetweenConstraint<VALUE> > shared_ptr;
BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
BetweenFactor<VALUE>(key1, key2, measured,
noiseModel::Constrained::All(traits<VALUE>::GetDimension(measured), std::abs(mu)))
{}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("BetweenFactor",
boost::serialization::base_object<BetweenFactor<VALUE> >(*this));
}
}; // \class BetweenConstraint
template<class VALUE>
struct traits<BetweenConstraint<VALUE> > : public Testable<BetweenConstraint<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
Lie.h
Base class and basic functions for Lie types.
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::IsLieGroup
Lie Group Concept.
Definition Lie.h:260
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
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, VALUE >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::BetweenFactor
A class for a measurement predicted by "between(config[key1],config[key2])".
Definition BetweenFactor.h:40
gtsam::BetweenFactor::BetweenFactor
BetweenFactor()
default constructor - only use for serialization
Definition BetweenFactor.h:66
gtsam::BetweenFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition BetweenFactor.h:98
gtsam::BetweenFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition BetweenFactor.h:79
gtsam::BetweenFactor::shared_ptr
boost::shared_ptr< BetweenFactor > shared_ptr
The measurement.
Definition BetweenFactor.h:60
gtsam::BetweenFactor::measured
const VALUE & measured() const
return the measurement
Definition BetweenFactor.h:128
gtsam::BetweenFactor::BetweenFactor
BetweenFactor(Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model=nullptr)
Constructor.
Definition BetweenFactor.h:69
gtsam::BetweenFactor::access
friend class boost::serialization::access
Serialization function.
Definition BetweenFactor.h:136
gtsam::BetweenFactor::evaluateError
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
gtsam::BetweenFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition BetweenFactor.h:87
gtsam::BetweenConstraint
Binary between constraint - forces between to a given value This constraint requires the underlying t...
Definition BetweenFactor.h:161
gtsam::BetweenConstraint::BetweenConstraint
BetweenConstraint(const VALUE &measured, Key key1, Key key2, double mu=1000.0)
Syntactic sugar for constrained version.
Definition BetweenFactor.h:166
gtsam::BetweenConstraint::access
friend class boost::serialization::access
Serialization function.
Definition BetweenFactor.h:174