Loading...
Searching...
No Matches
NonlinearEquality.h
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/*
* @file NonlinearEquality.h
* @brief Factor to handle enforced equality between factors
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <boost/bind/bind.hpp>
#include <limits>
#include <iostream>
#include <cmath>
namespace gtsam {
template<class VALUE>
class NonlinearEquality: public NoiseModelFactorN<VALUE> {
public:
typedef VALUE T;
private:
// feasible value
T feasible_;
// error handling flag
bool allow_error_;
// error gain in allow error case
double error_gain_;
// typedef to this class
using This = NonlinearEquality<VALUE>;
// typedef to base class
using Base = NoiseModelFactorN<VALUE>;
public:
using CompareFunction = std::function<bool(const T&, const T&)>;
CompareFunction compare_;
NonlinearEquality() {
}
~NonlinearEquality() override {
}
NonlinearEquality(Key j, const T& feasible,
const CompareFunction &_compare = std::bind(traits<T>::Equals,
std::placeholders::_1, std::placeholders::_2, 1e-9)) :
Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
compare_(_compare) {
}
NonlinearEquality(Key j, const T& feasible, double error_gain,
const CompareFunction &_compare = std::bind(traits<T>::Equals,
std::placeholders::_1, std::placeholders::_2, 1e-9)) :
Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
compare_(_compare) {
}
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << (s.empty() ? s : s + " ") << "Constraint: on ["
<< keyFormatter(this->key()) << "]\n";
traits<VALUE>::Print(feasible_, "Feasible Point:\n");
std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_)
<< std::endl;
}
bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
const This* e = dynamic_cast<const This*>(&f);
return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol)
&& std::abs(error_gain_ - e->error_gain_) < tol;
}
double error(const Values& c) const override {
const T& xj = c.at<T>(this->key());
Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) {
return error_gain_ * dot(e, e);
} else {
return 0.0;
}
}
Vector evaluateError(const T& xj,
boost::optional<Matrix&> H = boost::none) const override {
const size_t nj = traits<T>::GetDimension(feasible_);
if (allow_error_) {
if (H)
*H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare
return traits<T>::Local(xj,feasible_);
} else if (compare_(feasible_, xj)) {
if (H)
*H = Matrix::Identity(nj,nj);
return Vector::Zero(nj); // set error to zero if equal
} else {
if (H)
throw std::invalid_argument(
"Linearization point not feasible for "
+ DefaultKeyFormatter(this->key()) + "!");
return Vector::Constant(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
}
}
GaussianFactor::shared_ptr linearize(const Values& x) const override {
const T& xj = x.at<T>(this->key());
Matrix A;
Vector b = evaluateError(xj, A);
SharedDiagonal model = noiseModel::Constrained::All(b.size());
return GaussianFactor::shared_ptr(
new JacobianFactor(this->key(), A, b, model));
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
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(feasible_);
ar & BOOST_SERIALIZATION_NVP(allow_error_);
ar & BOOST_SERIALIZATION_NVP(error_gain_);
}
};
// \class NonlinearEquality
template <typename VALUE>
struct traits<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {};
/* ************************************************************************* */
template<class VALUE>
class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
public:
typedef VALUE X;
protected:
typedef NoiseModelFactorN<VALUE> Base;
typedef NonlinearEquality1<VALUE> This;
NonlinearEquality1() {
}
X value_;
GTSAM_CONCEPT_MANIFOLD_TYPE(X)
GTSAM_CONCEPT_TESTABLE_TYPE(X)
public:
typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
NonlinearEquality1(const X& value, Key key, double mu = 1000.0)
: Base(noiseModel::Constrained::All(traits<X>::GetDimension(value),
std::abs(mu)),
key),
value_(value) {}
~NonlinearEquality1() override {
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
Vector evaluateError(const X& x1,
boost::optional<Matrix&> H = boost::none) const override {
if (H)
(*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
// manifold equivalent of h(x)-z -> log(z,h(x))
return traits<X>::Local(value_,x1);
}
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
<< ")," << "\n";
this->noiseModel_->print();
traits<X>::Print(value_, "Value");
}
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
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(value_);
}
};
// \NonlinearEquality1
template <typename VALUE>
struct traits<NonlinearEquality1<VALUE> >
: Testable<NonlinearEquality1<VALUE> > {};
/* ************************************************************************* */
template <class T>
class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
protected:
using Base = NoiseModelFactorN<T, T>;
using This = NonlinearEquality2<T>;
GTSAM_CONCEPT_MANIFOLD_TYPE(T)
NonlinearEquality2() {}
public:
typedef boost::shared_ptr<NonlinearEquality2<T>> shared_ptr;
NonlinearEquality2(Key key1, Key key2, double mu = 1e4)
: Base(noiseModel::Constrained::All(traits<T>::dimension, std::abs(mu)),
key1, key2) {}
~NonlinearEquality2() override {}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
Vector evaluateError(
const T& x1, const T& x2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
static const size_t p = traits<T>::dimension;
if (H1) *H1 = -Matrix::Identity(p, p);
if (H2) *H2 = Matrix::Identity(p, p);
return traits<T>::Local(x1, x2);
}
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
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));
}
};
// \NonlinearEquality2
template <typename VALUE>
struct traits<NonlinearEquality2<VALUE>> : Testable<NonlinearEquality2<VALUE>> {
};
}// namespace gtsam
Testable.h
Concept check for values that can be used in unit tests.
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Manifold.h
Base class and basic functions for Manifold types.
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
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::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Fully constrained variations.
Definition NoiseModel.h:457
gtsam::NonlinearEquality
An equality factor that forces either one variable to a constant, or a set of variables to be equal t...
Definition NonlinearEquality.h:45
gtsam::NonlinearEquality::error
double error(const Values &c) const override
Actual error function calculation.
Definition NonlinearEquality.h:130
gtsam::NonlinearEquality::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition NonlinearEquality.h:109
gtsam::NonlinearEquality::CompareFunction
std::function< bool(const T &, const T &)> CompareFunction
Function that compares two values.
Definition NonlinearEquality.h:70
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality()
Default constructor - only for serialization.
Definition NonlinearEquality.h:74
gtsam::NonlinearEquality::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition NonlinearEquality.h:119
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality(Key j, const T &feasible, double error_gain, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
Constructor - allows inexact evaluation.
Definition NonlinearEquality.h:97
gtsam::NonlinearEquality::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:172
gtsam::NonlinearEquality::evaluateError
Vector evaluateError(const T &xj, boost::optional< Matrix & > H=boost::none) const override
Error function.
Definition NonlinearEquality.h:141
gtsam::NonlinearEquality::access
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:184
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality(Key j, const T &feasible, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
Constructor - forces exact evaluation.
Definition NonlinearEquality.h:86
gtsam::NonlinearEquality::linearize
GaussianFactor::shared_ptr linearize(const Values &x) const override
Linearize is over-written, because base linearization tries to whiten.
Definition NonlinearEquality.h:162
gtsam::NonlinearEquality1
Simple unary equality constraint - fixes a value for a variable.
Definition NonlinearEquality.h:207
gtsam::NonlinearEquality1::evaluateError
Vector evaluateError(const X &x1, boost::optional< Matrix & > H=boost::none) const override
g(x) with optional derivative
Definition NonlinearEquality.h:251
gtsam::NonlinearEquality1::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:245
gtsam::NonlinearEquality1::shared_ptr
boost::shared_ptr< NonlinearEquality1< VALUE > > shared_ptr
fixed value for variable
Definition NonlinearEquality.h:227
gtsam::NonlinearEquality1::NonlinearEquality1
NonlinearEquality1(const X &value, Key key, double mu=1000.0)
Constructor.
Definition NonlinearEquality.h:235
gtsam::NonlinearEquality1::NonlinearEquality1
NonlinearEquality1()
Default constructor to allow for serialization.
Definition NonlinearEquality.h:217
gtsam::NonlinearEquality1::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition NonlinearEquality.h:260
gtsam::NonlinearEquality1::access
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:273
gtsam::NonlinearEquality2
Simple binary equality constraint - this constraint forces two variables to be the same.
Definition NonlinearEquality.h:295
gtsam::NonlinearEquality2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:320
gtsam::NonlinearEquality2::evaluateError
Vector evaluateError(const T &x1, const T &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
g(x) with optional derivative2
Definition NonlinearEquality.h:326
gtsam::NonlinearEquality2::NonlinearEquality2
NonlinearEquality2(Key key1, Key key2, double mu=1e4)
Constructor.
Definition NonlinearEquality.h:314
gtsam::NonlinearEquality2::access
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:339
gtsam::NonlinearEquality2::NonlinearEquality2
NonlinearEquality2()
Default constructor to allow for serialization.
Definition NonlinearEquality.h:303
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactorN< VALUE >::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Definition NonlinearFactor.h:542
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