gtsam  4.1.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
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 
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 override {
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 override {
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 override {
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 override {
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
gtsam::BoundingConstraint1::value
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...
gtsam::BoundingConstraint2::active
bool active(const Values &c) const override
active when constraint NOT met
Definition: BoundingConstraint.h:129
gtsam::BoundingConstraint2::value
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...
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::BoundingConstraint2::access
friend class boost::serialization::access
Serialization function.
Definition: BoundingConstraint.h:158
gtsam::NoiseModelFactor2::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::BoundingConstraint2::BoundingConstraint2
BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition: BoundingConstraint.h:110
gtsam::BoundingConstraint1
Definition: BoundingConstraint.h:33
gtsam::BoundingConstraint1::evaluateError
Vector evaluateError(const X &x, boost::optional< Matrix & > H=boost::none) const override
Override this method to finish implementing a unary factor.
Definition: BoundingConstraint.h:67
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
gtsam::BoundingConstraint1::active
bool active(const Values &c) const override
active when constraint NOT met
Definition: BoundingConstraint.h:61
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::BoundingConstraint1::BoundingConstraint1
BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition: BoundingConstraint.h:41
gtsam::BoundingConstraint2
Binary scalar inequality constraint, with a similar value() function to implement for specific system...
Definition: BoundingConstraint.h:100
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::BoundingConstraint1::access
friend class boost::serialization::access
Serialization function.
Definition: BoundingConstraint.h:85
gtsam::BoundingConstraint2::evaluateError
Vector evaluateError(const X1 &x1, const X2 &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Override this method to finish implementing a binary factor.
Definition: BoundingConstraint.h:135
Lie.h
Base class and basic functions for Lie types.
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition: NonlinearFactor.cpp:119