NonlinearConstraint.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/inference/FactorGraph.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/base_object.hpp>
#endif
 
namespace gtsam {
 
class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor {
 public:
  typedef NoiseModelFactor Base;
 
  using Base::Base;
 
  using NonlinearFactor::error;
 
  virtual ~NonlinearConstraint() {}
 
  virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const {
    return cloneWithNewNoiseModel(penaltyNoise(mu));
  }
 
  virtual double violation(const Values& x) const { return sqrt(2 * error(x)); }
 
  virtual bool feasible(const Values& x, const double tolerance = 1e-5) const {
    return violation(x) <= tolerance;
  }
 
  const Vector sigmas() const { return noiseModel()->sigmas(); }
 
  // return the hessian of unwhitened error function in each dimension.
  virtual std::vector<Matrix> unwhitenedHessian(const Values& x) const {
    throw std::runtime_error("hessian not implemented");
  }
 
 protected:
  SharedDiagonal penaltyNoise(const double mu) const {
    return noiseModel::Diagonal::Sigmas(noiseModel()->sigmas() / sqrt(mu));
  }
 
  static SharedNoiseModel constrainedNoise(const Vector& sigmas) {
    return noiseModel::Constrained::MixedSigmas(1.0, sigmas);
  }
 
 private:
#if 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("NonlinearConstraint",
                                       boost::serialization::base_object<Base>(*this));
  }
#endif
};
 
}  // namespace gtsam