gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
23namespace gtsam {
24
32template<class 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 ~BoundingConstraint1() override {}
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
82private:
83
86 template<class ARCHIVE>
87 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
88 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
89 ar & boost::serialization::make_nvp("NoiseModelFactor1",
90 boost::serialization::base_object<Base>(*this));
91 ar & BOOST_SERIALIZATION_NVP(threshold_);
92 ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
93 }
94};
95
100template<class VALUE1, class VALUE2>
101struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
102 typedef VALUE1 X1;
103 typedef VALUE2 X2;
104
106 typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
107
108 double threshold_;
109 bool isGreaterThan_;
110
111 BoundingConstraint2(Key key1, Key key2, double threshold,
112 bool isGreaterThan, double mu = 1000.0)
113 : Base(noiseModel::Constrained::All(1, mu), key1, key2),
114 threshold_(threshold), isGreaterThan_(isGreaterThan) {}
115
116 ~BoundingConstraint2() override {}
117
118 inline double threshold() const { return threshold_; }
119 inline bool isGreaterThan() const { return isGreaterThan_; }
120
125 virtual double value(const X1& x1, const X2& x2,
126 boost::optional<Matrix&> H1 = boost::none,
127 boost::optional<Matrix&> H2 = boost::none) const = 0;
128
130 bool active(const Values& c) const override {
131 // note: still active at equality to avoid zigzagging
132 double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
133 return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
134 }
135
136 Vector evaluateError(const X1& x1, const X2& x2,
137 boost::optional<Matrix&> H1 = boost::none,
138 boost::optional<Matrix&> H2 = boost::none) const override {
139 Matrix D1, D2;
140 double error = value(x1, x2, D1, D2) - threshold_;
141 if (H1) {
142 if (isGreaterThan_) *H1 = D1;
143 else *H1 = -1.0 * D1;
144 }
145 if (H2) {
146 if (isGreaterThan_) *H2 = D2;
147 else *H2 = -1.0 * D2;
148 }
149
150 if (isGreaterThan_)
151 return (Vector(1) << error).finished();
152 else
153 return -1.0 * (Vector(1) << error).finished();
154 }
155
156private:
157
160 template<class ARCHIVE>
161 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
162 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
163 ar & boost::serialization::make_nvp("NoiseModelFactor2",
164 boost::serialization::base_object<Base>(*this));
165 ar & BOOST_SERIALIZATION_NVP(threshold_);
166 ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
167 }
168};
169
170} // \namespace gtsam
Base class and basic functions for Lie types.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
Unary inequality constraint forcing a scalar to be greater/less than a fixed threshold.
Definition BoundingConstraint.h:33
bool active(const Values &c) const override
active when constraint NOT met
Definition BoundingConstraint.h:61
BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition BoundingConstraint.h:41
friend class boost::serialization::access
Serialization function.
Definition BoundingConstraint.h:85
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...
Binary scalar inequality constraint, with a similar value() function to implement for specific system...
Definition BoundingConstraint.h:101
bool active(const Values &c) const override
active when constraint NOT met
Definition BoundingConstraint.h:130
BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition BoundingConstraint.h:111
friend class boost::serialization::access
Serialization function.
Definition BoundingConstraint.h:159
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...