BoundingConstraint.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/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

namespace gtsam {

template<class VALUE>
struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {
  typedef VALUE X;
  typedef NoiseModelFactor1<VALUE> Base;
  typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;

  double threshold_;
  bool isGreaterThan_; 

  BoundingConstraint1(Key key, double threshold,
      bool isGreaterThan, double mu = 1000.0) :
        Base(noiseModel::Constrained::All(1, mu), key),
        threshold_(threshold), isGreaterThan_(isGreaterThan) {
  }

  virtual ~BoundingConstraint1() {}

  inline double threshold() const { return threshold_; }
  inline bool isGreaterThan() const { return isGreaterThan_; }

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

  bool active(const Values& c) const {
    // note: still active at equality to avoid zigzagging
    double x = value(c.at<X>(this->key()));
    return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
  }

  Vector evaluateError(const X& x, boost::optional<Matrix&> H =
      boost::none) const {
    Matrix D;
    double error = value(x, D) - threshold_;
    if (H) {
      if (isGreaterThan_) *H = D;
      else *H = -1.0 * D;
    }

    if (isGreaterThan_)
      return (Vector(1) << error).finished();
    else
      return -1.0 * (Vector(1) << error).finished();
  }

private:

  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("NoiseModelFactor1",
        boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(threshold_);
    ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
  }
};

template<class VALUE1, class VALUE2>
struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
  typedef VALUE1 X1;
  typedef VALUE2 X2;

  typedef NoiseModelFactor2<VALUE1, VALUE2> Base;
  typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;

  double threshold_;
  bool isGreaterThan_; 

  BoundingConstraint2(Key key1, Key key2, double threshold,
      bool isGreaterThan, double mu = 1000.0)
  : Base(noiseModel::Constrained::All(1, mu), key1, key2),
    threshold_(threshold), isGreaterThan_(isGreaterThan) {}

  virtual ~BoundingConstraint2() {}

  inline double threshold() const { return threshold_; }
  inline bool isGreaterThan() const { return isGreaterThan_; }

  virtual double value(const X1& x1, const X2& x2,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none) const = 0;

  bool active(const Values& c) const {
    // note: still active at equality to avoid zigzagging
    double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
    return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
  }

  Vector evaluateError(const X1& x1, const X2& x2,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none) const {
    Matrix D1, D2;
    double error = value(x1, x2, D1, D2) - threshold_;
    if (H1) {
      if (isGreaterThan_)  *H1 = D1;
      else *H1 = -1.0 * D1;
    }
    if (H2) {
      if (isGreaterThan_) *H2 = D2;
      else *H2 = -1.0 * D2;
    }

    if (isGreaterThan_)
      return (Vector(1) << error).finished();
    else
      return -1.0 * (Vector(1) << error).finished();
  }

private:

  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("NoiseModelFactor2",
        boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(threshold_);
    ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
  }
};

} // \namespace gtsam