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 <boost/serialization/base_object.hpp>
#include <boost/assign/list_of.hpp>

#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Factor.h>


#define ADD_CLONE_NONLINEAR_FACTOR(Derived) \
  virtual gtsam::NonlinearFactor::shared_ptr clone() const { \
  return boost::static_pointer_cast<gtsam::NonlinearFactor>( \
      gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); }

namespace gtsam {

using boost::assign::cref_list_of;

/* ************************************************************************* */
class NonlinearFactor: public Factor {

protected:

  // Some handy typedefs
  typedef Factor Base;
  typedef NonlinearFactor This;

public:

  typedef boost::shared_ptr<This> shared_ptr;


  NonlinearFactor() {}

  template<typename CONTAINER>
  NonlinearFactor(const CONTAINER& keys) :
    Base(keys) {}


  virtual void print(const std::string& s = "",
    const KeyFormatter& keyFormatter = DefaultKeyFormatter) const
  {
    std::cout << s << "  keys = { ";
    BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
    std::cout << "}" << std::endl;
  }

  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
    return Base::equals(f);
  }


  virtual ~NonlinearFactor() {}


  virtual double error(const Values& c) const = 0;

  virtual size_t dim() const = 0;

  virtual bool active(const Values& c) const { return true; }

  virtual boost::shared_ptr<GaussianFactor>
  linearize(const Values& c) const = 0;

  //virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
  //  std::vector<Index> indices(this->size());
  //  for(size_t j=0; j<this->size(); ++j)
  //    indices[j] = ordering[this->keys()[j]];
  //  return IndexFactor::shared_ptr(new IndexFactor(indices));
  //}

  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();
  }

  shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const {
    shared_ptr new_factor = clone();
    for (size_t i=0; i<new_factor->size(); ++i) {
      Key& cur_key = new_factor->keys()[i];
      std::map<Key,Key>::const_iterator mapping = rekey_mapping.find(cur_key);
      if (mapping != rekey_mapping.end())
        cur_key = mapping->second;
    }
    return new_factor;
  }

  shared_ptr rekey(const std::vector<Key>& new_keys) const {
    assert(new_keys.size() == this->keys().size());
    shared_ptr new_factor = clone();
    new_factor->keys() = new_keys;
    return new_factor;
  }


}; // \class NonlinearFactor

/* ************************************************************************* */
class NoiseModelFactor: public NonlinearFactor {

protected:

  // handy typedefs
  typedef NonlinearFactor Base;
  typedef NoiseModelFactor This;

  SharedNoiseModel noiseModel_; 
public:

  typedef boost::shared_ptr<This> shared_ptr;

  NoiseModelFactor() {}

  virtual ~NoiseModelFactor() {}

  template<typename CONTAINER>
  NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) :
    Base(keys), noiseModel_(noiseModel) {}

protected:

  NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}

public:

  virtual void print(const std::string& s = "",
    const KeyFormatter& keyFormatter = DefaultKeyFormatter) const
  {
    Base::print(s, keyFormatter);
    this->noiseModel_->print("  noise model: ");
  }

  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
    const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
    return e && Base::equals(f, tol) &&
      ((!noiseModel_ && !e->noiseModel_) ||
      (noiseModel_ && e->noiseModel_ && noiseModel_->equals(*e->noiseModel_, tol)));
  }

  virtual size_t dim() const {
    return noiseModel_->dim();
  }

  SharedNoiseModel get_noiseModel() const {
    return noiseModel_;
  }

  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;

  Vector whitenedError(const Values& c) const {
    const Vector unwhitenedErrorVec = unwhitenedError(c);
    if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
      throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
    return noiseModel_->whiten(unwhitenedErrorVec);
  }

  virtual double error(const Values& c) const {
    if (this->active(c)) {
      const Vector unwhitenedErrorVec = unwhitenedError(c);
      if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
        throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
      return 0.5 * noiseModel_->distance(unwhitenedErrorVec);
    } else {
      return 0.0;
    }
  }

  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
    // Only linearize if the factor is active
    if (!this->active(x))
      return boost::shared_ptr<JacobianFactor>();

    // Create the set of terms - Jacobians for each index
    Vector b;
    // Call evaluate error to get Jacobians and b vector
    std::vector<Matrix> A(this->size());
    b = -unwhitenedError(x, A);
    if(noiseModel_)
    {
      if((size_t) b.size() != noiseModel_->dim())
        throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");

      this->noiseModel_->WhitenSystem(A,b);
    }

    std::vector<std::pair<Key, Matrix> > terms(this->size());
    // Fill in terms
    for(size_t j=0; j<this->size(); ++j) {
      terms[j].first = this->keys()[j];
      terms[j].second.swap(A[j]);
    }

    if(noiseModel_)
    {
      // TODO pass unwhitened + noise model to Gaussian factor
      noiseModel::Constrained::shared_ptr constrained =
          boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
      if(constrained)
        return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit()));
      else
        return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
    }
    else
      return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
  }

