82 lines
2.5 KiB
C++
82 lines
2.5 KiB
C++
/*
|
|
* @file SQPOptimizer-inl.h
|
|
* @brief Implementation of the SQP Optimizer
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <boost/foreach.hpp>
|
|
#include "GaussianFactorGraph.h"
|
|
#include "SQPOptimizer.h"
|
|
|
|
using namespace std;
|
|
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)
|
|
{
|
|
// TODO: assign a value to the lagrange config
|
|
|
|
}
|
|
|
|
/* **************************************************************** */
|
|
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)
|
|
{
|
|
|
|
}
|
|
|
|
/* **************************************************************** */
|
|
template<class G, class C>
|
|
SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
|
|
bool verbose = v == SQPOptimizer<G, C>::FULL;
|
|
|
|
// local typedefs
|
|
typedef typename G::const_iterator const_iterator;
|
|
typedef NonlinearConstraint<C> NLConstraint;
|
|
typedef boost::shared_ptr<NLConstraint > shared_c;
|
|
|
|
// linearize the graph
|
|
GaussianFactorGraph fg;
|
|
|
|
// 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);
|
|
if (constraint == NULL) {
|
|
// if a regular factor, linearize using the default linearization
|
|
GaussianFactor::shared_ptr f = (*factor)->linearize(*config_);
|
|
if (verbose) f->print("Regular Factor");
|
|
fg.push_back(f);
|
|
} else {
|
|
// if a constraint, linearize using the constraint method (2 configs)
|
|
GaussianFactor::shared_ptr f, c;
|
|
boost::tie(f,c) = constraint->linearize(*config_, *lagrange_config_);
|
|
if (verbose) f->print("Constrained Factor");
|
|
if (verbose) c->print("Constraint");
|
|
fg.push_back(f);
|
|
fg.push_back(c);
|
|
}
|
|
}
|
|
if (verbose) fg.print("Before Optimization");
|
|
|
|
// optimize linear graph to get full delta config
|
|
VectorConfig delta = fg.optimize(*ordering_).scale(-1.0);
|
|
|
|
if (verbose) delta.print("Delta Config");
|
|
|
|
// update both state variables
|
|
shared_config newConfig(new C(config_->exmap(delta)));
|
|
shared_vconfig newLamConfig(new VectorConfig(lagrange_config_->exmap(delta)));
|
|
|
|
// construct a new optimizer
|
|
return SQPOptimizer<G, C>(*graph_, *ordering_, newConfig, newLamConfig);
|
|
}
|
|
|
|
}
|