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 
225  X value_;
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>
292 struct traits<NonlinearEquality1<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
NoiseModelFactorN< VALUE > Base
NonlinearEquality1()
Default constructor to allow for serialization.
Concept check for values that can be used in unit tests.
gtsam::NonlinearFactor::shared_ptr clone() const override
gtsam::NonlinearFactor::shared_ptr clone() const override
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
std::string serialize(const T &input)
serializes to a string
noiseModel::Diagonal::shared_ptr model
const ValueType at(Key j) const
Definition: Values-inl.h:204
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
double mu
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::NonlinearFactor::shared_ptr clone() const override
std::shared_ptr< NonlinearEquality2< T > > shared_ptr
NonlinearEquality1(const X &value, Key key, double mu=1000.0)
Definition: BFloat16.h:88
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
Vector evaluateError(const T &x1, const T &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
g(x) with optional derivative2
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
#define GTSAM_CONCEPT_TESTABLE_TYPE(T)
Definition: Testable.h:177
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
GaussianFactor::shared_ptr linearize(const Values &x) const override
Linearize is over-written, because base linearization tries to whiten.
const SharedNoiseModel & noiseModel() const
access to the noise model
Matrix * OptionalMatrixType
Eigen::VectorXd Vector
Definition: Vector.h:38
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Base class and basic functions for Manifold types.
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
NonlinearEquality2(Key key1, Key key2, double mu=1e4)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
NonlinearEquality(Key j, const T &feasible, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
RealScalar s
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
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
const G & b
Definition: Group.h:86
std::shared_ptr< NonlinearEquality1< VALUE > > shared_ptr
fixed value for variable
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Vector evaluateError(const T &xj, OptionalMatrixType H) const override
Error function.
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
SharedNoiseModel noiseModel_
Non-linear factor base classes.
Vector evaluateError(const X &x1, OptionalMatrixType H) const override
g(x) with optional derivative
NonlinearEquality< VALUE > This
NonlinearEquality()
Default constructor - only for serialization.
Pose3 x1
Definition: testPose3.cpp:663
std::shared_ptr< This > shared_ptr
float * p
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T)
Definition: Manifold.h:178
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:284
const G double tol
Definition: Group.h:86
double error(const Values &c) const override
Actual error function calculation.
NonlinearEquality1< VALUE > This
std::function< bool(const T &, const T &)> CompareFunction
Function that compares two values.
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
#define abs(x)
Definition: datatypes.h:17
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:62
std::ptrdiff_t j
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))
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
static shared_ptr All(size_t dim)
Definition: NoiseModel.h:466


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:57