Added nonlinearconstraint and tests
krunalchande
thduynguyen
parent
7d2f69335d
commit
3af06ef029
|
|
@ -0,0 +1,544 @@
|
|||
/**
|
||||
* @file NonlinearConstraint.h
|
||||
* @brief
|
||||
* @author Duy-Nguyen Ta
|
||||
* @date Sep 30, 2013
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
//#include "DualKeyGenerator.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class NonlinearConstraint {
|
||||
Key dualKey_;
|
||||
|
||||
public:
|
||||
typedef boost::shared_ptr<NonlinearConstraint> 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 VALUE>
|
||||
class NonlinearConstraint1: public NoiseModelFactor1<VALUE>, public NonlinearConstraint {
|
||||
|
||||
public:
|
||||
|
||||
// typedefs for value types pulled from keys
|
||||
typedef VALUE X;
|
||||
|
||||
protected:
|
||||
|
||||
typedef NoiseModelFactor1<VALUE> Base;
|
||||
typedef NonlinearConstraint1<VALUE> 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<Matrix&> 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<Matrix> 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<HessianFactor>(Base::key(), lG11sum,
|
||||
zero(x1.dim()), 100.0);
|
||||
}
|
||||
|
||||
/** evaluate Hessians for lambda factors */
|
||||
virtual void evaluateHessians(const X& x1, std::vector<Matrix>& G11) const {
|
||||
|
||||
static const bool debug = false;
|
||||
|
||||
boost::function<Vector(const X&)> 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<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar
|
||||
& boost::serialization::make_nvp("NoiseModelFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
};
|
||||
// \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 NonlinearConstraint2: public NoiseModelFactor2<VALUE1, VALUE2>, public NonlinearConstraint {
|
||||
|
||||
public:
|
||||
|
||||
// typedefs for value types pulled from keys
|
||||
typedef VALUE1 X1;
|
||||
typedef VALUE2 X2;
|
||||
|
||||
protected:
|
||||
|
||||
typedef NoiseModelFactor2<VALUE1, VALUE2> Base;
|
||||
typedef NonlinearConstraint2<VALUE1, VALUE2> 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<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> 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<Matrix> 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<HessianFactor>(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<Matrix>& G11, std::vector<Matrix>& G12,
|
||||
std::vector<Matrix>& G22) const {
|
||||
|
||||
static const bool debug = false;
|
||||
|
||||
boost::function<Vector(const X1&, const X2&)> 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<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar
|
||||
& boost::serialization::make_nvp("NoiseModelFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
};
|
||||
// \class NonlinearConstraint2
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** A convenient base class for creating your own NonlinearConstraint with 3
|
||||
* variables. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUE1, class VALUE2, class VALUE3>
|
||||
class NonlinearConstraint3: public NoiseModelFactor3<VALUE1, VALUE2, VALUE3>, public NonlinearConstraint {
|
||||
|
||||
public:
|
||||
|
||||
// typedefs for value types pulled from keys
|
||||
typedef VALUE1 X1;
|
||||
typedef VALUE2 X2;
|
||||
typedef VALUE3 X3;
|
||||
|
||||
protected:
|
||||
|
||||
typedef NoiseModelFactor3<VALUE1, VALUE2, VALUE3> Base;
|
||||
typedef NonlinearConstraint3<VALUE1, VALUE2, VALUE3> 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<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none,
|
||||
boost::optional<Matrix&> 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<Matrix> 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<HessianFactor>(
|
||||
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<Matrix>& G11, std::vector<Matrix>& G12,
|
||||
std::vector<Matrix>& G13, std::vector<Matrix>& G22,
|
||||
std::vector<Matrix>& G23, std::vector<Matrix>& G33) const {
|
||||
|
||||
static const bool debug = false;
|
||||
|
||||
boost::function<Vector(const X1&, const X2&, const X3&)> 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<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar
|
||||
& boost::serialization::make_nvp("NoiseModelFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
};
|
||||
// \class NonlinearConstraint3
|
||||
|
||||
} /* namespace gtsam */
|
||||
|
|
@ -0,0 +1,176 @@
|
|||
/**
|
||||
* @file testNonlinearConstraints.cpp
|
||||
* @brief
|
||||
* @author Duy-Nguyen Ta
|
||||
* @date Oct 26, 2013
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/LieScalar.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#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<LieScalar, LieScalar,
|
||||
LieScalar> {
|
||||
typedef NonlinearConstraint3<LieScalar, LieScalar, LieScalar> 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<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> 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<Matrix>& G11, std::vector<Matrix>& G12,
|
||||
std::vector<Matrix>& G13, std::vector<Matrix>& G22,
|
||||
std::vector<Matrix>& G23, std::vector<Matrix>& 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<Matrix> expectedG11, expectedG12, expectedG13, expectedG22,
|
||||
expectedG23, expectedG33;
|
||||
constraint.expectedHessians(x1, x2, x3, expectedG11, expectedG12, expectedG13,
|
||||
expectedG22, expectedG23, expectedG33);
|
||||
std::vector<Matrix> 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<Point2, Point2, Point2> {
|
||||
typedef NonlinearConstraint3<Point2, Point2, Point2> 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<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
|
||||
boost::none, boost::optional<Matrix&> 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<Matrix>& G11, std::vector<Matrix>& G12,
|
||||
std::vector<Matrix>& G13, std::vector<Matrix>& G22,
|
||||
std::vector<Matrix>& G23, std::vector<Matrix>& 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<Matrix> expectedG11, expectedG12, expectedG13, expectedG22,
|
||||
expectedG23, expectedG33;
|
||||
constraint.expectedHessians(x1, x2, x3, expectedG11, expectedG12, expectedG13,
|
||||
expectedG22, expectedG23, expectedG33);
|
||||
std::vector<Matrix> G11, G12, G13, G22, G23, G33;
|
||||
constraint.evaluateHessians(x1, x2, x3, G11, G12, G13, G22, G23, G33);
|
||||
for (size_t i = 0; i<G11.size(); ++i) {
|
||||
EXPECT(assert_equal(expectedG11[i], G11[i], 1e-5));
|
||||
EXPECT(assert_equal(expectedG12[i], G12[i], 1e-5));
|
||||
EXPECT(assert_equal(expectedG13[i], G13[i], 1e-5));
|
||||
EXPECT(assert_equal(expectedG22[i], G22[i], 1e-5));
|
||||
EXPECT(assert_equal(expectedG23[i], G23[i], 1e-5));
|
||||
EXPECT(assert_equal(expectedG33[i], G33[i], 1e-5));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue