Implemented linearization and equality for unary NonlinearConstraints. Current tests use a scalar example. Split out implementation into a separate implementation file.
Alex Cunningham
parent
4ca1dbf325
commit
ee4a066275
|
|
@ -115,7 +115,7 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
|
|||
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
|
||||
sources += NonlinearFactor.cpp
|
||||
sources += NonlinearEquality.h
|
||||
sources += NonlinearConstraint.h
|
||||
sources += NonlinearConstraint.h NonlinearConstraint-inl.h
|
||||
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint
|
||||
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
|
||||
testNonlinearFactor_LDADD = libgtsam.la
|
||||
|
|
|
|||
|
|
@ -0,0 +1,60 @@
|
|||
/*
|
||||
* @file NonlinearConstraint-inl.h
|
||||
* @brief Implementation for NonlinearConstraints
|
||||
* @author alexgc
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <iostream>
|
||||
#include "NonlinearConstraint.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Implementations of unary nonlinear constraints
|
||||
*/
|
||||
|
||||
template <class Config>
|
||||
void NonlinearConstraint1<Config>::print(const std::string& s) const {
|
||||
std::cout << "NonlinearConstraint [" << s << "]:\n"
|
||||
<< " Variable: " << key_ << "\n"
|
||||
<< " p: " << this->p_ << "\n"
|
||||
<< " lambda key: " << this->lagrange_key_ << std::endl;
|
||||
}
|
||||
|
||||
template <class Config>
|
||||
bool NonlinearConstraint1<Config>::equals(const Factor<Config>& f, double tol) const {
|
||||
const NonlinearConstraint1<Config>* p = dynamic_cast<const NonlinearConstraint1<Config>*> (&f);
|
||||
if (p == NULL) return false;
|
||||
if (key_ != p->key_) return false;
|
||||
if (this->lagrange_key_ != p->lagrange_key_) return false;
|
||||
if (g_ != p->g_) return false;
|
||||
if (gradG_ != p->gradG_) return false;
|
||||
return this->p_ == p->p_;
|
||||
}
|
||||
|
||||
template <class Config>
|
||||
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
|
||||
NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig& lagrange) const {
|
||||
// extract lagrange multiplier
|
||||
Vector lambda = lagrange[this->lagrange_key_];
|
||||
|
||||
// find the error
|
||||
Vector g = g_(config, key_);
|
||||
|
||||
// construct the gradient
|
||||
Matrix grad = gradG_(config, key_);
|
||||
|
||||
// construct probabilistic factor
|
||||
Matrix A1 = vector_scale(grad, lambda);
|
||||
GaussianFactor::shared_ptr factor(new
|
||||
GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
|
||||
|
||||
// construct the constraint
|
||||
GaussianFactor::shared_ptr constraint(new GaussianFactor("x", grad, g, 0.0));
|
||||
|
||||
return std::make_pair(factor, constraint);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -8,6 +8,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <iostream>
|
||||
#include "NonlinearFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -81,7 +82,7 @@ public:
|
|||
* A unary constraint with arbitrary cost and gradient functions
|
||||
*/
|
||||
template <class Config>
|
||||
class NonlinearConstraint1 : NonlinearConstraint<Config> {
|
||||
class NonlinearConstraint1 : public NonlinearConstraint<Config> {
|
||||
|
||||
private:
|
||||
/** calculates the constraint function of the current config
|
||||
|
|
@ -122,21 +123,17 @@ public:
|
|||
const std::string& lagrange_key="") :
|
||||
NonlinearConstraint<Config>(lagrange_key, dim_constraint),
|
||||
g_(g), gradG_(gradG), key_(key) {
|
||||
// set a good lagrange key here - should do something smart to find a unique one
|
||||
// set a good lagrange key here
|
||||
// TODO:should do something smart to find a unique one
|
||||
if (lagrange_key == "")
|
||||
this->lagrange_key_ = "L_" + key;
|
||||
}
|
||||
|
||||
/** Print */
|
||||
void print(const std::string& s = "") const {
|
||||
//FIXME: dummy implementation
|
||||
}
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/** Check if two factors are equal */
|
||||
bool equals(const Factor<Config>& f, double tol=1e-9) const {
|
||||
//FIXME: dummy implementation
|
||||
return false;
|
||||
}
|
||||
bool equals(const Factor<Config>& f, double tol=1e-9) const;
|
||||
|
||||
/** error function - returns the result of the constraint function */
|
||||
inline Vector error_vector(const Config& c) const {
|
||||
|
|
@ -151,13 +148,7 @@ public:
|
|||
* @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint)
|
||||
*/
|
||||
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
|
||||
linearize(const Config& config, const VectorConfig& lagrange) const {
|
||||
//FIXME: dummy implementation
|
||||
GaussianFactor::shared_ptr factor(new GaussianFactor);
|
||||
GaussianFactor::shared_ptr constraint(new GaussianFactor);
|
||||
return std::make_pair(factor, constraint);
|
||||
}
|
||||
|
||||
linearize(const Config& config, const VectorConfig& lagrange) const;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -7,14 +7,12 @@
|
|||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <VectorConfig.h>
|
||||
#include <NonlinearConstraint.h>
|
||||
#include <NonlinearConstraint-inl.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
// define some functions to use
|
||||
// these examples use scalar variables and a single constraint
|
||||
|
||||
/* ************************************************************************* */
|
||||
// unary functions
|
||||
// unary functions with scalar variables
|
||||
/* ************************************************************************* */
|
||||
namespace test1 {
|
||||
/** p = 1, gradG(x) = 2x */
|
||||
|
|
@ -31,7 +29,7 @@ Vector g_func(const VectorConfig& config, const std::string& key) {
|
|||
} // \namespace test1
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint, unary_construction ) {
|
||||
TEST( NonlinearConstraint, unary_scalar_construction ) {
|
||||
// construct a constraint on x
|
||||
// the lagrange multipliers will be expected on L_x1
|
||||
// and there is only one multiplier
|
||||
|
|
@ -48,40 +46,46 @@ TEST( NonlinearConstraint, unary_construction ) {
|
|||
CHECK(assert_equal(actual, expected, 1e-5));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( NonlinearConstraint, unary_linearize ) {
|
||||
// // constuct a constraint
|
||||
// Vector lambda = Vector_(1, 1.0);
|
||||
// NonlinearConstraint1<VectorConfig> c1("x", *grad_g1, *g1_func, lambda);
|
||||
//
|
||||
// // get a configuration to use for linearization
|
||||
// VectorConfig config;
|
||||
// config.insert("x", Vector_(1, 1.0));
|
||||
//
|
||||
// // determine the gradient of the function
|
||||
// Matrix
|
||||
//
|
||||
//}
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint, unary_scalar_linearize ) {
|
||||
// construct a constraint on x
|
||||
// the lagrange multipliers will be expected on L_x1
|
||||
// and there is only one multiplier
|
||||
size_t p = 1;
|
||||
NonlinearConstraint1<VectorConfig> c1("x", *test1::grad_g, *test1::g_func, p, "L_x1");
|
||||
|
||||
///* ************************************************************************* */
|
||||
//// binary functions
|
||||
//Matrix grad1_g2(const VectorConfig& config) {
|
||||
// return eye(1);
|
||||
//}
|
||||
//
|
||||
//Matrix grad2_g2(const VectorConfig& config) {
|
||||
// return eye(1);
|
||||
//}
|
||||
//
|
||||
//Vector g2_func(const VectorConfig& config) {
|
||||
// return zero(1);
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//TEST( NonlinearConstraint, binary_construction ) {
|
||||
// Vector lambda = Vector_(1, 1.0);
|
||||
// NonlinearConstraint2<VectorConfig> c1("x", *grad1_g2, "y", *grad2_g2, *g2_func, lambda);
|
||||
//}
|
||||
// get a configuration to use for linearization
|
||||
VectorConfig realconfig;
|
||||
realconfig.insert("x", Vector_(1, 1.0));
|
||||
|
||||
// get a configuration of Lagrange multipliers
|
||||
VectorConfig lagrangeConfig;
|
||||
lagrangeConfig.insert("L_x1", Vector_(1, 3.0));
|
||||
|
||||
// linearize the system
|
||||
GaussianFactor::shared_ptr actFactor, actConstraint;
|
||||
boost::tie(actFactor, actConstraint) = c1.linearize(realconfig, lagrangeConfig);
|
||||
|
||||
// verify
|
||||
GaussianFactor expFactor("x", Matrix_(1,1, 6.0), "L_x1", eye(1), zero(1), 1.0);
|
||||
GaussianFactor expConstraint("x", Matrix_(1,1, 2.0), Vector_(1,-4.0), 0.0);
|
||||
CHECK(assert_equal(*actFactor, expFactor));
|
||||
CHECK(assert_equal(*actConstraint, expConstraint));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint, unary_scalar_equal ) {
|
||||
NonlinearConstraint1<VectorConfig>
|
||||
c1("x", *test1::grad_g, *test1::g_func, 1, "L_x1"),
|
||||
c2("x", *test1::grad_g, *test1::g_func, 1, "L_x1"),
|
||||
c3("x", *test1::grad_g, *test1::g_func, 2, "L_x1"),
|
||||
c4("y", *test1::grad_g, *test1::g_func, 1, "L_x1");
|
||||
|
||||
CHECK(assert_equal(c1, c2));
|
||||
CHECK(assert_equal(c2, c1));
|
||||
CHECK(!c1.equals(c3));
|
||||
CHECK(!c1.equals(c4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
|
|
|
|||
|
|
@ -192,7 +192,7 @@ TEST (SQP, problem1_sqp ) {
|
|||
* to zero
|
||||
* lam*gradG*dx + dlam + lam
|
||||
* formulated in matrix form as:
|
||||
* [lam*gradG eye(1)] [dx; dlam] = lam
|
||||
* [lam*gradG eye(1)] [dx; dlam] = zero
|
||||
*/
|
||||
GaussianFactor::shared_ptr f2(
|
||||
new GaussianFactor("x", lam*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
|
||||
|
|
|
|||
Loading…
Reference in New Issue