12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added basic inequality constraint handling to NonlinearConstraint. Demo still in progress, but now constraints are explicitly handled as either equality or inequality constraints, with an active() function to determine if it is necessary to use the constraint. 

Created testOrdering for new subtract() function to remove parts of an ordering.
release/4.3a0
Alex Cunningham 2009-11-28 18:35:36 +00:00
parent 674d35855b
commit 107c6846fb
11 changed files with 285 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
.cproject
View File

@ -300,6 +300,7 @@
<buildTargets>
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -307,6 +308,7 @@
</target>
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -322,6 +324,7 @@
</target>
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -337,7 +340,6 @@
</target>
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVSLAMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -345,6 +347,7 @@
</target>
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -352,7 +355,6 @@
</target>
<target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianConditional.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -360,6 +362,7 @@
</target>
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -367,6 +370,7 @@
</target>
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -374,7 +378,6 @@
</target>
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -382,6 +385,7 @@
</target>
<target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -389,6 +393,7 @@
</target>
<target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -396,7 +401,6 @@
</target>
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -404,6 +408,7 @@
</target>
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -411,7 +416,6 @@
</target>
<target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVectorConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -419,7 +423,6 @@
</target>
<target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -427,6 +430,7 @@
</target>
<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -434,6 +438,7 @@
</target>
<target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>timeGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -441,6 +446,7 @@
</target>
<target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>timeGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -448,6 +454,7 @@
</target>
<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -455,7 +462,6 @@
</target>
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -463,6 +469,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -470,7 +477,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -478,6 +484,7 @@
</target>
<target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -485,6 +492,7 @@
</target>
<target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -492,7 +500,6 @@
</target>
<target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVSLAMGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -500,7 +507,6 @@
</target>
<target name="testNonlinearEquality.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearEquality.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -508,6 +514,7 @@
</target>
<target name="testSQP.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSQP.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -515,7 +522,6 @@
</target>
<target name="testNonlinearConstraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -523,6 +529,7 @@
</target>
<target name="testSQPOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSQPOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -530,6 +537,7 @@
</target>
<target name="testVSLAMConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVSLAMConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -537,6 +545,7 @@
</target>
<target name="testControlConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testControlConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -544,6 +553,7 @@
</target>
<target name="testControlPoint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testControlPoint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -551,15 +561,21 @@
</target>
<target name="testControlGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testControlGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testOrdering.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testOrdering.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -567,7 +583,6 @@
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -575,7 +590,6 @@
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>

4
cpp/Makefile.am
View File

@ -64,13 +64,15 @@ example = smallExample.cpp
headers += inference.h inference-inl.h
headers += FactorGraph.h FactorGraph-inl.h
headers += BayesNet.h BayesNet-inl.h BayesTree.h BayesTree-inl.h
check_PROGRAMS += testFactorgraph testBayesTree testInference
check_PROGRAMS += testFactorgraph testBayesTree testInference testOrdering
testFactorgraph_SOURCES = testFactorgraph.cpp
testBayesTree_SOURCES = $(example) testBayesTree.cpp
testInference_SOURCES = $(example) testInference.cpp
testFactorgraph_LDADD = libgtsam.la
testBayesTree_LDADD = libgtsam.la
testInference_LDADD = libgtsam.la
testOrdering_SOURCES = testOrdering.cpp
testOrdering_LDADD = libgtsam.la
# Symbolic Inference
headers += SymbolicConditional.h

34
cpp/NonlinearConstraint-inl.h
View File

