/* * @file NonlinearConstraint-inl.h * @brief Implementation for NonlinearConstraints * @author Alex Cunningham */ #pragma once #include #include #include "NonlinearConstraint.h" namespace gtsam { /* ************************************************************************* */ // Implementations of base class /* ************************************************************************* */ /* ************************************************************************* */ template NonlinearConstraint::NonlinearConstraint(const LagrangeKey& lagrange_key, size_t dim_lagrange, Vector (*g)(const Config& config), bool isEquality) : NonlinearFactor(1.0), lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality), g_(boost::bind(g, _1)) {} /* ************************************************************************* */ template NonlinearConstraint::NonlinearConstraint(const LagrangeKey& lagrange_key, size_t dim_lagrange, boost::function g, bool isEquality) : NonlinearFactor(noiseModel::Constrained::All(dim_lagrange)), lagrange_key_(lagrange_key), p_(dim_lagrange), g_(g), isEquality_(isEquality) {} /* ************************************************************************* */ template bool NonlinearConstraint::active(const Config& config) const { return !(!isEquality_ && greaterThanOrEqual(unwhitenedError(config), zero(p_))); } /* ************************************************************************* */ // Implementations of unary nonlinear constraints /* ************************************************************************* */ template NonlinearConstraint1::NonlinearConstraint1( Vector (*g)(const Config& config), const Key& key, Matrix (*gradG)(const Config& config), size_t dim_constraint, const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), G_(boost::bind(gradG, _1)), key_(key) { } /* ************************************************************************* */ template NonlinearConstraint1::NonlinearConstraint1( boost::function g, const Key& key, boost::function gradG, size_t dim_constraint, const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), G_(gradG), key_(key) { } /* ************************************************************************* */ template void NonlinearConstraint1::print(const std::string& s) const { std::cout << "NonlinearConstraint1 [" << s << "]:\n"; // << " key: " << key_ << "\n" // << " p: " << this->p_ << "\n" // << " lambda key: " << this->lagrange_key_ << "\n"; // if (this->isEquality_) // std::cout << " Equality Factor" << std::endl; // else // std::cout << " Inequality Factor" << std::endl; } /* ************************************************************************* */ template bool NonlinearConstraint1::equals(const Factor& f, double tol) const { const NonlinearConstraint1* p = dynamic_cast*> (&f); if (p == NULL) return false; if (!(key_ == p->key_)) return false; if (!(this->lagrange_key_.equals(p->lagrange_key_))) return false; if (this->isEquality_ != p->isEquality_) return false; return this->p_ == p->p_; } /* ************************************************************************* */ template GaussianFactor::shared_ptr NonlinearConstraint1::linearize(const Config& config) const { const size_t p = this->p_; // extract lagrange multiplier Vector lambda = config[this->lagrange_key_]; // find the error Vector g = g_(config); // construct the gradient Matrix grad = G_(config); // construct combined factor Matrix Ax = zeros(grad.size1()*2, grad.size2()); insertSub(Ax, vector_scale(lambda, grad), 0, 0); insertSub(Ax, grad, grad.size1(), 0); Matrix AL = eye(p*2, p); Vector rhs = zero(p*2); subInsert(rhs, -1*g, p); // construct a mixed constraint model Vector sigmas = zero(p*2); subInsert(sigmas, ones(p), 0); SharedDiagonal mixedConstraint = noiseModel::Constrained::MixedSigmas(sigmas); GaussianFactor::shared_ptr factor(new GaussianFactor(key_, Ax, this->lagrange_key_, AL, rhs, mixedConstraint)); return factor; } /* ************************************************************************* */ // Implementations of binary nonlinear constraints /* ************************************************************************* */ /* ************************************************************************* */ template NonlinearConstraint2::NonlinearConstraint2( Vector (*g)(const Config& config), const Key1& key1, Matrix (*G1)(const Config& config), const Key2& key2, Matrix (*G2)(const Config& config), size_t dim_constraint, const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), key1_(key1), key2_(key2) { } /* ************************************************************************* */ template NonlinearConstraint2::NonlinearConstraint2( boost::function g, const Key1& key1, boost::function G1, const Key2& key2, boost::function G2, size_t dim_constraint, const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), G1_(G1), G2_(G2), key1_(key1), key2_(key2) { } /* ************************************************************************* */ template void NonlinearConstraint2::print(const std::string& s) const { std::cout << "NonlinearConstraint2 [" << s << "]:\n"; // << " key1: " << key1_ << "\n" // << " key2: " << key2_ << "\n" // << " p: " << this->p_ << "\n" // << " lambda key: " << this->lagrange_key_ << std::endl; } /* ************************************************************************* */ template bool NonlinearConstraint2::equals(const Factor& f, double tol) const { const NonlinearConstraint2* p = dynamic_cast*> (&f); if (p == NULL) return false; if (!(key1_ == p->key1_)) return false; if (!(key2_ == p->key2_)) return false; if (!(this->lagrange_key_.equals(p->lagrange_key_))) return false; if (this->isEquality_ != p->isEquality_) return false; return this->p_ == p->p_; } /* ************************************************************************* */ template GaussianFactor::shared_ptr NonlinearConstraint2::linearize(const Config& config) const { // extract lagrange multiplier Vector lambda = config[this->lagrange_key_]; // find the error Vector g = g_(config); // construct the gradients Matrix grad1 = G1_(config); Matrix grad2 = G2_(config); // construct probabilistic factor Matrix A1 = vector_scale(lambda, grad1); Matrix A2 = vector_scale(lambda, grad2); SharedDiagonal probModel = sharedSigma(this->p_,1.0); GaussianFactor::shared_ptr factor(new GaussianFactor(key1_, A1, key2_, A2, this->lagrange_key_, eye(this->p_), zero(this->p_), probModel)); // construct the constraint SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, -1.0 * g, constraintModel)); return factor; } }