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

NonlinearConstraints are now implemented using the simple linearization method with quadratic merit function

release/4.3a0
Alex Cunningham 2010-07-18 19:55:53 +00:00
parent 864b66ea93
commit a3da89b63a
9 changed files with 848 additions and 740 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

329
.cproject
View File

@ -302,7 +302,6 @@
<buildTargets>
<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -310,7 +309,6 @@
</target>
<target name="clean" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -318,7 +316,6 @@
</target>
<target name="check" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -326,7 +323,6 @@
</target>
<target name="testGaussianConditional.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianConditional.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -334,7 +330,6 @@
</target>
<target name="testGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -342,7 +337,6 @@
</target>
<target name="timeGaussianFactor.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>timeGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -350,7 +344,6 @@
</target>
<target name="timeVectorConfig.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>timeVectorConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -358,7 +351,6 @@
</target>
<target name="testVectorBTree.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVectorBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -366,7 +358,6 @@
</target>
<target name="testVectorMap.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVectorMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -374,7 +365,6 @@
</target>
<target name="testNoiseModel.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNoiseModel.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -382,7 +372,6 @@
</target>
<target name="testBayesNetPreconditioner.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBayesNetPreconditioner.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -390,13 +379,12 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
@ -404,8 +392,16 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -413,6 +409,7 @@
</target>
<target name="clean" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -420,6 +417,7 @@
</target>
<target name="testBTree.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -427,6 +425,7 @@
</target>
<target name="testDSF.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -434,6 +433,7 @@
</target>
<target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -441,6 +441,7 @@
</target>
<target name="testMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -448,6 +449,7 @@
</target>
<target name="testSPQRUtil.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSPQRUtil.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -455,6 +457,7 @@
</target>
<target name="testVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -462,6 +465,7 @@
</target>
<target name="timeMatrix.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>timeMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -469,6 +473,7 @@
</target>
<target name="all" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -476,7 +481,6 @@
</target>
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -484,7 +488,6 @@
</target>
<target name="clean" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -500,7 +503,6 @@
</target>
<target name="testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -508,7 +510,6 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -516,7 +517,6 @@
</target>
<target name="testFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -524,7 +524,6 @@
</target>
<target name="testISAM.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -532,7 +531,6 @@
</target>
<target name="testJunctionTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -540,7 +538,6 @@
</target>
<target name="testKey.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testKey.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -548,7 +545,6 @@
</target>
<target name="testOrdering.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testOrdering.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -556,7 +552,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -564,7 +559,6 @@
</target>
<target name="testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -572,7 +566,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -580,7 +573,6 @@
</target>
<target name="timeSymbolMaps.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>timeSymbolMaps.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -588,7 +580,6 @@
</target>
<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -596,7 +587,6 @@
</target>
<target name="testClusterTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testClusterTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -604,7 +594,6 @@
</target>
<target name="testJunctionTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -612,7 +601,6 @@
</target>
<target name="testEliminationTree.run" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testEliminationTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -620,6 +608,7 @@
</target>
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -627,6 +616,7 @@
</target>
<target name="testGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -634,6 +624,7 @@
</target>
<target name="testGaussianISAM.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -641,6 +632,7 @@
</target>
<target name="testGaussianISAM2.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianISAM2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -648,6 +640,7 @@
</target>
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -655,6 +648,7 @@
</target>
<target name="testIterative.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testIterative.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -662,6 +656,7 @@
</target>
<target name="testNonlinearEquality.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearEquality.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -669,6 +664,7 @@
</target>
<target name="testNonlinearFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -676,6 +672,7 @@
</target>
<target name="testNonlinearFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -683,6 +680,7 @@
</target>
<target name="testNonlinearOptimizer.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -690,6 +688,7 @@
</target>
<target name="testSQP.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSQP.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -697,6 +696,7 @@
</target>
<target name="testSubgraphPreconditioner.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSubgraphPreconditioner.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -704,6 +704,7 @@
</target>
<target name="testTupleConfig.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testTupleConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -711,6 +712,7 @@
</target>
<target name="timeGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>timeGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -718,6 +720,7 @@
</target>
<target name="testBayesNetPreconditioner.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBayesNetPreconditioner.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -725,6 +728,7 @@
</target>
<target name="testConstraintOptimizer.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConstraintOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -732,6 +736,7 @@
</target>
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -739,6 +744,7 @@
</target>
<target name="testGaussianBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -746,6 +752,7 @@
</target>
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -753,6 +760,7 @@
</target>
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -760,6 +768,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -767,6 +776,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -774,7 +784,6 @@
</target>
<target name="clean" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -782,7 +791,6 @@
</target>
<target name="all" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -790,7 +798,6 @@
</target>
<target name="testNonlinearConstraint.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -798,7 +805,6 @@
</target>
<target name="testLieConfig.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testLieConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -806,7 +812,6 @@
</target>
<target name="testConstraintOptimizer.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConstraintOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -814,7 +819,6 @@
</target>
<target name="install" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -822,7 +826,6 @@
</target>
<target name="clean" path="CCOLAMD" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -830,6 +833,7 @@
</target>
<target name="all" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -837,6 +841,7 @@
</target>
<target name="clean" path="colamd" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -844,6 +849,7 @@
</target>
<target name="all" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -851,6 +857,7 @@
</target>
<target name="clean" path="ldl" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -858,6 +865,7 @@
</target>
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -865,6 +873,7 @@
</target>
<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -872,7 +881,6 @@
</target>
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -880,7 +888,6 @@
</target>
<target name="clean" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -888,6 +895,7 @@
</target>
<target name="rebuild" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -895,6 +903,7 @@
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -902,6 +911,7 @@
</target>
<target name="check" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -909,6 +919,7 @@
</target>
<target name="clean" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -916,6 +927,7 @@
</target>
<target name="testPlanarSLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPlanarSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -923,6 +935,7 @@
</target>
<target name="testPose2Config.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose2Config.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -930,6 +943,7 @@
</target>
<target name="testPose2Factor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose2Factor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -937,6 +951,7 @@
</target>
<target name="testPose2Prior.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose2Prior.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -944,6 +959,7 @@
</target>
<target name="testPose2SLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose2SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -951,6 +967,7 @@
</target>
<target name="testPose3Config.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose3Config.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -958,6 +975,7 @@
</target>
<target name="testPose3SLAM.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose3SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -965,6 +983,7 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -972,6 +991,7 @@
</target>
<target name="testVSLAMConfig.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVSLAMConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -979,6 +999,7 @@
</target>
<target name="testVSLAMFactor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVSLAMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -986,6 +1007,7 @@
</target>
<target name="testVSLAMGraph.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVSLAMGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -993,6 +1015,7 @@
</target>
<target name="testPose3Factor.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose3Factor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -1000,6 +1023,7 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1007,11 +1031,36 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1030,96 +1079,11 @@
</target>
<target name="clean" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testRot2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>timeRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testCal3_S2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testHomography2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testPoint2.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>-j2</buildArguments>
@ -1146,6 +1110,7 @@
</target>
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -1153,12 +1118,85 @@
</target>
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>dist</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testRot2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>timeRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testCal3_S2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testHomography2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
@ -1166,15 +1204,7 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
@ -1182,9 +1212,37 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -1192,7 +1250,6 @@
</target>
<target name="all" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>

134
nonlinear/NonlinearConstraint-inl.h
View File

