clean up and refactor NonlinearEquality and BoundingConstraint
parent
80fcff1ed5
commit
9b0fa1210e
|
@ -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
|
||||
*/
|
||||
|
|
|
@ -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 */
|
||||
|
||||
|
|
|
@ -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 */
|
||||
|
||||
|
|
|
@ -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,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 {
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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_;
|
||||
|
|
|
@ -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
|
||||
*/
|
||||
|
|
|
@ -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
|
||||
*/
|
||||
|
|
|
@ -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);
|
||||
}
|
|
@ -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:
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue