addressed MR comments on nonlinearfactor
parent
9c56c73c1a
commit
5575dc1f69
|
@ -75,7 +75,7 @@ public:
|
|||
* Calls the errorFunction closure, which is a std::function object
|
||||
* One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array
|
||||
*/
|
||||
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = OptionalMatrixVecNone) const override;
|
||||
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = nullptr) const override;
|
||||
|
||||
/** print */
|
||||
void print(const std::string &s,
|
||||
|
|
|
@ -99,7 +99,7 @@ protected:
|
|||
* both the function evaluation and its derivative(s) in H.
|
||||
*/
|
||||
Vector unwhitenedError(const Values& x,
|
||||
OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
|
||||
OptionalMatrixVecType H = nullptr) 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
|
||||
|
|
|
@ -29,25 +29,32 @@
|
|||
#include <gtsam/base/utilities.h> // boost::index_sequence
|
||||
|
||||
#include <boost/serialization/base_object.hpp>
|
||||
#include <cstddef>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
// These typedefs and aliases will help with making the evaluateError interface
|
||||
// independent of boost
|
||||
using OptionalNoneType = std::nullptr_t;
|
||||
// TODO: Change this to OptionalMatrixNone
|
||||
/* These typedefs and aliases will help with making the evaluateError interface
|
||||
* independent of boost
|
||||
* TODO(kartikarcot): Change this to OptionalMatrixNone
|
||||
* This typedef is used to indicate that the Jacobian is not required
|
||||
* and the default value used for optional matrix pointer arguments in evaluateError.
|
||||
* Had to use the static_cast of a nullptr, because the compiler is not able to
|
||||
* deduce the type of the nullptr when expanding the evaluateError templates.
|
||||
*/
|
||||
#define OptionalNone static_cast<Matrix*>(nullptr)
|
||||
template <typename T = void>
|
||||
using OptionalMatrixTypeT = Matrix*;
|
||||
template <typename T = void>
|
||||
using MatrixTypeT = Matrix;
|
||||
|
||||
/* This typedef will be used everywhere boost::optional<Matrix&> reference was used
|
||||
* previously. This is used to indicate that the Jacobian is optional. In the future
|
||||
* we will change this to OptionalJacobian
|
||||
*/
|
||||
using OptionalMatrixType = Matrix*;
|
||||
// These typedefs and aliases will help with making the unwhitenedError interface
|
||||
// independent of boost
|
||||
|
||||
/* The OptionalMatrixVecType is a pointer to a vector of matrices. It will
|
||||
* be used in situations where a vector of matrices is optional, like in
|
||||
* unwhitenedError. */
|
||||
using OptionalMatrixVecType = std::vector<Matrix>*;
|
||||
#define OptionalMatrixVecNone static_cast<std::vector<Matrix>*>(nullptr)
|
||||
|
||||
/**
|
||||
* Nonlinear factor base class
|
||||
|
@ -244,7 +251,7 @@ public:
|
|||
* If the optional arguments is specified, it should compute
|
||||
* both the function evaluation and its derivative(s) in H.
|
||||
*/
|
||||
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const = 0;
|
||||
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;
|
||||
|
||||
// support taking in the actual vector instead of the pointer as well
|
||||
// to get access to this version of the function from derived classes
|
||||
|
@ -452,13 +459,22 @@ protected:
|
|||
/* Like std::void_t, except produces `OptionalMatrixType` instead of
|
||||
* `void`. Used to expand fixed-type parameter-packs with same length as
|
||||
* ValueTypes. */
|
||||
template <typename T = void>
|
||||
using OptionalMatrixTypeT = Matrix*;
|
||||
|
||||
/* Like std::void_t, except produces `Key` instead of `void`. Used to expand
|
||||
* fixed-type parameter-packs with same length as ValueTypes. */
|
||||
template <typename T>
|
||||
using KeyType = Key;
|
||||
|
||||
public:
|
||||
/* Like std::void_t, except produces `Matrix` instead of
|
||||
* `void`. Used to expand fixed-type parameter-packs with same length as
|
||||
* ValueTypes. This helps in creating an evaluateError overload that accepts
|
||||
* Matrices instead of pointers to matrices */
|
||||
template <typename T = void>
|
||||
using MatrixTypeT = Matrix;
|
||||
|
||||
public:
|
||||
/**
|
||||
* The type of the I'th template param can be obtained as ValueType<I>.
|
||||
* I is 1-indexed for backwards compatibility/consistency! So for example,
|
||||
|
@ -563,7 +579,7 @@ protected:
|
|||
*/
|
||||
Vector unwhitenedError(
|
||||
const Values& x,
|
||||
OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
|
||||
OptionalMatrixVecType H = nullptr) const override {
|
||||
return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x,
|
||||
H);
|
||||
}
|
||||
|
@ -603,6 +619,7 @@ protected:
|
|||
// one will need to use the "using" keyword and specify that like this:
|
||||
// public:
|
||||
// using NoiseModelFactorN<list the value types here>::evaluateError;
|
||||
|
||||
Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const {
|
||||
return evaluateError(x..., (&H)...);
|
||||
}
|
||||
|
@ -631,7 +648,7 @@ protected:
|
|||
constexpr bool are_all_mat = (... && (std::is_same<Matrix, std::decay_t<OptionalJacArgs>>::value));
|
||||
// The pointers can either be of std::nonetype_t or of Matrix* type
|
||||
constexpr bool are_all_ptrs = (... && (std::is_same<OptionalMatrixType, std::decay_t<OptionalJacArgs>>::value ||
|
||||
std::is_same<OptionalNoneType, std::decay_t<OptionalJacArgs>>::value));
|
||||
std::is_same<std::nullptr_t, std::decay_t<OptionalJacArgs>>::value));
|
||||
static_assert((are_all_mat || are_all_ptrs),
|
||||
"Arguments that are passed to the evaluateError function can only be of following the types: Matrix, "
|
||||
"or Matrix*");
|
||||
|
@ -659,7 +676,7 @@ protected:
|
|||
inline Vector unwhitenedError(
|
||||
boost::mp11::index_sequence<Indices...>, //
|
||||
const Values& x,
|
||||
OptionalMatrixVecType H = OptionalMatrixVecNone) const {
|
||||
OptionalMatrixVecType H = nullptr) const {
|
||||
if (this->active(x)) {
|
||||
if (H) {
|
||||
return evaluateError(x.at<ValueTypes>(keys_[Indices])...,
|
||||
|
|
|
@ -63,7 +63,9 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
|
|||
OptionalMatrixType H2 = OptionalNone) const {
|
||||
std::vector<Matrix> Hs(2);
|
||||
const auto& keys = Factor::keys();
|
||||
const Vector error = Base::unwhitenedError({{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, Hs);
|
||||
const Vector error = Base::unwhitenedError(
|
||||
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
|
||||
Hs);
|
||||
if (H1) *H1 = Hs[0];
|
||||
if (H2) *H2 = Hs[1];
|
||||
return error;
|
||||
|
|
|
@ -145,7 +145,7 @@ public:
|
|||
|
||||
/* ************************************************************************* */
|
||||
Vector whitenedError(const Values& x,
|
||||
OptionalMatrixVecType H = OptionalMatrixVecNone) const {
|
||||
OptionalMatrixVecType H = nullptr) const {
|
||||
|
||||
bool debug = true;
|
||||
|
||||
|
|
|
@ -130,7 +130,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/// Evaluate error h(x)-z and optionally derivatives
|
||||
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
|
||||
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
|
||||
|
||||
Vector a;
|
||||
return a;
|
||||
|
|
|
@ -146,7 +146,7 @@ class SmartRangeFactor: public NoiseModelFactor {
|
|||
/**
|
||||
* Error function *without* the NoiseModel, \f$ z-h(x) \f$.
|
||||
*/
|
||||
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
|
||||
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
|
||||
size_t n = size();
|
||||
if (n < 3) {
|
||||
if (H) {
|
||||
|
|
|
@ -156,7 +156,7 @@ namespace gtsam {
|
|||
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Vector whitenedError(const gtsam::Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const {
|
||||
gtsam::Vector whitenedError(const gtsam::Values& x, OptionalMatrixVecType H = nullptr) const {
|
||||
|
||||
T orgA_T_currA = valA_.at<T>(keyA_);
|
||||
T orgB_T_currB = valB_.at<T>(keyB_);
|
||||
|
|
|
@ -179,7 +179,7 @@ namespace gtsam {
|
|||
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector whitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const {
|
||||
Vector whitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const {
|
||||
|
||||
bool debug = true;
|
||||
|
||||
|
|
Loading…
Reference in New Issue