NonlinearEquality.h

/* ----------------------------------------------------------------------------

 * 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 NonlinearEquality.h
 * @brief Factor to handle enforced equality between factors
 * @author Alex Cunningham
 */

#pragma once

#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>

#include <limits>
#include <iostream>
#include <cmath>

namespace gtsam {

template<class VALUE>
class NonlinearEquality: public NoiseModelFactorN<VALUE> {

public:
  typedef VALUE T;

  // Provide access to the Matrix& version of evaluateError:
  using NoiseModelFactor1<VALUE>::evaluateError;

private:

  // feasible value
  T feasible_;

  // error handling flag
  bool allow_error_;

  // error gain in allow error case
  double error_gain_;

  // typedef to this class
  using This = NonlinearEquality<VALUE>;

  // typedef to base class
  using Base = NoiseModelFactorN<VALUE>;

public:

  using CompareFunction = std::function<bool(const T&, const T&)>;
  CompareFunction compare_;

  NonlinearEquality() {
  }

  ~NonlinearEquality() override {
  }


  NonlinearEquality(Key j, const T& feasible,
      const CompareFunction &_compare = std::bind(traits<T>::Equals,
          std::placeholders::_1, std::placeholders::_2, 1e-9)) :
      Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
          j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
      compare_(_compare) {
  }

  NonlinearEquality(Key j, const T& feasible, double error_gain,
      const CompareFunction &_compare = std::bind(traits<T>::Equals,
          std::placeholders::_1, std::placeholders::_2, 1e-9)) :
      Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
          j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
      compare_(_compare) {
  }


  void print(const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    std::cout << (s.empty() ? s : s + " ") << "Constraint: on ["
              << keyFormatter(this->key()) << "]\n";
    traits<VALUE>::Print(feasible_, "Feasible Point:\n");
    std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_)
              << std::endl;
  }

  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
    const This* e = dynamic_cast<const This*>(&f);
    return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol)
        && std::abs(error_gain_ - e->error_gain_) < tol;
  }


  double error(const Values& c) const override {
    const T& xj = c.at<T>(this->key());
    Vector e = this->unwhitenedError(c);
    if (allow_error_ || !compare_(xj, feasible_)) {
      return error_gain_ * dot(e, e);
    } else {
      return 0.0;
    }
  }

  Vector evaluateError(const T& xj, OptionalMatrixType H) const override {
    const size_t nj = traits<T>::GetDimension(feasible_);
    if (allow_error_) {
      if (H)
        *H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare
      return traits<T>::Local(xj,feasible_);
    } else if (compare_(feasible_, xj)) {
      if (H)
        *H = Matrix::Identity(nj,nj);
      return Vector::Zero(nj); // set error to zero if equal
    } else {
      if (H)
        throw std::invalid_argument(
            "Linearization point not feasible for "
                + DefaultKeyFormatter(this->key()) + "!");
      return Vector::Constant(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
    }
  }

  GaussianFactor::shared_ptr linearize(const Values& x) const override {
    const T& xj = x.at<T>(this->key());
    Matrix A;
    Vector b = evaluateError(xj, A);
    SharedDiagonal model = noiseModel::Constrained::All(b.size());
    return GaussianFactor::shared_ptr(
        new JacobianFactor(this->key(), A, b, model));
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
  }


  GTSAM_MAKE_ALIGNED_OPERATOR_NEW

private:

#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
    ar
        & boost::serialization::make_nvp("NoiseModelFactor1",
            boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(feasible_);
    ar & BOOST_SERIALIZATION_NVP(allow_error_);
    ar & BOOST_SERIALIZATION_NVP(error_gain_);
  }
#endif

};
// \class NonlinearEquality

template <typename VALUE>
struct traits<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {};

/* ************************************************************************* */
template<class VALUE>
class NonlinearEquality1: public NoiseModelFactorN<VALUE> {

public:
  typedef VALUE X;

  // Provide access to Matrix& version of evaluateError:
  using NoiseModelFactor1<VALUE>::evaluateError;

protected:
  typedef NoiseModelFactorN<VALUE> Base;
  typedef NonlinearEquality1<VALUE> This;

  NonlinearEquality1() {
  }

  X value_; 

  GTSAM_CONCEPT_MANIFOLD_TYPE(X)
  GTSAM_CONCEPT_TESTABLE_TYPE(X)

public:

  typedef std::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;

  NonlinearEquality1(const X& value, Key key, double mu = 1000.0)
      : Base(noiseModel::Constrained::All(traits<X>::GetDimension(value),
                                          std::abs(mu)),
             key),
        value_(value) {}

  ~NonlinearEquality1() override {
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
  }

  Vector evaluateError(const X& x1, OptionalMatrixType H) const override {
    if (H)
      (*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
    // manifold equivalent of h(x)-z -> log(z,h(x))
    return traits<X>::Local(value_,x1);
  }

  void print(const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
        << ")," << "\n";
    this->noiseModel_->print();
    traits<X>::Print(value_, "Value");
  }

  GTSAM_MAKE_ALIGNED_OPERATOR_NEW

private:

#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
    ar
        & boost::serialization::make_nvp("NoiseModelFactor1",
            boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(value_);
  }
#endif
};
// \NonlinearEquality1

template <typename VALUE>
struct traits<NonlinearEquality1<VALUE> >
    : Testable<NonlinearEquality1<VALUE> > {};

/* ************************************************************************* */
template <class T>
class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
 protected:
  using Base = NoiseModelFactorN<T, T>;
  using This = NonlinearEquality2<T>;

  GTSAM_CONCEPT_MANIFOLD_TYPE(T)

  
  NonlinearEquality2() {}

 public:
  typedef std::shared_ptr<NonlinearEquality2<T>> shared_ptr;

  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;


  NonlinearEquality2(Key key1, Key key2, double mu = 1e4)
      : Base(noiseModel::Constrained::All(traits<T>::dimension, std::abs(mu)),
             key1, key2) {}
  ~NonlinearEquality2() override {}

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
  }

  Vector evaluateError(
      const T& x1, const T& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override {
    static const size_t p = traits<T>::dimension;
    if (H1) *H1 = -Matrix::Identity(p, p);
    if (H2) *H2 = Matrix::Identity(p, p);
    return traits<T>::Local(x1, x2);
  }

  GTSAM_MAKE_ALIGNED_OPERATOR_NEW

 private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
    ar& boost::serialization::make_nvp(
        "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
  }
#endif
};
// \NonlinearEquality2

template <typename VALUE>
struct traits<NonlinearEquality2<VALUE>> : Testable<NonlinearEquality2<VALUE>> {
};

}// namespace gtsam