Merge branch 'unified_constraints'
Conflicts: gtsam/linear/NoiseModel.cpp gtsam/linear/NoiseModel.h gtsam/linear/tests/testNoiseModel.cpprelease/4.3a0
Alex Cunningham
parent
70afdfb7d3
commit
98410ca5c9
229
.cproject
229
.cproject
|
|
@ -375,6 +375,14 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -401,7 +409,6 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments/>
|
|
||||||
<buildTarget>tests/testBayesTree.run</buildTarget>
|
<buildTarget>tests/testBayesTree.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -409,7 +416,6 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments/>
|
|
||||||
<buildTarget>testBinaryBayesNet.run</buildTarget>
|
<buildTarget>testBinaryBayesNet.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -457,7 +463,6 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments/>
|
|
||||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -465,7 +470,6 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments/>
|
|
||||||
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
|
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -473,7 +477,6 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments/>
|
|
||||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -489,20 +492,11 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments/>
|
|
||||||
<buildTarget>tests/testBayesTree</buildTarget>
|
<buildTarget>tests/testBayesTree</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -529,6 +523,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testGraph.run</buildTarget>
|
<buildTarget>testGraph.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -600,6 +595,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testInference.run</buildTarget>
|
<buildTarget>testInference.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -607,6 +603,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -614,6 +611,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testJunctionTree.run</buildTarget>
|
<buildTarget>testJunctionTree.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -621,6 +619,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -628,6 +627,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -681,22 +681,6 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>tests/testPose2.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>tests/testPose3.run</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -713,6 +697,22 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>tests/testPose2.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>tests/testPose3.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -737,15 +737,7 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>all</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
<buildTarget>check</buildTarget>
|
<buildTarget>check</buildTarget>
|
||||||
|
|
@ -753,14 +745,6 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>clean</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -801,7 +785,15 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>all</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
<buildTarget>check</buildTarget>
|
<buildTarget>check</buildTarget>
|
||||||
|
|
@ -809,6 +801,14 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>clean</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -1025,6 +1025,22 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="testNonlinearEquality.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>testNonlinearEquality.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="testNonlinearEqualityConstraint.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>testNonlinearEqualityConstraint.run</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -1123,7 +1139,6 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments/>
|
|
||||||
<buildTarget>testErrors.run</buildTarget>
|
<buildTarget>testErrors.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1531,6 +1546,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1570,6 +1586,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testSimulated2D.run</buildTarget>
|
<buildTarget>testSimulated2D.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1577,6 +1594,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>testSimulated3D.run</buildTarget>
|
<buildTarget>testSimulated3D.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1824,6 +1842,7 @@
|
||||||
</target>
|
</target>
|
||||||
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments/>
|
||||||
<buildTarget>tests/testGaussianISAM2</buildTarget>
|
<buildTarget>tests/testGaussianISAM2</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>false</useDefaultCommand>
|
<useDefaultCommand>false</useDefaultCommand>
|
||||||
|
|
@ -1845,46 +1864,6 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>install</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>clean</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>check</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>all</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>dist</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -1981,6 +1960,62 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>install</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>clean</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>check</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>all</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>dist</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>check</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
|
<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j2</buildArguments>
|
||||||
|
<buildTarget>install</buildTarget>
|
||||||
|
<stopOnError>true</stopOnError>
|
||||||
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
</target>
|
||||||
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -2021,22 +2056,6 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>check</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
|
||||||
<buildCommand>make</buildCommand>
|
|
||||||
<buildArguments>-j2</buildArguments>
|
|
||||||
<buildTarget>install</buildTarget>
|
|
||||||
<stopOnError>true</stopOnError>
|
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
|
||||||
<runAllBuilders>true</runAllBuilders>
|
|
||||||
</target>
|
|
||||||
</buildTargets>
|
</buildTargets>
|
||||||
</storageModule>
|
</storageModule>
|
||||||
</cproject>
|
</cproject>
|
||||||
|
|
|
||||||
|
|
@ -500,27 +500,43 @@ Matrix collect(size_t nrMatrices, ...)
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// row scaling, in-place
|
// row scaling, in-place
|
||||||
void vector_scale_inplace(const Vector& v, Matrix& A) {
|
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
|
||||||
const size_t m = A.rows();
|
const size_t m = A.rows();
|
||||||
|
if (inf_mask) {
|
||||||
|
for (size_t i=0; i<m; ++i) {
|
||||||
|
const double& vi = v(i);
|
||||||
|
if (!isnan(vi) && !isinf(vi))
|
||||||
|
A.row(i) *= vi;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
for (size_t i=0; i<m; ++i)
|
for (size_t i=0; i<m; ++i)
|
||||||
A.row(i) *= v(i);
|
A.row(i) *= v(i);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// row scaling
|
// row scaling
|
||||||
Matrix vector_scale(const Vector& v, const Matrix& A) {
|
Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask) {
|
||||||
Matrix M(A);
|
Matrix M(A);
|
||||||
vector_scale_inplace(v, M);
|
vector_scale_inplace(v, M, inf_mask);
|
||||||
return M;
|
return M;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// column scaling
|
// column scaling
|
||||||
Matrix vector_scale(const Matrix& A, const Vector& v) {
|
Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) {
|
||||||
Matrix M(A);
|
Matrix M(A);
|
||||||
const size_t n = A.cols();
|
const size_t n = A.cols();
|
||||||
|
if (inf_mask) {
|
||||||
|
for (size_t j=0; j<n; ++j) {
|
||||||
|
const double& vj = v(j);
|
||||||
|
if (!isnan(vj) && !isinf(vj))
|
||||||
|
M.col(j) *= vj;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
for (size_t j=0; j<n; ++j)
|
for (size_t j=0; j<n; ++j)
|
||||||
M.col(j) *= v(j);
|
M.col(j) *= v(j);
|
||||||
|
}
|
||||||
return M;
|
return M;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -377,10 +377,11 @@ Matrix collect(size_t nrMatrices, ...);
|
||||||
* scales a matrix row or column by the values in a vector
|
* scales a matrix row or column by the values in a vector
|
||||||
* Arguments (Matrix, Vector) scales the columns,
|
* Arguments (Matrix, Vector) scales the columns,
|
||||||
* (Vector, Matrix) scales the rows
|
* (Vector, Matrix) scales the rows
|
||||||
|
* @param inf_mask when true, will not scale with a NaN or inf value
|
||||||
*/
|
*/
|
||||||
void vector_scale_inplace(const Vector& v, Matrix& A); // row
|
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
|
||||||
Matrix vector_scale(const Vector& v, const Matrix& A); // row
|
Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row
|
||||||
Matrix vector_scale(const Matrix& A, const Vector& v); // column
|
Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask = false); // column
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* skew symmetric matrix returns this:
|
* skew symmetric matrix returns this:
|
||||||
|
|
|
||||||
|
|
@ -272,8 +272,8 @@ Vector ediv_(const Vector &a, const Vector &b) {
|
||||||
assert (b.size()==a.size());
|
assert (b.size()==a.size());
|
||||||
Vector c(n);
|
Vector c(n);
|
||||||
for( size_t i = 0; i < n; i++ ) {
|
for( size_t i = 0; i < n; i++ ) {
|
||||||
double ai = a(i), bi = b(i);
|
const double &ai = a(i), &bi = b(i);
|
||||||
c(i) = (bi==0.0 && ai==0.0) ? 0.0 : a(i)/b(i);
|
c(i) = (bi==0.0 && ai==0.0) ? 0.0 : ai/bi;
|
||||||
}
|
}
|
||||||
return c;
|
return c;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -337,6 +337,44 @@ TEST( matrix, scale_rows )
|
||||||
EXPECT(assert_equal(actual, expected));
|
EXPECT(assert_equal(actual, expected));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( matrix, scale_rows_mask )
|
||||||
|
{
|
||||||
|
Matrix A(3, 4);
|
||||||
|
A(0, 0) = 1.;
|
||||||
|
A(0, 1) = 1.;
|
||||||
|
A(0, 2) = 1.;
|
||||||
|
A(0, 3) = 1.;
|
||||||
|
A(1, 0) = 1.;
|
||||||
|
A(1, 1) = 1.;
|
||||||
|
A(1, 2) = 1.;
|
||||||
|
A(1, 3) = 1.;
|
||||||
|
A(2, 0) = 1.;
|
||||||
|
A(2, 1) = 1.;
|
||||||
|
A(2, 2) = 1.;
|
||||||
|
A(2, 3) = 1.;
|
||||||
|
|
||||||
|
Vector v = Vector_(3, 2., std::numeric_limits<double>::infinity(), 4.);
|
||||||
|
|
||||||
|
Matrix actual = vector_scale(v, A, true);
|
||||||
|
|
||||||
|
Matrix expected(3, 4);
|
||||||
|
expected(0, 0) = 2.;
|
||||||
|
expected(0, 1) = 2.;
|
||||||
|
expected(0, 2) = 2.;
|
||||||
|
expected(0, 3) = 2.;
|
||||||
|
expected(1, 0) = 1.;
|
||||||
|
expected(1, 1) = 1.;
|
||||||
|
expected(1, 2) = 1.;
|
||||||
|
expected(1, 3) = 1.;
|
||||||
|
expected(2, 0) = 4.;
|
||||||
|
expected(2, 1) = 4.;
|
||||||
|
expected(2, 2) = 4.;
|
||||||
|
expected(2, 3) = 4.;
|
||||||
|
|
||||||
|
EXPECT(assert_equal(actual, expected));
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( matrix, equal )
|
TEST( matrix, equal )
|
||||||
{
|
{
|
||||||
|
|
|
||||||
|
|
@ -291,13 +291,35 @@ Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu, const Vector&
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void Constrained::print(const std::string& name) const {
|
void Constrained::print(const std::string& name) const {
|
||||||
gtsam::print(sigmas_, name + ": constrained sigmas");
|
gtsam::print(sigmas_, name + ": constrained sigmas");
|
||||||
|
gtsam::print(mu_, name + ": constrained mu");
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Vector Constrained::whiten(const Vector& v) const {
|
Vector Constrained::whiten(const Vector& v) const {
|
||||||
// ediv_ does the right thing with the errors
|
// ediv_ does the right thing with the errors
|
||||||
// FIXME: solve this more effectively
|
// return ediv_(v, sigmas_);
|
||||||
return ediv_(v, sigmas_);
|
const Vector& a = v;
|
||||||
|
const Vector& b = sigmas_;
|
||||||
|
size_t n = a.size();
|
||||||
|
assert (b.size()==a.size());
|
||||||
|
Vector c(n);
|
||||||
|
for( size_t i = 0; i < n; i++ ) {
|
||||||
|
const double& ai = a(i), &bi = b(i);
|
||||||
|
c(i) = (bi==0.0) ? ai : ai/bi;
|
||||||
|
}
|
||||||
|
return c;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
double Constrained::distance(const Vector& v) const {
|
||||||
|
Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
|
||||||
|
for (size_t i=0; i<dim_; ++i) { // add mu weights on constrained variables
|
||||||
|
if (isinf(w[i])) // whiten makes constrained variables infinite
|
||||||
|
w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
|
||||||
|
if (isnan(w[i])) // ensure no other invalid values make it through
|
||||||
|
w[i] = v[i];
|
||||||
|
}
|
||||||
|
return w.dot(w);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -311,14 +333,23 @@ double Constrained::distance(const Vector& v) const {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Matrix Constrained::Whiten(const Matrix& H) const {
|
Matrix Constrained::Whiten(const Matrix& H) const {
|
||||||
// FIXME: replace with pass-through
|
// selective scaling
|
||||||
throw logic_error("noiseModel::Constrained cannot Whiten");
|
return vector_scale(invsigmas(), H, true);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void Constrained::WhitenInPlace(Matrix& H) const {
|
void Constrained::WhitenInPlace(Matrix& H) const {
|
||||||
// FIXME: replace with pass-through
|
// selective scaling
|
||||||
throw logic_error("noiseModel::Constrained cannot Whiten");
|
vector_scale_inplace(invsigmas(), H, true);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Constrained::shared_ptr Constrained::unit() const {
|
||||||
|
Vector sigmas = ones(dim());
|
||||||
|
for (size_t i=0; i<dim(); ++i)
|
||||||
|
if (this->sigmas_(i) == 0.0)
|
||||||
|
sigmas(i) = 0.0;
|
||||||
|
return MixedSigmas(mu_, sigmas);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -401,7 +432,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
|
||||||
}
|
}
|
||||||
toc(4, "constrained_QR write back into Ab");
|
toc(4, "constrained_QR write back into Ab");
|
||||||
|
|
||||||
return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions);
|
// Must include mus, as the defaults might be higher, resulting in non-convergence
|
||||||
|
return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -60,7 +60,7 @@ namespace gtsam {
|
||||||
/// Dimensionality
|
/// Dimensionality
|
||||||
inline size_t dim() const { return dim_;}
|
inline size_t dim() const { return dim_;}
|
||||||
|
|
||||||
virtual void print(const std::string& name) const = 0;
|
virtual void print(const std::string& name = "") const = 0;
|
||||||
|
|
||||||
virtual bool equals(const Base& expected, double tol=1e-9) const = 0;
|
virtual bool equals(const Base& expected, double tol=1e-9) const = 0;
|
||||||
|
|
||||||
|
|
@ -327,8 +327,11 @@ namespace gtsam {
|
||||||
* All other Gaussian models are guaranteed to have a non-singular square-root
|
* All other Gaussian models are guaranteed to have a non-singular square-root
|
||||||
* information matrix, but this class is specifically equipped to deal with
|
* information matrix, but this class is specifically equipped to deal with
|
||||||
* singular noise models, specifically: whiten will return zero on those
|
* singular noise models, specifically: whiten will return zero on those
|
||||||
* components that have zero sigma *and* zero error, infinity otherwise.
|
* components that have zero sigma *and* zero error, unchanged otherwise.
|
||||||
* FIXME: the "otherwise return infinity" does not solve anything
|
*
|
||||||
|
* While a hard constraint may seem to be a case in which there is infinite error,
|
||||||
|
* we do not ever produce an error value of infinity to allow for constraints
|
||||||
|
* to actually be optimized rather than self-destructing if not initialized correctly.
|
||||||
*
|
*
|
||||||
* The distance function in this function provides an error model
|
* The distance function in this function provides an error model
|
||||||
* for a penalty function with a scaling function, assuming a mask of
|
* for a penalty function with a scaling function, assuming a mask of
|
||||||
|
|
@ -387,8 +390,10 @@ namespace gtsam {
|
||||||
/**
|
/**
|
||||||
* A diagonal noise model created by specifying a Vector of
|
* A diagonal noise model created by specifying a Vector of
|
||||||
* standard devations, some of which might be zero
|
* standard devations, some of which might be zero
|
||||||
* TODO: allow for mu
|
|
||||||
*/
|
*/
|
||||||
|
static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) {
|
||||||
|
return shared_ptr(new Constrained(mu, esqrt(variances)));
|
||||||
|
}
|
||||||
static shared_ptr MixedVariances(const Vector& variances) {
|
static shared_ptr MixedVariances(const Vector& variances) {
|
||||||
return shared_ptr(new Constrained(esqrt(variances)));
|
return shared_ptr(new Constrained(esqrt(variances)));
|
||||||
}
|
}
|
||||||
|
|
@ -398,6 +403,9 @@ namespace gtsam {
|
||||||
* precisions, some of which might be inf
|
* precisions, some of which might be inf
|
||||||
* TODO: allow for mu
|
* TODO: allow for mu
|
||||||
*/
|
*/
|
||||||
|
static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions) {
|
||||||
|
return MixedVariances(mu, reciprocal(precisions));
|
||||||
|
}
|
||||||
static shared_ptr MixedPrecisions(const Vector& precisions) {
|
static shared_ptr MixedPrecisions(const Vector& precisions) {
|
||||||
return MixedVariances(reciprocal(precisions));
|
return MixedVariances(reciprocal(precisions));
|
||||||
}
|
}
|
||||||
|
|
@ -429,9 +437,8 @@ namespace gtsam {
|
||||||
/// Calculates error vector with weights applied
|
/// Calculates error vector with weights applied
|
||||||
virtual Vector whiten(const Vector& v) const;
|
virtual Vector whiten(const Vector& v) const;
|
||||||
|
|
||||||
// Whitening Jacobians does not make sense for possibly constrained
|
/// Whitening functions will perform partial whitening on rows
|
||||||
// noise model and will throw an exception.
|
/// with a non-zero sigma. Other rows remain untouched.
|
||||||
// FIXME: change to allow for use a normal linearization function
|
|
||||||
virtual Matrix Whiten(const Matrix& H) const;
|
virtual Matrix Whiten(const Matrix& H) const;
|
||||||
virtual void WhitenInPlace(Matrix& H) const;
|
virtual void WhitenInPlace(Matrix& H) const;
|
||||||
|
|
||||||
|
|
@ -446,6 +453,12 @@ namespace gtsam {
|
||||||
*/
|
*/
|
||||||
virtual bool isConstrained() const {return true;}
|
virtual bool isConstrained() const {return true;}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Returns a Unit version of a constrained noisemodel in which
|
||||||
|
* constrained sigmas remain constrained and the rest are unit scaled
|
||||||
|
*/
|
||||||
|
shared_ptr unit() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
|
|
|
||||||
|
|
@ -187,7 +187,8 @@ TEST(NoiseModel, ConstrainedMixed )
|
||||||
Vector feasible = Vector_(3, 1.0, 0.0, 1.0),
|
Vector feasible = Vector_(3, 1.0, 0.0, 1.0),
|
||||||
infeasible = Vector_(3, 1.0, 1.0, 1.0);
|
infeasible = Vector_(3, 1.0, 1.0, 1.0);
|
||||||
Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma));
|
Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma));
|
||||||
EXPECT(assert_equal(Vector_(3, 0.5, inf, 0.5),d->whiten(infeasible)));
|
// NOTE: we catch constrained variables elsewhere, so whitening does nothing
|
||||||
|
EXPECT(assert_equal(Vector_(3, 0.5, 1.0, 0.5),d->whiten(infeasible)));
|
||||||
EXPECT(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible)));
|
EXPECT(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible)));
|
||||||
|
|
||||||
DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
|
DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
|
||||||
|
|
@ -201,7 +202,8 @@ TEST(NoiseModel, ConstrainedAll )
|
||||||
infeasible = Vector_(3, 1.0, 1.0, 1.0);
|
infeasible = Vector_(3, 1.0, 1.0, 1.0);
|
||||||
|
|
||||||
Constrained::shared_ptr i = Constrained::All(3);
|
Constrained::shared_ptr i = Constrained::All(3);
|
||||||
EXPECT(assert_equal(Vector_(3, inf, inf, inf),i->whiten(infeasible)));
|
// NOTE: we catch constrained variables elsewhere, so whitening does nothing
|
||||||
|
EXPECT(assert_equal(Vector_(3, 1.0, 1.0, 1.0),i->whiten(infeasible)));
|
||||||
EXPECT(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible)));
|
EXPECT(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible)));
|
||||||
|
|
||||||
DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
|
DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
|
||||||
|
|
|
||||||
|
|
@ -11,8 +11,7 @@
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* @file NonlinearConstraint.h
|
* @file NonlinearConstraint.h
|
||||||
* @brief Implements nonlinear constraints that can be linearized using
|
* @brief Implements simple cases of constraints
|
||||||
* direct linearization and solving through a quadratic merit function
|
|
||||||
* @author Alex Cunningham
|
* @author Alex Cunningham
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
@ -22,566 +21,48 @@
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/**
|
|
||||||
* Base class for nonlinear constraints
|
|
||||||
* This allows for both equality and inequality constraints,
|
|
||||||
* where equality constraints are active all the time (even slightly
|
|
||||||
* nonzero constraint functions will still be active - inequality
|
|
||||||
* constraints should be sure to force to actual zero)
|
|
||||||
*
|
|
||||||
* 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 VALUES>
|
|
||||||
class NonlinearConstraint : public NonlinearFactor<VALUES> {
|
|
||||||
|
|
||||||
protected:
|
|
||||||
typedef NonlinearConstraint<VALUES> This;
|
|
||||||
typedef NonlinearFactor<VALUES> Base;
|
|
||||||
typedef Factor<Symbol> BaseFactor;
|
|
||||||
|
|
||||||
/** default constructor to allow for serialization */
|
|
||||||
NonlinearConstraint() {}
|
|
||||||
|
|
||||||
double mu_; /// gain for quadratic merit function
|
|
||||||
size_t dim_; /// dimension of the constraint
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
/** Constructor - sets the cost function and the lagrange multipliers
|
|
||||||
* @param dim is the dimension of the factor
|
|
||||||
* @param keys is a boost::tuple containing the keys, e.g. \c make_tuple(key1,key2,key3)
|
|
||||||
* @param mu is the gain used at error evaluation (forced to be positive)
|
|
||||||
*/
|
|
||||||
template<class U1, class U2>
|
|
||||||
NonlinearConstraint(const boost::tuples::cons<U1,U2>& keys, size_t dim, double mu = 1000.0)
|
|
||||||
: Base(keys), mu_(fabs(mu)), dim_(dim) {
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Constructor
|
|
||||||
* @param keys The variables involved in this factor
|
|
||||||
*/
|
|
||||||
template<class ITERATOR>
|
|
||||||
NonlinearConstraint(ITERATOR beginKeys, ITERATOR endKeys, size_t dim, double mu = 1000.0)
|
|
||||||
: Base(beginKeys, endKeys), mu_(fabs(mu)), dim_(dim) {
|
|
||||||
}
|
|
||||||
|
|
||||||
virtual ~NonlinearConstraint() {}
|
|
||||||
|
|
||||||
/** returns the gain mu */
|
|
||||||
double mu() const { return mu_; }
|
|
||||||
|
|
||||||
/** get the dimension of the factor (number of rows on linearization) */
|
|
||||||
virtual size_t dim() const {
|
|
||||||
return dim_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Print */
|
|
||||||
virtual void print(const std::string& s = "") const {
|
|
||||||
std::cout << "NonlinearConstraint " << s << " mu: " << this->mu_ << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Check if two factors are equal */
|
|
||||||
virtual bool equals(const NonlinearFactor<VALUES>& f, double tol=1e-9) const {
|
|
||||||
const This* p = dynamic_cast<const This*> (&f);
|
|
||||||
if (p == NULL) return false;
|
|
||||||
return BaseFactor::equals(*p, tol) && (fabs(mu_ - p->mu_) <= tol) && dim_ == p->dim_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/** error function - returns the quadratic merit function */
|
|
||||||
virtual double error(const VALUES& c) const {
|
|
||||||
if (active(c))
|
|
||||||
return mu_ * unwhitenedError(c).squaredNorm();
|
|
||||||
else
|
|
||||||
return 0.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* active set check, defines what type of constraint this is
|
|
||||||
*
|
|
||||||
* In an inequality/bounding constraint, this active() returns true
|
|
||||||
* when the constraint is *NOT* fulfilled.
|
|
||||||
* @return true if the constraint is active
|
|
||||||
*/
|
|
||||||
virtual bool active(const VALUES& c) const=0;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Error function \f$ z-c(x) \f$.
|
|
||||||
* Override this method to finish implementing an N-way factor.
|
|
||||||
* If any of the optional Matrix reference arguments are specified, it should compute
|
|
||||||
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
|
|
||||||
*/
|
|
||||||
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Linearizes around a given config
|
|
||||||
* @param config is the values structure
|
|
||||||
* @return a combined linear factor containing both the constraint and the constraint factor
|
|
||||||
*/
|
|
||||||
virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
|
|
||||||
if (!active(c))
|
|
||||||
return boost::shared_ptr<JacobianFactor>();
|
|
||||||
|
|
||||||
// Create the set of terms - Jacobians for each index
|
|
||||||
Vector b;
|
|
||||||
// Call evaluate error to get Jacobians and b vector
|
|
||||||
std::vector<Matrix> A(this->size());
|
|
||||||
b = -unwhitenedError(c, A);
|
|
||||||
|
|
||||||
std::vector<std::pair<Index, Matrix> > terms(this->size());
|
|
||||||
// Fill in terms
|
|
||||||
for(size_t j=0; j<this->size(); ++j) {
|
|
||||||
terms[j].first = ordering[this->keys()[j]];
|
|
||||||
terms[j].second.swap(A[j]);
|
|
||||||
}
|
|
||||||
|
|
||||||
return GaussianFactor::shared_ptr(
|
|
||||||
new JacobianFactor(terms, b, noiseModel::Constrained::All(dim_)));
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
ar & boost::serialization::make_nvp("NonlinearFactor",
|
|
||||||
boost::serialization::base_object<Base>(*this));
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(mu_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(dim_);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* A unary constraint that defaults to an equality constraint
|
|
||||||
*/
|
|
||||||
template <class VALUES, class KEY>
|
|
||||||
class NonlinearConstraint1 : public NonlinearConstraint<VALUES> {
|
|
||||||
|
|
||||||
public:
|
|
||||||
typedef typename KEY::Value X;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
typedef NonlinearConstraint1<VALUES,KEY> This;
|
|
||||||
typedef NonlinearConstraint<VALUES> Base;
|
|
||||||
|
|
||||||
/** default constructor to allow for serialization */
|
|
||||||
NonlinearConstraint1() {}
|
|
||||||
|
|
||||||
/** key for the constrained variable */
|
|
||||||
KEY key_;
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Basic constructor
|
|
||||||
* @param key is the identifier for the variable constrained
|
|
||||||
* @param dim is the size of the constraint (p)
|
|
||||||
* @param mu is the gain for the factor
|
|
||||||
*/
|
|
||||||
NonlinearConstraint1(const KEY& key, size_t dim, double mu = 1000.0)
|
|
||||||
: Base(make_tuple(key), dim, mu), key_(key) { }
|
|
||||||
virtual ~NonlinearConstraint1() {}
|
|
||||||
|
|
||||||
/** Calls the 1-key specific version of evaluateError, which is pure virtual
|
|
||||||
* so must be implemented in the derived class. */
|
|
||||||
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
|
||||||
if(this->active(x)) {
|
|
||||||
const X& x1 = x[key_];
|
|
||||||
if(H) {
|
|
||||||
return evaluateError(x1, (*H)[0]);
|
|
||||||
} else {
|
|
||||||
return evaluateError(x1);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
return zero(this->dim());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Print */
|
|
||||||
virtual void print(const std::string& s = "") const {
|
|
||||||
std::cout << s << ": NonlinearConstraint("
|
|
||||||
<< (std::string) this->key_ << ")," <<
|
|
||||||
" mu = " << this->mu_ <<
|
|
||||||
" dim = " << this->dim_ << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Override this method to finish implementing a unary factor.
|
|
||||||
* If the optional Matrix reference argument is specified, it should compute
|
|
||||||
* both the function evaluation and its derivative in X.
|
|
||||||
*/
|
|
||||||
virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
|
|
||||||
boost::none) const = 0;
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
ar & boost::serialization::make_nvp("NonlinearConstraint",
|
|
||||||
boost::serialization::base_object<Base>(*this));
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(key_);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* Unary Equality constraint - simply forces the value of active() to true
|
|
||||||
*/
|
|
||||||
template <class VALUES, class KEY>
|
|
||||||
class NonlinearEqualityConstraint1 : public NonlinearConstraint1<VALUES, KEY> {
|
|
||||||
|
|
||||||
public:
|
|
||||||
typedef typename KEY::Value X;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
typedef NonlinearEqualityConstraint1<VALUES,KEY> This;
|
|
||||||
typedef NonlinearConstraint1<VALUES,KEY> Base;
|
|
||||||
|
|
||||||
/** default constructor to allow for serialization */
|
|
||||||
NonlinearEqualityConstraint1() {}
|
|
||||||
|
|
||||||
public:
|
|
||||||
NonlinearEqualityConstraint1(const KEY& key, size_t dim, double mu = 1000.0)
|
|
||||||
: Base(key, dim, mu) {}
|
|
||||||
virtual ~NonlinearEqualityConstraint1() {}
|
|
||||||
|
|
||||||
/** Always active, so fixed value for active() */
|
|
||||||
virtual bool active(const VALUES& c) const { return true; }
|
|
||||||
|
|
||||||
/** Print */
|
|
||||||
virtual void print(const std::string& s = "") const {
|
|
||||||
std::cout << s << ": NonlinearEqualityConstraint("
|
|
||||||
<< (std::string) this->key_ << ")," <<
|
|
||||||
" mu = " << this->mu_ <<
|
|
||||||
" dim = " << this->dim_ << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
ar & boost::serialization::make_nvp("NonlinearConstraint1",
|
|
||||||
boost::serialization::base_object<Base>(*this));
|
|
||||||
}
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* A binary constraint with arbitrary cost and jacobian functions
|
|
||||||
*/
|
|
||||||
template <class VALUES, class KEY1, class KEY2>
|
|
||||||
class NonlinearConstraint2 : public NonlinearConstraint<VALUES> {
|
|
||||||
|
|
||||||
public:
|
|
||||||
typedef typename KEY1::Value X1;
|
|
||||||
typedef typename KEY2::Value X2;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
typedef NonlinearConstraint2<VALUES,KEY1,KEY2> This;
|
|
||||||
typedef NonlinearConstraint<VALUES> Base;
|
|
||||||
|
|
||||||
/** default constructor to allow for serialization */
|
|
||||||
NonlinearConstraint2() {}
|
|
||||||
|
|
||||||
/** keys for the constrained variables */
|
|
||||||
KEY1 key1_;
|
|
||||||
KEY2 key2_;
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Basic constructor
|
|
||||||
* @param key1 is the identifier for the first variable constrained
|
|
||||||
* @param key2 is the identifier for the second variable constrained
|
|
||||||
* @param dim is the size of the constraint (p)
|
|
||||||
* @param mu is the gain for the factor
|
|
||||||
*/
|
|
||||||
NonlinearConstraint2(const KEY1& key1, const KEY2& key2, size_t dim, double mu = 1000.0) :
|
|
||||||
Base(make_tuple(key1, key2), dim, mu), key1_(key1), key2_(key2) { }
|
|
||||||
virtual ~NonlinearConstraint2() {}
|
|
||||||
|
|
||||||
/** Calls the 2-key specific version of evaluateError, which is pure virtual
|
|
||||||
* so must be implemented in the derived class. */
|
|
||||||
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
|
||||||
if(this->active(x)) {
|
|
||||||
const X1& x1 = x[key1_];
|
|
||||||
const X2& x2 = x[key2_];
|
|
||||||
if(H) {
|
|
||||||
return evaluateError(x1, x2, (*H)[0], (*H)[1]);
|
|
||||||
} else {
|
|
||||||
return evaluateError(x1, x2);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
return zero(this->dim());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Print */
|
|
||||||
virtual void print(const std::string& s = "") const {
|
|
||||||
std::cout << s << ": NonlinearConstraint("
|
|
||||||
<< (std::string) this->key1_ << ","
|
|
||||||
<< (std::string) this->key2_ << ")," <<
|
|
||||||
" mu = " << this->mu_ <<
|
|
||||||
" dim = " << this->dim_ << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Override this method to finish implementing a binary factor.
|
|
||||||
* If any of the optional Matrix reference arguments are specified, it should compute
|
|
||||||
* both the function evaluation and its derivative(s) in X1 (and/or X2).
|
|
||||||
*/
|
|
||||||
virtual Vector
|
|
||||||
evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 =
|
|
||||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const = 0;
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
ar & boost::serialization::make_nvp("NonlinearConstraint",
|
|
||||||
boost::serialization::base_object<Base>(*this));
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(key1_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(key2_);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* Binary Equality constraint - simply forces the value of active() to true
|
|
||||||
*/
|
|
||||||
template <class VALUES, class KEY1, class KEY2>
|
|
||||||
class NonlinearEqualityConstraint2 : public NonlinearConstraint2<VALUES, KEY1, KEY2> {
|
|
||||||
|
|
||||||
public:
|
|
||||||
typedef typename KEY1::Value X1;
|
|
||||||
typedef typename KEY2::Value X2;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
typedef NonlinearEqualityConstraint2<VALUES,KEY1,KEY2> This;
|
|
||||||
typedef NonlinearConstraint2<VALUES,KEY1,KEY2> Base;
|
|
||||||
|
|
||||||
/** default constructor to allow for serialization */
|
|
||||||
NonlinearEqualityConstraint2() {}
|
|
||||||
|
|
||||||
public:
|
|
||||||
NonlinearEqualityConstraint2(const KEY1& key1, const KEY2& key2, size_t dim, double mu = 1000.0)
|
|
||||||
: Base(key1, key2, dim, mu) {}
|
|
||||||
virtual ~NonlinearEqualityConstraint2() {}
|
|
||||||
|
|
||||||
/** Always active, so fixed value for active() */
|
|
||||||
virtual bool active(const VALUES& c) const { return true; }
|
|
||||||
|
|
||||||
/** Print */
|
|
||||||
virtual void print(const std::string& s = "") const {
|
|
||||||
std::cout << s << ": NonlinearEqualityConstraint("
|
|
||||||
<< (std::string) this->key1_ << ","
|
|
||||||
<< (std::string) this->key2_ << ")," <<
|
|
||||||
" mu = " << this->mu_ <<
|
|
||||||
" dim = " << this->dim_ << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
ar & boost::serialization::make_nvp("NonlinearConstraint2",
|
|
||||||
boost::serialization::base_object<Base>(*this));
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* A ternary constraint
|
|
||||||
*/
|
|
||||||
template <class VALUES, class KEY1, class KEY2, class KEY3>
|
|
||||||
class NonlinearConstraint3 : public NonlinearConstraint<VALUES> {
|
|
||||||
|
|
||||||
public:
|
|
||||||
typedef typename KEY1::Value X1;
|
|
||||||
typedef typename KEY2::Value X2;
|
|
||||||
typedef typename KEY3::Value X3;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
typedef NonlinearConstraint3<VALUES,KEY1,KEY2,KEY3> This;
|
|
||||||
typedef NonlinearConstraint<VALUES> Base;
|
|
||||||
|
|
||||||
/** default constructor to allow for serialization */
|
|
||||||
NonlinearConstraint3() {}
|
|
||||||
|
|
||||||
/** keys for the constrained variables */
|
|
||||||
KEY1 key1_;
|
|
||||||
KEY2 key2_;
|
|
||||||
KEY3 key3_;
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Basic constructor
|
|
||||||
* @param key1 is the identifier for the first variable constrained
|
|
||||||
* @param key2 is the identifier for the second variable constrained
|
|
||||||
* @param key3 is the identifier for the second variable constrained
|
|
||||||
* @param dim is the size of the constraint (p)
|
|
||||||
* @param mu is the gain for the factor
|
|
||||||
*/
|
|
||||||
NonlinearConstraint3(const KEY1& key1, const KEY2& key2, const KEY3& key3,
|
|
||||||
size_t dim, double mu = 1000.0) :
|
|
||||||
Base(make_tuple(key1, key2, key3), dim, mu), key1_(key1), key2_(key2), key3_(key3) { }
|
|
||||||
virtual ~NonlinearConstraint3() {}
|
|
||||||
|
|
||||||
/** Calls the 2-key specific version of evaluateError, which is pure virtual
|
|
||||||
* so must be implemented in the derived class. */
|
|
||||||
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
|
||||||
if(this->active(x)) {
|
|
||||||
const X1& x1 = x[key1_];
|
|
||||||
const X2& x2 = x[key2_];
|
|
||||||
const X3& x3 = x[key3_];
|
|
||||||
if(H) {
|
|
||||||
return evaluateError(x1, x2, x3, (*H)[0], (*H)[1], (*H)[2]);
|
|
||||||
} else {
|
|
||||||
return evaluateError(x1, x2, x3);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
return zero(this->dim());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Print */
|
|
||||||
virtual void print(const std::string& s = "") const {
|
|
||||||
std::cout << s << ": NonlinearConstraint("
|
|
||||||
<< (std::string) this->key1_ << ","
|
|
||||||
<< (std::string) this->key2_ << ","
|
|
||||||
<< (std::string) this->key3_ << ")," <<
|
|
||||||
" mu = " << this->mu_ <<
|
|
||||||
" dim = " << this->dim_ << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Override this method to finish implementing a trinary factor.
|
|
||||||
* If any of the optional Matrix reference arguments are specified, it should compute
|
|
||||||
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3).
|
|
||||||
*/
|
|
||||||
virtual Vector
|
|
||||||
evaluateError(const X1&, const X2&, const X3&,
|
|
||||||
boost::optional<Matrix&> H1 = boost::none,
|
|
||||||
boost::optional<Matrix&> H2 = boost::none,
|
|
||||||
boost::optional<Matrix&> H3 = boost::none) const = 0;
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
ar & boost::serialization::make_nvp("NonlinearConstraint",
|
|
||||||
boost::serialization::base_object<Base>(*this));
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(key1_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(key2_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(key3_);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* Ternary Equality constraint - simply forces the value of active() to true
|
|
||||||
*/
|
|
||||||
template <class VALUES, class KEY1, class KEY2, class KEY3>
|
|
||||||
class NonlinearEqualityConstraint3 : public NonlinearConstraint3<VALUES, KEY1, KEY2, KEY3> {
|
|
||||||
|
|
||||||
public:
|
|
||||||
typedef typename KEY1::Value X1;
|
|
||||||
typedef typename KEY2::Value X2;
|
|
||||||
typedef typename KEY3::Value X3;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
typedef NonlinearEqualityConstraint3<VALUES,KEY1,KEY2,KEY3> This;
|
|
||||||
typedef NonlinearConstraint3<VALUES,KEY1,KEY2,KEY3> Base;
|
|
||||||
|
|
||||||
/** default constructor to allow for serialization */
|
|
||||||
NonlinearEqualityConstraint3() {}
|
|
||||||
|
|
||||||
public:
|
|
||||||
NonlinearEqualityConstraint3(const KEY1& key1, const KEY2& key2, const KEY3& key3,
|
|
||||||
size_t dim, double mu = 1000.0)
|
|
||||||
: Base(key1, key2, key3, dim, mu) {}
|
|
||||||
virtual ~NonlinearEqualityConstraint3() {}
|
|
||||||
|
|
||||||
/** Always active, so fixed value for active() */
|
|
||||||
virtual bool active(const VALUES& c) const { return true; }
|
|
||||||
|
|
||||||
/** Print */
|
|
||||||
virtual void print(const std::string& s = "") const {
|
|
||||||
std::cout << s << ": NonlinearEqualityConstraint("
|
|
||||||
<< (std::string) this->key1_ << ","
|
|
||||||
<< (std::string) this->key2_ << ","
|
|
||||||
<< (std::string) this->key3_ << ")," <<
|
|
||||||
" mu = " << this->mu_ <<
|
|
||||||
" dim = " << this->dim_ << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
ar & boost::serialization::make_nvp("NonlinearConstraint3",
|
|
||||||
boost::serialization::base_object<Base>(*this));
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
/**
|
/**
|
||||||
* Simple unary equality constraint - fixes a value for a variable
|
* Simple unary equality constraint - fixes a value for a variable
|
||||||
*/
|
*/
|
||||||
template<class VALUES, class KEY>
|
template<class VALUES, class KEY>
|
||||||
class NonlinearEquality1 : public NonlinearEqualityConstraint1<VALUES, KEY> {
|
class NonlinearEquality1 : public NonlinearFactor1<VALUES, KEY> {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef typename KEY::Value X;
|
typedef typename KEY::Value X;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
typedef NonlinearEqualityConstraint1<VALUES, KEY> Base;
|
typedef NonlinearFactor1<VALUES, KEY> Base;
|
||||||
|
|
||||||
/** default constructor to allow for serialization */
|
/** default constructor to allow for serialization */
|
||||||
NonlinearEquality1() {}
|
NonlinearEquality1() {}
|
||||||
|
|
||||||
X value_; /// fixed value for variable
|
X value_; /// fixed value for variable
|
||||||
|
|
||||||
|
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
|
||||||
|
GTSAM_CONCEPT_TESTABLE_TYPE(X);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
typedef boost::shared_ptr<NonlinearEquality1<VALUES, KEY> > shared_ptr;
|
typedef boost::shared_ptr<NonlinearEquality1<VALUES, KEY> > shared_ptr;
|
||||||
|
|
||||||
NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
|
NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
|
||||||
: Base(key1, X::Dim(), mu), value_(value) {}
|
: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
|
||||||
|
|
||||||
virtual ~NonlinearEquality1() {}
|
virtual ~NonlinearEquality1() {}
|
||||||
|
|
||||||
/** g(x) with optional derivative */
|
/** g(x) with optional derivative */
|
||||||
Vector evaluateError(const X& x1, boost::optional<Matrix&> H1 = boost::none) const {
|
Vector evaluateError(const X& x1, boost::optional<Matrix&> H = boost::none) const {
|
||||||
const size_t p = X::Dim();
|
if (H) (*H) = eye(x1.dim());
|
||||||
if (H1) *H1 = eye(p);
|
// manifold equivalent of h(x)-z -> log(z,h(x))
|
||||||
return value_.localCoordinates(x1);
|
return value_.localCoordinates(x1);
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Print */
|
/** Print */
|
||||||
virtual void print(const std::string& s = "") const {
|
virtual void print(const std::string& s = "") const {
|
||||||
std::cout << s << ": NonlinearEquality("
|
std::cout << s << ": NonlinearEquality1("
|
||||||
<< (std::string) this->key_ << "),"<<
|
<< (std::string) this->key_ << "),"<< "\n";
|
||||||
" mu = " << this->mu_ <<
|
this->noiseModel_->print();
|
||||||
" dim = " << this->dim_ << "\n";
|
|
||||||
value_.print("Value");
|
value_.print("Value");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -591,7 +72,7 @@ private:
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
ar & boost::serialization::make_nvp("NonlinearEqualityConstraint1",
|
ar & boost::serialization::make_nvp("NonlinearFactor1",
|
||||||
boost::serialization::base_object<Base>(*this));
|
boost::serialization::base_object<Base>(*this));
|
||||||
ar & BOOST_SERIALIZATION_NVP(value_);
|
ar & BOOST_SERIALIZATION_NVP(value_);
|
||||||
}
|
}
|
||||||
|
|
@ -600,15 +81,17 @@ private:
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
/**
|
/**
|
||||||
* Simple binary equality constraint - this constraint forces two factors to
|
* Simple binary equality constraint - this constraint forces two factors to
|
||||||
* be the same. This constraint requires the underlying type to a Lie type
|
* be the same.
|
||||||
*/
|
*/
|
||||||
template<class VALUES, class KEY>
|
template<class VALUES, class KEY>
|
||||||
class NonlinearEquality2 : public NonlinearEqualityConstraint2<VALUES, KEY, KEY> {
|
class NonlinearEquality2 : public NonlinearFactor2<VALUES, KEY, KEY> {
|
||||||
public:
|
public:
|
||||||
typedef typename KEY::Value X;
|
typedef typename KEY::Value X;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
typedef NonlinearEqualityConstraint2<VALUES, KEY, KEY> Base;
|
typedef NonlinearFactor2<VALUES, KEY, KEY> Base;
|
||||||
|
|
||||||
|
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
|
||||||
|
|
||||||
/** default constructor to allow for serialization */
|
/** default constructor to allow for serialization */
|
||||||
NonlinearEquality2() {}
|
NonlinearEquality2() {}
|
||||||
|
|
@ -618,7 +101,7 @@ public:
|
||||||
typedef boost::shared_ptr<NonlinearEquality2<VALUES, KEY> > shared_ptr;
|
typedef boost::shared_ptr<NonlinearEquality2<VALUES, KEY> > shared_ptr;
|
||||||
|
|
||||||
NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)
|
NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)
|
||||||
: Base(key1, key2, X::Dim(), mu) {}
|
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
|
||||||
virtual ~NonlinearEquality2() {}
|
virtual ~NonlinearEquality2() {}
|
||||||
|
|
||||||
/** g(x) with optional derivative2 */
|
/** g(x) with optional derivative2 */
|
||||||
|
|
@ -637,7 +120,7 @@ private:
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2",
|
ar & boost::serialization::make_nvp("NonlinearFactor2",
|
||||||
boost::serialization::base_object<Base>(*this));
|
boost::serialization::base_object<Base>(*this));
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
|
||||||
|
|
@ -101,7 +101,8 @@ namespace gtsam {
|
||||||
/** Check if two factors are equal */
|
/** Check if two factors are equal */
|
||||||
bool equals(const NonlinearEquality<VALUES,KEY>& f, double tol = 1e-9) const {
|
bool equals(const NonlinearEquality<VALUES,KEY>& f, double tol = 1e-9) const {
|
||||||
if (!Base::equals(f)) return false;
|
if (!Base::equals(f)) return false;
|
||||||
return compare_(feasible_, f.feasible_);
|
return feasible_.equals(f.feasible_, tol) &&
|
||||||
|
fabs(error_gain_ - f.error_gain_) < tol;
|
||||||
}
|
}
|
||||||
|
|
||||||
/** actual error function calculation */
|
/** actual error function calculation */
|
||||||
|
|
|
||||||
|
|
@ -112,6 +112,18 @@ public:
|
||||||
/** get the dimension of the factor (number of rows on linearization) */
|
/** get the dimension of the factor (number of rows on linearization) */
|
||||||
virtual size_t dim() const = 0;
|
virtual size_t dim() const = 0;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Checks whether a factor should be used based on a set of values.
|
||||||
|
* This is primarily used to implment inequality constraints that
|
||||||
|
* require a variable active set. For all others, the default implementation
|
||||||
|
* returning true solves this problem.
|
||||||
|
*
|
||||||
|
* In an inequality/bounding constraint, this active() returns true
|
||||||
|
* when the constraint is *NOT* fulfilled.
|
||||||
|
* @return true if the constraint is active
|
||||||
|
*/
|
||||||
|
virtual bool active(const VALUES& c) const { return true; }
|
||||||
|
|
||||||
/** linearize to a GaussianFactor */
|
/** linearize to a GaussianFactor */
|
||||||
virtual boost::shared_ptr<GaussianFactor>
|
virtual boost::shared_ptr<GaussianFactor>
|
||||||
linearize(const VALUES& c, const Ordering& ordering) const = 0;
|
linearize(const VALUES& c, const Ordering& ordering) const = 0;
|
||||||
|
|
@ -138,7 +150,7 @@ public:
|
||||||
* the derived class implements \f$ \mathtt{error\_vector}(x) = h(x)-z \approx A \delta x - b \f$
|
* the derived class implements \f$ \mathtt{error\_vector}(x) = h(x)-z \approx A \delta x - b \f$
|
||||||
* This allows a graph to have factors with measurements of mixed type.
|
* This allows a graph to have factors with measurements of mixed type.
|
||||||
|
|
||||||
* The noise model is typically Gaussian, but robust error models are also supported.
|
* The noise model is typically Gaussian, but robust and constrained error models are also supported.
|
||||||
*/
|
*/
|
||||||
template<class VALUES>
|
template<class VALUES>
|
||||||
class NoiseModelFactor: public NonlinearFactor<VALUES> {
|
class NoiseModelFactor: public NonlinearFactor<VALUES> {
|
||||||
|
|
@ -237,7 +249,10 @@ public:
|
||||||
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
|
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
|
||||||
*/
|
*/
|
||||||
virtual double error(const VALUES& c) const {
|
virtual double error(const VALUES& c) const {
|
||||||
|
if (active(c))
|
||||||
return 0.5 * noiseModel_->distance(unwhitenedError(c));
|
return 0.5 * noiseModel_->distance(unwhitenedError(c));
|
||||||
|
else
|
||||||
|
return 0.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
@ -246,6 +261,9 @@ public:
|
||||||
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
|
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
|
||||||
*/
|
*/
|
||||||
boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
|
boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
|
||||||
|
// Only linearize if the factor is active
|
||||||
|
if (!active(x))
|
||||||
|
return boost::shared_ptr<JacobianFactor>();
|
||||||
|
|
||||||
// Create the set of terms - Jacobians for each index
|
// Create the set of terms - Jacobians for each index
|
||||||
Vector b;
|
Vector b;
|
||||||
|
|
@ -253,10 +271,6 @@ public:
|
||||||
std::vector<Matrix> A(this->size());
|
std::vector<Matrix> A(this->size());
|
||||||
b = -unwhitenedError(x, A);
|
b = -unwhitenedError(x, A);
|
||||||
|
|
||||||
// TODO pass unwhitened + noise model to Gaussian factor
|
|
||||||
SharedDiagonal constrained =
|
|
||||||
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
|
|
||||||
if(!constrained)
|
|
||||||
this->noiseModel_->WhitenSystem(A,b);
|
this->noiseModel_->WhitenSystem(A,b);
|
||||||
|
|
||||||
std::vector<std::pair<Index, Matrix> > terms(this->size());
|
std::vector<std::pair<Index, Matrix> > terms(this->size());
|
||||||
|
|
@ -266,9 +280,12 @@ public:
|
||||||
terms[j].second.swap(A[j]);
|
terms[j].second.swap(A[j]);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// TODO pass unwhitened + noise model to Gaussian factor
|
||||||
|
noiseModel::Constrained::shared_ptr constrained =
|
||||||
|
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
|
||||||
if(constrained)
|
if(constrained)
|
||||||
return GaussianFactor::shared_ptr(
|
return GaussianFactor::shared_ptr(
|
||||||
new JacobianFactor(terms, b, constrained));
|
new JacobianFactor(terms, b, constrained->unit()));
|
||||||
else
|
else
|
||||||
return GaussianFactor::shared_ptr(
|
return GaussianFactor::shared_ptr(
|
||||||
new JacobianFactor(terms, b, noiseModel::Unit::Create(b.size())));
|
new JacobianFactor(terms, b, noiseModel::Unit::Create(b.size())));
|
||||||
|
|
@ -328,10 +345,16 @@ public:
|
||||||
/** Calls the 1-key specific version of evaluateError, which is pure virtual
|
/** Calls the 1-key specific version of evaluateError, which is pure virtual
|
||||||
* so must be implemented in the derived class. */
|
* so must be implemented in the derived class. */
|
||||||
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||||
if(H)
|
if(this->active(x)) {
|
||||||
return evaluateError(x[key_], (*H)[0]);
|
const X& x1 = x[key_];
|
||||||
else
|
if(H) {
|
||||||
return evaluateError(x[key_]);
|
return evaluateError(x1, (*H)[0]);
|
||||||
|
} else {
|
||||||
|
return evaluateError(x1);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
return zero(this->dim());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Print */
|
/** Print */
|
||||||
|
|
@ -406,10 +429,17 @@ public:
|
||||||
/** Calls the 2-key specific version of evaluateError, which is pure virtual
|
/** Calls the 2-key specific version of evaluateError, which is pure virtual
|
||||||
* so must be implemented in the derived class. */
|
* so must be implemented in the derived class. */
|
||||||
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||||
if(H)
|
if(this->active(x)) {
|
||||||
return evaluateError(x[key1_], x[key2_], (*H)[0], (*H)[1]);
|
const X1& x1 = x[key1_];
|
||||||
else
|
const X2& x2 = x[key2_];
|
||||||
return evaluateError(x[key1_], x[key2_]);
|
if(H) {
|
||||||
|
return evaluateError(x1, x2, (*H)[0], (*H)[1]);
|
||||||
|
} else {
|
||||||
|
return evaluateError(x1, x2);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
return zero(this->dim());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Print */
|
/** Print */
|
||||||
|
|
@ -491,10 +521,14 @@ public:
|
||||||
/** Calls the 3-key specific version of evaluateError, which is pure virtual
|
/** Calls the 3-key specific version of evaluateError, which is pure virtual
|
||||||
* so must be implemented in the derived class. */
|
* so must be implemented in the derived class. */
|
||||||
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||||
|
if(this->active(x)) {
|
||||||
if(H)
|
if(H)
|
||||||
return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
|
return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
|
||||||
else
|
else
|
||||||
return evaluateError(x[key1_], x[key2_], x[key3_]);
|
return evaluateError(x[key1_], x[key2_], x[key3_]);
|
||||||
|
} else {
|
||||||
|
return zero(this->dim());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Print */
|
/** Print */
|
||||||
|
|
@ -585,10 +619,14 @@ public:
|
||||||
/** Calls the 4-key specific version of evaluateError, which is pure virtual
|
/** Calls the 4-key specific version of evaluateError, which is pure virtual
|
||||||
* so must be implemented in the derived class. */
|
* so must be implemented in the derived class. */
|
||||||
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||||
|
if(this->active(x)) {
|
||||||
if(H)
|
if(H)
|
||||||
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
|
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
|
||||||
else
|
else
|
||||||
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]);
|
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]);
|
||||||
|
} else {
|
||||||
|
return zero(this->dim());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Print */
|
/** Print */
|
||||||
|
|
@ -685,10 +723,14 @@ public:
|
||||||
/** Calls the 5-key specific version of evaluateError, which is pure virtual
|
/** Calls the 5-key specific version of evaluateError, which is pure virtual
|
||||||
* so must be implemented in the derived class. */
|
* so must be implemented in the derived class. */
|
||||||
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||||
|
if(this->active(x)) {
|
||||||
if(H)
|
if(H)
|
||||||
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
|
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
|
||||||
else
|
else
|
||||||
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]);
|
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]);
|
||||||
|
} else {
|
||||||
|
return zero(this->dim());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Print */
|
/** Print */
|
||||||
|
|
@ -792,10 +834,14 @@ public:
|
||||||
/** Calls the 6-key specific version of evaluateError, which is pure virtual
|
/** Calls the 6-key specific version of evaluateError, which is pure virtual
|
||||||
* so must be implemented in the derived class. */
|
* so must be implemented in the derived class. */
|
||||||
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||||
|
if(this->active(x)) {
|
||||||
if(H)
|
if(H)
|
||||||
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
|
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
|
||||||
else
|
else
|
||||||
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]);
|
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]);
|
||||||
|
} else {
|
||||||
|
return zero(this->dim());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Print */
|
/** Print */
|
||||||
|
|
|
||||||
|
|
@ -1,73 +0,0 @@
|
||||||
/* ----------------------------------------------------------------------------
|
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
||||||
* Atlanta, Georgia 30332-0415
|
|
||||||
* All Rights Reserved
|
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
||||||
|
|
||||||
* See LICENSE for the license information
|
|
||||||
|
|
||||||
* -------------------------------------------------------------------------- */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file BetweenConstraint.h
|
|
||||||
* @brief Implements a constraint to force a between
|
|
||||||
* @author Alex Cunningham
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <gtsam/nonlinear/NonlinearConstraint.h>
|
|
||||||
|
|
||||||
namespace gtsam {
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Binary between constraint - forces between to a given value
|
|
||||||
* This constraint requires the underlying type to a Lie type
|
|
||||||
*/
|
|
||||||
template<class VALUES, class KEY>
|
|
||||||
class BetweenConstraint : public NonlinearEqualityConstraint2<VALUES, KEY, KEY> {
|
|
||||||
public:
|
|
||||||
typedef typename KEY::Value X;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
typedef NonlinearEqualityConstraint2<VALUES, KEY, KEY> Base;
|
|
||||||
|
|
||||||
X measured_; /// fixed between value
|
|
||||||
|
|
||||||
/** concept check by type */
|
|
||||||
GTSAM_CONCEPT_TESTABLE_TYPE(X)
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
typedef boost::shared_ptr<BetweenConstraint<VALUES, KEY> > shared_ptr;
|
|
||||||
|
|
||||||
BetweenConstraint(const X& measured, const KEY& key1, const KEY& key2, double mu = 1000.0)
|
|
||||||
: Base(key1, key2, X::Dim(), mu), measured_(measured) {}
|
|
||||||
|
|
||||||
/** g(x) with optional derivative2 */
|
|
||||||
Vector evaluateError(const X& x1, const X& x2,
|
|
||||||
boost::optional<Matrix&> H1 = boost::none,
|
|
||||||
boost::optional<Matrix&> H2 = boost::none) const {
|
|
||||||
X hx = x1.between(x2, H1, H2);
|
|
||||||
return measured_.localCoordinates(hx);
|
|
||||||
}
|
|
||||||
|
|
||||||
inline const X measured() const {
|
|
||||||
return measured_;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
||||||
ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2",
|
|
||||||
boost::serialization::base_object<Base>(*this));
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(measured_);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
} // \namespace gtsam
|
|
||||||
|
|
@ -40,6 +40,7 @@ namespace gtsam {
|
||||||
T measured_; /** The measurement */
|
T measured_; /** The measurement */
|
||||||
|
|
||||||
/** concept check by type */
|
/** concept check by type */
|
||||||
|
GTSAM_CONCEPT_LIE_TYPE(T)
|
||||||
GTSAM_CONCEPT_TESTABLE_TYPE(T)
|
GTSAM_CONCEPT_TESTABLE_TYPE(T)
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
@ -106,6 +107,33 @@ namespace gtsam {
|
||||||
boost::serialization::base_object<Base>(*this));
|
boost::serialization::base_object<Base>(*this));
|
||||||
ar & BOOST_SERIALIZATION_NVP(measured_);
|
ar & BOOST_SERIALIZATION_NVP(measured_);
|
||||||
}
|
}
|
||||||
};
|
}; // \class BetweenFactor
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Binary between constraint - forces between to a given value
|
||||||
|
* This constraint requires the underlying type to a Lie type
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
template<class VALUES, class KEY>
|
||||||
|
class BetweenConstraint : public BetweenFactor<VALUES, KEY> {
|
||||||
|
public:
|
||||||
|
typedef boost::shared_ptr<BetweenConstraint<VALUES, KEY> > shared_ptr;
|
||||||
|
|
||||||
|
/** Syntactic sugar for constrained version */
|
||||||
|
BetweenConstraint(const typename KEY::Value& measured,
|
||||||
|
const KEY& key1, const KEY& key2, double mu = 1000.0)
|
||||||
|
: BetweenFactor<VALUES, KEY>(key1, key2, measured,
|
||||||
|
noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
/** Serialization function */
|
||||||
|
friend class boost::serialization::access;
|
||||||
|
template<class ARCHIVE>
|
||||||
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
|
ar & boost::serialization::make_nvp("BetweenFactor",
|
||||||
|
boost::serialization::base_object<BetweenFactor<VALUES, KEY> >(*this));
|
||||||
|
}
|
||||||
|
}; // \class BetweenConstraint
|
||||||
|
|
||||||
} /// namespace gtsam
|
} /// namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -29,9 +29,9 @@ namespace gtsam {
|
||||||
* a scalar for comparison.
|
* a scalar for comparison.
|
||||||
*/
|
*/
|
||||||
template<class VALUES, class KEY>
|
template<class VALUES, class KEY>
|
||||||
struct BoundingConstraint1: public NonlinearConstraint1<VALUES, KEY> {
|
struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> {
|
||||||
typedef typename KEY::Value X;
|
typedef typename KEY::Value X;
|
||||||
typedef NonlinearConstraint1<VALUES, KEY> Base;
|
typedef NonlinearFactor1<VALUES, KEY> Base;
|
||||||
typedef boost::shared_ptr<BoundingConstraint1<VALUES, KEY> > shared_ptr;
|
typedef boost::shared_ptr<BoundingConstraint1<VALUES, KEY> > shared_ptr;
|
||||||
|
|
||||||
double threshold_;
|
double threshold_;
|
||||||
|
|
@ -39,7 +39,8 @@ struct BoundingConstraint1: public NonlinearConstraint1<VALUES, KEY> {
|
||||||
|
|
||||||
BoundingConstraint1(const KEY& key, double threshold,
|
BoundingConstraint1(const KEY& key, double threshold,
|
||||||
bool isGreaterThan, double mu = 1000.0) :
|
bool isGreaterThan, double mu = 1000.0) :
|
||||||
Base(key, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {
|
Base(noiseModel::Constrained::All(1, mu), key),
|
||||||
|
threshold_(threshold), isGreaterThan_(isGreaterThan) {
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual ~BoundingConstraint1() {}
|
virtual ~BoundingConstraint1() {}
|
||||||
|
|
@ -83,7 +84,7 @@ private:
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
ar & boost::serialization::make_nvp("NonlinearConstraint1",
|
ar & boost::serialization::make_nvp("NonlinearFactor1",
|
||||||
boost::serialization::base_object<Base>(*this));
|
boost::serialization::base_object<Base>(*this));
|
||||||
ar & BOOST_SERIALIZATION_NVP(threshold_);
|
ar & BOOST_SERIALIZATION_NVP(threshold_);
|
||||||
ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
|
ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
|
||||||
|
|
@ -95,11 +96,11 @@ private:
|
||||||
* to implement for specific systems
|
* to implement for specific systems
|
||||||
*/
|
*/
|
||||||
template<class VALUES, class KEY1, class KEY2>
|
template<class VALUES, class KEY1, class KEY2>
|
||||||
struct BoundingConstraint2: public NonlinearConstraint2<VALUES, KEY1, KEY2> {
|
struct BoundingConstraint2: public NonlinearFactor2<VALUES, KEY1, KEY2> {
|
||||||
typedef typename KEY1::Value X1;
|
typedef typename KEY1::Value X1;
|
||||||
typedef typename KEY2::Value X2;
|
typedef typename KEY2::Value X2;
|
||||||
|
|
||||||
typedef NonlinearConstraint2<VALUES, KEY1, KEY2> Base;
|
typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;
|
||||||
typedef boost::shared_ptr<BoundingConstraint2<VALUES, KEY1, KEY2> > shared_ptr;
|
typedef boost::shared_ptr<BoundingConstraint2<VALUES, KEY1, KEY2> > shared_ptr;
|
||||||
|
|
||||||
double threshold_;
|
double threshold_;
|
||||||
|
|
@ -107,7 +108,8 @@ struct BoundingConstraint2: public NonlinearConstraint2<VALUES, KEY1, KEY2> {
|
||||||
|
|
||||||
BoundingConstraint2(const KEY1& key1, const KEY2& key2, double threshold,
|
BoundingConstraint2(const KEY1& key1, const KEY2& key2, double threshold,
|
||||||
bool isGreaterThan, double mu = 1000.0)
|
bool isGreaterThan, double mu = 1000.0)
|
||||||
: Base(key1, key2, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {}
|
: Base(noiseModel::Constrained::All(1, mu), key1, key2),
|
||||||
|
threshold_(threshold), isGreaterThan_(isGreaterThan) {}
|
||||||
|
|
||||||
virtual ~BoundingConstraint2() {}
|
virtual ~BoundingConstraint2() {}
|
||||||
|
|
||||||
|
|
@ -155,7 +157,7 @@ private:
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
ar & boost::serialization::make_nvp("NonlinearConstraint2",
|
ar & boost::serialization::make_nvp("NonlinearFactor2",
|
||||||
boost::serialization::base_object<Base>(*this));
|
boost::serialization::base_object<Base>(*this));
|
||||||
ar & BOOST_SERIALIZATION_NVP(threshold_);
|
ar & BOOST_SERIALIZATION_NVP(threshold_);
|
||||||
ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
|
ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
|
||||||
|
|
|
||||||
|
|
@ -40,7 +40,7 @@ headers += BetweenFactor.h PriorFactor.h PartialPriorFactor.h
|
||||||
headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h
|
headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h
|
||||||
|
|
||||||
# Generic constraint headers
|
# Generic constraint headers
|
||||||
headers += BetweenConstraint.h BoundingConstraint.h
|
headers += BoundingConstraint.h
|
||||||
|
|
||||||
# 2D Pose SLAM
|
# 2D Pose SLAM
|
||||||
sources += pose2SLAM.cpp dataset.cpp
|
sources += pose2SLAM.cpp dataset.cpp
|
||||||
|
|
|
||||||
|
|
@ -22,7 +22,7 @@
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
|
|
||||||
#include <gtsam/nonlinear/NonlinearConstraint.h>
|
#include <gtsam/nonlinear/NonlinearConstraint.h>
|
||||||
#include <gtsam/slam/BetweenConstraint.h>
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
#include <gtsam/slam/BoundingConstraint.h>
|
#include <gtsam/slam/BoundingConstraint.h>
|
||||||
#include <gtsam/slam/simulated2D.h>
|
#include <gtsam/slam/simulated2D.h>
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -96,8 +96,8 @@ TEST( testBoundingConstraint, unary_basics_active1 ) {
|
||||||
EXPECT(assert_equal(repeat(1,-5.0), constraint2.evaluateError(pt2), tol));
|
EXPECT(assert_equal(repeat(1,-5.0), constraint2.evaluateError(pt2), tol));
|
||||||
EXPECT(assert_equal(repeat(1,-3.0), constraint1.unwhitenedError(config), tol));
|
EXPECT(assert_equal(repeat(1,-3.0), constraint1.unwhitenedError(config), tol));
|
||||||
EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol));
|
EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol));
|
||||||
EXPECT_DOUBLES_EQUAL(90.0, constraint1.error(config), tol);
|
EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol);
|
||||||
EXPECT_DOUBLES_EQUAL(250.0, constraint2.error(config), tol);
|
EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -111,8 +111,8 @@ TEST( testBoundingConstraint, unary_basics_active2 ) {
|
||||||
EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol));
|
EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol));
|
||||||
EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol));
|
EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol));
|
||||||
EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol));
|
EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol));
|
||||||
EXPECT_DOUBLES_EQUAL(10.0, constraint3.error(config), tol);
|
EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol);
|
||||||
EXPECT_DOUBLES_EQUAL(10.0, constraint4.error(config), tol);
|
EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -224,7 +224,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
|
||||||
config1.update(key2, pt4);
|
config1.update(key2, pt4);
|
||||||
EXPECT(rangeBound.active(config1));
|
EXPECT(rangeBound.active(config1));
|
||||||
EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1)));
|
EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1)));
|
||||||
EXPECT_DOUBLES_EQUAL(1.0*mu, rangeBound.error(config1), tol);
|
EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -56,7 +56,7 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) {
|
||||||
EXPECT(constraint.active(config2));
|
EXPECT(constraint.active(config2));
|
||||||
EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.evaluateError(ptBad1), tol));
|
EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.evaluateError(ptBad1), tol));
|
||||||
EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.unwhitenedError(config2), tol));
|
EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.unwhitenedError(config2), tol));
|
||||||
EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol);
|
EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -88,7 +88,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
|
||||||
// ensure that the hard constraint overrides the soft constraint
|
// ensure that the hard constraint overrides the soft constraint
|
||||||
Point2 truth_pt(1.0, 2.0);
|
Point2 truth_pt(1.0, 2.0);
|
||||||
simulated2D::PoseKey key(1);
|
simulated2D::PoseKey key(1);
|
||||||
double mu = 1000.0;
|
double mu = 10.0;
|
||||||
eq2D::UnaryEqualityConstraint::shared_ptr constraint(
|
eq2D::UnaryEqualityConstraint::shared_ptr constraint(
|
||||||
new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));
|
new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));
|
||||||
|
|
||||||
|
|
@ -103,10 +103,16 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
|
||||||
shared_values initValues(new simulated2D::Values());
|
shared_values initValues(new simulated2D::Values());
|
||||||
initValues->insert(key, badPt);
|
initValues->insert(key, badPt);
|
||||||
|
|
||||||
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
|
// verify error values
|
||||||
|
EXPECT(constraint->active(*initValues));
|
||||||
|
|
||||||
simulated2D::Values expected;
|
simulated2D::Values expected;
|
||||||
expected.insert(key, truth_pt);
|
expected.insert(key, truth_pt);
|
||||||
CHECK(assert_equal(expected, *actual, tol));
|
EXPECT(constraint->active(expected));
|
||||||
|
EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);
|
||||||
|
|
||||||
|
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
|
||||||
|
EXPECT(assert_equal(expected, *actual, tol));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -132,7 +138,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
|
||||||
EXPECT(constraint.active(config2));
|
EXPECT(constraint.active(config2));
|
||||||
EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol));
|
EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol));
|
||||||
EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.unwhitenedError(config2), tol));
|
EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.unwhitenedError(config2), tol));
|
||||||
EXPECT_DOUBLES_EQUAL(2000.0, constraint.error(config2), tol);
|
EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -209,7 +209,7 @@ TEST( NonlinearFactor, linearize_constraint1 )
|
||||||
// create expected
|
// create expected
|
||||||
Ordering ord(*config.orderingArbitrary());
|
Ordering ord(*config.orderingArbitrary());
|
||||||
Vector b = Vector_(2, 0., -3.);
|
Vector b = Vector_(2, 0., -3.);
|
||||||
JacobianFactor expected(ord["x1"], eye(2), b, constraint);
|
JacobianFactor expected(ord["x1"], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint);
|
||||||
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
|
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -229,8 +229,9 @@ TEST( NonlinearFactor, linearize_constraint2 )
|
||||||
|
|
||||||
// create expected
|
// create expected
|
||||||
Ordering ord(*config.orderingArbitrary());
|
Ordering ord(*config.orderingArbitrary());
|
||||||
Vector b = Vector_(2, -3., -3.);
|
Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0);
|
||||||
JacobianFactor expected(ord["x1"], -1*eye(2), ord["l1"], eye(2), b, constraint);
|
Vector b = Vector_(2, -15., -3.);
|
||||||
|
JacobianFactor expected(ord["x1"], -1*A, ord["l1"], A, b, constraint);
|
||||||
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
|
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue