147 lines
4.0 KiB
C++
147 lines
4.0 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 NonlinearInequality.h
|
|
* @brief
|
|
* @author Duy-Nguyen Ta
|
|
* @date Sep 30, 2013
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam_unstable/nonlinear/NonlinearConstraint.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
/** A convenient base class for creating a nonlinear equality constraint with 1
|
|
* variables. To derive from this class, implement evaluateError(). */
|
|
template<class VALUE>
|
|
class NonlinearInequality1: public NonlinearConstraint1<VALUE> {
|
|
|
|
public:
|
|
|
|
// typedefs for value types pulled from keys
|
|
typedef VALUE X;
|
|
|
|
protected:
|
|
|
|
typedef NonlinearConstraint1<VALUE> Base;
|
|
typedef NonlinearInequality1<VALUE> This;
|
|
|
|
private:
|
|
static const int X1Dim = traits<VALUE>::dimension;
|
|
|
|
public:
|
|
|
|
/**
|
|
* Default Constructor for I/O
|
|
*/
|
|
NonlinearInequality1() {
|
|
}
|
|
|
|
/**
|
|
* Constructor
|
|
* @param j key of the variable
|
|
* @param constraintDim number of dimensions of the constraint error function
|
|
*/
|
|
NonlinearInequality1(Key key, Key dualKey) :
|
|
Base(key, dualKey, 1) {
|
|
}
|
|
|
|
virtual ~NonlinearInequality1() {
|
|
}
|
|
|
|
/**
|
|
* Override this method to finish implementing a binary factor.
|
|
* If any of the optional Matrix reference arguments are specified, it should compute
|
|
* both the function evaluation and its derivative(s) in X1 (and/or X2).
|
|
*/
|
|
virtual double
|
|
computeError(const X&, boost::optional<Matrix&> H1 = boost::none) const = 0;
|
|
|
|
/** predefine evaluateError to return a 1-dimension vector */
|
|
virtual Vector
|
|
evaluateError(const X& x, boost::optional<Matrix&> H1 = boost::none) const {
|
|
return (Vector(1) << computeError(x, H1)).finished();
|
|
}
|
|
//
|
|
// virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x,
|
|
// const VectorValues& duals) const {
|
|
// return Base::multipliedHessian(x, duals);
|
|
// }
|
|
|
|
};
|
|
// \class NonlinearConstraint1
|
|
|
|
/* ************************************************************************* */
|
|
/** A convenient base class for creating your own NonlinearConstraint with 2
|
|
* variables. To derive from this class, implement evaluateError(). */
|
|
template<class VALUE1, class VALUE2>
|
|
class NonlinearInequality2: public NonlinearConstraint2<VALUE1, VALUE2> {
|
|
|
|
public:
|
|
|
|
// typedefs for value types pulled from keys
|
|
typedef VALUE1 X1;
|
|
typedef VALUE2 X2;
|
|
|
|
protected:
|
|
|
|
typedef NonlinearConstraint2<VALUE1, VALUE2> Base;
|
|
typedef NonlinearInequality2<VALUE1, VALUE2> This;
|
|
|
|
private:
|
|
static const int X1Dim = traits<VALUE1>::dimension;
|
|
static const int X2Dim = traits<VALUE2>::dimension;
|
|
|
|
public:
|
|
|
|
/**
|
|
* Default Constructor for I/O
|
|
*/
|
|
NonlinearInequality2() {
|
|
}
|
|
|
|
/**
|
|
* Constructor
|
|
* @param j1 key of the first variable
|
|
* @param j2 key of the second variable
|
|
* @param constraintDim number of dimensions of the constraint error function
|
|
*/
|
|
NonlinearInequality2(Key j1, Key j2, Key dualKey) :
|
|
Base(j1, j2, 1) {
|
|
}
|
|
|
|
virtual ~NonlinearInequality2() {
|
|
}
|
|
|
|
/**
|
|
* Override this method to finish implementing a binary factor.
|
|
* If any of the optional Matrix reference arguments are specified, it should compute
|
|
* both the function evaluation and its derivative(s) in X1 (and/or X2).
|
|
*/
|
|
virtual double
|
|
computeError(const X1&, const X2&, boost::optional<Matrix&> H1 = boost::none,
|
|
boost::optional<Matrix&> H2 = boost::none) const = 0;
|
|
|
|
/** predefine evaluateError to return a 1-dimension vector */
|
|
virtual Vector
|
|
evaluateError(const X1& x1, const X2& x2, boost::optional<Matrix&> H1 = boost::none,
|
|
boost::optional<Matrix&> H2 = boost::none) const {
|
|
return (Vector(1) << computeError(x1, x2, H1, H2)).finished();
|
|
}
|
|
};
|
|
// \class NonlinearConstraint2
|
|
|
|
|
|
} /* namespace gtsam */
|