36 typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
42 bool isGreaterThan,
double mu = 1000.0) :
44 threshold_(threshold), isGreaterThan_(isGreaterThan) {
49 inline double threshold()
const {
return threshold_; }
50 inline bool isGreaterThan()
const {
return isGreaterThan_; }
57 virtual double value(
const X& x, boost::optional<Matrix&> H =
58 boost::none)
const = 0;
63 double x =
value(c.
at<X>(this->key()));
64 return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
67 Vector evaluateError(
const X& x, boost::optional<Matrix&> H =
68 boost::none)
const override {
72 if (isGreaterThan_) *H = D;
77 return (Vector(1) <<
error).finished();
79 return -1.0 * (Vector(1) <<
error).finished();
86 template<
class ARCHIVE>
87 void serialize(ARCHIVE & ar,
const unsigned int ) {
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_);
100template<
class VALUE1,
class VALUE2>
106 typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
112 bool isGreaterThan,
double mu = 1000.0)
114 threshold_(threshold), isGreaterThan_(isGreaterThan) {}
118 inline double threshold()
const {
return threshold_; }
119 inline bool isGreaterThan()
const {
return isGreaterThan_; }
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;
132 double x =
value(c.
at<X1>(this->key1()), c.
at<X2>(this->key2()));
133 return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
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 {
140 double error =
value(x1, x2, D1, D2) - threshold_;
142 if (isGreaterThan_) *H1 = D1;
143 else *H1 = -1.0 * D1;
146 if (isGreaterThan_) *H2 = D2;
147 else *H2 = -1.0 * D2;
151 return (Vector(1) <<
error).finished();
153 return -1.0 * (Vector(1) <<
error).finished();
160 template<
class ARCHIVE>
161 void serialize(ARCHIVE & ar,
const unsigned int ) {
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_);
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...