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

Removed SQP optimizer and moved remaining SQP optimizer tests into testSQP. All equality constraints should be fully functional now. Inequality constraints still to come.

release/4.3a0
Alex Cunningham 2010-02-06 14:48:46 +00:00
parent 219dfd262d
commit f88438bab4
6 changed files with 325 additions and 701 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
.cproject
View File

@ -16,35 +16,35 @@
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactName="gtsam" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" name="MacOSX GCC" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
<additionalInput kind="additionalinput" paths="$(LIBS)"/>
</inputType>
</tool>
<tool id="cdt.managedbuild.tool.gnu.assembler.macosx.base.85834184" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.macosx.base">
<inputType id="cdt.managedbuild.tool.gnu.assembler.input.1787895621" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.archiver.macosx.base.217086069" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.macosx.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base.1629258328" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base">
<option id="gnu.cpp.compiler.option.include.paths.1552452888" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;${HOME}/include&quot;"/>
</option>
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1833545667" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.macosx.base.1347102680" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.macosx.base">
<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1718446861" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
</tool>
</toolChain>
</folderInfo>
</configuration>
</storageModule>
<configuration artifactName="gtsam" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" name="MacOSX GCC" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
<additionalInput kind="additionalinput" paths="$(LIBS)"/>
</inputType>
</tool>
<tool id="cdt.managedbuild.tool.gnu.assembler.macosx.base.85834184" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.macosx.base">
<inputType id="cdt.managedbuild.tool.gnu.assembler.input.1787895621" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.archiver.macosx.base.217086069" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.macosx.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base.1629258328" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base">
<option id="gnu.cpp.compiler.option.include.paths.1552452888" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;${HOME}/include&quot;"/>
</option>
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1833545667" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.macosx.base.1347102680" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.macosx.base">
<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1718446861" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
</tool>
</toolChain>
</folderInfo>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
@ -538,14 +538,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -822,6 +814,6 @@
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="gtsam.null.1344331286" name="gtsam"/>
</storageModule>
<project id="gtsam.null.1344331286" name="gtsam"/>
</storageModule>
</cproject>

5
cpp/Makefile.am
View File

@ -139,12 +139,9 @@ testNonlinearEquality_SOURCES = testNonlinearEquality.cpp
testNonlinearEquality_LDADD = libgtsam.la
# SQP
headers += SQPOptimizer.h SQPOptimizer-inl.h
check_PROGRAMS += testSQP testSQPOptimizer
check_PROGRAMS += testSQP
testSQP_SOURCES = $(example) testSQP.cpp
testSQP_LDADD = libgtsam.la
testSQPOptimizer_SOURCES = testSQPOptimizer.cpp
testSQPOptimizer_LDADD = libgtsam.la
# geometry
headers += Lie.h Lie-inl.h

177
cpp/SQPOptimizer-inl.h
View File

@ -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");
}
}

115
cpp/SQPOptimizer.h
View File

@ -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
View File

@ -816,6 +816,299 @@ TEST (SQP, stereo_sqp_noisy ) {
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); }
/* ************************************************************************* */

366
cpp/testSQPOptimizer.cpp
View File

@ -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); }
/* ************************************************************************* */