88 lines
2.6 KiB
C++
88 lines
2.6 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 Expression.h
|
|
* @date September 18, 2014
|
|
* @author Frank Dellaert
|
|
* @author Paul Furgale
|
|
* @brief Expressions for Block Automatic Differentiation
|
|
*/
|
|
|
|
#include <gtsam_unstable/nonlinear/Expression.h>
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* BAD Factor that supports arbitrary expressions via AD
|
|
*/
|
|
template<class T>
|
|
class BADFactor: NonlinearFactor {
|
|
|
|
const T measurement_;
|
|
const Expression<T> expression_;
|
|
|
|
/// get value from expression and calculate error with respect to measurement
|
|
Vector unwhitenedError(const Values& values) const {
|
|
const T& value = expression_.value(values);
|
|
return value.localCoordinates(measurement_);
|
|
}
|
|
|
|
public:
|
|
|
|
/// Constructor
|
|
BADFactor(const T& measurement, const Expression<T>& expression) :
|
|
measurement_(measurement), expression_(expression) {
|
|
}
|
|
/// Constructor
|
|
BADFactor(const T& measurement, const ExpressionNode<T>& expression) :
|
|
measurement_(measurement), expression_(expression) {
|
|
}
|
|
/**
|
|
* Calculate the error of the factor.
|
|
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
|
|
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
|
|
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
|
|
*/
|
|
virtual double error(const Values& values) const {
|
|
if (this->active(values)) {
|
|
const Vector e = unwhitenedError(values);
|
|
return 0.5 * e.squaredNorm();
|
|
} else {
|
|
return 0.0;
|
|
}
|
|
}
|
|
|
|
/// get the dimension of the factor (number of rows on linearization)
|
|
size_t dim() const {
|
|
return 0;
|
|
}
|
|
|
|
/// linearize to a GaussianFactor
|
|
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
|
|
// We will construct an n-ary factor below, where terms is a container whose
|
|
// value type is std::pair<Key, Matrix>, specifying the
|
|
// collection of keys and matrices making up the factor.
|
|
std::map<Key, Matrix> terms;
|
|
expression_.value(values, terms);
|
|
Vector b = unwhitenedError(values);
|
|
SharedDiagonal model = SharedDiagonal();
|
|
return boost::shared_ptr<JacobianFactor>(
|
|
new JacobianFactor(terms, b, model));
|
|
}
|
|
|
|
};
|
|
// BADFactor
|
|
|
|
}
|
|
|