NonlinearFactor.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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
 
 * -------------------------------------------------------------------------- */
 
// \callgraph
 
#pragma once
 
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/utilities.h>
 
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/base_object.hpp>
#endif
#include <cstddef>
#include <type_traits>
 
namespace gtsam {
 
/* ************************************************************************* */
 
#define OptionalNone static_cast<gtsam::Matrix*>(nullptr)
 
using OptionalMatrixType = Matrix*;
 
using OptionalMatrixVecType = std::vector<Matrix>*;
 
class GTSAM_EXPORT NonlinearFactor: public Factor {
 
protected:
 
  // Some handy typedefs
  typedef Factor Base;
  typedef NonlinearFactor This;
 
public:
 
  typedef std::shared_ptr<This> shared_ptr;
 
 
  NonlinearFactor() {}
 
  template<typename CONTAINER>
  NonlinearFactor(const CONTAINER& keys) :
    Base(keys) {}
 
 
  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
                                            DefaultKeyFormatter) const override;
 
  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
 
 
  virtual double error(const Values& c) const;
 
  double error(const HybridValues& c) const override;
 
  virtual size_t dim() const = 0;
 
  virtual bool active(const Values& /*c*/) const { return true; }
 
  virtual std::shared_ptr<GaussianFactor>
  linearize(const Values& c) const = 0;
 
  virtual shared_ptr clone() const {
    // TODO: choose better exception to throw here
    throw std::runtime_error("NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!");
    return shared_ptr();
  }
 
  virtual shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
 
  virtual shared_ptr rekey(const KeyVector& new_keys) const;
 
   virtual bool sendable() const {
    return true;
  }
 
}; // \class NonlinearFactor
 
template<> struct traits<NonlinearFactor> : public Testable<NonlinearFactor> {
};
 
/* ************************************************************************* */
class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor {
 
protected:
 
  // handy typedefs
  typedef NonlinearFactor Base;
  typedef NoiseModelFactor This;
 
  SharedNoiseModel noiseModel_; 
public:
 
  typedef std::shared_ptr<This> shared_ptr;
 
  NoiseModelFactor() {}
 
  ~NoiseModelFactor() override {}
 
  template<typename CONTAINER>
  NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) :
    Base(keys), noiseModel_(noiseModel) {}
 
protected:
 
  NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}
 
public:
  void print(const std::string& s = "",
    const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
 
  size_t dim() const override {
    return noiseModel_->dim();
  }
 
  const SharedNoiseModel& noiseModel() const {
    return noiseModel_;
  }
 
  virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;
 
  Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) const {
    return unwhitenedError(x, &H);
  }
 
  Vector whitenedError(const Values& c) const;
 
  Vector unweightedWhitenedError(const Values& c) const;
 
  double weight(const Values& c) const;
 
  double error(const Values& c) const override;
 
  std::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
 
  shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
 
 private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("NonlinearFactor",
         boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(noiseModel_);
  }
#endif
 
}; // \class NoiseModelFactor
 
/* ************************************************************************* */
namespace detail {
template <typename, typename...>
struct NoiseModelFactorAliases {};
template <typename T1>
struct NoiseModelFactorAliases<T1> {
  using X = T1;
  using X1 = T1;
};
template <typename T1, typename T2>
struct NoiseModelFactorAliases<T1, T2> {
  using X1 = T1;
  using X2 = T2;
};
template <typename T1, typename T2, typename T3>
struct NoiseModelFactorAliases<T1, T2, T3> {
  using X1 = T1;
  using X2 = T2;
  using X3 = T3;
};
template <typename T1, typename T2, typename T3, typename T4>
struct NoiseModelFactorAliases<T1, T2, T3, T4> {
  using X1 = T1;
  using X2 = T2;
  using X3 = T3;
  using X4 = T4;
};
template <typename T1, typename T2, typename T3, typename T4, typename T5>
struct NoiseModelFactorAliases<T1, T2, T3, T4, T5> {
  using X1 = T1;
  using X2 = T2;
  using X3 = T3;
  using X4 = T4;
  using X5 = T5;
};
template <typename T1, typename T2, typename T3, typename T4, typename T5,
          typename T6, typename... TExtra>
struct NoiseModelFactorAliases<T1, T2, T3, T4, T5, T6, TExtra...> {
  using X1 = T1;
  using X2 = T2;
  using X3 = T3;
  using X4 = T4;
  using X5 = T5;
  using X6 = T6;
};
}  // namespace detail
 
