From 3af06ef029fb1655a80d1758ee8944dd5e6af1d5 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Thu, 18 Dec 2014 20:48:10 -0500 Subject: [PATCH] Added nonlinearconstraint and tests --- .../nonlinear/NonlinearConstraint.h | 544 ++++++++++++++++++ .../tests/testNonlinearConstraints.cpp | 176 ++++++ 2 files changed, 720 insertions(+) create mode 100644 gtsam_unstable/nonlinear/NonlinearConstraint.h create mode 100644 gtsam_unstable/nonlinear/tests/testNonlinearConstraints.cpp diff --git a/gtsam_unstable/nonlinear/NonlinearConstraint.h b/gtsam_unstable/nonlinear/NonlinearConstraint.h new file mode 100644 index 000000000..3d1bf7eb0 --- /dev/null +++ b/gtsam_unstable/nonlinear/NonlinearConstraint.h @@ -0,0 +1,544 @@ +/** + * @file NonlinearConstraint.h + * @brief + * @author Duy-Nguyen Ta + * @date Sep 30, 2013 + */ + +#pragma once + +#include +#include +#include +#include +#include +//#include "DualKeyGenerator.h" + +namespace gtsam { + +class NonlinearConstraint { + Key dualKey_; + +public: + typedef boost::shared_ptr shared_ptr; +public: + /// Construct with dual key + NonlinearConstraint(Key dualKey) : dualKey_(dualKey) {} + + /** + * compute the HessianFactor of the (-dual * constraintHessian) for the qp subproblem's objective function + */ + virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x, + const VectorValues& duals) const = 0; + + /// return the dual key + Key dualKey() const { return dualKey_; } +}; + +/* ************************************************************************* */ +/** A convenient base class for creating a nonlinear equality constraint with 1 + * variables. To derive from this class, implement evaluateError(). */ +template +class NonlinearConstraint1: public NoiseModelFactor1, public NonlinearConstraint { + +public: + + // typedefs for value types pulled from keys + typedef VALUE X; + +protected: + + typedef NoiseModelFactor1 Base; + typedef NonlinearConstraint1 This; + +public: + + /** + * Default Constructor for I/O + */ + NonlinearConstraint1() { + } + + /** + * Constructor + * @param j key of the variable + * @param constraintDim number of dimensions of the constraint error function + */ + NonlinearConstraint1(Key key, Key dualKey, size_t constraintDim = 1) : + Base(noiseModel::Constrained::All(constraintDim), key), NonlinearConstraint(dualKey) { + } + + virtual ~NonlinearConstraint1() { + } + + /** + * 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 Vector + evaluateError(const X&, boost::optional H1 = boost::none) const = 0; + + /** produce a Gaussian factor graph containing all Hessian Factors, scaled by lambda, + * corresponding to this constraint */ + virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x, + const VectorValues& duals) const { + if (!this->active(x)) { + return GaussianFactor::shared_ptr(); + } + const X& x1 = x.at < X > (Base::key()); + const Vector& lambda = duals.at(this->dualKey()); + + std::vector G11; + evaluateHessians(x1, G11); + + if (dim(lambda) != G11.size()) { + throw std::runtime_error( + "Error in evaluateHessians: the number of returned Gij matrices must be the same as the constraint dimension!"); + } + + // Combine the Lagrange-multiplier part into this constraint factor + Matrix lG11sum = zeros(G11[0].rows(), G11[0].cols()); + for (size_t i = 0; i < lambda.rows(); ++i) { + lG11sum += -lambda[i] * G11[i]; + } + + return boost::make_shared(Base::key(), lG11sum, + zero(x1.dim()), 100.0); + } + + /** evaluate Hessians for lambda factors */ + virtual void evaluateHessians(const X& x1, std::vector& G11) const { + + static const bool debug = false; + + boost::function vecH1( + boost::bind(&This::vectorizeH1t, this, _1)); + + Matrix G11all = numericalDerivative11(vecH1, x1, 1e-5); + + if (debug) { + gtsam::print(G11all, "G11all: "); + std::cout << "x1dim: " << x1.dim() << std::endl; + } + + for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) { + G11.push_back(G11all.block(i * x1.dim(), 0, x1.dim(), x1.dim())); + if (debug) + gtsam::print(G11[i], "G11: "); + } + } + +private: + /** Vectorize the transpose of Jacobian H1 to compute the Hessian numerically */ + Vector vectorizeH1t(const X& x1) const { + Matrix H1; + evaluateError(x1, H1); + Matrix H1t = H1.transpose(); + H1t.resize(H1t.size(), 1); + return Vector(H1t); + } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); + } +}; +// \class NonlinearConstraint1 + +/* ************************************************************************* */ +/** A convenient base class for creating your own NonlinearConstraint with 2 + * variables. To derive from this class, implement evaluateError(). */ +template +class NonlinearConstraint2: public NoiseModelFactor2, public NonlinearConstraint { + +public: + + // typedefs for value types pulled from keys + typedef VALUE1 X1; + typedef VALUE2 X2; + +protected: + + typedef NoiseModelFactor2 Base; + typedef NonlinearConstraint2 This; + +public: + + /** + * Default Constructor for I/O + */ + NonlinearConstraint2() { + } + + /** + * 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 + */ + NonlinearConstraint2(Key j1, Key j2, Key dualKey, size_t constraintDim = 1) : + Base(noiseModel::Constrained::All(constraintDim), j1, j2), NonlinearConstraint(dualKey) { + } + + virtual ~NonlinearConstraint2() { + } + + /** + * 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 Vector + evaluateError(const X1&, const X2&, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const = 0; + + /** produce a Gaussian factor graph containing all Hessian Factors, scaled by lambda, + * corresponding to this constraint */ + virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x, + const VectorValues& duals) const { + if (!this->active(x)) { + return GaussianFactor::shared_ptr(); + } + const X1& x1 = x.at < X1 > (Base::keys_[0]); + const X2& x2 = x.at < X2 > (Base::keys_[1]); + const Vector& lambda = duals.at(this->dualKey()); + + std::vector G11, G12, G22; + evaluateHessians(x1, x2, G11, G12, G22); + + if (dim(lambda) != G11.size() || dim(lambda) != G12.size() + || dim(lambda) != G22.size()) { + throw std::runtime_error( + "Error in evaluateHessians: the number of returned Gij matrices must be the same as the constraint dimension!"); + } + + // Combine the Lagrange-multiplier part into this constraint factor + Matrix lG11sum = zeros(G11[0].rows(), G11[0].cols()), lG12sum = zeros( + G12[0].rows(), G12[0].cols()), lG22sum = zeros(G22[0].rows(), + G22[0].cols()); + for (size_t i = 0; i < lambda.rows(); ++i) { + lG11sum += -lambda[i] * G11[i]; + lG12sum += -lambda[i] * G12[i]; + lG22sum += -lambda[i] * G22[i]; + } + + return boost::make_shared(Base::keys_[0], Base::keys_[1], + lG11sum, lG12sum, zero(x1.dim()), lG22sum, zero(x2.dim()), 0.0); + } + + /** evaluate Hessians for lambda factors */ + virtual void evaluateHessians(const X1& x1, const X2& x2, + std::vector& G11, std::vector& G12, + std::vector& G22) const { + + static const bool debug = false; + + boost::function vecH1( + boost::bind(&This::vectorizeH1t, this, _1, _2)), vecH2( + boost::bind(&This::vectorizeH2t, this, _1, _2)); + + Matrix G11all = numericalDerivative21(vecH1, x1, x2, 1e-5); + Matrix G12all = numericalDerivative22(vecH1, x1, x2, 1e-5); + Matrix G22all = numericalDerivative22(vecH2, x1, x2, 1e-5); + + if (debug) { + gtsam::print(G11all, "G11all: "); + gtsam::print(G12all, "G12all: "); + gtsam::print(G22all, "G22all: "); + std::cout << "x1dim: " << x1.dim() << std::endl; + std::cout << "x2dim: " << x2.dim() << std::endl; + } + + for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) { + G11.push_back(G11all.block(i * x1.dim(), 0, x1.dim(), x1.dim())); + if (debug) + gtsam::print(G11[i], "G11: "); + + G12.push_back(G12all.block(i * x1.dim(), 0, x1.dim(), x2.dim())); + if (debug) + gtsam::print(G12[i], "G12: "); + + G22.push_back(G22all.block(i * x2.dim(), 0, x2.dim(), x2.dim())); + if (debug) + gtsam::print(G22[i], "G22: "); + } + } + +private: + /** Vectorize the transpose of Jacobian H1 to compute the Hessian numerically */ + Vector vectorizeH1t(const X1& x1, const X2& x2) const { + Matrix H1; + Vector err = evaluateError(x1, x2, H1); + Matrix H1t = H1.transpose(); + H1t.resize(H1t.size(), 1); + return Vector(H1t); + } + + /** Vectorize the transpose of Jacobian H2 to compute the Hessian numerically */ + Vector vectorizeH2t(const X1& x1, const X2& x2) const { + Matrix H2; + Vector err = evaluateError(x1, x2, boost::none, H2); + Matrix H2t = H2.transpose(); + H2t.resize(H2t.size(), 1); + return Vector(H2t); + } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); + } +}; +// \class NonlinearConstraint2 + +/* ************************************************************************* */ +/** A convenient base class for creating your own NonlinearConstraint with 3 + * variables. To derive from this class, implement evaluateError(). */ +template +class NonlinearConstraint3: public NoiseModelFactor3, public NonlinearConstraint { + +public: + + // typedefs for value types pulled from keys + typedef VALUE1 X1; + typedef VALUE2 X2; + typedef VALUE3 X3; + +protected: + + typedef NoiseModelFactor3 Base; + typedef NonlinearConstraint3 This; + +public: + + /** + * Default Constructor for I/O + */ + NonlinearConstraint3() { + } + + /** + * 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 + */ + NonlinearConstraint3(Key j1, Key j2, Key j3, Key dualKey, size_t constraintDim = 1) : + Base(noiseModel::Constrained::All(constraintDim), j1, j2, j3), NonlinearConstraint(dualKey) { + } + + virtual ~NonlinearConstraint3() { + } + + /** + * 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 Vector + evaluateError(const X1&, const X2&, const X3&, boost::optional H1 = + boost::none, boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const = 0; + + /** produce a Gaussian factor graph containing all Hessian Factors, scaled by lambda, + * corresponding to this constraint */ + virtual GaussianFactor::shared_ptr multipliedHessian( + const Values& x, const VectorValues& duals) const { + if (!this->active(x)) { + return GaussianFactor::shared_ptr(); + } + const X1& x1 = x.at < X1 > (Base::keys_[0]); + const X2& x2 = x.at < X2 > (Base::keys_[1]); + const X3& x3 = x.at < X3 > (Base::keys_[2]); + const Vector& lambda = duals.at(this->dualKey()); + + std::vector G11, G12, G13, G22, G23, G33; + evaluateHessians(x1, x2, x3, G11, G12, G13, G22, G23, G33); + + if (dim(lambda) != G11.size() || dim(lambda) != G12.size() + || dim(lambda) != G13.size() || dim(lambda) != G22.size() + || dim(lambda) != G23.size() || dim(lambda) != G33.size()) { + throw std::runtime_error( + "Error in evaluateHessians: the number of returned Gij matrices must be the same as the constraint dimension!"); + } + + // Combine the Lagrange-multiplier part into this constraint factor + Matrix lG11sum = zeros(G11[0].rows(), G11[0].cols()), lG12sum = zeros( + G12[0].rows(), G12[0].cols()), lG13sum = zeros(G13[0].rows(), + G13[0].cols()), lG22sum = zeros(G22[0].rows(), G22[0].cols()), lG23sum = + zeros(G23[0].rows(), G23[0].cols()), lG33sum = zeros(G33[0].rows(), + G33[0].cols()); + for (size_t i = 0; i < lambda.rows(); ++i) { + lG11sum += -lambda[i] * G11[i]; + lG12sum += -lambda[i] * G12[i]; + lG13sum += -lambda[i] * G13[i]; + lG22sum += -lambda[i] * G22[i]; + lG23sum += -lambda[i] * G23[i]; + lG33sum += -lambda[i] * G33[i]; + } + + return boost::shared_ptr( + new HessianFactor(Base::keys_[0], Base::keys_[1], Base::keys_[2], + lG11sum, lG12sum, lG13sum, zero(x1.dim()), lG22sum, lG23sum, + zero(x2.dim()), lG33sum, zero(x3.dim()), 0.0)); + } + + /** + * Default Hessian computation using numerical derivatives + * + * As an example, assuming we have f(x1,x2,x3) where dim(f) = 2, dim(x1) = 3, dim(x2) = 2, dim(x3) = 1 + * + * The Jacobian is: + * f1x1 f1x1 f1x1 | f1x2 f1x2 | f1x3 + * f2x1 f2x1 f2x1 | f2x2 f2x2 | f2x3 + * + * We transpose it to have the gradients: + * f1x1 f2x1 + * f1x1 f2x1 + * f1x1 f2x1 + * f1x2 f2x2 + * f1x2 f2x2 + * f1x3 f2x3 + * Then we vectorize this gradient to have: + * [f1x1 + * f1x1 + * f1x1 + * f1x2 + * f1x2 + * f1x3 + * f2x1 + * f2x1 + * f2x1 + * f2x2 + * f2x2 + * f2x3] + * + * The Derivative of this gradient is then: + * [f1x1x1 f1x1x1 f1x1x1 | f1x1x2 f1x1x2 | f1x1x3 + * f1x1x1 f1x1x1 f1x1x1 | f1x1x2 f1x1x2 | f1x1x3 + * f1x1x1 f1x1x1 f1x1x1 | f1x1x2 f1x1x2 | f1x1x3 + * ---------------------|---------------|------- + * f1x2x1 f1x2x1 f1x2x1 | f1x2x2 f1x2x2 | f1x2x3 + * f1x2x1 f1x2x1 f1x2x1 | f1x2x2 f1x2x2 | f1x2x3 + * ---------------------|---------------|------- + * f1x3x1 f1x3x1 f1x3x1 | f1x3x2 f1x3x2 | f1x3x3 + * ============================================= + * f2x1x1 f2x1x1 f2x1x1 | f2x1x2 f2x1x2 | f2x1x3 + * f2x1x1 f2x1x1 f2x1x1 | f2x1x2 f2x1x2 | f2x1x3 + * f2x1x1 f2x1x1 f2x1x1 | f2x1x2 f2x1x2 | f2x1x3 + * ---------------------|---------------|------- + * f2x2x1 f2x2x1 f2x2x1 | f2x2x2 f2x2x2 | f2x2x3 + * f2x2x1 f2x2x1 f2x2x1 | f2x2x2 f2x2x2 | f2x2x3 + * ---------------------|---------------|------- + * f2x3x1 f2x3x1 f2x3x1 | f2x3x2 f2x3x2 | f2x3x3 ] + * + * It is the combination of the Hessian of each component of f + * stacking on top of each other. + * + * */ + virtual void evaluateHessians(const X1& x1, const X2& x2, const X3& x3, + std::vector& G11, std::vector& G12, + std::vector& G13, std::vector& G22, + std::vector& G23, std::vector& G33) const { + + static const bool debug = false; + + boost::function vecH1( + boost::bind(&This::vectorizeH1t, this, _1, _2, _3)), vecH2( + boost::bind(&This::vectorizeH2t, this, _1, _2, _3)), vecH3( + boost::bind(&This::vectorizeH3t, this, _1, _2, _3)); + + Matrix G11all = numericalDerivative31(vecH1, x1, x2, x3, 1e-5); + Matrix G12all = numericalDerivative32(vecH1, x1, x2, x3, 1e-5); + Matrix G13all = numericalDerivative33(vecH1, x1, x2, x3, 1e-5); + Matrix G22all = numericalDerivative32(vecH2, x1, x2, x3, 1e-5); + Matrix G23all = numericalDerivative33(vecH2, x1, x2, x3, 1e-5); + Matrix G33all = numericalDerivative33(vecH3, x1, x2, x3, 1e-5); + + if (debug) { + gtsam::print(G11all, "G11all: "); + gtsam::print(G12all, "G12all: "); + gtsam::print(G13all, "G13all: "); + gtsam::print(G22all, "G22all: "); + gtsam::print(G23all, "G23all: "); + gtsam::print(G33all, "G33all: "); + std::cout << "x1dim: " << x1.dim() << std::endl; + std::cout << "x2dim: " << x2.dim() << std::endl; + std::cout << "x3dim: " << x3.dim() << std::endl; + } + + for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) { + G11.push_back(G11all.block(i * x1.dim(), 0, x1.dim(), x1.dim())); + if (debug) + gtsam::print(G11[i], "G11: "); + + G12.push_back(G12all.block(i * x1.dim(), 0, x1.dim(), x2.dim())); + if (debug) + gtsam::print(G12[i], "G12: "); + + G13.push_back(G13all.block(i * x1.dim(), 0, x1.dim(), x3.dim())); + if (debug) + gtsam::print(G13[i], "G13: "); + + G22.push_back(G22all.block(i * x2.dim(), 0, x2.dim(), x2.dim())); + if (debug) + gtsam::print(G22[i], "G22: "); + + G23.push_back(G23all.block(i * x2.dim(), 0, x2.dim(), x3.dim())); + if (debug) + gtsam::print(G23[i], "G23: "); + + G33.push_back(G33all.block(i * x3.dim(), 0, x3.dim(), x3.dim())); + if (debug) + gtsam::print(G33[i], "G33: "); + } + } + +private: + /** Vectorize the transpose of Jacobian H1 to compute the Hessian numerically */ + Vector vectorizeH1t(const X1& x1, const X2& x2, const X3& x3) const { + Matrix H1; + Vector err = evaluateError(x1, x2, x3, H1); + Matrix H1t = H1.transpose(); + H1t.resize(H1t.size(), 1); + return Vector(H1t); + } + + /** Vectorize the transpose of Jacobian H2 to compute the Hessian numerically */ + Vector vectorizeH2t(const X1& x1, const X2& x2, const X3& x3) const { + Matrix H2; + Vector err = evaluateError(x1, x2, x3, boost::none, H2); + Matrix H2t = H2.transpose(); + H2t.resize(H2t.size(), 1); + return Vector(H2t); + } + + /** Vectorize the transpose of Jacobian H3 to compute the Hessian numerically */ + Vector vectorizeH3t(const X1& x1, const X2& x2, const X3& x3) const { + Matrix H3; + Vector err = evaluateError(x1, x2, x3, boost::none, boost::none, H3); + Matrix H3t = H3.transpose(); + H3t.resize(H3t.size(), 1); + return Vector(H3t); + } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); + } +}; +// \class NonlinearConstraint3 + +} /* namespace gtsam */ diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearConstraints.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearConstraints.cpp new file mode 100644 index 000000000..522f4096d --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testNonlinearConstraints.cpp @@ -0,0 +1,176 @@ +/** + * @file testNonlinearConstraints.cpp + * @brief + * @author Duy-Nguyen Ta + * @date Oct 26, 2013 + */ + +#include +#include +#include +#include +#include +#include "../NonlinearConstraint.h" + +using namespace gtsam; +using namespace gtsam::symbol_shorthand; +using namespace std; + +TEST(Vector, vectorize) { + Matrix m = (Matrix(4,3) << 1,2,3,4,5,6,7,8,9,10,11,12).finished(); + Matrix m2 = m.transpose(); + m2.resize(12,1); + Vector v = Vector(m2); + Vector expectedV = (Vector(12) << 1,2,3,4,5,6,7,8,9,10,11,12).finished(); + EXPECT(assert_equal(expectedV, v)); +} + +/* ************************************************************************* */ +/** + * Test 3-way equality nonlinear constraint + * x(1) + x(2)^2 + x(3)^3 = 3 + */ +class Constraint: public NonlinearConstraint3 { + typedef NonlinearConstraint3 Base; + +public: + Constraint(Key key1, Key key2, Key key3, Key dualKey) : + Base(key1, key2, key3, dualKey, 1) { + } + + Vector evaluateError(const LieScalar& x1, const LieScalar& x2, + const LieScalar& x3, boost::optional H1 = boost::none, + boost::optional H2 = boost::none, boost::optional H3 = + boost::none) const { + if (H1) { + *H1 = (Matrix(1, 1) << 1.0).finished(); + } + if (H2) { + *H2 = (Matrix(1, 1) << 2.0 * x2.value()).finished(); + } + if (H3) { + *H3 = (Matrix(1, 1) << 3.0 * x3.value() * x3.value()).finished(); + } + + return (Vector(1) << + x1.value() + x2.value() * x2.value() + + x3.value() * x3.value() * x3.value() - 3.0).finished(); + } + + void expectedHessians(const LieScalar& x1, const LieScalar& x2, + const LieScalar& x3, std::vector& G11, std::vector& G12, + std::vector& G13, std::vector& G22, + std::vector& G23, std::vector& G33) const { + G11.push_back(zeros(1, 1)); + G12.push_back(zeros(1, 1)); + G13.push_back(zeros(1, 1)); + G22.push_back((Matrix(1, 1) << 2.0).finished()); + G23.push_back(zeros(1, 1)); + G33.push_back((Matrix(1, 1) << 6.0 * x3.value()).finished()); + } +}; + +/* ************************************************************************* */ + +TEST(NonlinearConstraint3, Hessian) { + LieScalar x1(2.0), x2(sqrt(2) - 1), x3(sqrt(2) - 1), x4(2.0), x5(0.5); + Constraint constraint(X(1), X(2), X(3), 0); + + std::vector expectedG11, expectedG12, expectedG13, expectedG22, + expectedG23, expectedG33; + constraint.expectedHessians(x1, x2, x3, expectedG11, expectedG12, expectedG13, + expectedG22, expectedG23, expectedG33); + std::vector G11, G12, G13, G22, G23, G33; + constraint.evaluateHessians(x1, x2, x3, G11, G12, G13, G22, G23, G33); + EXPECT(assert_equal(expectedG11[0], G11[0], 1e-5)); + EXPECT(assert_equal(expectedG12[0], G12[0], 1e-5)); + EXPECT(assert_equal(expectedG13[0], G13[0], 1e-5)); + EXPECT(assert_equal(expectedG22[0], G22[0], 1e-5)); + EXPECT(assert_equal(expectedG23[0], G23[0], 1e-5)); + EXPECT(assert_equal(expectedG33[0], G33[0], 1e-5)); +} + +/* ************************************************************************* */ +/** + * Test 3-way nonlinear equality multi-values constraint + * p(1).x + p(2).x^2 + p(3).y^3 = 3 + * p(1).x^2 + p(2).y + p(3).x^3 = 5 + */ +class Constraint2d: public NonlinearConstraint3 { + typedef NonlinearConstraint3 Base; + +public: + Constraint2d(Key key1, Key key2, Key key3, Key dualKey) : + Base(key1, key2, key3, dualKey, 1) { + } + + Vector evaluateError(const Point2& p1, const Point2& p2, const Point2& p3, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none, boost::optional H3 = boost::none) const { + if (H1) { + *H1 = (Matrix(2, 2) << 1.0, 0.0, 2 * p1.x(), 0.0).finished(); + } + if (H2) { + *H2 = (Matrix(2, 2) << 2.0 * p2.x(), 0.0, 0.0, 1.0).finished(); + } + if (H3) { + *H3 = (Matrix(2, 2) << 0.0, 3.0 * p3.y() * p3.y(), 3.0 * p3.x() * p3.x(), 0.0).finished(); + } + + return (Vector(2) << p1.x() + p2.x() * p2.x() + p3.y() * p3.y() * p3.y() - 3.0, + p1.x() * p1.x() + p2.y() + p3.x() * p3.x() * p3.x() - 5.0).finished(); + } + + void expectedHessians(const Point2& x1, const Point2& x2, const Point2& x3, + std::vector& G11, std::vector& G12, + std::vector& G13, std::vector& G22, + std::vector& G23, std::vector& G33) const { + G11.push_back(zeros(2, 2)); + G11.push_back((Matrix(2,2) << 2.0, 0.0, 0.0, 0.0).finished()); + + G12.push_back(zeros(2,2)); + G12.push_back(zeros(2,2)); + + G13.push_back(zeros(2,2)); + G13.push_back(zeros(2,2)); + + G22.push_back((Matrix(2,2) << 2.0, 0.0, 0.0, 0.0).finished()); + G22.push_back(zeros(2,2)); + + G23.push_back(zeros(2, 2)); + G23.push_back(zeros(2, 2)); + + G33.push_back((Matrix(2, 2) << 0.0, 0.0, 0.0, 6.0 * x3.y() ).finished()); + G33.push_back((Matrix(2, 2) << 6.0 * x3.x(), 0.0, 0.0, 0.0 ).finished()); + } +}; + +/* ************************************************************************* */ + +TEST(NonlinearConstraint3, Hessian2) { + Point2 x1(2.0, 2.0), x2(sqrt(2) - 1, 3.0), x3(4.0, sqrt(2) - 2); + Constraint2d constraint(X(1), X(2), X(3), 0); + + std::vector expectedG11, expectedG12, expectedG13, expectedG22, + expectedG23, expectedG33; + constraint.expectedHessians(x1, x2, x3, expectedG11, expectedG12, expectedG13, + expectedG22, expectedG23, expectedG33); + std::vector G11, G12, G13, G22, G23, G33; + constraint.evaluateHessians(x1, x2, x3, G11, G12, G13, G22, G23, G33); + for (size_t i = 0; i