Added testSQP demo to show a simple problem solved by Sequential Quadratic Programming. To see the outputs, set verbose=true at the start of the test.
Alex Cunningham
parent
e549dd5305
commit
20830a1534
38
.cproject
38
.cproject
|
|
@ -300,6 +300,7 @@
|
|||
<buildTargets>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -307,6 +308,7 @@
|
|||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -322,6 +324,7 @@
|
|||
</target>
|
||||
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -337,7 +340,6 @@
|
|||
</target>
|
||||
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -345,6 +347,7 @@
|
|||
</target>
|
||||
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -352,7 +355,6 @@
|
|||
</target>
|
||||
<target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianConditional.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -360,6 +362,7 @@
|
|||
</target>
|
||||
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -367,6 +370,7 @@
|
|||
</target>
|
||||
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -374,7 +378,6 @@
|
|||
</target>
|
||||
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearOptimizer.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -382,6 +385,7 @@
|
|||
</target>
|
||||
<target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -389,6 +393,7 @@
|
|||
</target>
|
||||
<target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -396,7 +401,6 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -404,6 +408,7 @@
|
|||
</target>
|
||||
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -411,7 +416,6 @@
|
|||
</target>
|
||||
<target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVectorConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -419,7 +423,6 @@
|
|||
</target>
|
||||
<target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -427,6 +430,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -434,6 +438,7 @@
|
|||
</target>
|
||||
<target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -441,6 +446,7 @@
|
|||
</target>
|
||||
<target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeGaussianFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -448,6 +454,7 @@
|
|||
</target>
|
||||
<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -455,7 +462,6 @@
|
|||
</target>
|
||||
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -463,6 +469,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -470,7 +477,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -478,6 +484,7 @@
|
|||
</target>
|
||||
<target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -485,6 +492,7 @@
|
|||
</target>
|
||||
<target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -492,7 +500,6 @@
|
|||
</target>
|
||||
<target name="testInference.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testInference.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -500,7 +507,6 @@
|
|||
</target>
|
||||
<target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -508,15 +514,21 @@
|
|||
</target>
|
||||
<target name="testNonlinearEquality.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearEquality.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSQP.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSQP.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -524,7 +536,6 @@
|
|||
</target>
|
||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -532,7 +543,6 @@
|
|||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -113,7 +113,7 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
|
|||
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
|
||||
sources += NonlinearFactor.cpp
|
||||
sources += NonlinearEquality.h
|
||||
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality
|
||||
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP
|
||||
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
|
||||
testNonlinearFactor_LDADD = libgtsam.la
|
||||
testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
|
||||
|
|
@ -122,6 +122,8 @@ testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp
|
|||
testNonlinearOptimizer_LDADD = libgtsam.la
|
||||
testNonlinearEquality_SOURCES = testNonlinearEquality.cpp
|
||||
testNonlinearEquality_LDADD = libgtsam.la
|
||||
testSQP_SOURCES = testSQP.cpp
|
||||
testSQP_LDADD = libgtsam.la
|
||||
|
||||
# geometry
|
||||
sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
|
||||
|
|
|
|||
|
|
@ -0,0 +1,125 @@
|
|||
/*
|
||||
* @file testSQP.cpp
|
||||
* @brief demos of SQP using existing gtsam components
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
#include <boost/assign/std/map.hpp> // for insert
|
||||
#include <boost/foreach.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <GaussianFactorGraph.h>
|
||||
#include <Ordering.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace boost::assign;
|
||||
|
||||
// trick from some reading group
|
||||
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
|
||||
/**
|
||||
* This example uses a nonlinear objective function and
|
||||
* nonlinear equality constraint. The formulation is actually
|
||||
* the Choleski form that creates the full Hessian explicitly,
|
||||
* which should really be avoided with our QR-based machinery
|
||||
*/
|
||||
TEST (SQP, problem1 ) {
|
||||
bool verbose = false;
|
||||
// use a nonlinear function of f(x) = x^2+y^2
|
||||
// nonlinear equality constraint: g(x) = x^2-5-y=0
|
||||
// Lagrangian: f(x) + lam*g(x)
|
||||
|
||||
// state structure: [x y lam]
|
||||
VectorConfig init, state;
|
||||
init.insert("x", Vector_(1, 1.0));
|
||||
init.insert("y", Vector_(1, 1.0));
|
||||
init.insert("lam", Vector_(1, 1.0));
|
||||
state = init;
|
||||
|
||||
if (verbose) init.print("Initial State");
|
||||
|
||||
// loop until convergence
|
||||
int maxIt = 50;
|
||||
for (int i = 0; i<maxIt; ++i) {
|
||||
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
|
||||
|
||||
// extract the states
|
||||
double x, y, lam;
|
||||
x = state["x"](0);
|
||||
y = state["y"](0);
|
||||
lam = state["lam"](0);
|
||||
|
||||
// calculate the components
|
||||
Matrix H1, H2, gradG;
|
||||
Vector gradL, gx;
|
||||
|
||||
// hessian of lagrangian function, in two columns:
|
||||
H1 = Matrix_(2,1,
|
||||
2.0+2.0*lam,
|
||||
0.0);
|
||||
H2 = Matrix_(2,1,
|
||||
0.0,
|
||||
2.0);
|
||||
|
||||
// deriviative of lagrangian function
|
||||
gradL = Vector_(2,
|
||||
2.0*x*(1+lam),
|
||||
2.0*y-lam);
|
||||
|
||||
// constraint derivatives
|
||||
gradG = Matrix_(2,1,
|
||||
2.0*x,
|
||||
0.0);
|
||||
|
||||
// constraint value
|
||||
gx = Vector_(1,
|
||||
x*x-5-y);
|
||||
|
||||
// create a factor for the states
|
||||
GaussianFactor::shared_ptr f1(new
|
||||
GaussianFactor("x", H1, "y", H2, "lam", gradG, gradL, 1.0));
|
||||
|
||||
// create a factor for the lagrange multiplier
|
||||
GaussianFactor::shared_ptr f2(new
|
||||
GaussianFactor("x", -sub(gradG, 0, 1, 0, 1),
|
||||
"y", -sub(gradG, 1, 2, 0, 1), -gx, 0.0));
|
||||
|
||||
// construct graph
|
||||
GaussianFactorGraph fg;
|
||||
fg.push_back(f1);
|
||||
fg.push_back(f2);
|
||||
if (verbose) fg.print("Graph");
|
||||
|
||||
// solve
|
||||
Ordering ord;
|
||||
ord += "x", "y", "lam";
|
||||
VectorConfig delta = fg.optimize(ord);
|
||||
if (verbose) delta.print("Delta");
|
||||
|
||||
// update initial estimate
|
||||
double gain = 0.3;
|
||||
VectorConfig newState;
|
||||
newState.insert("x", state["x"]-gain*delta["x"]);
|
||||
newState.insert("y", state["y"]-gain*delta["y"]);
|
||||
newState.insert("lam", state["lam"]-gain*delta["lam"]);
|
||||
state = newState;
|
||||
|
||||
if (verbose) state.print("Updated State");
|
||||
}
|
||||
|
||||
// verify that it converges to the nearest optimal point
|
||||
VectorConfig expected;
|
||||
expected.insert("x", Vector_(1, 2.12));
|
||||
expected.insert("y", Vector_(1, -0.5));
|
||||
CHECK(assert_equal(state["x"], expected["x"], 1e-2));
|
||||
CHECK(assert_equal(state["y"], expected["y"], 1e-2));
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue