73 lines
2.0 KiB
C++
73 lines
2.0 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>
|
|
#include <gtsam/base/Testable.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* BAD Factor that supports arbitrary expressions via AD
|
|
*/
|
|
template<class T>
|
|
class BADFactor: public NoiseModelFactor {
|
|
|
|
const T measurement_;
|
|
const Expression<T> expression_;
|
|
|
|
public:
|
|
|
|
/// Constructor
|
|
BADFactor(const SharedNoiseModel& noiseModel, //
|
|
const T& measurement, const Expression<T>& 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<std::vector<Matrix>&> H = boost::none) const {
|
|
if (H) {
|
|
assert(H->size()==size());
|
|
typedef std::map<Key, Matrix> MapType;
|
|
MapType terms;
|
|
Augmented<T> augmented = expression_.augmented(x);
|
|
// move terms to H, which is pre-allocated to correct size
|
|
size_t j = 0;
|
|
MapType::iterator it = augmented.jacobians().begin();
|
|
for (; it != augmented.jacobians().end(); ++it)
|
|
it->second.swap((*H)[j++]);
|
|
return measurement_.localCoordinates(augmented.value());
|
|
} else {
|
|
const T& value = expression_.value(x);
|
|
return measurement_.localCoordinates(value);
|
|
}
|
|
}
|
|
|
|
};
|
|
// BADFactor
|
|
|
|
}
|
|
|