private:

  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_);
  }

}; // \class NoiseModelFactor


/* ************************************************************************* */
template<class VALUE>
class NoiseModelFactor1: public NoiseModelFactor {

public:

  // typedefs for value types pulled from keys
  typedef VALUE X;

protected:

  typedef NoiseModelFactor Base;
  typedef NoiseModelFactor1<VALUE> This;

public:

  NoiseModelFactor1() {}

  virtual ~NoiseModelFactor1() {}

  inline Key key() const { return keys_[0]; }

  NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) :
    Base(noiseModel, cref_list_of<1>(key1)) {}

  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
    if(this->active(x)) {
      const X& x1 = x.at<X>(keys_[0]);
      if(H) {
        return evaluateError(x1, (*H)[0]);
      } else {
        return evaluateError(x1);
      }
    } else {
      return zero(this->dim());
    }
  }

  virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
      boost::none) const = 0;

private:

  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));
  }
};// \class NoiseModelFactor1


/* ************************************************************************* */
template<class VALUE1, class VALUE2>
class NoiseModelFactor2: public NoiseModelFactor {

public:

  // typedefs for value types pulled from keys
  typedef VALUE1 X1;
  typedef VALUE2 X2;

protected:

  typedef NoiseModelFactor Base;
  typedef NoiseModelFactor2<VALUE1, VALUE2> This;

public:

  NoiseModelFactor2() {}

  NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
    Base(noiseModel, cref_list_of<2>(j1)(j2)) {}

  virtual ~NoiseModelFactor2() {}

  inline Key key1() const { return keys_[0];  }
  inline Key key2() const {  return keys_[1];  }

  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
    if(this->active(x)) {
      const X1& x1 = x.at<X1>(keys_[0]);
      const X2& x2 = x.at<X2>(keys_[1]);
      if(H) {
        return evaluateError(x1, x2, (*H)[0], (*H)[1]);
      } else {
        return evaluateError(x1, x2);
      }
    } else {
      return zero(this->dim());
    }
  }

  virtual Vector
  evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 =
      boost::none, boost::optional<Matrix&> H2 = boost::none) const = 0;

private:

  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));
  }
}; // \class NoiseModelFactor2

/* ************************************************************************* */
template<class VALUE1, class VALUE2, class VALUE3>
class NoiseModelFactor3: public NoiseModelFactor {

public:

  // typedefs for value types pulled from keys
  typedef VALUE1 X1;
  typedef VALUE2 X2;
  typedef VALUE3 X3;

protected:

  typedef NoiseModelFactor Base;
  typedef NoiseModelFactor3<VALUE1, VALUE2, VALUE3> This;

public:

  NoiseModelFactor3() {}

  NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) :
    Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {}

  virtual ~NoiseModelFactor3() {}

  inline Key key1() const { return keys_[0]; }
  inline Key key2() const { return keys_[1]; }
  inline Key key3() const { return keys_[2]; }

  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
    if(this->active(x)) {
      if(H)
        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
      else
        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]));
    } else {
      return zero(this->dim());
    }
  }

  virtual Vector
  evaluateError(const X1&, const X2&, const X3&,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none,
      boost::optional<Matrix&> H3 = boost::none) const = 0;

