BearingFactor.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
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <gtsam/nonlinear/ExpressionFactor.h>
 
namespace gtsam {
 
// forward declaration of Bearing functor, assumed partially specified
template <typename A1, typename A2>
struct Bearing;
 
template <typename A1, typename A2,
          typename T = typename Bearing<A1, A2>::result_type>
struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
  typedef ExpressionFactorN<T, A1, A2> Base;
 
  BearingFactor() {}
 
  BearingFactor(Key key1, Key key2, const T& measured,
                const SharedNoiseModel& model)
      : Base({key1, key2}, model, measured) {
    this->initialize(expression({key1, key2}));
  }
 
  // Return measurement expression
  Expression<T> expression(const typename Base::ArrayNKeys &keys) const override {
    Expression<A1> a1_(keys[0]);
    Expression<A2> a2_(keys[1]);
    return Expression<T>(Bearing<A1, A2>(), a1_, a2_);
  }
 
  void print(const std::string& s = "",
             const KeyFormatter& kf = DefaultKeyFormatter) const override {
    std::cout << s << "BearingFactor" << std::endl;
    Base::print(s, kf);
  }
  
  Vector evaluateError(const A1& a1, const A2& a2,
    boost::optional<Matrix&> H1 = boost::none,
    boost::optional<Matrix&> H2 = boost::none) const
  {
    std::vector<Matrix> Hs(2);
    const auto &keys = Factor::keys();
    const Vector error = unwhitenedError(
      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
      Hs);
    if (H1) *H1 = Hs[0];
    if (H2) *H2 = Hs[1];
    return error;
  }
 
 
 private:
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    ar& boost::serialization::make_nvp(
        "Base", boost::serialization::base_object<Base>(*this));
  }
};  // BearingFactor
 
template <typename A1, typename A2, typename T>
struct traits<BearingFactor<A1, A2, T> >
    : public Testable<BearingFactor<A1, A2, T> > {};
 
}  // namespace gtsam