Loading...
Searching...
No Matches
BoundingConstraint.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
template<class VALUE>
struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
typedef VALUE X;
typedef NoiseModelFactorN<VALUE> Base;
typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
double threshold_;
bool isGreaterThan_;
BoundingConstraint1(Key key, double threshold,
bool isGreaterThan, double mu = 1000.0) :
Base(noiseModel::Constrained::All(1, mu), key),
threshold_(threshold), isGreaterThan_(isGreaterThan) {
}
~BoundingConstraint1() override {}
inline double threshold() const { return threshold_; }
inline bool isGreaterThan() const { return isGreaterThan_; }
virtual double value(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
bool active(const Values& c) const override {
// note: still active at equality to avoid zigzagging
double x = value(c.at<X>(this->key()));
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const override {
Matrix D;
double error = value(x, D) - threshold_;
if (H) {
if (isGreaterThan_) *H = D;
else *H = -1.0 * D;
}
if (isGreaterThan_)
return (Vector(1) << error).finished();
else
return -1.0 * (Vector(1) << error).finished();
}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(threshold_);
ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
}
};
template<class VALUE1, class VALUE2>
struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
double threshold_;
bool isGreaterThan_;
BoundingConstraint2(Key key1, Key key2, double threshold,
bool isGreaterThan, double mu = 1000.0)
: Base(noiseModel::Constrained::All(1, mu), key1, key2),
threshold_(threshold), isGreaterThan_(isGreaterThan) {}
~BoundingConstraint2() override {}
inline double threshold() const { return threshold_; }
inline bool isGreaterThan() const { return isGreaterThan_; }
virtual double value(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const = 0;
bool active(const Values& c) const override {
// note: still active at equality to avoid zigzagging
double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
Vector evaluateError(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
Matrix D1, D2;
double error = value(x1, x2, D1, D2) - threshold_;
if (H1) {
if (isGreaterThan_) *H1 = D1;
else *H1 = -1.0 * D1;
}
if (H2) {
if (isGreaterThan_) *H2 = D2;
else *H2 = -1.0 * D2;
}
if (isGreaterThan_)
return (Vector(1) << error).finished();
else
return -1.0 * (Vector(1) << error).finished();
}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(threshold_);
ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
}
};
} // \namespace gtsam
Lie.h
Base class and basic functions for Lie types.
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition LossFunctions.cpp:27
gtsam::Factor::error
virtual double error(const HybridValues &c) const
All factor types need to implement an error function.
Definition Factor.cpp:47
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
gtsam::NoiseModelFactorN< VALUE >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN< VALUE >::key
Key key() const
Definition NonlinearFactor.h:518
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
gtsam::BoundingConstraint1
Unary inequality constraint forcing a scalar to be greater/less than a fixed threshold.
Definition BoundingConstraint.h:33
gtsam::BoundingConstraint1::active
bool active(const Values &c) const override
active when constraint NOT met
Definition BoundingConstraint.h:61
gtsam::BoundingConstraint1::BoundingConstraint1
BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition BoundingConstraint.h:41
gtsam::BoundingConstraint1::access
friend class boost::serialization::access
Serialization function.
Definition BoundingConstraint.h:85
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
Binary scalar inequality constraint, with a similar value() function to implement for specific system...
Definition BoundingConstraint.h:101
gtsam::BoundingConstraint2::active
bool active(const Values &c) const override
active when constraint NOT met
Definition BoundingConstraint.h:130
gtsam::BoundingConstraint2::BoundingConstraint2
BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition BoundingConstraint.h:111
gtsam::BoundingConstraint2::access
friend class boost::serialization::access
Serialization function.
Definition BoundingConstraint.h:159
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...