@ -22,18 +22,17 @@ namespace gtsam {
/* ************************************************************************* */
template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange,
bool isEquality) :
NonlinearFactor<Config>(noiseModel::Unit::Create(2*dim_lagrange)),
lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) {
this->keys_.push_back(lagrange_key_);
NonlinearConstraint<Config>::NonlinearConstraint(size_t dim, double mu) :
NonlinearFactor<Config>(noiseModel::Constrained::All(dim)),
mu_(fabs(mu))
{
}
/* ************************************************************************* */
template <class Config>
bool NonlinearConstraint<Config>::active(const Config& config) const {
return !(!isEquality_ && greaterThanOrEqual(unwhitenedError(config), zero(p_)));
double NonlinearConstraint<Config>::error(const Config& c) const {
const Vector error_vector = unwhitenedError(c);
return mu_ * inner_prod(error_vector, error_vector);
}
/* ************************************************************************* */
@ -45,10 +44,8 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
Vector (*g)(const Config& config),
const Key& key,
Matrix (*gradG)(const Config& config),
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
size_t dim, double mu) :
NonlinearConstraint<Config>(dim, mu),
G_(boost::bind(gradG, _1)), g_(boost::bind(g, _1)), key_(key)
{
this->keys_.push_back(key);
@ -60,10 +57,8 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
boost::function<Vector(const Config& config)> g,
const Key& key,
boost::function<Matrix(const Config& config)> gradG,
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
size_t dim, double mu) :
NonlinearConstraint<Config>(dim, mu),
G_(gradG), g_(g), key_(key)
{
this->keys_.push_back(key);
@ -72,13 +67,9 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
/* ************************************************************************* */
template <class Config, class Key, class X>
void NonlinearConstraint1<Config, Key, X>::print(const std::string& s) const {
std::cout << "NonlinearConstraint1 [" << s << "]: Dim: " << this->p_ << "\n"
<< " Key : " << (std::string) this->key_ << "\n"
<< " Lagrange Key: " << (std::string) this->lagrange_key_ << "\n";
if (this->isEquality_)
std::cout << " Equality Factor" << std::endl;
else
std::cout << " Inequality Factor" << std::endl;
std::cout << "NonlinearConstraint1 [" << s << "]: Dim: " << this->dim()
<< " mu: " << this->mu_ << "\n"
<< " Key : " << (std::string) this->key_ << "\n";
}
/* ************************************************************************* */
@ -87,44 +78,18 @@ bool NonlinearConstraint1<Config, Key, X>::equals(const Factor<Config>& f, doubl
const NonlinearConstraint1<Config, Key, X>* p = dynamic_cast<const NonlinearConstraint1<Config, Key, X>*> (&f);
if (p == NULL) return false;
if (!(key_ == p->key_)) return false;
if (!(this->lagrange_key_.equals(p->lagrange_key_))) return false;
if (this->isEquality_ != p->isEquality_) return false;
return this->p_ == p->p_;
if (fabs(this->mu_ - p->mu_ ) > tol) return false;
return this->dim() == p->dim();
}
/* ************************************************************************* */
template <class Config, class Key, class X>
GaussianFactor::shared_ptr
NonlinearConstraint1<Config, Key, X>::linearize(const Config& config) const {
const size_t p = this->p_;
// extract lagrange multiplier
Vector lambda = config[this->lagrange_key_];
// find the error
Vector g = g_(config);
// construct the gradient
Vector g = -1.0 * g_(config);
Matrix grad = G_(config);
// construct combined factor
Matrix Ax = zeros(grad.size1()*2, grad.size2());
insertSub(Ax, vector_scale(lambda, grad), 0, 0);
insertSub(Ax, grad, grad.size1(), 0);
Matrix AL = eye(p*2, p);
Vector rhs = zero(p*2);
subInsert(rhs, -1*g, p);
// construct a mixed constraint model
Vector sigmas = zero(p*2);
subInsert(sigmas, ones(p), 0);
SharedDiagonal mixedConstraint = noiseModel::Constrained::MixedSigmas(sigmas);
GaussianFactor::shared_ptr factor(new
GaussianFactor(key_, Ax, this->lagrange_key_, AL, rhs, mixedConstraint));
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
GaussianFactor::shared_ptr factor(new GaussianFactor(this->key_, grad, g, model));
return factor;
}
@ -140,10 +105,8 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
Matrix (*G1)(const Config& config),
const Key2& key2,
Matrix (*G2)(const Config& config),
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
size_t dim, double mu)
: NonlinearConstraint<Config>(dim, mu),
G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), g_(boost::bind(g, _1)),
key1_(key1), key2_(key2)
{
@ -159,10 +122,8 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
boost::function<Matrix(const Config& config)> G1,
const Key2& key2,
boost::function<Matrix(const Config& config)> G2,
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
size_t dim, double mu)
: NonlinearConstraint<Config>(dim, mu),
G1_(G1), G2_(G2), g_(g),
key1_(key1), key2_(key2)
{
@ -173,14 +134,10 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
/* ************************************************************************* */
template <class Config, class Key1, class X1, class Key2, class X2>
void NonlinearConstraint2<Config, Key1, X1, Key2, X2>::print(const std::string& s) const {
std::cout << "NonlinearConstraint2 [" << s << "]: Dim: " << this->p_ << "\n"
std::cout << "NonlinearConstraint2 [" << s << "]: Dim: " << this->dim()
<< " mu: " << this->mu_ << "\n"
<< " Key1 : " << (std::string) this->key1_ << "\n"
<< " Key2 : " << (std::string) this->key2_ << "\n"
<< " Lagrange Key: " << (std::string) this->lagrange_key_ << "\n";
if (this->isEquality_)
std::cout << " Equality Factor" << std::endl;
else
std::cout << " Inequality Factor" << std::endl;
<< " Key2 : " << (std::string) this->key2_ << "\n";
}
/* ************************************************************************* */
@ -190,48 +147,19 @@ bool NonlinearConstraint2<Config, Key1, X1, Key2, X2>::equals(const Factor<Confi
if (p == NULL) return false;
if (!(key1_ == p->key1_)) return false;
if (!(key2_ == p->key2_)) return false;
if (!(this->lagrange_key_.equals(p->lagrange_key_))) return false;
if (this->isEquality_ != p->isEquality_) return false;
return this->p_ == p->p_;
if (fabs(this->mu_ - p->mu_ ) > tol) return false;
return this->dim() == p->dim();
}
/* ************************************************************************* */
template<class Config, class Key1, class X1, class Key2, class X2>
GaussianFactor::shared_ptr
NonlinearConstraint2<Config, Key1, X1, Key2, X2>::linearize(const Config& config) const {
const size_t p = this->p_;
// extract lagrange multiplier
Vector lambda = config[this->lagrange_key_];
// find the error
Vector g = g_(config);
// construct the gradients
Vector g = -1.0 * g_(config);
Matrix grad1 = G1_(config);
Matrix grad2 = G2_(config);
// create matrices
Matrix Ax1 = zeros(grad1.size1()*2, grad1.size2()),
Ax2 = zeros(grad2.size1()*2, grad2.size2()),
AL = eye(p*2, p);
// insert matrix components
insertSub(Ax1, vector_scale(lambda, grad1), 0, 0);
insertSub(Ax1, grad1, grad1.size1(), 0);
insertSub(Ax2, vector_scale(lambda, grad2), 0, 0);
insertSub(Ax2, grad2, grad2.size1(), 0);
Vector rhs = zero(p*2);
subInsert(rhs, -1*g, p);
// construct a mixed constraint model
Vector sigmas = zero(p*2);
subInsert(sigmas, ones(p), 0);
SharedDiagonal mixedConstraint = noiseModel::Constrained::MixedSigmas(sigmas);
GaussianFactor::shared_ptr factor(new
GaussianFactor(key1_, Ax1, key2_, Ax2, this->lagrange_key_, AL, rhs, mixedConstraint));
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
GaussianFactor::shared_ptr factor(new GaussianFactor(this->key1_, grad1, this->key2_, grad2, g, model));
return factor;
}

110
nonlinear/NonlinearConstraint.h
View File

@ -1,7 +1,7 @@
/*
* @file NonlinearConstraint.h
* @brief Implements nonlinear constraints that can be linearized and
* inserted into an existing nonlinear graph and solved via SQP
* @brief Implements nonlinear constraints that can be linearized using
* direct linearization and solving through a quadratic merit function
* @author Alex Cunningham
*/
@ -13,9 +13,6 @@
namespace gtsam {
/** Typedef for Lagrange key type - must be present in factors and config */
typedef TypedSymbol<Vector, 'L'> LagrangeKey;
/**
* Base class for nonlinear constraints
* This allows for both equality and inequality constraints,
@ -23,48 +20,30 @@ typedef TypedSymbol<Vector, 'L'> LagrangeKey;
* nonzero constraint functions will still be active - inequality
* constraints should be sure to force to actual zero)
*
* The measurement z in the underlying NonlinearFactor is the
* set of Lagrange multipliers.
* NOTE: inequality constraints removed for now
*
* Note on NoiseModel:
* The nonlinear constraint actually uses a Unit noisemodel so that
* it is possible to have a finite error value when the constraint is
* not fulfilled. Using a constrained noisemodel will immediately cause
* infinite error and break optimization.
* Nonlinear constraints evaluate their error as a part of a quadratic
* error function: ||h(x)-z||^2 + mu * ||c(x)|| where mu is a gain
* on the constraint function that should be made high enough to be
* significant
*/
template <class Config>
class NonlinearConstraint : public NonlinearFactor<Config> {
protected:
/** key for the lagrange multipliers */
LagrangeKey lagrange_key_;
/** number of lagrange multipliers */
size_t p_;
/** type of constraint */
bool isEquality_;
double mu_; // gain for quadratic merit function
public:
/** Constructor - sets the cost function and the lagrange multipliers
* @param lagrange_key is the label for the associated lagrange multipliers
* @param dim_lagrange is the number of associated constraints
* @param isEquality is true if the constraint is an equality constraint
* @param dim is the dimension of the factor
* @param mu is the gain used at error evaluation (forced to be positive)
*/
NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange,
bool isEquality=true);
NonlinearConstraint(size_t dim, double mu = 1000.0);
/** returns the key used for the Lagrange multipliers */
LagrangeKey lagrangeKey() const { return lagrange_key_; }
/** returns the number of lagrange multipliers */
size_t nrConstraints() const { return p_; }
/** returns the type of constraint */
bool isEquality() const { return isEquality_; }
/** returns the gain mu */
double mu() const { return mu_; }
/** Print */
virtual void print(const std::string& s = "") const=0;
@ -72,19 +51,12 @@ public:
/** Check if two factors are equal */
virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
/** error function - returns the result of the constraint function */
virtual Vector unwhitenedError(const Config& c) const=0;
/** error function - returns the quadratic merit function */
virtual double error(const Config& c) const;
/**
* Determines whether the constraint is active given a particular configuration
* @param config is the input to the g(x) function
* @return true if constraint needs to be linearized
*/
bool active(const Config& config) const;
/**
* Real linearize, given a config that includes Lagrange multipliers
* @param config is the configuration (with lagrange multipliers)
* Linearizes around a given config
* @param config is the configuration
* @return a combined linear factor containing both the constraint and the constraint factor
*/
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const=0;
@ -128,34 +100,30 @@ public:
* @param key is the identifier for the variable constrained
* @param G gives the jacobian of the constraint function
* @param g is the constraint function
* @param dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier
* @param isEquality is true if the constraint is an equality constraint
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
NonlinearConstraint1(
Vector (*g)(const Config& config),
const Key& key,
Matrix (*G)(const Config& config),
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality=true);
size_t dim,
double mu = 1000.0);
/**
* Basic constructor with boost function pointers
* @param key is the identifier for the variable constrained
* @param G gives the jacobian of the constraint function
* @param g is the constraint function as a boost function pointer
* @param dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier
* @param isEquality is true if the constraint is an equality constraint
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
NonlinearConstraint1(
boost::function<Vector(const Config& config)> g,
const Key& key,
boost::function<Matrix(const Config& config)> G,
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality=true);
size_t dim,
double mu = 1000.0);
/** Print */
void print(const std::string& s = "") const;
@ -166,9 +134,7 @@ public:
/** Error function */
virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); }
/**
* Linearize from config - must have Lagrange multipliers
*/
/** Linearize from config */
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
};
@ -209,9 +175,8 @@ public:
* @param key is the identifier for the variable constrained
* @param G gives the jacobian of the constraint function
* @param g is the constraint function
* @param dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier
* @param isEquality is true if the constraint is an equality constraint
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
NonlinearConstraint2(
Vector (*g)(const Config& config),
@ -219,9 +184,8 @@ public:
Matrix (*G1)(const Config& config),
const Key2& key2,
Matrix (*G2)(const Config& config),
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality=true);
size_t dim,
double mu = 1000.0);
/**
* Basic constructor with direct function objects
@ -229,9 +193,8 @@ public:
* @param key is the identifier for the variable constrained
* @param G gives the jacobian of the constraint function
* @param g is the constraint function
* @param dim_constraint is the size of the constraint (p)
* @param lagrange_key is the identifier for the lagrange multiplier
* @param isEquality is true if the constraint is an equality constraint
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
NonlinearConstraint2(
boost::function<Vector(const Config& config)> g,
@ -239,9 +202,8 @@ public:
boost::function<Matrix(const Config& config)> G1,
const Key2& key2,
boost::function<Matrix(const Config& config)> G2,
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality=true);
size_t dim,
double mu = 1000.0);
/** Print */
void print(const std::string& s = "") const;
@ -252,9 +214,7 @@ public:
/** Error function */
virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); }
/**
* Linearize from config - must have Lagrange multipliers
*/
/** Linearize from config */
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
};

