gtsam  4.1.0
gtsam
NonlinearEquality.h
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
12 /*
13  * @file NonlinearEquality.h
14  * @brief Factor to handle enforced equality between factors
15  * @author Alex Cunningham
16  */
17 
18 #pragma once
19 
21 #include <gtsam/base/Testable.h>
22 #include <gtsam/base/Manifold.h>
23 
24 #include <boost/bind.hpp>
25 
26 #include <limits>
27 #include <iostream>
28 #include <cmath>
29 
30 namespace gtsam {
31 
44 template<class VALUE>
45 class NonlinearEquality: public NoiseModelFactor1<VALUE> {
46 
47 public:
48  typedef VALUE T;
49 
50 private:
51 
52  // feasible value
53  T feasible_;
54 
55  // error handling flag
56  bool allow_error_;
57 
58  // error gain in allow error case
59  double error_gain_;
60 
61  // typedef to this class
63 
64  // typedef to base class
66 
67 public:
68 
72  typedef boost::function<bool(const T&, const T&)> CompareFunction;
73  CompareFunction compare_;
74 // bool (*compare_)(const T& a, const T& b);
75 
78  }
79 
80  virtual ~NonlinearEquality() {
81  }
82 
85 
89  NonlinearEquality(Key j, const T& feasible,
90  const CompareFunction &_compare = boost::bind(traits<T>::Equals,_1,_2,1e-9)) :
91  Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
92  j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
93  compare_(_compare) {
94  }
95 
99  NonlinearEquality(Key j, const T& feasible, double error_gain,
100  const CompareFunction &_compare = boost::bind(traits<T>::Equals,_1,_2,1e-9)) :
101  Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
102  j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
103  compare_(_compare) {
104  }
105 
109 
110  void print(const std::string& s = "",
111  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
112  std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
113  traits<VALUE>::Print(feasible_, "Feasible Point:\n");
114  std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_) << std::endl;
115  }
116 
118  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
119  const This* e = dynamic_cast<const This*>(&f);
120  return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol)
121  && std::abs(error_gain_ - e->error_gain_) < tol;
122  }
123 
127 
129  double error(const Values& c) const override {
130  const T& xj = c.at<T>(this->key());
131  Vector e = this->unwhitenedError(c);
132  if (allow_error_ || !compare_(xj, feasible_)) {
133  return error_gain_ * dot(e, e);
134  } else {
135  return 0.0;
136  }
137  }
138 
140  Vector evaluateError(const T& xj,
141  boost::optional<Matrix&> H = boost::none) const override {
142  const size_t nj = traits<T>::GetDimension(feasible_);
143  if (allow_error_) {
144  if (H)
145  *H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare
146  return traits<T>::Local(xj,feasible_);
147  } else if (compare_(feasible_, xj)) {
148  if (H)
149  *H = Matrix::Identity(nj,nj);
150  return Vector::Zero(nj); // set error to zero if equal
151  } else {
152  if (H)
153  throw std::invalid_argument(
154  "Linearization point not feasible for "
155  + DefaultKeyFormatter(this->key()) + "!");
156  return Vector::Constant(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
157  }
158  }
159 
160  // Linearize is over-written, because base linearization tries to whiten
161  GaussianFactor::shared_ptr linearize(const Values& x) const override {
162  const T& xj = x.at<T>(this->key());
163  Matrix A;
164  Vector b = evaluateError(xj, A);
165  SharedDiagonal model = noiseModel::Constrained::All(b.size());
167  new JacobianFactor(this->key(), A, b, model));
168  }
169 
171  gtsam::NonlinearFactor::shared_ptr clone() const override {
172  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
173  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
174  }
175 
177 
179 
180 private:
181 
184  template<class ARCHIVE>
185  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
186  ar
187  & boost::serialization::make_nvp("NoiseModelFactor1",
188  boost::serialization::base_object<Base>(*this));
189  ar & BOOST_SERIALIZATION_NVP(feasible_);
190  ar & BOOST_SERIALIZATION_NVP(allow_error_);
191  ar & BOOST_SERIALIZATION_NVP(error_gain_);
192  }
193 
194 };
195 // \class NonlinearEquality
196 
197 template<typename VALUE>
198 struct traits<NonlinearEquality<VALUE> > : Testable<NonlinearEquality<VALUE> > {
199 };
200 
201 /* ************************************************************************* */
205 template<class VALUE>
206 class NonlinearEquality1: public NoiseModelFactor1<VALUE> {
207 
208 public:
209  typedef VALUE X;
210 
211 protected:
214 
217  }
218 
219  X value_;
220 
221  GTSAM_CONCEPT_MANIFOLD_TYPE(X)
222 
223  GTSAM_CONCEPT_TESTABLE_TYPE(X)
224 
225 public:
226 
227  typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
228 
236  NonlinearEquality1(const X& value, Key key, double mu = 1000.0) :
237  Base( noiseModel::Constrained::All(traits<X>::GetDimension(value),
238  std::abs(mu)), key), value_(value) {
239  }
240 
241  virtual ~NonlinearEquality1() {
242  }
243 
245  gtsam::NonlinearFactor::shared_ptr clone() const override {
246  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
247  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
248  }
249 
251  Vector evaluateError(const X& x1,
252  boost::optional<Matrix&> H = boost::none) const override {
253  if (H)
254  (*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
255  // manifold equivalent of h(x)-z -> log(z,h(x))
256  return traits<X>::Local(value_,x1);
257  }
258 
260  void print(const std::string& s = "",
261  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
262  std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
263  << ")," << "\n";
264  this->noiseModel_->print();
265  traits<X>::Print(value_, "Value");
266  }
267 
269 
270 private:
271 
274  template<class ARCHIVE>
275  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
276  ar
277  & boost::serialization::make_nvp("NoiseModelFactor1",
278  boost::serialization::base_object<Base>(*this));
279  ar & BOOST_SERIALIZATION_NVP(value_);
280  }
281 };
282 // \NonlinearEquality1
283 
284 template<typename VALUE>
285 struct traits<NonlinearEquality1<VALUE> > : Testable<NonlinearEquality1<VALUE> > {
286 };
287 
288 /* ************************************************************************* */
293 template<class VALUE>
294 class NonlinearEquality2: public NoiseModelFactor2<VALUE, VALUE> {
295 public:
296  typedef VALUE X;
297 
298 protected:
301 
302  GTSAM_CONCEPT_MANIFOLD_TYPE(X)
303 
304 
306  }
307 
308 public:
309 
310  typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
311 
313  NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) :
314  Base(noiseModel::Constrained::All(traits<X>::dimension, std::abs(mu)), key1, key2) {
315  }
316  virtual ~NonlinearEquality2() {
317  }
318 
320  gtsam::NonlinearFactor::shared_ptr clone() const override {
321  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
322  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
323  }
324 
326  Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
327  boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
328  static const size_t p = traits<X>::dimension;
329  if (H1) *H1 = -Matrix::Identity(p,p);
330  if (H2) *H2 = Matrix::Identity(p,p);
331  return traits<X>::Local(x1,x2);
332  }
333 
335 
336 private:
337 
340  template<class ARCHIVE>
341  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
342  ar
343  & boost::serialization::make_nvp("NoiseModelFactor2",
344  boost::serialization::base_object<Base>(*this));
345  }
346 };
347 // \NonlinearEquality2
348 
349 template<typename VALUE>
350 struct traits<NonlinearEquality2<VALUE> > : Testable<NonlinearEquality2<VALUE> > {
351 };
352 
353 
354 }// namespace gtsam
355 
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
Testable.h
Concept check for values that can be used in unit tests.
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Fully constrained variations.
Definition: NoiseModel.h:467
gtsam::NonlinearEquality::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: NonlinearEquality.h:110
gtsam::NoiseModelFactor1::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Calls the 1-key specific version of evaluateError below, which is pure virtual so must be implemented...
Definition: NonlinearFactor.h:310
gtsam::NonlinearEquality2::NonlinearEquality2
NonlinearEquality2(Key key1, Key key2, double mu=1000.0)
TODO: comment.
Definition: NonlinearEquality.h:313
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::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:203
gtsam::NonlinearEquality1::evaluateError
Vector evaluateError(const X &x1, boost::optional< Matrix & > H=boost::none) const override
g(x) with optional derivative
Definition: NonlinearEquality.h:251
gtsam::NoiseModelFactor2< VALUE, VALUE >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
gtsam::NonlinearEquality2::evaluateError
Vector evaluateError(const X &x1, const X &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
g(x) with optional derivative2
Definition: NonlinearEquality.h:326
gtsam::NonlinearEquality::error
double error(const Values &c) const override
actual error function calculation
Definition: NonlinearEquality.h:129
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam::NonlinearEquality::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearEquality.h:118
gtsam::NonlinearEquality1::NonlinearEquality1
NonlinearEquality1()
default constructor to allow for serialization
Definition: NonlinearEquality.h:216
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::NonlinearEquality2::access
friend class boost::serialization::access
Serialization function.
Definition: NonlinearEquality.h:339
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
gtsam::NonlinearEquality1
Simple unary equality constraint - fixes a value for a variable.
Definition: NonlinearEquality.h:206
gtsam::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::NonlinearEquality
An equality factor that forces either one variable to a constant, or a set of variables to be equal t...
Definition: NonlinearEquality.h:45
gtsam::KeyFormatter
boost::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::NonlinearEquality::access
friend class boost::serialization::access
Serialization function.
Definition: NonlinearEquality.h:183
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality(Key j, const T &feasible, double error_gain, const CompareFunction &_compare=boost::bind(traits< T >::Equals, _1, _2, 1e-9))
Constructor - allows inexact evaluation.
Definition: NonlinearEquality.h:99
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::NonlinearEquality2
Simple binary equality constraint - this constraint forces two factors to be the same.
Definition: NonlinearEquality.h:294
gtsam::NonlinearEquality1::shared_ptr
boost::shared_ptr< NonlinearEquality1< VALUE > > shared_ptr
fixed value for variable
Definition: NonlinearEquality.h:227
gtsam::NonlinearEquality2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: NonlinearEquality.h:320
gtsam::NonlinearEquality1::NonlinearEquality1
NonlinearEquality1(const X &value, Key key, double mu=1000.0)
Constructor.
Definition: NonlinearEquality.h:236
gtsam::NonlinearEquality::evaluateError
Vector evaluateError(const T &xj, boost::optional< Matrix & > H=boost::none) const override
error function
Definition: NonlinearEquality.h:140
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality()
default constructor - only for serialization
Definition: NonlinearEquality.h:77
gtsam::NonlinearEquality::linearize
GaussianFactor::shared_ptr linearize(const Values &x) const override
linearize to a GaussianFactor
Definition: NonlinearEquality.h:161
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::NonlinearEquality1::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: NonlinearEquality.h:245
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality(Key j, const T &feasible, const CompareFunction &_compare=boost::bind(traits< T >::Equals, _1, _2, 1e-9))
Constructor - forces exact evaluation.
Definition: NonlinearEquality.h:89
Manifold.h
Base class and basic functions for Manifold types.
gtsam::NonlinearEquality::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: NonlinearEquality.h:171
gtsam::NonlinearEquality1::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearEquality.h:260
gtsam::NonlinearEquality::CompareFunction
boost::function< bool(const T &, const T &)> CompareFunction
Function that compares two values.
Definition: NonlinearEquality.h:72
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:194
gtsam::NonlinearEquality1::access
friend class boost::serialization::access
Serialization function.
Definition: NonlinearEquality.h:273