@ -11,6 +11,15 @@
namespace gtsam {
/* ************************************************************************* */
template <class Config>
bool NonlinearConstraint<Config>::active(const Config& config) const {
if (!isEquality_ && zero(error_vector(config)))
return false;
else
return true;
}
/* ************************************************************************* */
// Implementations of unary nonlinear constraints
/* ************************************************************************* */
@ -21,8 +30,9 @@ NonlinearConstraint1<Config>::NonlinearConstraint1(
Matrix (*gradG)(const Config& config, const std::string& key),
Vector (*g)(const Config& config, const std::string& key),
size_t dim_constraint,
const std::string& lagrange_key) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint),
const std::string& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
g_(g), gradG_(gradG), key_(key) {
// set a good lagrange key here
// TODO:should do something smart to find a unique one
@ -36,7 +46,11 @@ void NonlinearConstraint1<Config>::print(const std::string& s) const {
std::cout << "NonlinearConstraint1 [" << s << "]:\n"
<< " key: " << key_ << "\n"
<< " p: " << this->p_ << "\n"
<< " lambda key: " << this->lagrange_key_ << std::endl;
<< " lambda key: " << this->lagrange_key_ << "\n";
if (this->isEquality_)
std::cout << " Equality Factor" << std::endl;
else
std::cout << " Inequality Factor" << std::endl;
}
/* ************************************************************************* */
@ -48,6 +62,7 @@ bool NonlinearConstraint1<Config>::equals(const Factor<Config>& f, double tol) c
if (this->lagrange_key_ != p->lagrange_key_) return false;
if (g_ != p->g_) return false;
if (gradG_ != p->gradG_) return false;
if (this->isEquality_ != p->isEquality_) return false;
return this->p_ == p->p_;
}
@ -61,13 +76,6 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
// find the error
Vector g = g_(config, key_);
// determine if this is an active constraint
if (zero(g)) {
GaussianFactor::shared_ptr factor(new GaussianFactor(Vector_(0)));
GaussianFactor::shared_ptr constraint(new GaussianFactor(Vector_(0)));
return std::make_pair(factor, constraint);
}
// construct the gradient
Matrix grad = gradG_(config, key_);
@ -95,8 +103,9 @@ NonlinearConstraint2<Config>::NonlinearConstraint2(
Matrix (*gradG2)(const Config& config, const std::string& key),
Vector (*g)(const Config& config, const std::string& key1, const std::string& key2),
size_t dim_constraint,
const std::string& lagrange_key) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint),
const std::string& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
g_(g), gradG1_(gradG1), gradG2_(gradG2), key1_(key1), key2_(key2) {
// set a good lagrange key here
// TODO:should do something smart to find a unique one
@ -125,6 +134,7 @@ bool NonlinearConstraint2<Config>::equals(const Factor<Config>& f, double tol) c
if (g_ != p->g_) return false;
if (gradG1_ != p->gradG1_) return false;
if (gradG2_ != p->gradG2_) return false;
if (this->isEquality_ != p->isEquality_) return false;
return this->p_ == p->p_;
}

33
cpp/NonlinearConstraint.h
View File

@ -8,7 +8,6 @@
#pragma once
#include <map>
#include <iostream>
#include "NonlinearFactor.h"
namespace gtsam {
@ -33,14 +32,20 @@ protected:
/** number of lagrange multipliers */
size_t p_;
/** type of constraint */
bool isEquality_;
public:
/** Constructor - sets the cost function and the lagrange multipliers
* @param lagrange_key is the label for the associated lagrange multipliers
* @param dim_lagrange is the number of associated constraints
* @param isEquality is true if the constraint is an equality constraint
*/
NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange) :
NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
lagrange_key_(lagrange_key), p_(dim_lagrange) {}
NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange,
bool isEquality=true)
: NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) {}
/** returns the key used for the Lagrange multipliers */
std::string lagrangeKey() const { return lagrange_key_; }
@ -48,6 +53,9 @@ public:
/** returns the number of lagrange multipliers */
size_t nrConstraints() const { return p_; }
/** returns the type of constraint */
bool isEquality() const { return isEquality_; }
/** Print */
virtual void print(const std::string& s = "") const =0;
@ -55,7 +63,14 @@ public:
virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
/** error function - returns the result of the constraint function */
virtual inline Vector error_vector(const Config& c) const=0;
virtual inline Vector error_vector(const Config& c) const { return zero(1); }
/**
* Determines whether the constraint is active given a particular configuration
* @param config is the input to the g(x) function
* @return true if constraint needs to be linearized
*/
bool active(const Config& config) const;
/**
* Linearize using a real Config and a VectorConfig of Lagrange multipliers
@ -114,13 +129,15 @@ public:
* @param g is the constraint function
* @param dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier
* @param isEquality is true if the constraint is an equality constraint
*/
NonlinearConstraint1(
const std::string& key,
Matrix (*gradG)(const Config& config, const std::string& key),
Vector (*g)(const Config& config, const std::string& key),
size_t dim_constraint,
const std::string& lagrange_key="");
const std::string& lagrange_key="",
bool isEquality=true);
/** Print */
void print(const std::string& s = "") const;
@ -184,6 +201,7 @@ public:
* @param g is the constraint function
* @param dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier
* @param isEquality is true if the constraint is an equality constraint
*/
NonlinearConstraint2(
const std::string& key1,
@ -192,7 +210,8 @@ public:
Matrix (*gradG2)(const Config& config, const std::string& key),
Vector (*g)(const Config& config, const std::string& key1, const std::string& key2),
size_t dim_constraint,
const std::string& lagrange_key="");
const std::string& lagrange_key="",
bool isEquality=true);
/** Print */
void print(const std::string& s = "") const;

