Loading...
Searching...
No Matches
ExpressionFactor.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
* -------------------------------------------------------------------------- */
#pragma once
#include <array>
#include <gtsam/config.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/Expression.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <numeric>
namespace gtsam {
template<typename T>
class ExpressionFactor: public NoiseModelFactor {
BOOST_CONCEPT_ASSERT((IsTestable<T>));
protected:
typedef ExpressionFactor<T> This;
static const int Dim = traits<T>::dimension;
T measured_;
Expression<T> expression_;
FastVector<int> dims_;
public:
typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
ExpressionFactor(const SharedNoiseModel& noiseModel, //
const T& measurement, const Expression<T>& expression)
: NoiseModelFactor(noiseModel), measured_(measurement) {
initialize(expression);
}
~ExpressionFactor() override {}
const T& measured() const { return measured_; }
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
NoiseModelFactor::print(s, keyFormatter);
traits<T>::Print(measured_, "ExpressionFactor with measurement: ");
}
bool equals(const NonlinearFactor& f, double tol) const override {
const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
return p && NoiseModelFactor::equals(f, tol) &&
traits<T>::Equals(measured_, p->measured_, tol) &&
dims_ == p->dims_;
}
Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const override {
if (H) {
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
// NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
// because it would use the tangent space of the measurement instead of the value.
return -traits<T>::Local(value, measured_);
} else {
const T value = expression_.value(x);
return -traits<T>::Local(value, measured_);
}
}
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
// Only linearize if the factor is active
if (!active(x))
return boost::shared_ptr<JacobianFactor>();
// In case noise model is constrained, we need to provide a noise model
SharedDiagonal noiseModel;
if (noiseModel_ && noiseModel_->isConstrained()) {
noiseModel = boost::static_pointer_cast<noiseModel::Constrained>(
noiseModel_)->unit();
}
// Create a writeable JacobianFactor in advance
boost::shared_ptr<JacobianFactor> factor(
new JacobianFactor(keys_, dims_, Dim, noiseModel));
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
VerticalBlockMatrix& Ab = factor->matrixObject();
internal::JacobianMap jacobianMap(keys_, Ab);
// Zero out Jacobian so we can simply add to it
Ab.matrix().setZero();
// Get value and Jacobians, writing directly into JacobianFactor
T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here !
// Evaluate error and set RHS vector b
Ab(size()).col(0) = traits<T>::Local(value, measured_);
// Whiten the corresponding system, Ab already contains RHS
if (noiseModel_) {
Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models
noiseModel_->WhitenSystem(Ab.matrix(), b);
}
return factor;
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
protected:
ExpressionFactor() {}
ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement)
: NoiseModelFactor(noiseModel), measured_(measurement) {
// Not properly initialized yet, need to call initialize
}
void initialize(const Expression<T>& expression) {
if (!noiseModel_)
throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
if (noiseModel_->dim() != Dim)
throw std::invalid_argument(
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
expression_ = expression;
// Get keys and dimensions for Jacobian matrices
// An Expression is assumed unmutable, so we do this now
if (keys_.empty()) {
// This is the case when called in ExpressionFactor Constructor.
// We then take the keys from the expression in sorted order.
boost::tie(keys_, dims_) = expression_.keysAndDims();
} else {
// This happens with classes derived from BinaryExpressionFactor etc.
// In that case, the keys_ are already defined and we just need to grab
// the dimensions in the correct order.
std::map<Key, int> keyedDims;
expression_.dims(keyedDims);
for (Key key : keys_) dims_.push_back(keyedDims[key]);
}
}
virtual Expression<T> expression() const {
throw std::runtime_error("ExpressionFactor::expression not provided: cannot deserialize.");
}
private:
template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
ar << boost::serialization::make_nvp("measured_", this->measured_);
}
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
ar >> boost::serialization::make_nvp("measured_", this->measured_);
this->initialize(expression());
}
// Indicate that we implement save/load separately, and be friendly to boost
BOOST_SERIALIZATION_SPLIT_MEMBER()
friend class boost::serialization::access;
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// ExpressionFactor
template <typename T>
struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
template <typename T, typename... Args>
class ExpressionFactorN : public ExpressionFactor<T> {
public:
static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;
~ExpressionFactorN() override = default;
// Don't provide backward compatible evaluateVector(), due to its problematic
// variable length of optional Jacobian arguments. Vector evaluateError(const
// Args... args,...);
virtual Expression<T> expression(const ArrayNKeys &keys) const {
throw std::runtime_error(
"ExpressionFactorN::expression not provided: cannot deserialize.");
}
protected:
ExpressionFactorN() = default;
ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel,
const T &measurement)
: ExpressionFactor<T>(noiseModel, measurement) {
for (const auto &key : keys)
Factor::keys_.push_back(key);
}
private:
Expression<T> expression() const override {
ArrayNKeys keys;
int idx = 0;
for (const auto &key : Factor::keys_)
keys[idx++] = key;
return expression(keys);
}
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &boost::serialization::make_nvp(
"ExpressionFactorN",
boost::serialization::base_object<ExpressionFactor<T>>(*this));
}
};
template <typename T, typename... Args>
struct traits<ExpressionFactorN<T, Args...>>
: public Testable<ExpressionFactorN<T, Args...>> {};
// ExpressionFactorN
#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V42)
template <typename T, typename A1, typename A2>
class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
public:
~ExpressionFactor2() override {}
Vector evaluateError(const A1 &a1, const A2 &a2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const {
Values values;
values.insert(this->keys_[0], a1);
values.insert(this->keys_[1], a2);
std::vector<Matrix> H(2);
Vector error = ExpressionFactor<T>::unwhitenedError(values, H);
if (H1) (*H1) = H[0];
if (H2) (*H2) = H[1];
return error;
}
virtual Expression<T> expression(Key key1, Key key2) const {
throw std::runtime_error(
"ExpressionFactor2::expression not provided: cannot deserialize.");
}
Expression<T>
expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys &keys)
const override {
return expression(keys[0], keys[1]);
}
protected:
ExpressionFactor2() {}
ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel,
const T &measurement)
: ExpressionFactorN<T, A1, A2>({key1, key2}, noiseModel, measurement) {}
};
// ExpressionFactor2
#endif
} // namespace gtsam
Testable.h
Concept check for values that can be used in unit tests.
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
NonlinearFactor.h
Non-linear factor base classes.
Expression.h
Expressions for Block Automatic Differentiation.
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:33
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
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::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
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::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock()).
Definition VerticalBlockMatrix.h:188
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition ExpressionFactor.h:44
gtsam::ExpressionFactor::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Error function without the NoiseModel, .
Definition ExpressionFactor.h:99
gtsam::ExpressionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print relies on Testable traits being defined for T
Definition ExpressionFactor.h:80
gtsam::ExpressionFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition ExpressionFactor.h:151
gtsam::ExpressionFactor::expression
virtual Expression< T > expression() const
Recreate expression from keys_ and measured_, used in load below.
Definition ExpressionFactor.h:193
gtsam::ExpressionFactor::expression_
Expression< T > expression_
the expression that is AD enabled
Definition ExpressionFactor.h:53
gtsam::ExpressionFactor::measured_
T measured_
the measurement to be compared with the expression
Definition ExpressionFactor.h:52
gtsam::ExpressionFactor::dims_
FastVector< int > dims_
dimensions of the Jacobian matrices
Definition ExpressionFactor.h:54
gtsam::ExpressionFactor::measured
const T & measured() const
return the measurement
Definition ExpressionFactor.h:77
gtsam::ExpressionFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
linearize to a GaussianFactor
Definition ExpressionFactor.h:112
gtsam::ExpressionFactor::~ExpressionFactor
~ExpressionFactor() override
Destructor.
Definition ExpressionFactor.h:74
gtsam::ExpressionFactor::equals
bool equals(const NonlinearFactor &f, double tol) const override
equals relies on Testable traits being defined for T
Definition ExpressionFactor.h:87
gtsam::ExpressionFactor::ExpressionFactor
ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement)
Default constructor, for serialization.
Definition ExpressionFactor.h:161
gtsam::ExpressionFactor::ExpressionFactor
ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement, const Expression< T > &expression)
Constructor: creates a factor from a measurement and measurement function.
Definition ExpressionFactor.h:67
gtsam::ExpressionFactor::initialize
void initialize(const Expression< T > &expression)
Initialize with constructor arguments.
Definition ExpressionFactor.h:167
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
gtsam::ExpressionFactorN
N-ary variadic template for ExpressionFactor meant as a base class for N-ary factors.
Definition ExpressionFactor.h:242
gtsam::ExpressionFactorN::expression
virtual Expression< T > expression(const ArrayNKeys &keys) const
Recreate expression from given keys_ and measured_, used in load Needed to deserialize a derived fact...
Definition ExpressionFactor.h:256
gtsam::ExpressionFactorN::~ExpressionFactorN
~ExpressionFactorN() override=default
Destructor.
gtsam::ExpressionFactorN::ExpressionFactorN
ExpressionFactorN()=default
Default constructor, for serialization.
gtsam::ExpressionFactorN::ExpressionFactorN
ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel, const T &measurement)
Constructor takes care of keys, but still need to call initialize.
Definition ExpressionFactor.h:266
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition NonlinearFactor.cpp:74
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition NonlinearFactor.cpp:82
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor()
Default constructor for I/O only.
Definition NonlinearFactor.h:189
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
NoiseModelFactor
noise model to the factor, and calculates the error by asking the user to implement the method