222 lines
6.7 KiB
C++
222 lines
6.7 KiB
C++
/*
|
|
* @file NonlinearConstraint.h
|
|
* @brief Implements nonlinear constraints that can be linearized using
|
|
* direct linearization and solving through a quadratic merit function
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <map>
|
|
#include <boost/function.hpp>
|
|
#include "NonlinearFactor.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* Base class for nonlinear constraints
|
|
* This allows for both equality and inequality constraints,
|
|
* where equality constraints are active all the time (even slightly
|
|
* nonzero constraint functions will still be active - inequality
|
|
* constraints should be sure to force to actual zero)
|
|
*
|
|
* NOTE: inequality constraints removed for now
|
|
*
|
|
* Nonlinear constraints evaluate their error as a part of a quadratic
|
|
* error function: ||h(x)-z||^2 + mu * ||c(x)|| where mu is a gain
|
|
* on the constraint function that should be made high enough to be
|
|
* significant
|
|
*/
|
|
template <class Config>
|
|
class NonlinearConstraint : public NonlinearFactor<Config> {
|
|
|
|
protected:
|
|
|
|
double mu_; // gain for quadratic merit function
|
|
|
|
public:
|
|
|
|
/** Constructor - sets the cost function and the lagrange multipliers
|
|
* @param dim is the dimension of the factor
|
|
* @param mu is the gain used at error evaluation (forced to be positive)
|
|
*/
|
|
NonlinearConstraint(size_t dim, double mu = 1000.0);
|
|
|
|
/** returns the gain mu */
|
|
double mu() const { return mu_; }
|
|
|
|
/** Print */
|
|
virtual void print(const std::string& s = "") const=0;
|
|
|
|
/** Check if two factors are equal */
|
|
virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
|
|
|
|
/** error function - returns the quadratic merit function */
|
|
virtual double error(const Config& c) const;
|
|
|
|
/**
|
|
* Linearizes around a given config
|
|
* @param config is the configuration
|
|
* @return a combined linear factor containing both the constraint and the constraint factor
|
|
*/
|
|
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const=0;
|
|
};
|
|
|
|
|
|
/**
|
|
* A unary constraint with arbitrary cost and jacobian functions
|
|
* This is an example class designed for easy testing, but real uses should probably
|
|
* subclass NonlinearConstraint and implement virtual functions directly
|
|
*/
|
|
template <class Config, class Key, class X>
|
|
class NonlinearConstraint1 : public NonlinearConstraint<Config> {
|
|
|
|
private:
|
|
|
|
/**
|
|
* Calculates the jacobian of the constraint function
|
|
* returns a pxn matrix
|
|
* Use boost.bind to create the function object
|
|
* @param config to use for linearization
|
|
* @return the jacobian of the constraint in terms of key
|
|
*/
|
|
boost::function<Matrix(const Config& config)> G_;
|
|
|
|
/** calculates the constraint function of the current config
|
|
* If the value is zero, the constraint is not active
|
|
* Use boost.bind to create the function object
|
|
* @param config is a configuration of all the variables
|
|
* @return the cost for each of p constraints, arranged in a vector
|
|
*/
|
|
boost::function<Vector(const Config& config)> g_;
|
|
|
|
/** key for the constrained variable */
|
|
Key key_;
|
|
|
|
public:
|
|
|
|
/**
|
|
* Basic constructor
|
|
* @param key is the identifier for the variable constrained
|
|
* @param G gives the jacobian of the constraint function
|
|
* @param g is the constraint function
|
|
* @param dim is the size of the constraint (p)
|
|
* @param mu is the gain for the factor
|
|
*/
|
|
NonlinearConstraint1(
|
|
Vector (*g)(const Config& config),
|
|
const Key& key,
|
|
Matrix (*G)(const Config& config),
|
|
size_t dim,
|
|
double mu = 1000.0);
|
|
|
|
/**
|
|
* Basic constructor with boost function pointers
|
|
* @param key is the identifier for the variable constrained
|
|
* @param G gives the jacobian of the constraint function
|
|
* @param g is the constraint function as a boost function pointer
|
|
* @param dim is the size of the constraint (p)
|
|
* @param mu is the gain for the factor
|
|
*/
|
|
NonlinearConstraint1(
|
|
boost::function<Vector(const Config& config)> g,
|
|
const Key& key,
|
|
boost::function<Matrix(const Config& config)> G,
|
|
size_t dim,
|
|
double mu = 1000.0);
|
|
|
|
/** Print */
|
|
void print(const std::string& s = "") const;
|
|
|
|
/** Check if two factors are equal */
|
|
bool equals(const Factor<Config>& f, double tol=1e-9) const;
|
|
|
|
/** Error function */
|
|
virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); }
|
|
|
|
/** Linearize from config */
|
|
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
|
|
};
|
|
|
|
/**
|
|
* A binary constraint with arbitrary cost and jacobian functions
|
|
*/
|
|
template <class Config, class Key1, class X1, class Key2, class X2>
|
|
class NonlinearConstraint2 : public NonlinearConstraint<Config> {
|
|
|
|
private:
|
|
|
|
/**
|
|
* Calculates the jacobians of the constraint function in terms of
|
|
* the first and second variables
|
|
* returns a pxn matrix
|
|
* @param config to use for linearization
|
|
* @return the jacobian of the constraint in terms of key
|
|
*/
|
|
boost::function<Matrix(const Config& config)> G1_;
|
|
boost::function<Matrix(const Config& config)> G2_;
|
|
|
|
/** calculates the constraint function of the current config
|
|
* If the value is zero, the constraint is not active
|
|
* Use boost.bind to create the function object
|
|
* @param config is a configuration of all the variables
|
|
* @return the cost for each of p constraints, arranged in a vector
|
|
*/
|
|
boost::function<Vector(const Config& config)> g_;
|
|
|
|
/** keys for the constrained variables */
|
|
Key1 key1_;
|
|
Key2 key2_;
|
|
|
|
public:
|
|
|
|
/**
|
|
* Basic constructor
|
|
* @param key is the identifier for the variable constrained
|
|
* @param G gives the jacobian of the constraint function
|
|
* @param g is the constraint function
|
|
* @param dim is the size of the constraint (p)
|
|
* @param mu is the gain for the factor
|
|
*/
|
|
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,
|
|
double mu = 1000.0);
|
|
|
|
/**
|
|
* Basic constructor with direct function objects
|
|
* Use boost.bind to construct the function objects
|
|
* @param key is the identifier for the variable constrained
|
|
* @param G gives the jacobian of the constraint function
|
|
* @param g is the constraint function
|
|
* @param dim is the size of the constraint (p)
|
|
* @param mu is the gain for the factor
|
|
*/
|
|
NonlinearConstraint2(
|
|
boost::function<Vector(const Config& config)> g,
|
|
const Key1& key1,
|
|
boost::function<Matrix(const Config& config)> G1,
|
|
const Key2& key2,
|
|
boost::function<Matrix(const Config& config)> G2,
|
|
size_t dim,
|
|
double mu = 1000.0);
|
|
|
|
/** Print */
|
|
void print(const std::string& s = "") const;
|
|
|
|
/** Check if two factors are equal */
|
|
bool equals(const Factor<Config>& f, double tol=1e-9) const;
|
|
|
|
/** Error function */
|
|
virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); }
|
|
|
|
/** Linearize from config */
|
|
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
|
|
};
|
|
|
|
}
|