NonlinearEquality.h
Go to the documentation of this file.
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 <limits>
25 #include <iostream>
26 #include <cmath>
27 
28 namespace gtsam {
29 
42 template<class VALUE>
43 class NonlinearEquality: public NoiseModelFactorN<VALUE> {
44 
45 public:
46  typedef VALUE T;
47 
48  // Provide access to the Matrix& version of evaluateError:
49  using NoiseModelFactor1<VALUE>::evaluateError;
50 
51 private:
52 
53  // feasible value
55 
56  // error handling flag
58 
59  // error gain in allow error case
60  double error_gain_;
61 
62  // typedef to this class
64 
65  // typedef to base class
67 
68 public:
69 
71  using CompareFunction = std::function<bool(const T&, const T&)>;
73 
76  }
77 
78  ~NonlinearEquality() override {
79  }
80 
83 
87  NonlinearEquality(Key j, const T& feasible,
88  const CompareFunction &_compare = std::bind(traits<T>::Equals,
89  std::placeholders::_1, std::placeholders::_2, 1e-9)) :
90  Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
91  j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
92  compare_(_compare) {
93  }
94 
98  NonlinearEquality(Key j, const T& feasible, double error_gain,
99  const CompareFunction &_compare = std::bind(traits<T>::Equals,
100  std::placeholders::_1, std::placeholders::_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.empty() ? s : s + " ") << "Constraint: on ["
113  << keyFormatter(this->key()) << "]\n";
114  traits<VALUE>::Print(feasible_, "Feasible Point:\n");
115  std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_)
116  << std::endl;
117  }
118 
120  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
121  const This* e = dynamic_cast<const This*>(&f);
122  return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol)
123  && std::abs(error_gain_ - e->error_gain_) < tol;
124  }
125 
129 
131  double error(const Values& c) const override {
132  const T& xj = c.at<T>(this->key());
133  Vector e = this->unwhitenedError(c);
134  if (allow_error_ || !compare_(xj, feasible_)) {
135  return error_gain_ * dot(e, e);
136  } else {
137  return 0.0;
138  }
139  }
140 
142  Vector evaluateError(const T& xj, OptionalMatrixType H) const override {
143  const size_t nj = traits<T>::GetDimension(feasible_);
144  if (allow_error_) {
145  if (H)
146  *H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare
147  return traits<T>::Local(xj,feasible_);
148  } else if (compare_(feasible_, xj)) {
149  if (H)
150  *H = Matrix::Identity(nj,nj);
151  return Vector::Zero(nj); // set error to zero if equal
152  } else {
153  if (H)
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()); // set error to infinity if not equal
158  }
159  }
160 
162  GaussianFactor::shared_ptr linearize(const Values& x) const override {
163  const T& xj = x.at<T>(this->key());
164  Matrix A;
165  Vector b = evaluateError(xj, A);
168  new JacobianFactor(this->key(), A, b, model));
169  }
170 
173  return std::static_pointer_cast<gtsam::NonlinearFactor>(
175  }
176 
178 
180 
181 private:
182 
183 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
184  friend class boost::serialization::access;
186  template<class ARCHIVE>
187  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
188  // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
189  ar
190  & boost::serialization::make_nvp("NoiseModelFactor1",
191  boost::serialization::base_object<Base>(*this));
192  ar & BOOST_SERIALIZATION_NVP(feasible_);
193  ar & BOOST_SERIALIZATION_NVP(allow_error_);
194  ar & BOOST_SERIALIZATION_NVP(error_gain_);
195  }
196 #endif
197 
198 };
199 // \class NonlinearEquality
200 
201 template <typename VALUE>
202 struct traits<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {};
203 
204 /* ************************************************************************* */
208 template<class VALUE>
209 class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
210 
211 public:
212  typedef VALUE X;
213 
214  // Provide access to Matrix& version of evaluateError:
215  using NoiseModelFactor1<VALUE>::evaluateError;
216 
217 protected:
220 
223  }
224 
226 
229 
230 public:
231 
232  typedef std::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
233 
240  NonlinearEquality1(const X& value, Key key, double mu = 1000.0)
241  : Base(noiseModel::Constrained::All(traits<X>::GetDimension(value),
242  std::abs(mu)),
243  key),
244  value_(value) {}
245 
246  ~NonlinearEquality1() override {
247  }
248 
251  return std::static_pointer_cast<gtsam::NonlinearFactor>(
253  }
254 
256  Vector evaluateError(const X& x1, OptionalMatrixType H) const override {
257  if (H)
258  (*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
259  // manifold equivalent of h(x)-z -> log(z,h(x))
260  return traits<X>::Local(value_,x1);
261  }
262 
264  void print(const std::string& s = "",
265  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
266  std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
267  << ")," << "\n";
268  this->noiseModel_->print();
269  traits<X>::Print(value_, "Value");
270  }
271 
273 
274 private:
275 
276 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
277  friend class boost::serialization::access;
279  template<class ARCHIVE>
280  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
281  // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
282  ar
283  & boost::serialization::make_nvp("NoiseModelFactor1",
284  boost::serialization::base_object<Base>(*this));
285  ar & BOOST_SERIALIZATION_NVP(value_);
286  }
287 #endif
288 };
289 // \NonlinearEquality1
290 
291 template <typename VALUE>
293  : Testable<NonlinearEquality1<VALUE> > {};
294 
295 /* ************************************************************************* */
300 template <class T>
301 class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
302  protected:
305 
307 
308 
310 
311  public:
312  typedef std::shared_ptr<NonlinearEquality2<T>> shared_ptr;
313 
314  // Provide access to the Matrix& version of evaluateError:
315  using Base::evaluateError;
316 
317 
325  : Base(noiseModel::Constrained::All(traits<T>::dimension, std::abs(mu)),
326  key1, key2) {}
327  ~NonlinearEquality2() override {}
328 
331  return std::static_pointer_cast<gtsam::NonlinearFactor>(
333  }
334 
337  const T& x1, const T& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override {
338  static const size_t p = traits<T>::dimension;
339  if (H1) *H1 = -Matrix::Identity(p, p);
340  if (H2) *H2 = Matrix::Identity(p, p);
341  return traits<T>::Local(x1, x2);
342  }
343 
345 
346  private:
347 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
348  friend class boost::serialization::access;
350  template <class ARCHIVE>
351  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
352  // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
353  ar& boost::serialization::make_nvp(
354  "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
355  }
356 #endif
357 };
358 // \NonlinearEquality2
359 
360 template <typename VALUE>
361 struct traits<NonlinearEquality2<VALUE>> : Testable<NonlinearEquality2<VALUE>> {
362 };
363 
364 }// namespace gtsam
gtsam::NonlinearEquality1::X
VALUE X
Definition: NonlinearEquality.h:212
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality()
Default constructor - only for serialization.
Definition: NonlinearEquality.h:75
gtsam::NonlinearEquality::T
VALUE T
Definition: NonlinearEquality.h:46
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::NonlinearEquality::compare_
CompareFunction compare_
Definition: NonlinearEquality.h:72
gtsam::NonlinearEquality2::This
NonlinearEquality2< T > This
Definition: NonlinearEquality.h:304
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Testable.h
Concept check for values that can be used in unit tests.
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality(Key j, const T &feasible, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
Definition: NonlinearEquality.h:87
mu
double mu
Definition: testBoundingConstraint.cpp:37
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::NoiseModelFactorN< T, T >::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::NonlinearEquality::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: NonlinearEquality.h:172
gtsam::NonlinearEquality1::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: NonlinearEquality.h:250
gtsam::NonlinearEquality::NonlinearEquality
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))
Definition: NonlinearEquality.h:98
gtsam::NonlinearEquality1::NonlinearEquality1
NonlinearEquality1(const X &value, Key key, double mu=1000.0)
Definition: NonlinearEquality.h:240
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::NonlinearEquality::CompareFunction
std::function< bool(const T &, const T &)> CompareFunction
Function that compares two values.
Definition: NonlinearEquality.h:71
gtsam::NonlinearEquality::feasible_
T feasible_
Definition: NonlinearEquality.h:54
gtsam::Factor
Definition: Factor.h:70
gtsam::NoiseModelFactor::noiseModel_
SharedNoiseModel noiseModel_
Definition: NonlinearFactor.h:205
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::NonlinearEquality
Definition: NonlinearEquality.h:43
gtsam::NonlinearEquality1::shared_ptr
std::shared_ptr< NonlinearEquality1< VALUE > > shared_ptr
fixed value for variable
Definition: NonlinearEquality.h:232
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::NonlinearEquality::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: NonlinearEquality.h:110
Eigen::internal::VALUE
@ VALUE
Definition: SpecialFunctionsImpl.h:729
gtsam::NonlinearEquality1
Definition: NonlinearEquality.h:209
gtsam::NonlinearEquality1::This
NonlinearEquality1< VALUE > This
Definition: NonlinearEquality.h:219
A
Definition: test_numpy_dtypes.cpp:298
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::NonlinearEquality1::Base
NoiseModelFactorN< VALUE > Base
Definition: NonlinearEquality.h:218
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:245
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::NoiseModelFactorN< VALUE >::unwhitenedError
virtual Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const =0
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::NonlinearEquality::error
double error(const Values &c) const override
Actual error function calculation.
Definition: NonlinearEquality.h:131
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
gtsam::dot
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Definition: NoiseModel.h:480
Manifold.h
Base class and basic functions for Manifold types.
gtsam::NonlinearEquality2::~NonlinearEquality2
~NonlinearEquality2() override
Definition: NonlinearEquality.h:327
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:80
Eigen::Triplet< double >
gtsam::NoiseModelFactorN< T, T >::key1
Key key1() const
Definition: NonlinearFactor.h:731
gtsam::NonlinearEquality2
Definition: NonlinearEquality.h:301
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::NonlinearEquality2::evaluateError
Vector evaluateError(const T &x1, const T &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
g(x) with optional derivative2
Definition: NonlinearEquality.h:336
gtsam::NonlinearEquality::evaluateError
Vector evaluateError(const T &xj, OptionalMatrixType H) const override
Error function.
Definition: NonlinearEquality.h:142
gtsam::NonlinearEquality1::evaluateError
Vector evaluateError(const X &x1, OptionalMatrixType H) const override
g(x) with optional derivative
Definition: NonlinearEquality.h:256
gtsam::NonlinearEquality2::shared_ptr
std::shared_ptr< NonlinearEquality2< T > > shared_ptr
Definition: NonlinearEquality.h:312
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
NonlinearFactor.h
Non-linear factor base classes.
gtsam::b
const G & b
Definition: Group.h:79
gtsam::NoiseModelFactorN< T, T >::key2
Key key2() const
Definition: NonlinearFactor.h:735
gtsam::NonlinearEquality1::~NonlinearEquality1
~NonlinearEquality1() override
Definition: NonlinearEquality.h:246
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::NonlinearEquality::~NonlinearEquality
~NonlinearEquality() override
Definition: NonlinearEquality.h:78
gtsam::traits
Definition: Group.h:36
gtsam::NonlinearEquality::error_gain_
double error_gain_
Definition: NonlinearEquality.h:60
gtsam::NonlinearEquality1::NonlinearEquality1
NonlinearEquality1()
Default constructor to allow for serialization.
Definition: NonlinearEquality.h:222
gtsam::NonlinearEquality2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: NonlinearEquality.h:330
gtsam::Values
Definition: Values.h:65
gtsam::NonlinearEquality2::NonlinearEquality2
NonlinearEquality2(Key key1, Key key2, double mu=1e4)
Definition: NonlinearEquality.h:324
gtsam::NonlinearEquality::This
NonlinearEquality< VALUE > This
Definition: NonlinearEquality.h:63
gtsam::NonlinearEquality1::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearEquality.h:264
std
Definition: BFloat16.h:88
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
GTSAM_CONCEPT_TESTABLE_TYPE
#define GTSAM_CONCEPT_TESTABLE_TYPE(T)
Definition: Testable.h:177
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
gtsam::NonlinearEquality::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearEquality.h:120
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::NonlinearEquality::allow_error_
bool allow_error_
Definition: NonlinearEquality.h:57
gtsam::NoiseModelFactorN< VALUE >::key
Key key() const
Definition: NonlinearFactor.h:582
gtsam::NonlinearEquality::linearize
GaussianFactor::shared_ptr linearize(const Values &x) const override
Linearize is over-written, because base linearization tries to whiten.
Definition: NonlinearEquality.h:162
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
GTSAM_CONCEPT_MANIFOLD_TYPE
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T)
Definition: Manifold.h:178
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
test_callbacks.value
value
Definition: test_callbacks.py:160
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam::NonlinearEquality1::value_
X value_
Definition: NonlinearEquality.h:225


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:03:12