/* ************************************************************************* */
template <class... ValueTypes>
class NoiseModelFactorN
    : public NoiseModelFactor,
      public detail::NoiseModelFactorAliases<ValueTypes...> {
 public:
  enum { N = sizeof...(ValueTypes) };
 
  using NoiseModelFactor::unwhitenedError;
 
protected:
  using Base = NoiseModelFactor;
  using This = NoiseModelFactorN<ValueTypes...>;
 
 
  template <typename From, typename To>
  using IsConvertible =
      typename std::enable_if<std::is_convertible<From, To>::value, void>::type;
 
  template <int I>
  using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N),
                                               void>::type;  // 1-indexed!
 
  template <typename Container>
  using ContainerElementType =
      typename std::decay<decltype(*std::declval<Container>().begin())>::type;
  template <typename Container>
  using IsContainerOfKeys = IsConvertible<ContainerElementType<Container>, Key>;
 
  template <typename Ret, typename ...Args>
  using AreAllMatrixRefs = std::enable_if_t<(... && 
      std::is_convertible<Args, Matrix&>::value), Ret>;
  
  template<typename Arg>
  using IsMatrixPointer = std::is_same<typename std::decay_t<Arg>, Matrix*>;
 
  template<typename Arg>
  using IsNullpointer = std::is_same<typename std::decay_t<Arg>, std::nullptr_t>;
 
  template <typename Ret, typename ...Args>
    using AreAllMatrixPtrs = std::enable_if_t<(... &&
            (IsMatrixPointer<Args>::value || IsNullpointer<Args>::value)), Ret>;
 
 
  /* 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;
 
  /* 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:
  template <int I, typename = IndexIsValid<I>>
  using ValueType =
      typename std::tuple_element<I - 1, std::tuple<ValueTypes...>>::type;
 
 public:
 
 
  NoiseModelFactorN() {}
 
  NoiseModelFactorN(const SharedNoiseModel& noiseModel,
                    KeyType<ValueTypes>... keys)
      : Base(noiseModel, std::array<Key, N>{keys...}) {}
 
  template <typename CONTAINER = std::initializer_list<Key>,
            typename = IsContainerOfKeys<CONTAINER>>
  NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys)
      : Base(noiseModel, keys) {
    if (keys.size() != N) {
      throw std::invalid_argument(
          "NoiseModelFactorN: wrong number of keys given");
    }
  }
 
 
  ~NoiseModelFactorN() override {}
 
  template <int I = 1>
  inline Key key() const {
    static_assert(I <= N, "Index out of bounds");
    return keys_[I - 1];
  }
 
 
  Vector unwhitenedError(
      const Values& x,
      OptionalMatrixVecType H = nullptr) const override {
    return unwhitenedError(gtsam::index_sequence_for<ValueTypes...>{}, x,
                           H);
  }
 
 
  virtual Vector evaluateError(const ValueTypes&... x,
                               OptionalMatrixTypeT<ValueTypes>... H) const = 0;
 
  Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const {
    return evaluateError(x..., (&H)...);
  }
 
 
  inline Vector evaluateError(const ValueTypes&... x) const {
    return evaluateError(x..., OptionalMatrixTypeT<ValueTypes>()...);
  }
 
  template <typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
  inline AreAllMatrixRefs<Vector, OptionalJacArgs...> evaluateError(const ValueTypes&... x,
                                                                  OptionalJacArgs&&... H) const {
    return evaluateError(x..., (&H)...);
  }
 
  template <typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
  inline AreAllMatrixPtrs<Vector, OptionalJacArgs...> evaluateError(const ValueTypes&... x,
                                                                    OptionalJacArgs&&... H) const {
    // If they are pointer version, ensure to cast them all to be Matrix* types
    // This will ensure any arguments inferred as std::nonetype_t are cast to (Matrix*) nullptr
    // This guides the compiler to the correct overload which is the one that takes pointers
    return evaluateError(x..., 
        std::forward<OptionalJacArgs>(H)..., static_cast<OptionalMatrixType>(OptionalNone));
  }
 
 
 private:
  template <std::size_t... Indices>
  inline Vector unwhitenedError(
      gtsam::index_sequence<Indices...>,  //
      const Values& x,
      OptionalMatrixVecType H = nullptr) const {
    if (this->active(x)) {
      if (H) {
        return evaluateError(x.at<ValueTypes>(keys_[Indices])...,
                             (*H)[Indices]...);
      } else {
        return evaluateError(x.at<ValueTypes>(keys_[Indices])...);
      }
    } else {
      return Vector::Zero(this->dim());
    }
  }
 
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    ar& boost::serialization::make_nvp(
        "NoiseModelFactor", boost::serialization::base_object<Base>(*this));
  }
#endif
 
 public:
 
  inline Key key1() const {
    return key<1>();
  }
  template <int I = 2>
  inline Key key2() const {
    static_assert(I <= N, "Index out of bounds");
    return key<2>();
  }
  template <int I = 3>
  inline Key key3() const {
    static_assert(I <= N, "Index out of bounds");
    return key<3>();
  }
  template <int I = 4>
  inline Key key4() const {
    static_assert(I <= N, "Index out of bounds");
    return key<4>();
  }
  template <int I = 5>
  inline Key key5() const {
    static_assert(I <= N, "Index out of bounds");
    return key<5>();
  }
  template <int I = 6>
  inline Key key6() const {
    static_assert(I <= N, "Index out of bounds");
    return key<6>();
  }
 
 
};  // \class NoiseModelFactorN
 
#define NoiseModelFactor1 NoiseModelFactorN
#define NoiseModelFactor2 NoiseModelFactorN
#define NoiseModelFactor3 NoiseModelFactorN
#define NoiseModelFactor4 NoiseModelFactorN
#define NoiseModelFactor5 NoiseModelFactorN
#define NoiseModelFactor6 NoiseModelFactorN
 
}  // namespace gtsam