gtsam  4.0.0 gtsam
BoundingConstraint.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10  * -------------------------------------------------------------------------- */
11
18 #pragma once
19
20 #include <gtsam/base/Lie.h>
22
23 namespace gtsam {
24
32 template<class VALUE>
33 struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {
34  typedef VALUE X;
36  typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
37
38  double threshold_;
39  bool isGreaterThan_;
40
41  BoundingConstraint1(Key key, double threshold,
42  bool isGreaterThan, double mu = 1000.0) :
43  Base(noiseModel::Constrained::All(1, mu), key),
44  threshold_(threshold), isGreaterThan_(isGreaterThan) {
45  }
46
47  virtual ~BoundingConstraint1() {}
48
49  inline double threshold() const { return threshold_; }
50  inline bool isGreaterThan() const { return isGreaterThan_; }
51
57  virtual double value(const X& x, boost::optional<Matrix&> H =
58  boost::none) const = 0;
59
61  bool active(const Values& c) const {
62  // note: still active at equality to avoid zigzagging
63  double x = value(c.at<X>(this->key()));
64  return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
65  }
66
67  Vector evaluateError(const X& x, boost::optional<Matrix&> H =
68  boost::none) const {
69  Matrix D;
70  double error = value(x, D) - threshold_;
71  if (H) {
72  if (isGreaterThan_) *H = D;
73  else *H = -1.0 * D;
74  }
75
76  if (isGreaterThan_)
77  return (Vector(1) << error).finished();
78  else
79  return -1.0 * (Vector(1) << error).finished();
80  }
81
82 private:
83
86  template<class ARCHIVE>
87  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
88  ar & boost::serialization::make_nvp("NoiseModelFactor1",
89  boost::serialization::base_object<Base>(*this));
90  ar & BOOST_SERIALIZATION_NVP(threshold_);
91  ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
92  }
93 };
94
99 template<class VALUE1, class VALUE2>
100 struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
101  typedef VALUE1 X1;
102  typedef VALUE2 X2;
103
105  typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
106
107  double threshold_;
108  bool isGreaterThan_;
109
110  BoundingConstraint2(Key key1, Key key2, double threshold,
111  bool isGreaterThan, double mu = 1000.0)
112  : Base(noiseModel::Constrained::All(1, mu), key1, key2),
113  threshold_(threshold), isGreaterThan_(isGreaterThan) {}
114
115  virtual ~BoundingConstraint2() {}
116
117  inline double threshold() const { return threshold_; }
118  inline bool isGreaterThan() const { return isGreaterThan_; }
119
124  virtual double value(const X1& x1, const X2& x2,
125  boost::optional<Matrix&> H1 = boost::none,
126  boost::optional<Matrix&> H2 = boost::none) const = 0;
127
129  bool active(const Values& c) const {
130  // note: still active at equality to avoid zigzagging
131  double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
132  return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
133  }
134
135  Vector evaluateError(const X1& x1, const X2& x2,
136  boost::optional<Matrix&> H1 = boost::none,
137  boost::optional<Matrix&> H2 = boost::none) const {
138  Matrix D1, D2;
139  double error = value(x1, x2, D1, D2) - threshold_;
140  if (H1) {
141  if (isGreaterThan_) *H1 = D1;
142  else *H1 = -1.0 * D1;
143  }
144  if (H2) {
145  if (isGreaterThan_) *H2 = D2;
146  else *H2 = -1.0 * D2;
147  }
148
149  if (isGreaterThan_)
150  return (Vector(1) << error).finished();
151  else
152  return -1.0 * (Vector(1) << error).finished();
153  }
154
155 private:
156
159  template<class ARCHIVE>
160  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
161  ar & boost::serialization::make_nvp("NoiseModelFactor2",
162  boost::serialization::base_object<Base>(*this));
163  ar & BOOST_SERIALIZATION_NVP(threshold_);
164  ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
165  }
166 };
167
168 } // \namespace gtsam
This is the base class for all factor types.
Definition: Factor.h:54
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:70
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:345
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
virtual double value(const X &x, boost::optional< Matrix & > H=boost::none) const =0
function producing a scalar value to compare to the threshold Must have optional argument for derivat...
friend class boost::serialization::access
Serialization function.
Definition: BoundingConstraint.h:158
virtual double value(const X1 &x1, const X2 &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const =0
function producing a scalar value to compare to the threshold Must have optional argument for derivat...
Vector evaluateError(const X1 &x1, const X2 &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
Override this method to finish implementing a binary factor.
Definition: BoundingConstraint.h:135
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:276
Binary scalar inequality constraint, with a similar value() function to implement for specific system...
Definition: BoundingConstraint.h:100
BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition: BoundingConstraint.h:110
const SharedNoiseModel & noiseModel() const
Definition: NonlinearFactor.h:210
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:377
Definition: BoundingConstraint.h:33
BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition: BoundingConstraint.h:41
Non-linear factor base classes.
bool active(const Values &c) const
active when constraint NOT met
Definition: BoundingConstraint.h:129
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
bool active(const Values &c) const
active when constraint NOT met
Definition: BoundingConstraint.h:61
Vector evaluateError(const X &x, boost::optional< Matrix & > H=boost::none) const
Override this method to finish implementing a unary factor.
Definition: BoundingConstraint.h:67
Base class and basic functions for Lie types.
virtual double error(const Values &c) const
Calculate the error of the factor.
Definition: NonlinearFactor.cpp:97
friend class boost::serialization::access
Serialization function.
Definition: BoundingConstraint.h:85