private:

  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));
  }
}; // \class NoiseModelFactor3

/* ************************************************************************* */
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
class NoiseModelFactor4: public NoiseModelFactor {

public:

  // typedefs for value types pulled from keys
  typedef VALUE1 X1;
  typedef VALUE2 X2;
  typedef VALUE3 X3;
  typedef VALUE4 X4;

protected:

  typedef NoiseModelFactor Base;
  typedef NoiseModelFactor4<VALUE1, VALUE2, VALUE3, VALUE4> This;

public:

  NoiseModelFactor4() {}

  NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) :
    Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4)) {}

  virtual ~NoiseModelFactor4() {}

  inline Key key1() const { return keys_[0]; }
  inline Key key2() const { return keys_[1]; }
  inline Key key3() const { return keys_[2]; }
  inline Key key4() const { return keys_[3]; }

  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
    if(this->active(x)) {
      if(H)
        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
      else
        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]));
    } else {
      return zero(this->dim());
    }
  }

  virtual Vector
  evaluateError(const X1&, const X2&, const X3&, const X4&,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none,
      boost::optional<Matrix&> H3 = boost::none,
      boost::optional<Matrix&> H4 = boost::none) const = 0;

private:

  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));
  }
}; // \class NoiseModelFactor4

/* ************************************************************************* */
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
class NoiseModelFactor5: public NoiseModelFactor {

public:

  // typedefs for value types pulled from keys
  typedef VALUE1 X1;
  typedef VALUE2 X2;
  typedef VALUE3 X3;
  typedef VALUE4 X4;
  typedef VALUE5 X5;

protected:

  typedef NoiseModelFactor Base;
  typedef NoiseModelFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5> This;

public:

  NoiseModelFactor5() {}

  NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) :
    Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {}

  virtual ~NoiseModelFactor5() {}

  inline Key key1() const { return keys_[0]; }
  inline Key key2() const { return keys_[1]; }
  inline Key key3() const { return keys_[2]; }
  inline Key key4() const { return keys_[3]; }
  inline Key key5() const { return keys_[4]; }

  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
    if(this->active(x)) {
      if(H)
        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
      else
        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]));
    } else {
      return zero(this->dim());
    }
  }

  virtual Vector
  evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none,
      boost::optional<Matrix&> H3 = boost::none,
      boost::optional<Matrix&> H4 = boost::none,
      boost::optional<Matrix&> H5 = boost::none) const = 0;

private:

  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));
  }
}; // \class NoiseModelFactor5

/* ************************************************************************* */
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
class NoiseModelFactor6: public NoiseModelFactor {

public:

  // typedefs for value types pulled from keys
  typedef VALUE1 X1;
  typedef VALUE2 X2;
  typedef VALUE3 X3;
  typedef VALUE4 X4;
  typedef VALUE5 X5;
  typedef VALUE6 X6;

protected:

  typedef NoiseModelFactor Base;
  typedef NoiseModelFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6> This;

public:

  NoiseModelFactor6() {}

  NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
    Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {}

  virtual ~NoiseModelFactor6() {}

  inline Key key1() const { return keys_[0]; }
  inline Key key2() const { return keys_[1]; }
  inline Key key3() const { return keys_[2]; }
  inline Key key4() const { return keys_[3]; }
  inline Key key5() const { return keys_[4]; }
  inline Key key6() const { return keys_[5]; }

  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
    if(this->active(x)) {
      if(H)
        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
      else
        return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]));
    } else {
      return zero(this->dim());
    }
  }

  virtual Vector
  evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none,
      boost::optional<Matrix&> H3 = boost::none,
      boost::optional<Matrix&> H4 = boost::none,
      boost::optional<Matrix&> H5 = boost::none,
      boost::optional<Matrix&> H6 = boost::none) const = 0;

private:

  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));
  }
}; // \class NoiseModelFactor6

/* ************************************************************************* */

} // \namespace gtsam