24#include <boost/bind/bind.hpp>
88 std::placeholders::_1, std::placeholders::_2, 1e-9)) :
90 j), feasible_(feasible), allow_error_(false), error_gain_(0.0),
99 std::placeholders::_1, std::placeholders::_2, 1e-9)) :
101 j), feasible_(feasible), allow_error_(true), error_gain_(error_gain),
109 void print(
const std::string& s =
"",
110 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
111 std::cout << (s.empty() ? s : s +
" ") <<
"Constraint: on ["
112 << keyFormatter(this->
key()) <<
"]\n";
120 const This* e =
dynamic_cast<const This*
>(&f);
122 && std::abs(error_gain_ - e->error_gain_) < tol;
131 const T& xj = c.
at<T>(this->
key());
133 if (allow_error_ || !compare_(xj, feasible_)) {
134 return error_gain_ *
dot(e, e);
142 boost::optional<Matrix&> H = boost::none)
const override {
146 *H = Matrix::Identity(nj,nj);
148 }
else if (compare_(feasible_, xj)) {
150 *H = Matrix::Identity(nj,nj);
151 return Vector::Zero(nj);
154 throw std::invalid_argument(
155 "Linearization point not feasible for "
156 + DefaultKeyFormatter(this->
key()) +
"!");
157 return Vector::Constant(nj, std::numeric_limits<double>::infinity());
163 const T& xj = x.
at<T>(this->
key());
172 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
173 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
174 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));
185 template<
class ARCHIVE>
186 void serialize(ARCHIVE & ar,
const unsigned int ) {
189 & boost::serialization::make_nvp(
"NoiseModelFactor1",
190 boost::serialization::base_object<Base>(*
this));
191 ar & BOOST_SERIALIZATION_NVP(feasible_);
192 ar & BOOST_SERIALIZATION_NVP(allow_error_);
193 ar & BOOST_SERIALIZATION_NVP(error_gain_);
199template <
typename VALUE>
222 GTSAM_CONCEPT_MANIFOLD_TYPE(X)
223 GTSAM_CONCEPT_TESTABLE_TYPE(X)
227 typedef boost::shared_ptr<NonlinearEquality1<VALUE> >
shared_ptr;
245 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
246 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
247 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));
252 boost::optional<Matrix&> H = boost::none)
const override {
260 void print(
const std::string& s =
"",
261 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
262 std::cout << s <<
": NonlinearEquality1(" << keyFormatter(this->
key())
264 this->noiseModel_->print();
274 template<
class ARCHIVE>
275 void serialize(ARCHIVE & ar,
const unsigned int ) {
278 & boost::serialization::make_nvp(
"NoiseModelFactor1",
279 boost::serialization::base_object<Base>(*
this));
280 ar & BOOST_SERIALIZATION_NVP(value_);
285template <
typename VALUE>
287 :
Testable<NonlinearEquality1<VALUE> > {};
300 GTSAM_CONCEPT_MANIFOLD_TYPE(T)
306 typedef boost::shared_ptr<NonlinearEquality2<T>> shared_ptr;
320 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
321 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
322 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));
327 const T& x1,
const T& x2, boost::optional<Matrix&> H1 = boost::none,
328 boost::optional<Matrix&> H2 = boost::none)
const override {
330 if (H1) *H1 = -Matrix::Identity(p, p);
331 if (H2) *H2 = Matrix::Identity(p, p);
340 template <
class ARCHIVE>
341 void serialize(ARCHIVE& ar,
const unsigned int ) {
343 ar& boost::serialization::make_nvp(
344 "NoiseModelFactor2", boost::serialization::base_object<Base>(*
this));
349template <
typename VALUE>
Concept check for values that can be used in unit tests.
Base class and basic functions for Manifold types.
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
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
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
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
static shared_ptr All(size_t dim)
Fully constrained variations.
Definition NoiseModel.h:457
An equality factor that forces either one variable to a constant, or a set of variables to be equal t...
Definition NonlinearEquality.h:45
double error(const Values &c) const override
Actual error function calculation.
Definition NonlinearEquality.h:130
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition NonlinearEquality.h:109
std::function< bool(const T &, const T &)> CompareFunction
Function that compares two values.
Definition NonlinearEquality.h:70
NonlinearEquality()
Default constructor - only for serialization.
Definition NonlinearEquality.h:74
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition NonlinearEquality.h:119
NonlinearEquality(Key j, const T &feasible, double error_gain, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
Constructor - allows inexact evaluation.
Definition NonlinearEquality.h:97
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:172
Vector evaluateError(const T &xj, boost::optional< Matrix & > H=boost::none) const override
Error function.
Definition NonlinearEquality.h:141
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:184
NonlinearEquality(Key j, const T &feasible, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
Constructor - forces exact evaluation.
Definition NonlinearEquality.h:86
GaussianFactor::shared_ptr linearize(const Values &x) const override
Linearize is over-written, because base linearization tries to whiten.
Definition NonlinearEquality.h:162
Simple unary equality constraint - fixes a value for a variable.
Definition NonlinearEquality.h:207
Vector evaluateError(const X &x1, boost::optional< Matrix & > H=boost::none) const override
g(x) with optional derivative
Definition NonlinearEquality.h:251
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:245
boost::shared_ptr< NonlinearEquality1< VALUE > > shared_ptr
fixed value for variable
Definition NonlinearEquality.h:227
NonlinearEquality1(const X &value, Key key, double mu=1000.0)
Constructor.
Definition NonlinearEquality.h:235
NonlinearEquality1()
Default constructor to allow for serialization.
Definition NonlinearEquality.h:217
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition NonlinearEquality.h:260
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:273
Simple binary equality constraint - this constraint forces two variables to be the same.
Definition NonlinearEquality.h:295
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:320
Vector evaluateError(const T &x1, const T &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
g(x) with optional derivative2
Definition NonlinearEquality.h:326
NonlinearEquality2(Key key1, Key key2, double mu=1e4)
Constructor.
Definition NonlinearEquality.h:314
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:339
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
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
This implements the unwhitenedError virtual function by calling the n-key specific version of evaluat...
Definition NonlinearFactor.h:542
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361