Removed SQP optimizer and moved remaining SQP optimizer tests into testSQP. All equality constraints should be fully functional now. Inequality constraints still to come.
Alex Cunningham
parent
219dfd262d
commit
f88438bab4
|
|
@ -538,14 +538,6 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="testSQPOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>testSQPOptimizer.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="testVSLAMConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testVSLAMConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
|
||||||
|
|
@ -139,12 +139,9 @@ testNonlinearEquality_SOURCES = testNonlinearEquality.cpp
|
||||||
testNonlinearEquality_LDADD = libgtsam.la
|
testNonlinearEquality_LDADD = libgtsam.la
|
||||||
|
|
||||||
# SQP
|
# SQP
|
||||||
headers += SQPOptimizer.h SQPOptimizer-inl.h
|
check_PROGRAMS += testSQP
|
||||||
check_PROGRAMS += testSQP testSQPOptimizer
|
|
||||||
testSQP_SOURCES = $(example) testSQP.cpp
|
testSQP_SOURCES = $(example) testSQP.cpp
|
||||||
testSQP_LDADD = libgtsam.la
|
testSQP_LDADD = libgtsam.la
|
||||||
testSQPOptimizer_SOURCES = testSQPOptimizer.cpp
|
|
||||||
testSQPOptimizer_LDADD = libgtsam.la
|
|
||||||
|
|
||||||
# geometry
|
# geometry
|
||||||
headers += Lie.h Lie-inl.h
|
headers += Lie.h Lie-inl.h
|
||||||
|
|
|
||||||
|
|
@ -1,177 +0,0 @@
|
||||||
/*
|
|
||||||
* @file SQPOptimizer-inl.h
|
|
||||||
* @brief Implementation of the SQP Optimizer
|
|
||||||
* @author Alex Cunningham
|
|
||||||
*/
|
|
||||||
|
|
||||||
#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 "NonlinearFactorGraph.h"
|
|
||||||
#include "SQPOptimizer.h"
|
|
||||||
|
|
||||||
// implementations
|
|
||||||
#include "NonlinearConstraint-inl.h"
|
|
||||||
#include "NonlinearFactorGraph-inl.h"
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
using namespace boost::assign;
|
|
||||||
|
|
||||||
namespace gtsam {
|
|
||||||
|
|
||||||
/* **************************************************************** */
|
|
||||||
template <class G, class C>
|
|
||||||
double constraintError(const G& graph, const C& config) {
|
|
||||||
// local typedefs
|
|
||||||
typedef typename G::const_iterator const_iterator;
|
|
||||||
typedef NonlinearConstraint<C> NLConstraint;
|
|
||||||
typedef boost::shared_ptr<NLConstraint > shared_c;
|
|
||||||
|
|
||||||
// accumulate error
|
|
||||||
double error = 0;
|
|
||||||
|
|
||||||
// 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) {
|
|
||||||
Vector e = constraint->unwhitenedError(config);
|
|
||||||
error += inner_prod(trans(e),e);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return error;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* **************************************************************** */
|
|
||||||
template <class G, class C>
|
|
||||||
SQPOptimizer<G,C>::SQPOptimizer(const G& graph, const Ordering& ordering,
|
|
||||||
shared_config config)
|
|
||||||
: graph_(&graph), ordering_(&ordering), full_ordering_(ordering),
|
|
||||||
config_(config), lagrange_config_(new VectorConfig), error_(graph.error(*config)),
|
|
||||||
constraint_error_(constraintError(graph, *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), full_ordering_(ordering),
|
|
||||||
config_(config), lagrange_config_(lagrange), error_(graph.error(*config)),
|
|
||||||
constraint_error_(constraintError(graph, *config))
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
/* **************************************************************** */
|
|
||||||
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;
|
|
||||||
|
|
||||||
// prepare an ordering of lagrange multipliers to remove
|
|
||||||
Ordering keysToRemove;
|
|
||||||
|
|
||||||
// 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 (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_);
|
|
||||||
if (verbose) f->print("Constrained Factor");
|
|
||||||
if (verbose) c->print("Constraint");
|
|
||||||
fg.push_back(f);
|
|
||||||
fg.push_back(c);
|
|
||||||
} else {
|
|
||||||
if (verbose) constraint->print("Skipping...");
|
|
||||||
keysToRemove += constraint->lagrangeKey();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (verbose) fg.print("Before Optimization");
|
|
||||||
|
|
||||||
// optimize linear graph to get full delta config
|
|
||||||
VectorConfig delta = fg.optimize(full_ordering_.subtract(keysToRemove));
|
|
||||||
|
|
||||||
if (verbose) delta.print("Delta Config");
|
|
||||||
|
|
||||||
// update both state variables
|
|
||||||
shared_config newConfig(new C(expmap(*config_, delta)));
|
|
||||||
shared_vconfig newLambdas(new VectorConfig(expmap(*lagrange_config_, delta)));
|
|
||||||
|
|
||||||
// construct a new optimizer
|
|
||||||
return SQPOptimizer<G, C>(*graph_, full_ordering_, newConfig, newLambdas);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* **************************************************************** */
|
|
||||||
template<class G, class C>
|
|
||||||
SQPOptimizer<G, C> SQPOptimizer<G, C>::iterateSolve(double relThresh, double absThresh,
|
|
||||||
double constraintThresh, size_t maxIterations, Verbosity v) const {
|
|
||||||
bool verbose = v == SQPOptimizer<G, C>::FULL;
|
|
||||||
|
|
||||||
// do an iteration
|
|
||||||
SQPOptimizer<G, C> next = iterate(v);
|
|
||||||
|
|
||||||
// if converged or out of iterations, return result
|
|
||||||
if (maxIterations == 1 ||
|
|
||||||
next.checkConvergence(relThresh, absThresh, constraintThresh,
|
|
||||||
error_, constraint_error_))
|
|
||||||
return next;
|
|
||||||
else // otherwise, recurse with a lower maxIterations
|
|
||||||
return next.iterateSolve(relThresh, absThresh, constraintThresh,
|
|
||||||
maxIterations-1, v);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* **************************************************************** */
|
|
||||||
template<class G, class C>
|
|
||||||
bool SQPOptimizer<G, C>::checkConvergence(double relThresh, double absThresh,
|
|
||||||
double constraintThresh, double full_error, double constraint_error) const {
|
|
||||||
// if error sufficiently low, then the system has converged
|
|
||||||
if (error_ < absThresh && constraint_error_ < constraintThresh)
|
|
||||||
return true;
|
|
||||||
|
|
||||||
// TODO: determine other cases
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* **************************************************************** */
|
|
||||||
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");
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
@ -1,115 +0,0 @@
|
||||||
/**
|
|
||||||
* @file SQPOptimizer.h
|
|
||||||
* @brief Interface for a generic SQP-based nonlinear optimization engine
|
|
||||||
* @author Alex Cunningham
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include "Ordering.h"
|
|
||||||
#include "VectorConfig.h"
|
|
||||||
|
|
||||||
namespace gtsam {
|
|
||||||
|
|
||||||
/**
|
|
||||||
* This class is an engine for performing SQP-based optimization
|
|
||||||
* It stores a graph, a config, and needs a specific ordering, and
|
|
||||||
* then will perform optimization iterations in a functional way.
|
|
||||||
*/
|
|
||||||
template<class FactorGraph, class Config>
|
|
||||||
class SQPOptimizer {
|
|
||||||
|
|
||||||
public:
|
|
||||||
// verbosity level
|
|
||||||
typedef enum {
|
|
||||||
SILENT,
|
|
||||||
FULL
|
|
||||||
} Verbosity;
|
|
||||||
|
|
||||||
// useful for storing configurations
|
|
||||||
typedef boost::shared_ptr<const Config> shared_config;
|
|
||||||
typedef boost::shared_ptr<VectorConfig> shared_vconfig;
|
|
||||||
|
|
||||||
private:
|
|
||||||
// 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_;
|
|
||||||
|
|
||||||
public:
|
|
||||||
/**
|
|
||||||
* Standard external constructor
|
|
||||||
* @param graph is the nonlinear graph to optimize
|
|
||||||
* @param ordering is the elimination ordering to use
|
|
||||||
* @param config is the initial configuration for the real variables
|
|
||||||
*/
|
|
||||||
SQPOptimizer(const FactorGraph& graph, const Ordering& ordering,
|
|
||||||
shared_config config);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Constructor that includes a lagrange initialization. Primarily
|
|
||||||
* for internal iterations, but if the user has an idea of what a good
|
|
||||||
* set of lagrange multipliers is, they can specify them, assuming that
|
|
||||||
* the naming convention is the same as the internal system.
|
|
||||||
* @param graph is the nonlinear graph to optimize
|
|
||||||
* @param ordering is the elimination ordering to use
|
|
||||||
* @param config is the initial configuration for the real variables
|
|
||||||
* @param lagrange is the configuration of lagrange multipliers
|
|
||||||
*/
|
|
||||||
SQPOptimizer(const FactorGraph& graph, const Ordering& ordering,
|
|
||||||
shared_config config, shared_vconfig lagrange);
|
|
||||||
|
|
||||||
/// Access functions
|
|
||||||
const FactorGraph* graph() const { return graph_; }
|
|
||||||
const Ordering* ordering() const { return ordering_; }
|
|
||||||
shared_config config() const { return config_; }
|
|
||||||
shared_vconfig configLagrange() const { return lagrange_config_; }
|
|
||||||
double error() const { return error_; }
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Primary optimization iteration, updates the configs
|
|
||||||
* @return a new optimization object with updated values
|
|
||||||
*/
|
|
||||||
SQPOptimizer<FactorGraph, Config> iterate(Verbosity verbosity=SILENT) const;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Iterates recursively until converence occurs
|
|
||||||
* @param relThresh minimum change in error between iterations
|
|
||||||
* @param absThresh minimum error necessary to converge
|
|
||||||
* @param constraintThresh minimum constraint error to be feasible
|
|
||||||
* @param maxIterations is the maximum number of iterations
|
|
||||||
* @param verbosity controls output print statements
|
|
||||||
* @return a new optimization object with final values
|
|
||||||
*/
|
|
||||||
SQPOptimizer<FactorGraph, Config>
|
|
||||||
iterateSolve(double relThresh, double absThresh, double constraintThresh,
|
|
||||||
size_t maxIterations = 10, Verbosity verbosity=SILENT) const;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Checks whether convergence has occurred, and returns true if
|
|
||||||
* the solution will not get better, based on the previous error conditions.
|
|
||||||
* @param full_error is the error all the factors and constraints
|
|
||||||
* @param constraint_error is the error of just the constraints
|
|
||||||
* @param relThresh is the relative threshold between
|
|
||||||
* @return true if the problem has converged
|
|
||||||
*/
|
|
||||||
bool checkConvergence(double relThresh, double absThresh,
|
|
||||||
double constraintThresh, double full_error, double constraint_error) const;
|
|
||||||
|
|
||||||
/** Standard print function with optional name */
|
|
||||||
void print(const std::string& s);
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
293
cpp/testSQP.cpp
293
cpp/testSQP.cpp
|
|
@ -816,6 +816,299 @@ TEST (SQP, stereo_sqp_noisy ) {
|
||||||
CHECK(assert_equal(*truthConfig,*actual, 1e-5));
|
CHECK(assert_equal(*truthConfig,*actual, 1e-5));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
|
||||||
|
|
||||||
|
// typedefs
|
||||||
|
//typedef simulated2D::Config Config2D;
|
||||||
|
//typedef boost::shared_ptr<Config2D> shared_config;
|
||||||
|
//typedef NonlinearFactorGraph<Config2D> NLGraph;
|
||||||
|
//typedef boost::shared_ptr<NonlinearFactor<Config2D> > shared;
|
||||||
|
|
||||||
|
namespace map_warp_example {
|
||||||
|
typedef NonlinearConstraint1<
|
||||||
|
Config2D, simulated2D::PoseKey, Point2> NLC1;
|
||||||
|
//typedef NonlinearConstraint2<
|
||||||
|
// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2;
|
||||||
|
} // \namespace map_warp_example
|
||||||
|
|
||||||
|
/* ********************************************************************* */
|
||||||
|
// Example that moves two separate maps into the same frame of reference
|
||||||
|
// Note that this is a linear example, so it should converge in one step
|
||||||
|
/* ********************************************************************* */
|
||||||
|
|
||||||
|
namespace sqp_LinearMapWarp2 {
|
||||||
|
// binary constraint between landmarks
|
||||||
|
/** g(x) = x-y = 0 */
|
||||||
|
Vector g_func(const Config2D& config, const simulated2D::PointKey& key1, const simulated2D::PointKey& key2) {
|
||||||
|
Point2 p = config[key1]-config[key2];
|
||||||
|
return Vector_(2, p.x(), p.y());
|
||||||
|
}
|
||||||
|
|
||||||
|
/** jacobian at l1 */
|
||||||
|
Matrix jac_g1(const Config2D& config) {
|
||||||
|
return eye(2);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** jacobian at l2 */
|
||||||
|
Matrix jac_g2(const Config2D& config) {
|
||||||
|
return -1*eye(2);
|
||||||
|
}
|
||||||
|
} // \namespace sqp_LinearMapWarp2
|
||||||
|
|
||||||
|
namespace sqp_LinearMapWarp1 {
|
||||||
|
// Unary Constraint on x1
|
||||||
|
/** g(x) = x -[1;1] = 0 */
|
||||||
|
Vector g_func(const Config2D& config, const simulated2D::PoseKey& key) {
|
||||||
|
Point2 p = config[key]-Point2(1.0, 1.0);
|
||||||
|
return Vector_(2, p.x(), p.y());
|
||||||
|
}
|
||||||
|
|
||||||
|
/** jacobian at x1 */
|
||||||
|
Matrix jac_g(const Config2D& config) {
|
||||||
|
return eye(2);
|
||||||
|
}
|
||||||
|
} // \namespace sqp_LinearMapWarp12
|
||||||
|
|
||||||
|
//typedef NonlinearOptimizer<NLGraph, Config2D> Optimizer;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates the graph with each robot seeing the landmark, and it is
|
||||||
|
* known that it is the same landmark
|
||||||
|
*/
|
||||||
|
boost::shared_ptr<Graph2D> linearMapWarpGraph() {
|
||||||
|
using namespace map_warp_example;
|
||||||
|
// keys
|
||||||
|
simulated2D::PoseKey x1(1), x2(2);
|
||||||
|
simulated2D::PointKey l1(1), l2(2);
|
||||||
|
|
||||||
|
// constant constraint on x1
|
||||||
|
LagrangeKey L1(1);
|
||||||
|
shared_ptr<NLC1> c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1),
|
||||||
|
x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1),
|
||||||
|
2, L1));
|
||||||
|
|
||||||
|
// measurement from x1 to l1
|
||||||
|
Point2 z1(0.0, 5.0);
|
||||||
|
shared f1(new simulated2D::GenericMeasurement<Config2D>(z1, sigma, x1,l1));
|
||||||
|
|
||||||
|
// measurement from x2 to l2
|
||||||
|
Point2 z2(-4.0, 0.0);
|
||||||
|
shared f2(new simulated2D::GenericMeasurement<Config2D>(z2, sigma, x2,l2));
|
||||||
|
|
||||||
|
// equality constraint between l1 and l2
|
||||||
|
LagrangeKey L12(12);
|
||||||
|
shared_ptr<NLC2> c2 (new NLC2(
|
||||||
|
boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2),
|
||||||
|
l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1),
|
||||||
|
l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1),
|
||||||
|
2, L12));
|
||||||
|
|
||||||
|
// construct the graph
|
||||||
|
boost::shared_ptr<Graph2D> graph(new Graph2D());
|
||||||
|
graph->push_back(c1);
|
||||||
|
graph->push_back(c2);
|
||||||
|
graph->push_back(f1);
|
||||||
|
graph->push_back(f2);
|
||||||
|
|
||||||
|
return graph;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ********************************************************************* */
|
||||||
|
TEST ( SQPOptimizer, map_warp_initLam ) {
|
||||||
|
bool verbose = false;
|
||||||
|
// get a graph
|
||||||
|
boost::shared_ptr<Graph2D> graph = linearMapWarpGraph();
|
||||||
|
|
||||||
|
// keys
|
||||||
|
simulated2D::PoseKey x1(1), x2(2);
|
||||||
|
simulated2D::PointKey l1(1), l2(2);
|
||||||
|
LagrangeKey L1(1), L12(12);
|
||||||
|
|
||||||
|
// create an initial estimate
|
||||||
|
shared_ptr<Config2D> initialEstimate(new Config2D);
|
||||||
|
initialEstimate->insert(x1, Point2(1.0, 1.0));
|
||||||
|
initialEstimate->insert(l1, Point2(1.0, 6.0));
|
||||||
|
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
|
||||||
|
initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin
|
||||||
|
initialEstimate->insert(L12, Vector_(2, 1.0, 1.0));
|
||||||
|
initialEstimate->insert(L1, Vector_(2, 1.0, 1.0));
|
||||||
|
|
||||||
|
// create an ordering
|
||||||
|
shared_ptr<Ordering> ordering(new Ordering());
|
||||||
|
*ordering += "x1", "x2", "l1", "l2", "L12", "L1";
|
||||||
|
|
||||||
|
// create an optimizer
|
||||||
|
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
|
||||||
|
Optimizer optimizer(graph, initialEstimate, solver);
|
||||||
|
|
||||||
|
// perform an iteration of optimization
|
||||||
|
Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
|
||||||
|
|
||||||
|
// get the config back out and verify
|
||||||
|
Config2D actual = *(oneIteration.config());
|
||||||
|
Config2D expected;
|
||||||
|
expected.insert(x1, Point2(1.0, 1.0));
|
||||||
|
expected.insert(l1, Point2(1.0, 6.0));
|
||||||
|
expected.insert(l2, Point2(1.0, 6.0));
|
||||||
|
expected.insert(x2, Point2(5.0, 6.0));
|
||||||
|
expected.insert(L1, Vector_(2, 1.0, 1.0));
|
||||||
|
expected.insert(L12, Vector_(2, 6.0, 7.0));
|
||||||
|
CHECK(assert_equal(expected, actual));
|
||||||
|
}
|
||||||
|
|
||||||
|
///* ********************************************************************* */
|
||||||
|
//// 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.
|
||||||
|
///* ********************************************************************* */
|
||||||
|
//
|
||||||
|
//typedef NonlinearConstraint2<Config2D, PoseKey, Point2, PointKey, Point2> AvoidConstraint;
|
||||||
|
//typedef shared_ptr<AvoidConstraint> shared_a;
|
||||||
|
//typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> PoseConstraint;
|
||||||
|
//typedef shared_ptr<PoseConstraint> shared_pc;
|
||||||
|
//typedef NonlinearEquality<Config2D, simulated2D::PointKey, Point2> ObstacleConstraint;
|
||||||
|
//typedef shared_ptr<ObstacleConstraint> shared_oc;
|
||||||
|
//
|
||||||
|
//
|
||||||
|
//namespace sqp_avoid1 {
|
||||||
|
//// avoidance radius
|
||||||
|
//double radius = 1.0;
|
||||||
|
//
|
||||||
|
//// binary avoidance constraint
|
||||||
|
///** g(x) = ||x2-obs||^2 - radius^2 > 0 */
|
||||||
|
//Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) {
|
||||||
|
// double dist2 = config[x].dist(config[obs]);
|
||||||
|
// double thresh = radius*radius;
|
||||||
|
// return Vector_(1, dist2-thresh);
|
||||||
|
//}
|
||||||
|
//
|
||||||
|
///** jacobian at pose */
|
||||||
|
//Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) {
|
||||||
|
// Point2 p = config[x]-config[obs];
|
||||||
|
// return Matrix_(1,2, 2.0*p.x(), 2.0*p.y());
|
||||||
|
//}
|
||||||
|
//
|
||||||
|
///** jacobian at obstacle */
|
||||||
|
//Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) {
|
||||||
|
// Point2 p = config[x]-config[obs];
|
||||||
|
// return Matrix_(1,2, -2.0*p.x(), -2.0*p.y());
|
||||||
|
//}
|
||||||
|
//}
|
||||||
|
//
|
||||||
|
//pair<NLGraph, Config2D> obstacleAvoidGraph() {
|
||||||
|
// // Keys
|
||||||
|
// PoseKey x1(1), x2(2), x3(3);
|
||||||
|
// PointKey l1(1);
|
||||||
|
// LagrangeKey L20(20);
|
||||||
|
//
|
||||||
|
// // Constrained Points
|
||||||
|
// Point2 pt_x1,
|
||||||
|
// pt_x3(10.0, 0.0),
|
||||||
|
// pt_l1(5.0, -0.5);
|
||||||
|
//
|
||||||
|
// shared_pc e1(new PoseConstraint(x1, pt_x1));
|
||||||
|
// shared_pc e2(new PoseConstraint(x3, pt_x3));
|
||||||
|
// shared_oc e3(new ObstacleConstraint(l1, pt_l1));
|
||||||
|
//
|
||||||
|
// // measurement from x1 to x2
|
||||||
|
// Point2 x1x2(5.0, 0.0);
|
||||||
|
// shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2));
|
||||||
|
//
|
||||||
|
// // measurement from x2 to x3
|
||||||
|
// Point2 x2x3(5.0, 0.0);
|
||||||
|
// shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3));
|
||||||
|
//
|
||||||
|
// // create a binary inequality constraint that forces the middle point away from
|
||||||
|
// // the obstacle
|
||||||
|
// shared_a c1(new AvoidConstraint(boost::bind(sqp_avoid1::g_func, _1, x2, l1),
|
||||||
|
// x2, boost::bind(sqp_avoid1::jac_g1, _1, x2, l1),
|
||||||
|
// l1,boost::bind(sqp_avoid1::jac_g2, _1, x2, l1),
|
||||||
|
// 1, L20, false));
|
||||||
|
//
|
||||||
|
// // 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);
|
||||||
|
//
|
||||||
|
// // make a config of the fixed values, for convenience
|
||||||
|
// Config2D config;
|
||||||
|
// config.insert(x1, pt_x1);
|
||||||
|
// config.insert(x3, pt_x3);
|
||||||
|
// config.insert(l1, pt_l1);
|
||||||
|
//
|
||||||
|
// return make_pair(graph, config);
|
||||||
|
//}
|
||||||
|
//
|
||||||
|
///* ********************************************************************* */
|
||||||
|
//TEST ( SQPOptimizer, inequality_avoid ) {
|
||||||
|
// // create the graph
|
||||||
|
// NLGraph graph; Config2D feasible;
|
||||||
|
// boost::tie(graph, feasible) = obstacleAvoidGraph();
|
||||||
|
//
|
||||||
|
// // create the rest of the config
|
||||||
|
// shared_ptr<Config2D> init(new Config2D(feasible));
|
||||||
|
// PoseKey x2(2);
|
||||||
|
// init->insert(x2, Point2(5.0, 100.0));
|
||||||
|
//
|
||||||
|
// // create an ordering
|
||||||
|
// Ordering ord;
|
||||||
|
// ord += "x1", "x2", "x3", "l1";
|
||||||
|
//
|
||||||
|
// // create an optimizer
|
||||||
|
// Optimizer optimizer(graph, ord, init);
|
||||||
|
//
|
||||||
|
// // perform an iteration of optimization
|
||||||
|
// // NOTE: the constraint will be inactive in the first iteration,
|
||||||
|
// // so it will violate the constraint after one iteration
|
||||||
|
// Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT);
|
||||||
|
//
|
||||||
|
// Config2D exp1(feasible);
|
||||||
|
// exp1.insert(x2, Point2(5.0, 0.0));
|
||||||
|
// CHECK(assert_equal(exp1, *(afterOneIteration.config())));
|
||||||
|
//
|
||||||
|
// // the second iteration will activate the constraint and force the
|
||||||
|
// // config to a viable configuration.
|
||||||
|
// Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT);
|
||||||
|
//
|
||||||
|
// Config2D exp2(feasible);
|
||||||
|
// exp2.insert(x2, Point2(5.0, 0.5));
|
||||||
|
// CHECK(assert_equal(exp2, *(after2ndIteration.config())));
|
||||||
|
//}
|
||||||
|
//
|
||||||
|
///* ********************************************************************* */
|
||||||
|
//TEST ( SQPOptimizer, inequality_avoid_iterative ) {
|
||||||
|
// // create the graph
|
||||||
|
// NLGraph graph; Config2D feasible;
|
||||||
|
// boost::tie(graph, feasible) = obstacleAvoidGraph();
|
||||||
|
//
|
||||||
|
// // create the rest of the config
|
||||||
|
// shared_ptr<Config2D> init(new Config2D(feasible));
|
||||||
|
// PoseKey x2(2);
|
||||||
|
// init->insert(x2, Point2(5.0, 100.0));
|
||||||
|
//
|
||||||
|
// // create an ordering
|
||||||
|
// Ordering ord;
|
||||||
|
// ord += "x1", "x2", "x3", "l1";
|
||||||
|
//
|
||||||
|
// // create an optimizer
|
||||||
|
// Optimizer optimizer(graph, ord, init);
|
||||||
|
//
|
||||||
|
// double relThresh = 1e-5; // minimum change in error between iterations
|
||||||
|
// double absThresh = 1e-5; // minimum error necessary to converge
|
||||||
|
// double constraintThresh = 1e-9; // minimum constraint error to be feasible
|
||||||
|
// Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh);
|
||||||
|
//
|
||||||
|
// // verify
|
||||||
|
// Config2D exp2(feasible);
|
||||||
|
// exp2.insert(x2, Point2(5.0, 0.5));
|
||||||
|
// CHECK(assert_equal(exp2, *(final.config())));
|
||||||
|
//}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -1,366 +0,0 @@
|
||||||
/*
|
|
||||||
* @file testSQPOptimizer.cpp
|
|
||||||
* @brief tests the optimization algorithm for nonlinear graphs with nonlinear constraints
|
|
||||||
* @author Alex Cunningham
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
|
||||||
#include <boost/assign/std/list.hpp> // for operator +=
|
|
||||||
#include <boost/assign/std/map.hpp> // for insert
|
|
||||||
#include <boost/bind.hpp>
|
|
||||||
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <simulated2D.h>
|
|
||||||
#include "NonlinearFactorGraph.h"
|
|
||||||
#include "NonlinearConstraint.h"
|
|
||||||
#include "NonlinearEquality.h"
|
|
||||||
#include "VectorConfig.h"
|
|
||||||
#include "Ordering.h"
|
|
||||||
//#include "SQPOptimizer.h"
|
|
||||||
|
|
||||||
// implementations
|
|
||||||
#include "NonlinearConstraint-inl.h"
|
|
||||||
//#include "SQPOptimizer-inl.h"
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
using namespace gtsam;
|
|
||||||
using namespace boost;
|
|
||||||
using namespace boost::assign;
|
|
||||||
using namespace simulated2D;
|
|
||||||
|
|
||||||
static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
|
|
||||||
|
|
||||||
//// typedefs
|
|
||||||
//typedef simulated2D::Config Config2D;
|
|
||||||
//typedef boost::shared_ptr<Config2D> shared_config;
|
|
||||||
//typedef NonlinearFactorGraph<Config2D> NLGraph;
|
|
||||||
//typedef boost::shared_ptr<NonlinearFactor<Config2D> > shared;
|
|
||||||
//
|
|
||||||
//namespace map_warp_example {
|
|
||||||
//typedef NonlinearConstraint1<
|
|
||||||
// Config2D, simulated2D::PoseKey, Point2> NLC1;
|
|
||||||
//typedef NonlinearConstraint2<
|
|
||||||
// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2;
|
|
||||||
//} // \namespace map_warp_example
|
|
||||||
//
|
|
||||||
///* ********************************************************************* */
|
|
||||||
//// Example that moves two separate maps into the same frame of reference
|
|
||||||
//// Note that this is a linear example, so it should converge in one step
|
|
||||||
///* ********************************************************************* */
|
|
||||||
//
|
|
||||||
//namespace sqp_LinearMapWarp2 {
|
|
||||||
//// binary constraint between landmarks
|
|
||||||
///** g(x) = x-y = 0 */
|
|
||||||
//Vector g_func(const Config2D& config, const PointKey& key1, const PointKey& key2) {
|
|
||||||
// Point2 p = config[key1]-config[key2];
|
|
||||||
// return Vector_(2, p.x(), p.y());
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
///** jacobian at l1 */
|
|
||||||
//Matrix jac_g1(const Config2D& config) {
|
|
||||||
// return eye(2);
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
///** jacobian at l2 */
|
|
||||||
//Matrix jac_g2(const Config2D& config) {
|
|
||||||
// return -1*eye(2);
|
|
||||||
//}
|
|
||||||
//} // \namespace sqp_LinearMapWarp2
|
|
||||||
//
|
|
||||||
//namespace sqp_LinearMapWarp1 {
|
|
||||||
//// Unary Constraint on x1
|
|
||||||
///** g(x) = x -[1;1] = 0 */
|
|
||||||
//Vector g_func(const Config2D& config, const PoseKey& key) {
|
|
||||||
// Point2 p = config[key]-Point2(1.0, 1.0);
|
|
||||||
// return Vector_(2, p.x(), p.y());
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
///** jacobian at x1 */
|
|
||||||
//Matrix jac_g(const Config2D& config) {
|
|
||||||
// return eye(2);
|
|
||||||
//}
|
|
||||||
//} // \namespace sqp_LinearMapWarp12
|
|
||||||
//
|
|
||||||
////typedef NonlinearOptimizer<NLGraph, Config2D> Optimizer;
|
|
||||||
//
|
|
||||||
///**
|
|
||||||
// * Creates the graph with each robot seeing the landmark, and it is
|
|
||||||
// * known that it is the same landmark
|
|
||||||
// */
|
|
||||||
//NLGraph linearMapWarpGraph() {
|
|
||||||
// using namespace map_warp_example;
|
|
||||||
// // keys
|
|
||||||
// PoseKey x1(1), x2(2);
|
|
||||||
// PointKey l1(1), l2(2);
|
|
||||||
//
|
|
||||||
// // constant constraint on x1
|
|
||||||
// list<Symbol> keyx; keyx += "x1";
|
|
||||||
// LagrangeKey L1(1);
|
|
||||||
// shared_ptr<NLC1> c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1),
|
|
||||||
// x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1),
|
|
||||||
// 2, L1));
|
|
||||||
//
|
|
||||||
// // measurement from x1 to l1
|
|
||||||
// Point2 z1(0.0, 5.0);
|
|
||||||
// shared f1(new simulated2D::Measurement(z1, sigma, 1,1));
|
|
||||||
//
|
|
||||||
// // measurement from x2 to l2
|
|
||||||
// Point2 z2(-4.0, 0.0);
|
|
||||||
// shared f2(new simulated2D::Measurement(z2, sigma, 2,2));
|
|
||||||
//
|
|
||||||
// // equality constraint between l1 and l2
|
|
||||||
// LagrangeKey L12(12);
|
|
||||||
// list<Symbol> keys; keys += "l1", "l2";
|
|
||||||
// shared_ptr<NLC2> c2 (new NLC2(
|
|
||||||
// boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2),
|
|
||||||
// l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1),
|
|
||||||
// l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1),
|
|
||||||
// 2, L12));
|
|
||||||
//
|
|
||||||
// // construct the graph
|
|
||||||
// NLGraph graph;
|
|
||||||
// graph.push_back(c1);
|
|
||||||
// graph.push_back(c2);
|
|
||||||
// graph.push_back(f1);
|
|
||||||
// graph.push_back(f2);
|
|
||||||
//
|
|
||||||
// return graph;
|
|
||||||
//}
|
|
||||||
|
|
||||||
///* ********************************************************************* */
|
|
||||||
//TEST ( SQPOptimizer, map_warp_initLam ) {
|
|
||||||
// bool verbose = false;
|
|
||||||
// // get a graph
|
|
||||||
// NLGraph graph = linearMapWarpGraph();
|
|
||||||
//
|
|
||||||
// // keys
|
|
||||||
// PoseKey x1(1), x2(2);
|
|
||||||
// PointKey l1(1), l2(2);
|
|
||||||
// LagrangeKey L1(1), L12(12);
|
|
||||||
//
|
|
||||||
// // create an initial estimate
|
|
||||||
// shared_config initialEstimate(new Config2D);
|
|
||||||
// initialEstimate->insert(x1, Point2(1.0, 1.0));
|
|
||||||
// initialEstimate->insert(l1, Point2(1.0, 6.0));
|
|
||||||
// initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
|
|
||||||
// initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin
|
|
||||||
// initialEstimate->insert(L12, Vector_(2, 1.0, 1.0));
|
|
||||||
// initialEstimate->insert(L1, Vector_(2, 1.0, 1.0));
|
|
||||||
//
|
|
||||||
// // create an ordering
|
|
||||||
// Ordering ordering;
|
|
||||||
// ordering += "x1", "x2", "l1", "l2", "L12", "L1";
|
|
||||||
//
|
|
||||||
// // create an optimizer
|
|
||||||
// Optimizer optimizer(graph, ordering, initialEstimate);
|
|
||||||
// if (verbose) optimizer.print("Initialized Optimizer");
|
|
||||||
//
|
|
||||||
// // perform an iteration of optimization
|
|
||||||
// Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
|
|
||||||
//
|
|
||||||
// // get the config back out and verify
|
|
||||||
// Config2D actual = *(oneIteration.config());
|
|
||||||
// Config2D expected;
|
|
||||||
// expected.insert(x1, Point2(1.0, 1.0));
|
|
||||||
// expected.insert(l1, Point2(1.0, 6.0));
|
|
||||||
// expected.insert(l2, Point2(1.0, 6.0));
|
|
||||||
// expected.insert(x2, Point2(5.0, 6.0));
|
|
||||||
// CHECK(assert_equal(expected, actual));
|
|
||||||
//}
|
|
||||||
|
|
||||||
|
|
||||||
///* ********************************************************************* */
|
|
||||||
//TEST ( SQPOptimizer, map_warp ) {
|
|
||||||
// bool verbose = false;
|
|
||||||
// // get a graph
|
|
||||||
// NLGraph graph = linearMapWarpGraph();
|
|
||||||
// if (verbose) graph.print("Initial map warp graph");
|
|
||||||
//
|
|
||||||
// // keys
|
|
||||||
// PoseKey x1(1), x2(2);
|
|
||||||
// PointKey l1(1), l2(2);
|
|
||||||
//
|
|
||||||
// // create an initial estimate
|
|
||||||
// shared_config initialEstimate(new Config2D);
|
|
||||||
// initialEstimate->insert(x1, Point2(1.0, 1.0));
|
|
||||||
// initialEstimate->insert(l1, Point2(.0, 6.0));
|
|
||||||
// initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
|
|
||||||
// initialEstimate->insert(x2, Point2(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
|
|
||||||
// Config2D actual = *(oneIteration.config());
|
|
||||||
// Config2D expected;
|
|
||||||
// expected.insert(x1, Point2(1.0, 1.0));
|
|
||||||
// expected.insert(l1, Point2(1.0, 6.0));
|
|
||||||
// expected.insert(l2, Point2(1.0, 6.0));
|
|
||||||
// expected.insert(x2, Point2(5.0, 6.0));
|
|
||||||
// CHECK(assert_equal(expected, actual));
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
///* ********************************************************************* */
|
|
||||||
//// 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.
|
|
||||||
///* ********************************************************************* */
|
|
||||||
//
|
|
||||||
//typedef NonlinearConstraint2<Config2D, PoseKey, Point2, PointKey, Point2> AvoidConstraint;
|
|
||||||
//typedef shared_ptr<AvoidConstraint> shared_a;
|
|
||||||
//typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> PoseConstraint;
|
|
||||||
//typedef shared_ptr<PoseConstraint> shared_pc;
|
|
||||||
//typedef NonlinearEquality<Config2D, simulated2D::PointKey, Point2> ObstacleConstraint;
|
|
||||||
//typedef shared_ptr<ObstacleConstraint> shared_oc;
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//namespace sqp_avoid1 {
|
|
||||||
//// avoidance radius
|
|
||||||
//double radius = 1.0;
|
|
||||||
//
|
|
||||||
//// binary avoidance constraint
|
|
||||||
///** g(x) = ||x2-obs||^2 - radius^2 > 0 */
|
|
||||||
//Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) {
|
|
||||||
// double dist2 = config[x].dist(config[obs]);
|
|
||||||
// double thresh = radius*radius;
|
|
||||||
// return Vector_(1, dist2-thresh);
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
///** jacobian at pose */
|
|
||||||
//Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) {
|
|
||||||
// Point2 p = config[x]-config[obs];
|
|
||||||
// return Matrix_(1,2, 2.0*p.x(), 2.0*p.y());
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
///** jacobian at obstacle */
|
|
||||||
//Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) {
|
|
||||||
// Point2 p = config[x]-config[obs];
|
|
||||||
// return Matrix_(1,2, -2.0*p.x(), -2.0*p.y());
|
|
||||||
//}
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//pair<NLGraph, Config2D> obstacleAvoidGraph() {
|
|
||||||
// // Keys
|
|
||||||
// PoseKey x1(1), x2(2), x3(3);
|
|
||||||
// PointKey l1(1);
|
|
||||||
// LagrangeKey L20(20);
|
|
||||||
//
|
|
||||||
// // Constrained Points
|
|
||||||
// Point2 pt_x1,
|
|
||||||
// pt_x3(10.0, 0.0),
|
|
||||||
// pt_l1(5.0, -0.5);
|
|
||||||
//
|
|
||||||
// shared_pc e1(new PoseConstraint(x1, pt_x1));
|
|
||||||
// shared_pc e2(new PoseConstraint(x3, pt_x3));
|
|
||||||
// shared_oc e3(new ObstacleConstraint(l1, pt_l1));
|
|
||||||
//
|
|
||||||
// // measurement from x1 to x2
|
|
||||||
// Point2 x1x2(5.0, 0.0);
|
|
||||||
// shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2));
|
|
||||||
//
|
|
||||||
// // measurement from x2 to x3
|
|
||||||
// Point2 x2x3(5.0, 0.0);
|
|
||||||
// shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3));
|
|
||||||
//
|
|
||||||
// // create a binary inequality constraint that forces the middle point away from
|
|
||||||
// // the obstacle
|
|
||||||
// shared_a c1(new AvoidConstraint(boost::bind(sqp_avoid1::g_func, _1, x2, l1),
|
|
||||||
// x2, boost::bind(sqp_avoid1::jac_g1, _1, x2, l1),
|
|
||||||
// l1,boost::bind(sqp_avoid1::jac_g2, _1, x2, l1),
|
|
||||||
// 1, L20, false));
|
|
||||||
//
|
|
||||||
// // 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);
|
|
||||||
//
|
|
||||||
// // make a config of the fixed values, for convenience
|
|
||||||
// Config2D config;
|
|
||||||
// config.insert(x1, pt_x1);
|
|
||||||
// config.insert(x3, pt_x3);
|
|
||||||
// config.insert(l1, pt_l1);
|
|
||||||
//
|
|
||||||
// return make_pair(graph, config);
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
///* ********************************************************************* */
|
|
||||||
//TEST ( SQPOptimizer, inequality_avoid ) {
|
|
||||||
// // create the graph
|
|
||||||
// NLGraph graph; Config2D feasible;
|
|
||||||
// boost::tie(graph, feasible) = obstacleAvoidGraph();
|
|
||||||
//
|
|
||||||
// // create the rest of the config
|
|
||||||
// shared_ptr<Config2D> init(new Config2D(feasible));
|
|
||||||
// PoseKey x2(2);
|
|
||||||
// init->insert(x2, Point2(5.0, 100.0));
|
|
||||||
//
|
|
||||||
// // create an ordering
|
|
||||||
// Ordering ord;
|
|
||||||
// ord += "x1", "x2", "x3", "l1";
|
|
||||||
//
|
|
||||||
// // create an optimizer
|
|
||||||
// Optimizer optimizer(graph, ord, init);
|
|
||||||
//
|
|
||||||
// // perform an iteration of optimization
|
|
||||||
// // NOTE: the constraint will be inactive in the first iteration,
|
|
||||||
// // so it will violate the constraint after one iteration
|
|
||||||
// Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT);
|
|
||||||
//
|
|
||||||
// Config2D exp1(feasible);
|
|
||||||
// exp1.insert(x2, Point2(5.0, 0.0));
|
|
||||||
// CHECK(assert_equal(exp1, *(afterOneIteration.config())));
|
|
||||||
//
|
|
||||||
// // the second iteration will activate the constraint and force the
|
|
||||||
// // config to a viable configuration.
|
|
||||||
// Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT);
|
|
||||||
//
|
|
||||||
// Config2D exp2(feasible);
|
|
||||||
// exp2.insert(x2, Point2(5.0, 0.5));
|
|
||||||
// CHECK(assert_equal(exp2, *(after2ndIteration.config())));
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
///* ********************************************************************* */
|
|
||||||
//TEST ( SQPOptimizer, inequality_avoid_iterative ) {
|
|
||||||
// // create the graph
|
|
||||||
// NLGraph graph; Config2D feasible;
|
|
||||||
// boost::tie(graph, feasible) = obstacleAvoidGraph();
|
|
||||||
//
|
|
||||||
// // create the rest of the config
|
|
||||||
// shared_ptr<Config2D> init(new Config2D(feasible));
|
|
||||||
// PoseKey x2(2);
|
|
||||||
// init->insert(x2, Point2(5.0, 100.0));
|
|
||||||
//
|
|
||||||
// // create an ordering
|
|
||||||
// Ordering ord;
|
|
||||||
// ord += "x1", "x2", "x3", "l1";
|
|
||||||
//
|
|
||||||
// // create an optimizer
|
|
||||||
// Optimizer optimizer(graph, ord, init);
|
|
||||||
//
|
|
||||||
// double relThresh = 1e-5; // minimum change in error between iterations
|
|
||||||
// double absThresh = 1e-5; // minimum error necessary to converge
|
|
||||||
// double constraintThresh = 1e-9; // minimum constraint error to be feasible
|
|
||||||
// Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh);
|
|
||||||
//
|
|
||||||
// // verify
|
|
||||||
// Config2D exp2(feasible);
|
|
||||||
// exp2.insert(x2, Point2(5.0, 0.5));
|
|
||||||
// CHECK(assert_equal(exp2, *(final.config())));
|
|
||||||
//}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
||||||
/* ************************************************************************* */
|
|
||||||
Loading…
Reference in New Issue