12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

clean up and refactor NonlinearEquality and BoundingConstraint

release/4.3a0
yetongumich 2024-08-06 15:42:09 -04:00
parent 80fcff1ed5
commit 9b0fa1210e
14 changed files with 363 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/constraint/NonlinearConstraint.h
View File

@ -11,7 +11,7 @@
/**
* @file NonlinearConstraint.h
* @brief Equality constraints in constrained optimization.
* @brief Nonlinear constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @date Aug 3, 2024
*/

4
gtsam/constraint/NonlinearEqualityConstraint-inl.h
View File

@ -10,8 +10,8 @@
* -------------------------------------------------------------------------- */
/**
* @file EqualityConstraint-inl.h
* @brief Equality constraints in constrained optimization.
* @file NonlinearEqualityConstraint-inl.h
* @brief Nonlinear equality constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @date Aug 3, 2024 */

4
gtsam/constraint/NonlinearEqualityConstraint.cpp
View File

@ -10,8 +10,8 @@
* -------------------------------------------------------------------------- */
/**
* @file EqualityConstraint.cpp
* @brief Equality constraints in constrained optimization.
* @file NonlinearEqualityConstraint.cpp
* @brief Nonlinear equality constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @date Aug 3, 2024 */

19
gtsam/constraint/NonlinearEqualityConstraint.h
View File

@ -10,8 +10,8 @@
* -------------------------------------------------------------------------- */
/**
* @file EqualityConstraint.h
* @brief Equality constraints in constrained optimization.
* @file NonlinearEqualityConstraint.h
* @brief Nonlinear equality constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @date Aug 3, 2024 */
@ -20,9 +20,6 @@
#include <gtsam/constraint/NonlinearConstraint.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/base_object.hpp>
#endif
namespace gtsam {
@ -81,6 +78,12 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint {
const Expression<T>& expression() const { return expression_; }
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
@ -119,6 +122,12 @@ class ZeroCostConstraint : public NonlinearEqualityConstraint {
virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */

17
gtsam/constraint/NonlinearInequalityConstraint.cpp
View File

@ -17,8 +17,7 @@
*/
#include <gtsam/constraint/NonlinearInequalityConstraint.h>
#include <memory>
#include "gtsam/constraint/RampFunction.h"
#include <gtsam/constraint/RampFunction.h>
namespace gtsam {
@ -44,6 +43,20 @@ bool NonlinearInequalityConstraint::active(const Values& x) const {
return (unwhitenedExpr(x).array() >= 0).any();
}
/* ************************************************************************* */
NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorSmooth(
SmoothRampFunction::shared_ptr func, const double mu) const {
/// Default behavior, this function should be overriden.
return penaltyFactor(mu);
}
/* ************************************************************************* */
NonlinearEqualityConstraint::shared_ptr NonlinearInequalityConstraint::createEqualityConstraint()
const {
/// Default behavior, this function should be overriden.
return nullptr;
}
/* ************************************************************************* */
NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality(
const double mu) const {

40
gtsam/constraint/NonlinearInequalityConstraint.h
View File

@ -25,7 +25,7 @@
namespace gtsam {
/**
* Inequality constraint base class.
* Inequality constraint base class, enforcing g(x) <= 0.
*/
class NonlinearInequalityConstraint : public NonlinearConstraint {
public:
@ -39,17 +39,21 @@ class NonlinearInequalityConstraint : public NonlinearConstraint {
/** Destructor. */
virtual ~NonlinearInequalityConstraint() {}
/** Return g(x). */
virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;
/** Return ramp(g(x)). */
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
/** Return true if g(x)>=0 in any dimension. */
virtual bool active(const Values& x) const override;
virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const = 0;
/** Return an equality constraint corresponding to g(x)=0. */
virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const;
/** Smooth approximation of the ramp function. */
virtual NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func,
const double mu = 1.0) const = 0;
const double mu = 1.0) const;
/** penalty function as if the constraint is equality, 0.5 * mu * ||g(x)||^2 */
virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const;
@ -67,7 +71,8 @@ class NonlinearInequalityConstraint : public NonlinearConstraint {
};
/** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued
* function. */
* nonlinear function.
*/
class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint {
public:
typedef NonlinearInequalityConstraint Base;
@ -82,32 +87,45 @@ class ScalarExpressionInequalityConstraint : public NonlinearInequalityConstrain
/**
* @brief Constructor.
*
* @param expression expression representing g(x).
* @param expression expression representing g(x) (or -g(x) for GeqZero).
* @param sigma scalar representing sigma.
*/
ScalarExpressionInequalityConstraint(const Double_& expression, const double& sigma);
// Create an inequality constraint g(x)>=0.
/** Create an inequality constraint g(x)/sigma >= 0, internally represented as -g(x)/sigma <= 0.
*/
static ScalarExpressionInequalityConstraint::shared_ptr GeqZero(const Double_& expression,
const double& sigma);
// Create an inequality constraint g(x)<=0.
/** Create an inequality constraint g(x)/sigma <= 0. */
static ScalarExpressionInequalityConstraint::shared_ptr LeqZero(const Double_& expression,
const double& sigma);
/** Compute g(x), or -g(x) for objects constructed from GeqZero. */
virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override;
/** Equality constraint representing g(x)/sigma = 0. */
NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const override;
/** Penalty function 0.5*mu*||ramp(g(x)/sigma||^2. */
NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
/** Penalty function using a smooth approxiamtion of the ramp funciton. */
NoiseModelFactor::shared_ptr penaltyFactorSmooth(SmoothRampFunction::shared_ptr func,
const double mu = 1.0) const override;
/** Penalty function as if the constraint is equality, 0.5 * mu * ||g(x)/sigma||^2. */
virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override;
/** Return expression g(x), or -g(x) for objects constructed from GeqZero. */
const Double_& expression() const { return expression_; }
/** return a deep copy of this factor. */
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
@ -131,17 +149,19 @@ class NonlinearInequalityConstraints : public FactorGraph<NonlinearInequalityCon
using Base::Base;
/// Return the total dimension of constraints.
/** Return the total dimension of constraints. */
size_t dim() const;
/// Evaluate the constraint violation as a vector
/** Evaluate the constraint violation as a vector. */
Vector violationVector(const Values& values, bool whiten = true) const;
/// Evaluate the constraint violation (as L2 norm).
/** Evaluate the constraint violation (as L2 norm). */
double violationNorm(const Values& values) const;
/** Return the penalty function corresponding to \sum_i||ramp(g_i(x))||^2. */
NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const;
/** Return the penalty function constructed using smooth approximations of the ramp function. */
NonlinearFactorGraph penaltyGraphSmooth(SmoothRampFunction::shared_ptr func,
const double mu = 1.0) const;

32
gtsam/constraint/RampFunction.cpp
View File

@ -10,9 +10,9 @@
* -------------------------------------------------------------------------- */
/**
* @file NonlinearInequalityConstraint.h
* @brief Nonlinear inequality constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @file RampFunction.cpp
* @brief Ramp function to compute inequality constraint violations.
* @author Yetong Zhang
* @date Aug 3, 2024
*/
@ -41,7 +41,7 @@ SmoothRampFunction::UnaryScalarFunc SmoothRampFunction::function() const {
}
/* ************************************************************************* */
double PolynomialRampFunction::operator()(const double& x, OptionalJacobian<1, 1> H) const {
double RampFunctionPoly2::operator()(const double& x, OptionalJacobian<1, 1> H) const {
if (x <= 0) {
if (H) {
H->setZero();
@ -49,9 +49,29 @@ double PolynomialRampFunction::operator()(const double& x, OptionalJacobian<1, 1
return 0;
} else if (x < epsilon_) {
if (H) {
H->setConstant(x / epsilon_);
H->setConstant(2 * a_ * x);
}
return (x * x) / (2 * epsilon_) + epsilon_ / 2;
return a_ * pow(x, 2);
} else {
if (H) {
H->setOnes();
}
return x - epsilon_ / 2;
}
}
/* ************************************************************************* */
double RampFunctionPoly3::operator()(const double& x, OptionalJacobian<1, 1> H) const {
if (x <= 0) {
if (H) {
H->setZero();
}
return 0;
} else if (x < epsilon_) {
if (H) {
H->setConstant(3 * a_ * pow(x, 2) + 2 * b_ * x);
}
return a_ * pow(x, 3) + b_ * pow(x, 2);
} else {
if (H) {
H->setOnes();

57
gtsam/constraint/RampFunction.h
View File

@ -10,9 +10,9 @@
* -------------------------------------------------------------------------- */
/**
* @file NonlinearInequalityConstraint.h
* @brief Nonlinear inequality constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @file RampFunction.h
* @brief Ramp function to compute inequality constraint violations.
* @author Yetong Zhang
* @date Aug 3, 2024
*/
@ -20,10 +20,14 @@
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/expressions.h>
#include <memory>
namespace gtsam {
/// Ramp function for create penalty factors.
/** Ramp function f : R -> R.
* f(x) = 0 for x <= 0
* x for x > 0
*/
double RampFunction(const double x, OptionalJacobian<1, 1> H = {});
/** Base class for smooth approximation of the ramp function. */
@ -43,20 +47,51 @@ class SmoothRampFunction {
UnaryScalarFunc function() const;
};
/** Ramp function approximated with a polynomial of degree 2.
* Function f(x) = 0 for x <= 0
* x^2/(2*e) + e/2 for 0 < x < epsilon
* x for x >= epsilon
/** Ramp function approximated with a polynomial of degree 2 in [0, epsilon].
* The coefficients are computed as
* a = 1 / (2 * epsilon)
* Function f(x) = 0 for x <= 0
* a * x^2 for 0 < x < epsilon
* x - epsilon/2 for x >= epsilon
*/
class PolynomialRampFunction : public SmoothRampFunction {
class RampFunctionPoly2 : public SmoothRampFunction {
public:
typedef SmoothRampFunction Base;
typedef RampFunctionPoly2 This;
typedef std::shared_ptr<This> shared_ptr;
protected:
double epsilon_;
double a_;
public:
PolynomialRampFunction(const double epsilon = 1) : Base(), epsilon_(epsilon) {}
RampFunctionPoly2(const double epsilon = 1) : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {}
virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override;
};
/** Ramp function approximated with a polynomial of degree 3 in [0, epsilon].
* The coefficients are computed as
* a = -1 / epsilon^2
* b = 2 / epsilon
* Function f(x) = 0 for x <= 0
* a * x^3 + b * x^2 for 0 < x < epsilon
* x for x >= epsilon
*/
class RampFunctionPoly3 : public SmoothRampFunction {
public:
typedef SmoothRampFunction Base;
typedef RampFunctionPoly3 This;
typedef std::shared_ptr<This> shared_ptr;
protected:
double epsilon_;
double a_;
double b_;
public:
RampFunctionPoly3(const double epsilon = 1)
: Base(), epsilon_(epsilon), a_(-1 / (epsilon * epsilon)), b_(2 / epsilon) {}
virtual double operator()(const double& x, OptionalJacobian<1, 1> H = {}) const override;
};
@ -65,6 +100,8 @@ class PolynomialRampFunction : public SmoothRampFunction {
class SoftPlusFunction : public SmoothRampFunction {
public:
typedef SmoothRampFunction Base;
typedef SoftPlusFunction This;
typedef std::shared_ptr<This> shared_ptr;
protected:
double k_;

4
gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp
View File

@ -10,8 +10,8 @@
* -------------------------------------------------------------------------- */
/**
* @file testEqualityConstraint.cpp
* @brief Equality constraints in constrained optimization.
* @file testNonlinearEqualityConstraint.cpp
* @brief unit tests for nonlinear equality constraints
* @author Yetong Zhang
* @date Aug 3, 2024
*/

4
gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp
View File

@ -10,8 +10,8 @@
* -------------------------------------------------------------------------- */
/**
* @file NonlinearInequalityConstraint.h
* @brief Nonlinear inequality constraints in constrained optimization.
* @file testNonlinearInequalityConstraint.h
* @brief unit tests for nonlinear inequality constraints
* @author Yetong Zhang
* @date Aug 3, 2024
*/

124
gtsam/constraint/tests/testRampFunction.cpp Normal file
View File

@ -0,0 +1,124 @@
/* ----------------------------------------------------------------------------
* 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 testRampFunction.h
* @brief unit tests for ramp functions
* @author Yetong Zhang
* @date Aug 3, 2024
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/constraint/RampFunction.h>
using namespace gtsam;
TEST(RampFunction, error_and_jacobian) {
/// Helper function for numerical Jacobian computation.
auto ramp_helper = [&](const double& x) { return RampFunction(x); };
/// Create a set of values to test the function.
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
static std::vector<double> expected_r_vec{0.0, 0.0, 1.0, 2.0, 3.0};
for (size_t i = 0; i < x_vec.size(); i++) {
double x = x_vec.at(i);
Matrix H;
double r = RampFunction(x, H);
/// Check function evaluation.
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9);
/// Check derivative.
if (abs(x) > 1e-6) { // function is not smooth at 0, so Jacobian is undefined.
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(ramp_helper, x, 1e-6);
EXPECT(assert_equal(expected_H, H));
}
}
}
TEST(RampFunctionPoly2, error_and_jacobian) {
/// Helper function for numerical Jacobian computation.
RampFunctionPoly2 p_ramp(2.0);
auto ramp_helper = [&](const double& x) { return p_ramp(x); };
/// Create a set of values to test the function.
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
static std::vector<double> expected_r_vec{0.0, 0.0, 0.25, 1.0, 2.0};
for (size_t i = 0; i < x_vec.size(); i++) {
double x = x_vec.at(i);
Matrix H;
double r = p_ramp(x, H);
/// Check function evaluation.
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9);
/// Check derivative.
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(ramp_helper, x, 1e-6);
EXPECT(assert_equal(expected_H, H, 1e-6));
}
}
TEST(RampFunctionPoly3, error_and_jacobian) {
/// Helper function for numerical Jacobian computation.
RampFunctionPoly3 p_ramp(2.0);
auto ramp_helper = [&](const double& x) { return p_ramp(x); };
/// Create a set of values to test the function.
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
static std::vector<double> expected_r_vec{0.0, 0.0, 0.75, 2.0, 3.0};
for (size_t i = 0; i < x_vec.size(); i++) {
double x = x_vec.at(i);
Matrix H;
double r = p_ramp(x, H);
/// Check function evaluation.
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9);
/// Check derivative.
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(ramp_helper, x, 1e-6);
EXPECT(assert_equal(expected_H, H, 1e-6));
}
}
TEST(SoftPlusFunction, error_and_jacobian) {
/// Helper function for numerical Jacobian computation.
SoftPlusFunction soft_plus(0.5);
auto soft_plus_helper = [&](const double& x) { return soft_plus(x); };
/// Create a set of values to test the function.
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
static std::vector<double> expected_r_vec{
0.40282656, 1.38629436, 1.94815397, 2.62652338, 3.40282656};
for (size_t i = 0; i < x_vec.size(); i++) {
double x = x_vec.at(i);
Matrix H;
double r = soft_plus(x, H);
/// Check function evaluation.
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-6);
/// Check derivative.
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(soft_plus_helper, x, 1e-6);
EXPECT(assert_equal(expected_H, H, 1e-6));
}
}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}

74
gtsam/nonlinear/NonlinearEquality.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/constraint/NonlinearEqualityConstraint.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
@ -40,14 +41,11 @@ namespace gtsam {
* \nosubgrouping
*/
template<class VALUE>
class NonlinearEquality: public NoiseModelFactorN<VALUE> {
class NonlinearEquality: public NonlinearEqualityConstraint {
public:
typedef VALUE T;
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor1<VALUE>::evaluateError;
private:
// feasible value
@ -63,7 +61,7 @@ private:
using This = NonlinearEquality<VALUE>;
// typedef to base class
using Base = NoiseModelFactorN<VALUE>;
using Base = NonlinearEqualityConstraint;
public:
@ -88,7 +86,7 @@ public:
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), //
KeyVector{j}), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
compare_(_compare) {
}
@ -99,10 +97,14 @@ public:
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), //
KeyVector{j}), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
compare_(_compare) {
}
Key key() const {
return keys().front();
}
/// @}
/// @name Testable
/// @{
@ -139,7 +141,7 @@ public:
}
/// Error function
Vector evaluateError(const T& xj, OptionalMatrixType H) const override {
Vector evaluateError(const T& xj, OptionalMatrixType H = nullptr) const {
const size_t nj = traits<T>::GetDimension(feasible_);
if (allow_error_) {
if (H)
@ -158,11 +160,20 @@ public:
}
}
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
VALUE x1 = x.at<VALUE>(key());
if (H) {
return evaluateError(x1, &(H->front()));
} else {
return evaluateError(x1);
}
}
/// Linearize is over-written, because base linearization tries to whiten
GaussianFactor::shared_ptr linearize(const Values& x) const override {
const T& xj = x.at<T>(this->key());
Matrix A;
Vector b = evaluateError(xj, A);
Vector b = evaluateError(xj, &A);
SharedDiagonal model = noiseModel::Constrained::All(b.size());
return GaussianFactor::shared_ptr(
new JacobianFactor(this->key(), A, b, model));
@ -206,16 +217,13 @@ struct traits<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {};
* Simple unary equality constraint - fixes a value for a variable
*/
template<class VALUE>
class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
class NonlinearEquality1: public NonlinearEqualityConstraint {
public:
typedef VALUE X;
// Provide access to Matrix& version of evaluateError:
using NoiseModelFactor1<VALUE>::evaluateError;
protected:
typedef NoiseModelFactorN<VALUE> Base;
typedef NonlinearEqualityConstraint Base;
typedef NonlinearEquality1<VALUE> This;
/// Default constructor to allow for serialization
@ -240,7 +248,7 @@ public:
NonlinearEquality1(const X& value, Key key, double mu = 1000.0)
: Base(noiseModel::Constrained::All(traits<X>::GetDimension(value),
std::abs(mu)),
key),
KeyVector{key}),
value_(value) {}
~NonlinearEquality1() override {
@ -252,14 +260,25 @@ public:
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
Key key() const { return keys().front(); }
/// g(x) with optional derivative
Vector evaluateError(const X& x1, OptionalMatrixType H) const override {
Vector evaluateError(const X& x1, OptionalMatrixType H = nullptr) const {
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);
}
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
X x1 = x.at<X>(key());
if (H) {
return evaluateError(x1, &(H->front()));
} else {
return evaluateError(x1);
}
}
/// Print
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
@ -298,10 +317,10 @@ struct traits<NonlinearEquality1<VALUE> >
* be the same.
*/
template <class T>
class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
class NonlinearEquality2 : public NonlinearEqualityConstraint {
protected:
using Base = NoiseModelFactorN<T, T>;
using This = NonlinearEquality2<T>;
typedef NonlinearEqualityConstraint Base;
typedef NonlinearEquality2<T> This;
GTSAM_CONCEPT_MANIFOLD_TYPE(T)
@ -311,9 +330,6 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
public:
typedef std::shared_ptr<NonlinearEquality2<T>> shared_ptr;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/**
* Constructor
@ -323,7 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
*/
NonlinearEquality2(Key key1, Key key2, double mu = 1e4)
: Base(noiseModel::Constrained::All(traits<T>::dimension, std::abs(mu)),
key1, key2) {}
KeyVector{key1, key2}) {}
~NonlinearEquality2() override {}
/// @return a deep copy of this factor
@ -334,13 +350,23 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
/// g(x) with optional derivative2
Vector evaluateError(
const T& x1, const T& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override {
const T& x1, const T& x2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const {
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);
}
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
T x1 = x.at<T>(keys().front());
T x2 = x.at<T>(keys().back());
if (H) {
return evaluateError(x1, x2, &(H->front()), &(H->back()));
} else {
return evaluateError(x1, x2);
}
}
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
private:

65
gtsam/slam/BoundingConstraint.h
View File

@ -19,6 +19,7 @@
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/constraint/NonlinearInequalityConstraint.h>
namespace gtsam {
@ -30,20 +31,17 @@ namespace gtsam {
* @ingroup slam
*/
template<class VALUE>
struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
struct BoundingConstraint1: public NonlinearInequalityConstraint {
typedef VALUE X;
typedef NoiseModelFactorN<VALUE> Base;
typedef NonlinearInequalityConstraint Base;
typedef std::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint1(Key key, double threshold,
bool isGreaterThan, double mu = 1000.0) :
Base(noiseModel::Constrained::All(1, mu), key),
Base(noiseModel::Constrained::All(1, mu), KeyVector{key}),
threshold_(threshold), isGreaterThan_(isGreaterThan) {
}
@ -51,6 +49,7 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
inline double threshold() const { return threshold_; }
inline bool isGreaterThan() const { return isGreaterThan_; }
inline Key key() const { return keys().front(); }
/**
* function producing a scalar value to compare to the threshold
@ -60,14 +59,23 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
virtual double value(const X& x, OptionalMatrixType H =
OptionalNone) const = 0;
/** active when constraint *NOT* met */
bool active(const Values& c) const override {
// note: still active at equality to avoid zigzagging
double x = value(c.at<X>(this->key()));
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override {
if (H) {
double d = value(x.at<X>(this->key()), &(H->front()));
if (isGreaterThan_) {
H->front() *= -1;
return Vector1(threshold_ - d);
} else {
return Vector1(d - threshold_);
}
} else {
double d = value(x.at<X>(this->key()));
return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_);
}
}
Vector evaluateError(const X& x, OptionalMatrixType H) const override {
/// TODO: This should be deprecated.
Vector evaluateError(const X& x, OptionalMatrixType H = nullptr) const {
Matrix D;
double error = value(x, &D) - threshold_;
if (H) {
@ -102,22 +110,19 @@ private:
* to implement for specific systems
*/
template<class VALUE1, class VALUE2>
struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
struct BoundingConstraint2: public NonlinearInequalityConstraint {
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
typedef NonlinearInequalityConstraint Base;
typedef std::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint2(Key key1, Key key2, double threshold,
bool isGreaterThan, double mu = 1000.0)
: Base(noiseModel::Constrained::All(1, mu), key1, key2),
: Base(noiseModel::Constrained::All(1, mu), KeyVector{key1, key2}),
threshold_(threshold), isGreaterThan_(isGreaterThan) {}
~BoundingConstraint2() override {}
@ -133,15 +138,27 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const = 0;
/** active when constraint *NOT* met */
bool active(const Values& c) const override {
// 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 unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override {
X1 x1 = x.at<X1>(keys().front());
X2 x2 = x.at<X2>(keys().back());
if (H) {
double d = value(x1, x2, &(H->front()), &(H->back()));
if (isGreaterThan_) {
H->front() *= -1;
H->back() *= -1;
return Vector1(threshold_ - d);
} else {
return Vector1(d - threshold_);
}
} else {
double d = value(x1, x2);
return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_);
}
}
/// TODO: This should be deprecated.
Vector evaluateError(const X1& x1, const X2& x2,
OptionalMatrixType H1, OptionalMatrixType H2) const override {
OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const {
Matrix D1, D2;
double error = value(x1, x2, &D1, &D2) - threshold_;
if (H1) {

14
tests/testBoundingConstraint.cpp
View File

@ -90,8 +90,8 @@ TEST( testBoundingConstraint, unary_basics_active1 ) {
EXPECT(constraint2.active(config));
EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol));
EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol));
EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.unwhitenedError(config), tol));
EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.unwhitenedError(config), tol));
EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint1.unwhitenedError(config), tol));
EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint2.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol);
EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol);
}
@ -103,10 +103,10 @@ TEST( testBoundingConstraint, unary_basics_active2 ) {
config.insert(key, pt1);
EXPECT(constraint3.active(config));
EXPECT(constraint4.active(config));
EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * I_1x1, constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(-1.0 * I_1x1, constraint4.unwhitenedError(config), tol));
// EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol));
// EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol));
EXPECT(assert_equal(1.0 * I_1x1, constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(1.0 * I_1x1, constraint4.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol);
EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol);
}
@ -213,7 +213,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
config1.update(key2, pt4);
EXPECT(rangeBound.active(config1));
EXPECT(assert_equal(-1.0*I_1x1, rangeBound.unwhitenedError(config1)));
EXPECT(assert_equal(1.0*I_1x1, rangeBound.unwhitenedError(config1)));
EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol);
}