5
nonlinear/NonlinearFactor.h
View File

@ -86,6 +86,11 @@ namespace gtsam {
return keys_;
}
/** get the dimension of the factor (number of rows on linearization) */
size_t dim() const {
return noiseModel_->dim();
}
/* return the begin iterator of keys */
std::list<Symbol>::const_iterator begin() const { return keys_.begin(); }

2
nonlinear/NonlinearOptimizer.cpp
View File

@ -5,8 +5,6 @@
* Created on: Jul 17, 2010
*/
#pragma once
#include "NonlinearOptimizer.h"
namespace gtsam {

303
nonlinear/tests/testNonlinearConstraint.cpp
View File

@ -21,8 +21,7 @@ using namespace gtsam;
using namespace boost::assign;
typedef TypedSymbol<Vector, 'x'> Key;
typedef TupleConfig2< LieConfig<LagrangeKey, Vector>,
LieConfig<Key, Vector> > VecConfig;
typedef LieConfig<Key, Vector> VecConfig;
typedef NonlinearConstraint1<VecConfig, Key, Vector> NLC1;
typedef NonlinearConstraint2<VecConfig, Key, Vector, Key, Vector> NLC2;
@ -48,15 +47,12 @@ namespace test1 {
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_construction ) {
// construct a constraint on x
// the lagrange multipliers will be expected on L_x1
// and there is only one multiplier
size_t p = 1;
Key x1(1);
list<Key> keys; keys += x1;
LagrangeKey L1(1);
NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys),
p, L1);
p, 10.0);
// get a configuration to use for finding the error
VecConfig config;
@ -68,7 +64,7 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
CHECK(assert_equal(actualVec, expectedVec, 1e-5));
double actError = c1.error(config);
double expError = 8.0;
double expError = 160.0;
DOUBLES_EQUAL(expError, actError, 1e-5);
}
@ -77,27 +73,22 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
size_t p = 1;
Key x1(1);
list<Key> keys; keys += x1;
LagrangeKey L1(1);
NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys),
p, L1);
p);
// get a configuration to use for linearization (with lagrange multipliers)
// get a configuration to use for linearization
VecConfig realconfig;
realconfig.insert(x1, Vector_(1, 1.0));
realconfig.insert(L1, Vector_(1, 3.0));
// linearize the system
GaussianFactor::shared_ptr linfactor = c1.linearize(realconfig);
// verify - probabilistic component goes on top
Vector sigmas = Vector_(2, 1.0, 0.0);
SharedDiagonal mixedModel = noiseModel::Constrained::MixedSigmas(sigmas);
// stack the matrices to combine
Matrix Ax1 = Matrix_(2,1, 6.0, 2.0),
AL1 = Matrix_(2,1, 1.0, 0.0);
Vector rhs = Vector_(2, 0.0, 4.0);
GaussianFactor expectedFactor(x1, Ax1, L1, AL1, rhs, mixedModel);
// verify
SharedDiagonal model = noiseModel::Constrained::All(p);
Matrix Ax1 = Matrix_(p,1, 2.0);
Vector rhs = Vector_(p, 4.0);
GaussianFactor expectedFactor(x1, Ax1, rhs, model);
CHECK(assert_equal(*linfactor, expectedFactor));
}
@ -106,12 +97,11 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
TEST( NonlinearConstraint1, unary_scalar_equal ) {
Key x(0), y(1);
list<Key> keys1, keys2; keys1 += x; keys2 += y;
LagrangeKey L1(1);
NLC1
c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, L1, true),
c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, L1),
c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2, L1),
c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1, L1);
c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1),
c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1),
c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2),
c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1);
CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1));
@ -147,17 +137,14 @@ namespace test2 {
/* ************************************************************************* */
TEST( NonlinearConstraint2, binary_scalar_construction ) {
// construct a constraint on x and y
// the lagrange multipliers will be expected on L_xy
// and there is only one multiplier
size_t p = 1;
Key x0(0), x1(1);
list<Key> keys; keys += x0, x1;
LagrangeKey L1(1);
NLC2 c1(
boost::bind(test2::g, _1, keys),
x0, boost::bind(test2::G1, _1, keys),
x1, boost::bind(test2::G1, _1, keys),
p, L1);
p);
// get a configuration to use for finding the error
VecConfig config;
@ -176,30 +163,26 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
size_t p = 1;
Key x0(0), x1(1);
list<Key> keys; keys += x0, x1;
LagrangeKey L1(1);
NLC2 c1(
boost::bind(test2::g, _1, keys),
x0, boost::bind(test2::G1, _1, keys),
x1, boost::bind(test2::G2, _1, keys),
p, L1);
p);
// get a configuration to use for finding the error
VecConfig realconfig;
realconfig.insert(x0, Vector_(1, 1.0));
realconfig.insert(x1, Vector_(1, 2.0));
realconfig.insert(L1, Vector_(1, 3.0));
// linearize the system
GaussianFactor::shared_ptr actualFactor = c1.linearize(realconfig);
// verify - probabilistic component goes on top
Matrix Ax0 = Matrix_(2,1, 6.0, 2.0),
Ax1 = Matrix_(2,1,-3.0,-1.0),
AL = Matrix_(2,1, 1.0, 0.0);
Vector rhs = Vector_(2, 0.0, 6.0),
sigmas = Vector_(2, 1.0, 0.0);
SharedDiagonal expModel = noiseModel::Constrained::MixedSigmas(sigmas);
GaussianFactor expFactor(x0,Ax0, x1, Ax1,L1, AL, rhs, expModel);
// verify
Matrix Ax0 = Matrix_(1,1, 2.0),
Ax1 = Matrix_(1,1,-1.0);
Vector rhs = Vector_(1, 6.0);
SharedDiagonal expModel = noiseModel::Constrained::All(p);
GaussianFactor expFactor(x0,Ax0, x1, Ax1,rhs, expModel);
CHECK(assert_equal(expFactor, *actualFactor));
}
@ -208,12 +191,11 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) {
list<Key> keys1, keys2, keys3;
Key x0(0), x1(1), x2(2);
keys1 += x0, x1; keys2 += x1, x0; keys3 += x0;
LagrangeKey L1(1);
NLC2
c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, L1),
c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, L1),
c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1, L1),
c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3, L1);
c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1),
c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1),
c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1),
c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3);
CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1));
@ -222,127 +204,120 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) {
}
/* ************************************************************************* */
// Inequality tests
// Inequality tests - DISABLED
/* ************************************************************************* */
namespace inequality1 {
/** p = 1, g(x) x^2 - 5 > 0 */
Vector g(const VecConfig& config, const Key& key) {
double x = config[key](0);
double g = x * x - 5;
return Vector_(1, g); // return the actual cost
}
/** p = 1, jacobianG(x) = 2*x */
Matrix G(const VecConfig& config, const Key& key) {
double x = config[key](0);
return Matrix_(1, 1, 2 * x);
}
} // \namespace inequality1
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality ) {
size_t p = 1;
Key x0(0);
LagrangeKey L1(1);
NLC1 c1(boost::bind(inequality1::g, _1, x0),
x0, boost::bind(inequality1::G, _1, x0),
p, L1,
false); // inequality constraint
// get configurations to use for evaluation
VecConfig config1, config2;
config1.insert(x0, Vector_(1, 10.0)); // should be inactive
config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
// check error
CHECK(!c1.active(config1));
Vector actualError2 = c1.unwhitenedError(config2);
CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
CHECK(c1.active(config2));
}
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality_linearize ) {
size_t p = 1;
Key x0(0);
LagrangeKey L1(1);
NLC1 c1(boost::bind(inequality1::g, _1, x0),
x0, boost::bind(inequality1::G, _1, x0),
p, L1,
false); // inequality constraint
// get configurations to use for linearization
VecConfig config1, config2;
config1.insert(x0, Vector_(1, 10.0)); // should have zero error
config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
config1.insert(L1, Vector_(1, 3.0));
config2.insert(L1, Vector_(1, 3.0));
// linearize for inactive constraint
GaussianFactor::shared_ptr actualFactor1 = c1.linearize(config1);
// check if the factor is active
CHECK(!c1.active(config1));
// linearize for active constraint
GaussianFactor::shared_ptr actualFactor2 = c1.linearize(config2);
CHECK(c1.active(config2));
// verify
Vector sigmas = Vector_(2, 1.0, 0.0);
SharedDiagonal model = noiseModel::Constrained::MixedSigmas(sigmas);
GaussianFactor expectedFactor(x0, Matrix_(2,1, 6.0, 2.0),
L1, Matrix_(2,1, 1.0, 0.0),
Vector_(2, 0.0, 4.0), model);
CHECK(assert_equal(*actualFactor2, expectedFactor));
}
//namespace inequality1 {
//
// /** p = 1, g(x) x^2 - 5 > 0 */
// Vector g(const VecConfig& config, const Key& key) {
// double x = config[key](0);
// double g = x * x - 5;
// return Vector_(1, g); // return the actual cost
// }
//
// /** p = 1, jacobianG(x) = 2*x */
// Matrix G(const VecConfig& config, const Key& key) {
// double x = config[key](0);
// return Matrix_(1, 1, 2 * x);
// }
//
//} // \namespace inequality1
//
///* ************************************************************************* */
//TEST( NonlinearConstraint1, unary_inequality ) {
// size_t p = 1;
// Key x0(0);
// NLC1 c1(boost::bind(inequality1::g, _1, x0),
// x0, boost::bind(inequality1::G, _1, x0),
// p, false); // inequality constraint
//
// // get configurations to use for evaluation
// VecConfig config1, config2;
// config1.insert(x0, Vector_(1, 10.0)); // should be inactive
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
//
// // check error
// CHECK(!c1.active(config1));
// Vector actualError2 = c1.unwhitenedError(config2);
// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
// CHECK(c1.active(config2));
//}
//
///* ************************************************************************* */
//TEST( NonlinearConstraint1, unary_inequality_linearize ) {
// size_t p = 1;
// Key x0(0);
// NLC1 c1(boost::bind(inequality1::g, _1, x0),
// x0, boost::bind(inequality1::G, _1, x0),
// p, false); // inequality constraint
//
// // get configurations to use for linearization
// VecConfig config1, config2;
// config1.insert(x0, Vector_(1, 10.0)); // should have zero error
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
//
// // linearize for inactive constraint
// GaussianFactor::shared_ptr actualFactor1 = c1.linearize(config1);
//
// // check if the factor is active
// CHECK(!c1.active(config1));
//
// // linearize for active constraint
// GaussianFactor::shared_ptr actualFactor2 = c1.linearize(config2);
// CHECK(c1.active(config2));
//
// // verify
// Vector sigmas = Vector_(2, 1.0, 0.0);
// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(sigmas);
// GaussianFactor expectedFactor(x0, Matrix_(2,1, 6.0, 2.0),
// L1, Matrix_(2,1, 1.0, 0.0),
// Vector_(2, 0.0, 4.0), model);
//
// CHECK(assert_equal(*actualFactor2, expectedFactor));
//}
/* ************************************************************************* */
// Binding arbitrary functions
/* ************************************************************************* */
namespace binding1 {
/** p = 1, g(x) x^2 - r > 0 */
Vector g(double r, const VecConfig& config, const Key& key) {
double x = config[key](0);
double g = x * x - r;
return Vector_(1, g); // return the actual cost
}
/** p = 1, jacobianG(x) = 2*x */
Matrix G(double coeff, const VecConfig& config,
const Key& key) {
double x = config[key](0);
return Matrix_(1, 1, coeff * x);
}
} // \namespace binding1
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_binding ) {
size_t p = 1;
double coeff = 2;
double radius = 5;
Key x0(0); LagrangeKey L1(1);
NLC1 c1(boost::bind(binding1::g, radius, _1, x0),
x0, boost::bind(binding1::G, coeff, _1, x0),
p, L1,
false); // inequality constraint
// get configurations to use for evaluation
VecConfig config1, config2;
config1.insert(x0, Vector_(1, 10.0)); // should have zero error
config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
// check error
CHECK(!c1.active(config1));
Vector actualError2 = c1.unwhitenedError(config2);
CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
CHECK(c1.active(config2));
}
//namespace binding1 {
//
// /** p = 1, g(x) x^2 - r > 0 */
// Vector g(double r, const VecConfig& config, const Key& key) {
// double x = config[key](0);
// double g = x * x - r;
// return Vector_(1, g); // return the actual cost
// }
//
// /** p = 1, jacobianG(x) = 2*x */
// Matrix G(double coeff, const VecConfig& config,
// const Key& key) {
// double x = config[key](0);
// return Matrix_(1, 1, coeff * x);
// }
//
//} // \namespace binding1
//
///* ************************************************************************* */
//TEST( NonlinearConstraint1, unary_binding ) {
// size_t p = 1;
// double coeff = 2;
// double radius = 5;
// Key x0(0);
// NLC1 c1(boost::bind(binding1::g, radius, _1, x0),
// x0, boost::bind(binding1::G, coeff, _1, x0),
// p, false); // inequality constraint
//
// // get configurations to use for evaluation
// VecConfig config1, config2;
// config1.insert(x0, Vector_(1, 10.0)); // should have zero error
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
//
// // check error
// CHECK(!c1.active(config1));
// Vector actualError2 = c1.unwhitenedError(config2);
// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
// CHECK(c1.active(config2));
//}
/* ************************************************************************* */
namespace binding2 {
@ -370,17 +345,15 @@ namespace binding2 {
/* ************************************************************************* */
TEST( NonlinearConstraint2, binary_binding ) {
// construct a constraint on x and y
// the lagrange multipliers will be expected on L_xy
// and there is only one multiplier
size_t p = 1;
double a = 2.0;
double b = 1.0;
double r = 5.0;
Key x0(0), x1(1); LagrangeKey L1(1);
Key x0(0), x1(1);
NLC2 c1(boost::bind(binding2::g, r, _1, x0, x1),
x0, boost::bind(binding2::G1, a, _1, x0),
x1, boost::bind(binding2::G2, b, _1),
p, L1);
p);
// get a configuration to use for finding the error
VecConfig config;
@ -393,6 +366,8 @@ TEST( NonlinearConstraint2, binary_binding ) {
CHECK(assert_equal(actual, expected, 1e-5));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

2
tests/Makefile.am
View File

@ -8,7 +8,7 @@ check_PROGRAMS += testGaussianBayesNet testGaussianFactor testGaussianFactorGrap
check_PROGRAMS += testGaussianISAM testGraph
check_PROGRAMS += testInference testIterative testGaussianJunctionTree
check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
check_PROGRAMS += testNonlinearOptimizer testSQP testSubgraphPreconditioner
check_PROGRAMS += testNonlinearOptimizer testNonlinearConstraint testSubgraphPreconditioner
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig
if USE_LDL

211
tests/testConstraintOptimizer.cpp
View File

@ -102,6 +102,217 @@ TEST( matrix, unconstrained_fg_ata ) {
// CHECK(assert_equal(expected,x, 1e-4));
//}
SharedDiagonal probModel1 = sharedSigma(1,1.0);
SharedDiagonal probModel2 = sharedSigma(2,1.0);
SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1);
/* *********************************************************************
* This example uses a nonlinear objective function and
* nonlinear equality constraint. The formulation is actually
* the Cholesky form that creates the full Hessian explicitly,
* which should really be avoided with our QR-based machinery.
*
* Note: the update equation used here has a fixed step size
* and gain that is rather arbitrarily chosen, and as such,
* will take a silly number of iterations.
*/
TEST (SQP, problem1_cholesky ) {
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) + \lambda*g(x)
// Symbols
Symbol x1("x1"), y1("y1"), L1("L1");
// state structure: [x y \lambda]
VectorConfig init, state;
init.insert(x1, Vector_(1, 1.0));
init.insert(y1, Vector_(1, 1.0));
init.insert(L1, Vector_(1, 1.0));
state = init;
if (verbose) init.print("Initial State");
// loop until convergence
int maxIt = 10;
for (int i = 0; i<maxIt; ++i) {
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
// extract the states
double x, y, lambda;
x = state[x1](0);
y = state[y1](0);
lambda = state[L1](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*lambda,
0.0);
H2 = Matrix_(2,1,
0.0,
2.0);
// deriviative of lagrangian function
gradL = Vector_(2,
2.0*x*(1+lambda),
2.0*y-lambda);
// 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(x1, H1, y1, H2, L1, gradG, gradL, probModel2));
// create a factor for the lagrange multiplier
GaussianFactor::shared_ptr f2(new
GaussianFactor(x1, -sub(gradG, 0, 1, 0, 1),
y1, -sub(gradG, 1, 2, 0, 1), -gx, constraintModel1));
// construct graph
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
if (verbose) fg.print("Graph");
// solve
Ordering ord;
ord += x1, y1, L1;
VectorConfig delta = fg.optimize(ord).scale(-1.0);
if (verbose) delta.print("Delta");
// update initial estimate
VectorConfig newState = expmap(state, delta);
state = newState;
if (verbose) state.print("Updated State");
}
// verify that it converges to the nearest optimal point
VectorConfig expected;
expected.insert(L1, Vector_(1, -1.0));
expected.insert(x1, Vector_(1, 2.12));
expected.insert(y1, Vector_(1, -0.5));
CHECK(assert_equal(expected,state, 1e-2));
}
/* *********************************************************************
* This example uses a nonlinear objective function and
* nonlinear equality constraint. This formulation splits
* the constraint into a factor and a linear constraint.
*
* This example uses the same silly number of iterations as the
* previous example.
*/
TEST (SQP, problem1_sqp ) {
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) + \lambda*g(x)
// Symbols
Symbol x1("x1"), y1("y1"), L1("L1");
// state structure: [x y \lambda]
VectorConfig init, state;
init.insert(x1, Vector_(1, 1.0));
init.insert(y1, Vector_(1, 1.0));
init.insert(L1, Vector_(1, 1.0));
state = init;
if (verbose) init.print("Initial State");
// loop until convergence
int maxIt = 5;
for (int i = 0; i<maxIt; ++i) {
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
// extract the states
double x, y, lambda;
x = state[x1](0);
y = state[y1](0);
lambda = state[L1](0);
/** create the linear factor
* ||h(x)-z||^2 => ||Ax-b||^2
* where:
* h(x) simply returns the inputs
* z zeros(2)
* A identity
* b linearization point
*/
Matrix A = eye(2);
Vector b = Vector_(2, x, y);
GaussianFactor::shared_ptr f1(
new GaussianFactor(x1, sub(A, 0,2, 0,1), // A(:,1)
y1, sub(A, 0,2, 1,2), // A(:,2)
b, // rhs of f(x)
probModel2)); // arbitrary sigma
/** create the constraint-linear factor
* Provides a mechanism to use variable gain to force the constraint
* \lambda*gradG*dx + d\lambda = zero
* formulated in matrix form as:
* [\lambda*gradG eye(1)] [dx; d\lambda] = zero
*/
Matrix gradG = Matrix_(1, 2,2*x, -1.0);
GaussianFactor::shared_ptr f2(
new GaussianFactor(x1, lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
y1, lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
L1, eye(1), // dlambda term
Vector_(1, 0.0), // rhs is zero
probModel1)); // arbitrary sigma
// create the actual constraint
// [gradG] [x; y] - g = 0
Vector g = Vector_(1,x*x-y-5);
GaussianFactor::shared_ptr c1(
new GaussianFactor(x1, sub(gradG, 0,1, 0,1), // slice first part of gradG
y1, sub(gradG, 0,1, 1,2), // slice second part of gradG
g, // value of constraint function
constraintModel1)); // force to constraint
// construct graph
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
fg.push_back(c1);
if (verbose) fg.print("Graph");
// solve
Ordering ord;
ord += x1, y1, L1;
VectorConfig delta = fg.optimize(ord);
if (verbose) delta.print("Delta");
// update initial estimate
VectorConfig newState = expmap(state, delta.scale(-1.0));
// set the state to the updated state
state = newState;
if (verbose) state.print("Updated State");
}
// verify that it converges to the nearest optimal point
VectorConfig expected;
expected.insert(x1, Vector_(1, 2.12));
expected.insert(y1, Vector_(1, -0.5));
CHECK(assert_equal(state[x1], expected[x1], 1e-2));
CHECK(assert_equal(state[y1], expected[y1], 1e-2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

482
tests/testSQP.cpp → tests/testNonlinearConstraint.cpp
View File

@ -41,223 +41,9 @@ SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1);
// 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 Cholesky form that creates the full Hessian explicitly,
* which should really be avoided with our QR-based machinery.
*
* Note: the update equation used here has a fixed step size
* and gain that is rather arbitrarily chosen, and as such,
* will take a silly number of iterations.
*/
TEST (SQP, problem1_cholesky ) {
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) + \lambda*g(x)
// Symbols
Symbol x1("x1"), y1("y1"), L1("L1");
// state structure: [x y \lambda]
VectorConfig init, state;
init.insert(x1, Vector_(1, 1.0));
init.insert(y1, Vector_(1, 1.0));
init.insert(L1, Vector_(1, 1.0));
state = init;
if (verbose) init.print("Initial State");
// loop until convergence
int maxIt = 10;
for (int i = 0; i<maxIt; ++i) {
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
// extract the states
double x, y, lambda;
x = state[x1](0);
y = state[y1](0);
lambda = state[L1](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*lambda,
0.0);
H2 = Matrix_(2,1,
0.0,
2.0);
// deriviative of lagrangian function
gradL = Vector_(2,
2.0*x*(1+lambda),
2.0*y-lambda);
// 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(x1, H1, y1, H2, L1, gradG, gradL, probModel2));
// create a factor for the lagrange multiplier
GaussianFactor::shared_ptr f2(new
GaussianFactor(x1, -sub(gradG, 0, 1, 0, 1),
y1, -sub(gradG, 1, 2, 0, 1), -gx, constraintModel1));
// construct graph
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
if (verbose) fg.print("Graph");
// solve
Ordering ord;
ord += x1, y1, L1;
VectorConfig delta = fg.optimize(ord).scale(-1.0);
if (verbose) delta.print("Delta");
// update initial estimate
VectorConfig newState = expmap(state, delta);
state = newState;
if (verbose) state.print("Updated State");
}
// verify that it converges to the nearest optimal point
VectorConfig expected;
expected.insert(L1, Vector_(1, -1.0));
expected.insert(x1, Vector_(1, 2.12));
expected.insert(y1, Vector_(1, -0.5));
CHECK(assert_equal(expected,state, 1e-2));
}
/* *********************************************************************
* This example uses a nonlinear objective function and
* nonlinear equality constraint. This formulation splits
* the constraint into a factor and a linear constraint.
*
* This example uses the same silly number of iterations as the
* previous example.
*/
TEST (SQP, problem1_sqp ) {
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) + \lambda*g(x)
// Symbols
Symbol x1("x1"), y1("y1"), L1("L1");
// state structure: [x y \lambda]
VectorConfig init, state;
init.insert(x1, Vector_(1, 1.0));
init.insert(y1, Vector_(1, 1.0));
init.insert(L1, Vector_(1, 1.0));
state = init;
if (verbose) init.print("Initial State");
// loop until convergence
int maxIt = 5;
for (int i = 0; i<maxIt; ++i) {
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
// extract the states
double x, y, lambda;
x = state[x1](0);
y = state[y1](0);
lambda = state[L1](0);
/** create the linear factor
* ||h(x)-z||^2 => ||Ax-b||^2
* where:
* h(x) simply returns the inputs
* z zeros(2)
* A identity
* b linearization point
*/
Matrix A = eye(2);
Vector b = Vector_(2, x, y);
GaussianFactor::shared_ptr f1(
new GaussianFactor(x1, sub(A, 0,2, 0,1), // A(:,1)
y1, sub(A, 0,2, 1,2), // A(:,2)
b, // rhs of f(x)
probModel2)); // arbitrary sigma
/** create the constraint-linear factor
* Provides a mechanism to use variable gain to force the constraint
* \lambda*gradG*dx + d\lambda = zero
* formulated in matrix form as:
* [\lambda*gradG eye(1)] [dx; d\lambda] = zero
*/
Matrix gradG = Matrix_(1, 2,2*x, -1.0);
GaussianFactor::shared_ptr f2(
new GaussianFactor(x1, lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
y1, lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
L1, eye(1), // dlambda term
Vector_(1, 0.0), // rhs is zero
probModel1)); // arbitrary sigma
// create the actual constraint
// [gradG] [x; y] - g = 0
Vector g = Vector_(1,x*x-y-5);
GaussianFactor::shared_ptr c1(
new GaussianFactor(x1, sub(gradG, 0,1, 0,1), // slice first part of gradG
y1, sub(gradG, 0,1, 1,2), // slice second part of gradG
g, // value of constraint function
constraintModel1)); // force to constraint
// construct graph
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
fg.push_back(c1);
if (verbose) fg.print("Graph");
// solve
Ordering ord;
ord += x1, y1, L1;
VectorConfig delta = fg.optimize(ord);
if (verbose) delta.print("Delta");
// update initial estimate
VectorConfig newState = expmap(state, delta.scale(-1.0));
// set the state to the updated state
state = newState;
if (verbose) state.print("Updated State");
}
// verify that it converges to the nearest optimal point
VectorConfig expected;
expected.insert(x1, Vector_(1, 2.12));
expected.insert(y1, Vector_(1, -0.5));
CHECK(assert_equal(state[x1], expected[x1], 1e-2));
CHECK(assert_equal(state[y1], expected[y1], 1e-2));
}
/* ********************************************************************* */
// Basic configs
typedef LieConfig<LagrangeKey, Vector> LagrangeConfig;
// full components
typedef TupleConfig3<LieConfig<simulated2D::PoseKey, Point2>,
LieConfig<simulated2D::PointKey, Point2>,
LieConfig<LagrangeKey, Vector> > Config2D;
//typedef TupleConfig<LagrangeConfig, TupleConfigEnd<simulated2D::Config> > Config2D;
typedef simulated2D::Config Config2D;
typedef NonlinearFactorGraph<Config2D> Graph2D;
typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> NLE;
typedef boost::shared_ptr<simulated2D::GenericMeasurement<Config2D> > shared;
@ -268,7 +54,7 @@ typedef NonlinearOptimizer<Graph2D, Config2D> Optimizer;
* with two poses seeing one landmark, with each pose
* constrained to a particular value
*/
TEST (SQP, two_pose_truth ) {
TEST (NonlinearConstraint, two_pose_truth ) {
bool verbose = false;
// create a graph
@ -379,7 +165,7 @@ typedef NonlinearConstraint2<
* should be the same. Note that this is a linear system,
* so it will converge in one step.
*/
TEST (SQP, two_pose ) {
TEST (NonlinearConstraint, two_pose ) {
bool verbose = false;
// create the graph
@ -407,13 +193,12 @@ TEST (SQP, two_pose ) {
graph->push_back(f2);
// equality constraint between l1 and l2
LagrangeKey L1(1);
list<simulated2D::PointKey> keys2; keys2 += l1, l2;
boost::shared_ptr<NLC2 > c2(new NLC2(
boost::bind(sqp_test1::g, _1, keys2),
l1, boost::bind(sqp_test1::G1, _1, keys2),
l2, boost::bind(sqp_test1::G2, _1, keys2),
2, L1));
2));
graph->push_back(c2);
if (verbose) graph->print("Initial nonlinear graph with constraints");
@ -424,7 +209,6 @@ TEST (SQP, two_pose ) {
initialEstimate->insert(x2, Point2());
initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
initialEstimate->insert(L1, Vector_(2, 1.0, 1.0)); // connect the landmarks
// create state config variables and initialize them
Config2D state(*initialEstimate);
@ -436,7 +220,7 @@ TEST (SQP, two_pose ) {
// create an ordering
Ordering ordering;
ordering += "x1", "x2", "l1", "l2", "L1";
ordering += "x1", "x2", "l1", "l2";
// optimize linear graph to get full delta config
GaussianBayesNet cbn = fg->eliminate(ordering);
@ -455,7 +239,6 @@ TEST (SQP, two_pose ) {
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, 6.0, 7.0));
CHECK(assert_equal(expected, state, 1e-5));
}
@ -474,9 +257,10 @@ using namespace boost;
// typedefs for visual SLAM example
typedef TypedSymbol<Pose3, 'x'> Pose3Key;
typedef TypedSymbol<Point3, 'l'> Point3Key;
typedef TupleConfig3<LieConfig<LagrangeKey, Vector>,
LieConfig<Pose3Key, Pose3>,
LieConfig<Point3Key, Point3> > VConfig;
//typedef TupleConfig3<LieConfig<LagrangeKey, Vector>,
// LieConfig<Pose3Key, Pose3>,
// LieConfig<Point3Key, Point3> > VConfig;
typedef visualSLAM::Config VConfig;
typedef NonlinearFactorGraph<VConfig> VGraph;
typedef boost::shared_ptr<GenericProjectionFactor<VConfig> > shared_vf;
typedef NonlinearOptimizer<VGraph,VConfig> VOptimizer;
@ -487,7 +271,7 @@ typedef NonlinearEquality<VConfig, Pose3Key, Pose3> Pose3Constraint;
/**
* Ground truth for a visual SLAM example with stereo vision
*/
TEST (SQP, stereo_truth ) {
TEST (NonlinearConstraint, stereo_truth ) {
bool verbose = false;
// create initial estimates
@ -555,7 +339,7 @@ TEST (SQP, stereo_truth ) {
* Ground truth for a visual SLAM example with stereo vision
* with some noise injected into the initial config
*/
TEST (SQP, stereo_truth_noisy ) {
TEST (NonlinearConstraint, stereo_truth_noisy ) {
bool verbose = false;
// setting to determine how far away the noisy landmark is,
@ -696,12 +480,11 @@ boost::shared_ptr<VGraph> stereoExampleGraph() {
// as the previous examples
visualSLAM::PointKey l1(1), l2(2);
list<Point3Key> keys; keys += l1, l2;
LagrangeKey L12(12);
shared_ptr<VNLC2> c2(
new VNLC2(boost::bind(sqp_stereo::g, _1, keys),
l1, boost::bind(sqp_stereo::G1, _1, keys),
l2, boost::bind(sqp_stereo::G2, _1, keys),
3, L12));
3));
graph->push_back(c2);
return graph;
@ -736,7 +519,7 @@ boost::shared_ptr<VConfig> stereoExampleTruthConfig() {
* SQP version of the above stereo example,
* with the initial case as the ground truth
*/
TEST (SQP, stereo_sqp ) {
TEST (NonlinearConstraint, stereo_sqp ) {
bool verbose = false;
// get a graph
@ -745,11 +528,10 @@ TEST (SQP, stereo_sqp ) {
// get the truth config
boost::shared_ptr<VConfig> truthConfig = stereoExampleTruthConfig();
truthConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0));
// create ordering
shared_ptr<Ordering> ord(new Ordering());
*ord += "x1", "x2", "l1", "l2", "L12";
*ord += "x1", "x2", "l1", "l2";
VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
// create optimizer
@ -766,7 +548,7 @@ TEST (SQP, stereo_sqp ) {
* SQP version of the above stereo example,
* with noise in the initial estimate
*/
TEST (SQP, stereo_sqp_noisy ) {
TEST (NonlinearConstraint, stereo_sqp_noisy ) {
// get a graph
boost::shared_ptr<VGraph> graph = stereoExampleGraph();
@ -787,11 +569,10 @@ TEST (SQP, stereo_sqp_noisy ) {
initConfig->insert(Pose3Key(2), pose2);
initConfig->insert(Point3Key(1), landmark1);
initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place
initConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0));
// create ordering
shared_ptr<Ordering> ord(new Ordering());
*ord += "x1", "x2", "l1", "l2", "L12";
*ord += "x1", "x2", "l1", "l2";
VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
// create optimizer
@ -809,7 +590,6 @@ TEST (SQP, stereo_sqp_noisy ) {
// get the truth config
boost::shared_ptr<VConfig> truthConfig = stereoExampleTruthConfig();
truthConfig->insert(LagrangeKey(12), Vector_(3, 0.0, 1.0, 1.0));
// check if correct
CHECK(assert_equal(*truthConfig,*actual, 1e-5));
@ -817,17 +597,9 @@ TEST (SQP, stereo_sqp_noisy ) {
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
/* ********************************************************************* */
@ -881,10 +653,9 @@ boost::shared_ptr<Graph2D> linearMapWarpGraph() {
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));
2));
// measurement from x1 to l1
Point2 z1(0.0, 5.0);
@ -895,12 +666,11 @@ boost::shared_ptr<Graph2D> linearMapWarpGraph() {
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));
2));
// construct the graph
boost::shared_ptr<Graph2D> graph(new Graph2D());
@ -920,7 +690,6 @@ TEST ( SQPOptimizer, map_warp_initLam ) {
// 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);
@ -928,12 +697,10 @@ TEST ( SQPOptimizer, map_warp_initLam ) {
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";
*ordering += "x1", "x2", "l1", "l2";
// create an optimizer
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
@ -949,8 +716,6 @@ TEST ( SQPOptimizer, map_warp_initLam ) {
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));
}
@ -1107,6 +872,215 @@ TEST ( SQPOptimizer, map_warp_initLam ) {
// CHECK(assert_equal(exp2, *(final.config())));
//}
/* *********************************************************************
* Example from SQP testing:
*
* This example uses a nonlinear objective function and
* nonlinear equality constraint. The formulation is actually
* the Cholesky form that creates the full Hessian explicitly,
* which should really be avoided with our QR-based machinery.
*
* Note: the update equation used here has a fixed step size
* and gain that is rather arbitrarily chosen, and as such,
* will take a silly number of iterations.
*/
TEST (NonlinearConstraint, problem1_cholesky ) {
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) + \lambda*g(x)
// Symbols
Symbol x1("x1"), y1("y1"), L1("L1");
// state structure: [x y \lambda]
VectorConfig init, state;
init.insert(x1, Vector_(1, 1.0));
init.insert(y1, Vector_(1, 1.0));
init.insert(L1, Vector_(1, 1.0));
state = init;
if (verbose) init.print("Initial State");
// loop until convergence
int maxIt = 10;
for (int i = 0; i<maxIt; ++i) {
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
// extract the states
double x, y, lambda;
x = state[x1](0);
y = state[y1](0);
lambda = state[L1](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*lambda,
0.0);
H2 = Matrix_(2,1,
0.0,
2.0);
// deriviative of lagrangian function
gradL = Vector_(2,
2.0*x*(1+lambda),
2.0*y-lambda);
// 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(x1, H1, y1, H2, L1, gradG, gradL, probModel2));
// create a factor for the lagrange multiplier
GaussianFactor::shared_ptr f2(new
GaussianFactor(x1, -sub(gradG, 0, 1, 0, 1),
y1, -sub(gradG, 1, 2, 0, 1), -gx, constraintModel1));
// construct graph
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
if (verbose) fg.print("Graph");
// solve
Ordering ord;
ord += x1, y1, L1;
VectorConfig delta = fg.optimize(ord).scale(-1.0);
if (verbose) delta.print("Delta");
// update initial estimate
VectorConfig newState = expmap(state, delta);
state = newState;
if (verbose) state.print("Updated State");
}
// verify that it converges to the nearest optimal point
VectorConfig expected;
expected.insert(L1, Vector_(1, -1.0));
expected.insert(x1, Vector_(1, 2.12));
expected.insert(y1, Vector_(1, -0.5));
CHECK(assert_equal(expected,state, 1e-2));
}
/* *********************************************************************
* This example uses a nonlinear objective function and
* nonlinear equality constraint. This formulation splits
* the constraint into a factor and a linear constraint.
*
* This example uses the same silly number of iterations as the
* previous example.
*/
TEST (NonlinearConstraint, problem1_sqp ) {
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) + \lambda*g(x)
// Symbols
Symbol x1("x1"), y1("y1"), L1("L1");
// state structure: [x y \lambda]
VectorConfig init, state;
init.insert(x1, Vector_(1, 1.0));
init.insert(y1, Vector_(1, 1.0));
init.insert(L1, Vector_(1, 1.0));
state = init;
if (verbose) init.print("Initial State");
// loop until convergence
int maxIt = 5;
for (int i = 0; i<maxIt; ++i) {
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
// extract the states
double x, y, lambda;
x = state[x1](0);
y = state[y1](0);
lambda = state[L1](0);
/** create the linear factor
* ||h(x)-z||^2 => ||Ax-b||^2
* where:
* h(x) simply returns the inputs
* z zeros(2)
* A identity
* b linearization point
*/
Matrix A = eye(2);
Vector b = Vector_(2, x, y);
GaussianFactor::shared_ptr f1(
new GaussianFactor(x1, sub(A, 0,2, 0,1), // A(:,1)
y1, sub(A, 0,2, 1,2), // A(:,2)
b, // rhs of f(x)
probModel2)); // arbitrary sigma
/** create the constraint-linear factor
* Provides a mechanism to use variable gain to force the constraint
* \lambda*gradG*dx + d\lambda = zero
* formulated in matrix form as:
* [\lambda*gradG eye(1)] [dx; d\lambda] = zero
*/
Matrix gradG = Matrix_(1, 2,2*x, -1.0);
GaussianFactor::shared_ptr f2(
new GaussianFactor(x1, lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
y1, lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
L1, eye(1), // dlambda term
Vector_(1, 0.0), // rhs is zero
probModel1)); // arbitrary sigma
// create the actual constraint
// [gradG] [x; y] - g = 0
Vector g = Vector_(1,x*x-y-5);
GaussianFactor::shared_ptr c1(
new GaussianFactor(x1, sub(gradG, 0,1, 0,1), // slice first part of gradG
y1, sub(gradG, 0,1, 1,2), // slice second part of gradG
g, // value of constraint function
constraintModel1)); // force to constraint
// construct graph
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
fg.push_back(c1);
if (verbose) fg.print("Graph");
// solve
Ordering ord;
ord += x1, y1, L1;
VectorConfig delta = fg.optimize(ord);
if (verbose) delta.print("Delta");
// update initial estimate
VectorConfig newState = expmap(state, delta.scale(-1.0));
// set the state to the updated state
state = newState;
if (verbose) state.print("Updated State");
}
// verify that it converges to the nearest optimal point
VectorConfig expected;
expected.insert(x1, Vector_(1, 2.12));
expected.insert(y1, Vector_(1, -0.5));
CHECK(assert_equal(state[x1], expected[x1], 1e-2));
CHECK(assert_equal(state[y1], expected[y1], 1e-2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */