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

SQPOptimizer can now do its own initialization of the Lagrange multipliers. 

Cleaned up NonlinearConstraint
release/4.3a0
Alex Cunningham 2009-11-23 22:10:52 +00:00
parent a5515d9d57
commit 31856ce598
5 changed files with 124 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
cpp/NonlinearConstraint-inl.h
View File

@ -15,6 +15,21 @@ namespace gtsam {
// Implementations of unary nonlinear constraints
/* ************************************************************************* */
template <class Config>
NonlinearConstraint1<Config>::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) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint),
g_(g), gradG_(gradG), key_(key) {
// set a good lagrange key here
// TODO:should do something smart to find a unique one
if (lagrange_key == "")
this->lagrange_key_ = "L_" + key;
}
/* ************************************************************************* */
template <class Config>
void NonlinearConstraint1<Config>::print(const std::string& s) const {
@ -64,6 +79,24 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
// Implementations of binary nonlinear constraints
/* ************************************************************************* */
/* ************************************************************************* */
template <class Config>
NonlinearConstraint2<Config>::NonlinearConstraint2(
const std::string& key1,
Matrix (*gradG1)(const Config& config, const std::string& key),
const std::string& key2,
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),
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
if (lagrange_key == "")
this->lagrange_key_ = "L_" + key1 + key2;
}
/* ************************************************************************* */
template <class Config>
void NonlinearConstraint2<Config>::print(const std::string& s) const {

20
cpp/NonlinearConstraint.h
View File

@ -43,7 +43,7 @@ public:
lagrange_key_(lagrange_key), p_(dim_lagrange) {}
/** returns the key used for the Lagrange multipliers */
std::string& lagrangeKey() const { return lagrange_key_; }
std::string lagrangeKey() const { return lagrange_key_; }
/** returns the number of lagrange multipliers */
size_t nrConstraints() const { return p_; }
@ -120,14 +120,7 @@ public:
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),
g_(g), gradG_(gradG), key_(key) {
// set a good lagrange key here
// TODO:should do something smart to find a unique one
if (lagrange_key == "")
this->lagrange_key_ = "L_" + key;
}
const std::string& lagrange_key="");
/** Print */
void print(const std::string& s = "") const;
@ -199,14 +192,7 @@ 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="") :
NonlinearConstraint<Config>(lagrange_key, dim_constraint),
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
if (lagrange_key == "")
this->lagrange_key_ = "L_" + key1 + key2;
}
const std::string& lagrange_key="");
/** Print */
void print(const std::string& s = "") const;

31
cpp/SQPOptimizer-inl.h
View File

@ -7,27 +7,48 @@
#pragma once
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/map.hpp> // for insert
#include "GaussianFactorGraph.h"
#include "SQPOptimizer.h"
using namespace std;
using namespace boost::assign;
namespace gtsam {
/* **************************************************************** */
template <class G, class C>
SQPOptimizer<G,C>::SQPOptimizer(const G& graph, const Ordering& ordering,
shared_config config)
: graph_(&graph), ordering_(&ordering), config_(config)
: graph_(&graph), ordering_(&ordering), full_ordering_(ordering),
config_(config), lagrange_config_(new VectorConfig)
{
// TODO: assign a value to the lagrange config
// local typedefs
typedef typename G::const_iterator const_iterator;
typedef NonlinearConstraint<C> NLConstraint;
typedef boost::shared_ptr<NLConstraint > shared_c;
// find the constraints
for (const_iterator factor = graph_->begin(); factor < graph_->end(); factor++) {
const shared_c constraint = boost::shared_dynamic_cast<NLConstraint >(*factor);
if (constraint != NULL) {
size_t p = constraint->nrConstraints();
// update ordering
string key = constraint->lagrangeKey();
full_ordering_ += key;
// initialize lagrange multipliers
lagrange_config_->insert(key, ones(p));
}
}
}
/* **************************************************************** */
template <class G, class C>
SQPOptimizer<G,C>::SQPOptimizer(const G& graph, const Ordering& ordering,
shared_config config, shared_vconfig lagrange)
: graph_(&graph), ordering_(&ordering), config_(config), lagrange_config_(lagrange)
: graph_(&graph), ordering_(&ordering), full_ordering_(ordering),
config_(config), lagrange_config_(lagrange)
{
}
@ -66,7 +87,7 @@ SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
if (verbose) fg.print("Before Optimization");
// optimize linear graph to get full delta config
VectorConfig delta = fg.optimize(*ordering_).scale(-1.0);
VectorConfig delta = fg.optimize(full_ordering_).scale(-1.0);
if (verbose) delta.print("Delta Config");
@ -75,7 +96,7 @@ SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
shared_vconfig newLamConfig(new VectorConfig(lagrange_config_->exmap(delta)));
// construct a new optimizer
return SQPOptimizer<G, C>(*graph_, *ordering_, newConfig, newLamConfig);
return SQPOptimizer<G, C>(*graph_, full_ordering_, newConfig, newLamConfig);
}
}

10
cpp/SQPOptimizer.h
View File

@ -27,16 +27,22 @@ public:
// useful for storing configurations
typedef boost::shared_ptr<const Config> shared_config;
typedef boost::shared_ptr<const VectorConfig> shared_vconfig;
typedef boost::shared_ptr<VectorConfig> shared_vconfig;
private:
// keep const references to the graph and the original ordering
// keep const references to the graph and initial ordering
const FactorGraph* graph_;
const Ordering* ordering_;
// keep configurations
shared_config config_;
shared_vconfig lagrange_config_;
// keep a configuration that has been updated to include the lagrange multipliers
Ordering full_ordering_;
// keep a set of errors for the overall system and just the constraints
double error_;
double constraint_error_;

74
cpp/testSQPOptimizer.cpp
View File

@ -43,7 +43,7 @@ TEST ( SQPOptimizer, basic ) {
CHECK(assert_equal(*config, *(optimizer.config())));
}
namespace sqpOptimizer_test1 {
namespace sqp_LinearMapWarp2 {
// binary constraint between landmarks
/** g(x) = x-y = 0 */
Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) {
@ -59,9 +59,9 @@ Matrix grad_g1(const VectorConfig& config, const std::string& key) {
Matrix grad_g2(const VectorConfig& config, const std::string& key) {
return -1*eye(2);
}
} // \namespace sqpOptimizer_test1
} // \namespace sqp_LinearMapWarp2
namespace sqpOptimizer_test2 {
namespace sqp_LinearMapWarp1 {
// Unary Constraint on x1
/** g(x) = x -[1;1] = 0 */
Vector g_func(const VectorConfig& config, const std::string& key) {
@ -72,24 +72,16 @@ Vector g_func(const VectorConfig& config, const std::string& key) {
Matrix grad_g(const VectorConfig& config, const std::string& key) {
return eye(2);
}
} // \namespace sqpOptimizer_test2
} // \namespace sqp_LinearMapWarp12
typedef SQPOptimizer<NLGraph, VectorConfig> Optimizer;
/**
* Test pulled from testSQP, with
*/
TEST ( SQPOptimizer, simple_case ) {
bool verbose = false;
// position (1, 1) constraint for x1
VectorConfig feas;
feas.insert("x1", Vector_(2, 1.0, 1.0));
NLGraph linearMapWarpGraph() {
// constant constraint on x1
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>(
"x1", *sqpOptimizer_test2::grad_g,
*sqpOptimizer_test2::g_func, 2, "L_x1"));
"x1", *sqp_LinearMapWarp1::grad_g,
*sqp_LinearMapWarp1::g_func, 2, "L_x1"));
// measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0);
@ -104,9 +96,9 @@ TEST ( SQPOptimizer, simple_case ) {
// equality constraint between l1 and l2
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>(
"l1", *sqpOptimizer_test1::grad_g1,
"l2", *sqpOptimizer_test1::grad_g2,
*sqpOptimizer_test1::g_func, 2, "L_l1l2"));
"l1", *sqp_LinearMapWarp2::grad_g1,
"l2", *sqp_LinearMapWarp2::grad_g2,
*sqp_LinearMapWarp2::g_func, 2, "L_l1l2"));
// construct the graph
NLGraph graph;
@ -115,9 +107,19 @@ TEST ( SQPOptimizer, simple_case ) {
graph.push_back(f1);
graph.push_back(f2);
return graph;
}
/* ********************************************************************* */
TEST ( SQPOptimizer, map_warp_initLam ) {
bool verbose = false;
// get a graph
NLGraph graph = linearMapWarpGraph();
// create an initial estimate
shared_config initialEstimate(new VectorConfig(feas)); // must start with feasible set
initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); // ground truth
shared_config initialEstimate(new VectorConfig);
initialEstimate->insert("x1", Vector_(2, 1.0, 1.0));
initialEstimate->insert("l1", Vector_(2, 1.0, 6.0));
initialEstimate->insert("l2", Vector_(2, -4.0, 0.0)); // starting with a separate reference frame
initialEstimate->insert("x2", Vector_(2, 0.0, 0.0)); // other pose starts at origin
@ -146,6 +148,38 @@ TEST ( SQPOptimizer, simple_case ) {
CHECK(assert_equal(actual, expected));
}
/* ********************************************************************* */
TEST ( SQPOptimizer, map_warp ) {
// get a graph
NLGraph graph = linearMapWarpGraph();
// create an initial estimate
shared_config initialEstimate(new VectorConfig);
initialEstimate->insert("x1", Vector_(2, 1.0, 1.0));
initialEstimate->insert("l1", Vector_(2, 1.0, 6.0));
initialEstimate->insert("l2", Vector_(2, -4.0, 0.0)); // starting with a separate reference frame
initialEstimate->insert("x2", Vector_(2, 0.0, 0.0)); // other pose starts at origin
// create an ordering
Ordering ordering;
ordering += "x1", "x2", "l1", "l2";
// create an optimizer
Optimizer optimizer(graph, ordering, initialEstimate);
// perform an iteration of optimization
Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
// get the config back out and verify
VectorConfig actual = *(oneIteration.config());
VectorConfig expected;
expected.insert("x1", Vector_(2, 1.0, 1.0));
expected.insert("l1", Vector_(2, 1.0, 6.0));
expected.insert("l2", Vector_(2, 1.0, 6.0));
expected.insert("x2", Vector_(2, 5.0, 6.0));
CHECK(assert_equal(actual, expected));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */