/* ---------------------------------------------------------------------------- * 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 #include #include namespace gtsam { /** * Factor that supports arbitrary expressions via AD */ template class ExpressionFactor: public NoiseModelFactor { const T measurement_; const Expression expression_; public: /// Constructor ExpressionFactor(const SharedNoiseModel& noiseModel, // const T& measurement, const Expression& expression) : NoiseModelFactor(noiseModel, expression.keys()), // measurement_(measurement), expression_(expression) { } /** * Error function *without* the NoiseModel, \f$ z-h(x) \f$. * We override this method to provide * both the function evaluation and its derivative(s) in H. */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if (H) { assert(H->size()==size()); Augmented augmented = expression_.augmented(x); // move terms to H, which is pre-allocated to correct size augmented.move(*H); return measurement_.localCoordinates(augmented.value()); } else { const T& value = expression_.value(x); return measurement_.localCoordinates(value); } } }; // ExpressionFactor }