11
cpp/Ordering.cpp
View File

@ -5,11 +5,22 @@
*/
#include <iostream>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/foreach.hpp>
#include "Ordering.h"
using namespace std;
using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
Ordering Ordering::subtract(const Ordering& keys) const {
Ordering newOrdering = *this;
BOOST_FOREACH(string key, keys) {
newOrdering.remove(key);
}
return newOrdering;
}
/* ************************************************************************* */
void Ordering::print(const string& s) const {

7
cpp/Ordering.h
View File

@ -38,6 +38,13 @@ namespace gtsam {
std::list<std::string>(strings_in) {
}
/**
* Remove a set of keys from an ordering
* @param keys to remove
* @return a new ordering without the selected keys
*/
Ordering subtract(const Ordering& keys) const;
void print(const std::string& s = "Ordering") const;
/**

20
cpp/SQPOptimizer-inl.h
View File

@ -55,7 +55,6 @@ SQPOptimizer<G,C>::SQPOptimizer(const G& graph, const Ordering& ordering,
: graph_(&graph), ordering_(&ordering), full_ordering_(ordering),
config_(config), lagrange_config_(lagrange), error_(graph.error(*config))
{
}
/* **************************************************************** */
@ -71,6 +70,9 @@ SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
// linearize the graph
GaussianFactorGraph fg;
// prepare an ordering of lagrange multipliers to remove
Ordering rem;
// iterate over all factors and linearize
for (const_iterator factor = graph_->begin(); factor < graph_->end(); factor++) {
const shared_c constraint = boost::shared_dynamic_cast<NLConstraint >(*factor);
@ -79,7 +81,7 @@ SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
GaussianFactor::shared_ptr f = (*factor)->linearize(*config_);
if (verbose) f->print("Regular Factor");
fg.push_back(f);
} else {
} else if (constraint->active(*config_)) {
// if a constraint, linearize using the constraint method (2 configs)
GaussianFactor::shared_ptr f, c;
boost::tie(f,c) = constraint->linearize(*config_, *lagrange_config_);
@ -87,12 +89,14 @@ SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
if (verbose) c->print("Constraint");
fg.push_back(f);
fg.push_back(c);
} else {
rem += constraint->lagrangeKey();
}
}
if (verbose) fg.print("Before Optimization");
// optimize linear graph to get full delta config
VectorConfig delta = fg.optimize(full_ordering_).scale(-1.0);
VectorConfig delta = fg.optimize(full_ordering_.subtract(rem)).scale(-1.0);
if (verbose) delta.print("Delta Config");
@ -104,4 +108,14 @@ SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
return SQPOptimizer<G, C>(*graph_, full_ordering_, newConfig, newLamConfig);
}
/* **************************************************************** */
template<class G, class C>
void SQPOptimizer<G, C>::print(const std::string& s) {
graph_->print("Nonlinear Graph");
ordering_->print("Initial Ordering");
full_ordering_.print("Ordering including all Lagrange Multipliers");
config_->print("Real Config");
lagrange_config_->print("Lagrange Multiplier Config");
}
}

2
cpp/SQPOptimizer.h
View File

@ -83,6 +83,8 @@ public:
*/
SQPOptimizer<FactorGraph, Config> iterate(Verbosity verbosity=SILENT) const;
/** Standard print function with optional name */
void print(const std::string& s);
};
}

16
cpp/testNonlinearConstraint.cpp
View File

@ -1,4 +1,4 @@
/*
/**
* @file testNonlinearConstraint.cpp
* @brief Tests for nonlinear constraints handled via SQP
* @author Alex Cunningham
@ -73,7 +73,7 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_equal ) {
NonlinearConstraint1<VectorConfig>
c1("x", *test1::grad_g, *test1::g_func, 1, "L_x1"),
c1("x", *test1::grad_g, *test1::g_func, 1, "L_x1", true),
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");
@ -202,7 +202,8 @@ Vector g_func(const VectorConfig& config, const std::string& key) {
TEST( NonlinearConstraint1, unary_inequality ) {
size_t p = 1;
NonlinearConstraint1<VectorConfig> c1("x", *inequality_unary::grad_g,
*inequality_unary::g_func, p, "L_x1");
*inequality_unary::g_func, p, "L_x1",
false); // inequality constraint
// get configurations to use for evaluation
VectorConfig config1, config2;
@ -220,7 +221,8 @@ TEST( NonlinearConstraint1, unary_inequality ) {
TEST( NonlinearConstraint1, unary_inequality_linearize ) {
size_t p = 1;
NonlinearConstraint1<VectorConfig> c1("x", *inequality_unary::grad_g,
*inequality_unary::g_func, p, "L_x");
*inequality_unary::g_func, p, "L_x",
false); // inequality constraint
// get configurations to use for linearization
VectorConfig config1, config2;
@ -235,13 +237,13 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
GaussianFactor::shared_ptr actFactor1, actConstraint1;
boost::tie(actFactor1, actConstraint1) = c1.linearize(config1, lagrangeConfig);
// verify empty factors
CHECK(actFactor1->empty());
CHECK(actConstraint1->empty());
// check if the factor is active
CHECK(!c1.active(config1));
// linearize for active constraint
GaussianFactor::shared_ptr actFactor2, actConstraint2;
boost::tie(actFactor2, actConstraint2) = c1.linearize(config2, lagrangeConfig);
CHECK(c1.active(config2));
// verify
GaussianFactor expFactor("x", Matrix_(1,1, 6.0), "L_x", eye(1), zero(1), 1.0);

33
cpp/testOrdering.cpp Normal file
View File

@ -0,0 +1,33 @@
/**
* @file testOrdering.cpp
* @author Alex Cunningham
*/
#include <boost/assign/std/list.hpp> // for operator +=
#include <CppUnitLite/TestHarness.h>
#include "Ordering.h"
using namespace std;
using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
TEST ( Ordering, subtract ) {
Ordering init, delta;
init += "a", "b", "c", "d", "e";
CHECK(assert_equal(init.subtract(delta), init));
delta += "b";
Ordering expected1;
expected1 += "a", "c", "d", "e";
CHECK(assert_equal(init.subtract(delta), expected1));
delta += "e";
Ordering expected2;
expected2 += "a", "c", "d";
CHECK(assert_equal(init.subtract(delta), expected2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

126
cpp/testSQPOptimizer.cpp
View File

@ -8,9 +8,11 @@
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/map.hpp> // for insert
#include <Simulated2DMeasurement.h>
#include <Simulated2DOdometry.h>
#include <simulated2D.h>
#include "NonlinearFactorGraph.h"
#include "NonlinearConstraint.h"
#include "NonlinearEquality.h"
#include "VectorConfig.h"
#include "Ordering.h"
#include "SQPOptimizer.h"
@ -143,6 +145,7 @@ TEST ( SQPOptimizer, map_warp_initLam ) {
// create an optimizer
Optimizer optimizer(graph, ordering, initialEstimate, initLagrange);
if (verbose) optimizer.print("Initialized Optimizer");
// perform an iteration of optimization
Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
@ -159,8 +162,10 @@ TEST ( SQPOptimizer, map_warp_initLam ) {
/* ********************************************************************* */
TEST ( SQPOptimizer, map_warp ) {
bool verbose = false;
// get a graph
NLGraph graph = linearMapWarpGraph();
if (verbose) graph.print("Initial map warp graph");
// create an initial estimate
shared_config initialEstimate(new VectorConfig);
@ -189,6 +194,127 @@ TEST ( SQPOptimizer, map_warp ) {
CHECK(assert_equal(actual, expected));
}
/* ********************************************************************* */
// This is an obstacle avoidance demo, where there is a trajectory of
// three points, where there is a circular obstacle in the middle. There
// is a binary inequality constraint connecting the obstacle to the
// states, which enforces a minimum distance.
/* ********************************************************************* */
bool vector_compare(const std::string& key,
const VectorConfig& feasible,
const VectorConfig& input) {
Vector feas, lin;
feas = feasible[key];
lin = input[key];
return equal_with_abs_tol(lin, feas, 1e-5);
}
typedef NonlinearConstraint1<VectorConfig> NLC1;
typedef boost::shared_ptr<NLC1> shared_NLC1;
typedef NonlinearConstraint2<VectorConfig> NLC2;
typedef boost::shared_ptr<NLC2> shared_NLC2;
typedef NonlinearEquality<VectorConfig> NLE;
typedef boost::shared_ptr<NLE> shared_NLE;
namespace sqp_avoid1 {
// avoidance radius
double radius = 1.0;
// binary avoidance constraint
/** g(x) = ||x2-obs||^2 > radius^2 */
Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) {
Vector delta = config[key1]-config[key2];
double dist2 = sum(emul(delta, delta));
double thresh = radius*radius;
if (dist2 <= thresh)
return Vector_(1, dist2-thresh);
else
return zero(1);
}
/** gradient at pose */
Matrix grad_g1(const VectorConfig& config, const std::string& key) {
Matrix grad;
return grad;
}
/** gradient at obstacle */
Matrix grad_g2(const VectorConfig& config, const std::string& key) {
return -1*eye(1);
}
}
pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
// fix start, end, obstacle positions
VectorConfig feasible;
feasible.insert("x1", Vector_(2, 0.0, 0.0));
feasible.insert("x3", Vector_(2, 10.0, 0.0));
feasible.insert("obs", Vector_(2, 5.0, -0.5));
shared_NLE e1(new NLE("x1", feasible, 2, *vector_compare));
shared_NLE e2(new NLE("x3", feasible, 2, *vector_compare));
shared_NLE e3(new NLE("obs", feasible, 2, *vector_compare));
// measurement from x1 to x2
Vector x1x2 = Vector_(2, 5.0, 0.0);
double sigma1 = 0.1;
shared f1(new Simulated2DOdometry(x1x2, sigma1, "x1", "x2"));
// measurement from x2 to x3
Vector x2x3 = Vector_(2, 5.0, 0.0);
double sigma2 = 0.1;
shared f2(new Simulated2DOdometry(x2x3, sigma2, "x2", "x3"));
// create a binary inequality constraint that forces the middle point away from
// the obstacle
shared_NLC2 c1(new NLC2("x2", *sqp_avoid1::grad_g1,
"obs", *sqp_avoid1::grad_g2,
*sqp_avoid1::g_func, 1, "L_x2obs"));
// construct the graph
NLGraph graph;
graph.push_back(e1);
graph.push_back(e2);
graph.push_back(e3);
graph.push_back(c1);
graph.push_back(f1);
graph.push_back(f2);
return make_pair(graph, feasible);
}
/* ********************************************************************* */
TEST ( SQPOptimizer, inequality_inactive ) {
// create the graph
NLGraph graph; VectorConfig feasible;
boost::tie(graph, feasible) = obstacleAvoidGraph();
// create the rest of the config
shared_config init(new VectorConfig(feasible));
init->insert("x2", Vector_(2, 5.0, 0.6));
// create an ordering
Ordering ord;
ord += "x1", "x2", "x3", "obs";
// create an optimizer
Optimizer optimizer(graph, ord, init);
// perform optimization
// FIXME: avoidance constraint not correctly implemented
//Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT);
}
/* ********************************************************************* */
TEST ( SQPOptimizer, inequality_active ) {
// create the graph
NLGraph graph; VectorConfig feasible;
boost::tie(graph, feasible) = obstacleAvoidGraph();
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */