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

Merge remote-tracking branch 'svn/trunk' into remove_slam_namespaces 

Conflicts:
	matlab/examples/Pose2SLAMExample_graph.m
release/4.3a0
Richard Roberts 2012-08-03 21:02:27 +00:00
parent da1a732eff f39bad0d13
commit 711b28ae01
19 changed files with 81046 additions and 190 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

334
.cproject
View File

@ -309,14 +309,6 @@
<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>
@ -343,6 +335,7 @@
</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>
@ -350,6 +343,7 @@
</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>
@ -397,6 +391,7 @@
</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>
@ -404,6 +399,7 @@
</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>
@ -411,6 +407,7 @@
</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>
@ -426,11 +423,20 @@
</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="testPoseRTV.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testPoseRTV.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -519,22 +525,6 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="all" path="CppUnitLite" 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="clean" path="CppUnitLite" 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/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -551,6 +541,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="all" path="CppUnitLite" 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="clean" path="CppUnitLite" 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="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>
@ -575,26 +581,26 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testValues.run" 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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>testValues.run</buildTarget> <buildTarget>all</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>testOrdering.run</buildTarget> <buildTarget>check</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>testKey.run</buildTarget> <buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -679,26 +685,26 @@
<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="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>all</buildTarget> <buildTarget>testValues.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<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"> <target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>check</buildTarget> <buildTarget>testOrdering.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<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"> <target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>clean</buildTarget> <buildTarget>testKey.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -775,6 +781,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testInvDepthFactor3.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthFactor3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -807,6 +821,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthCamera3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -953,6 +975,7 @@
</target> </target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="build/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>
@ -960,6 +983,7 @@
</target> </target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testJunctionTree.run" path="build/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>
@ -967,6 +991,7 @@
</target> </target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget> <buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1102,6 +1127,7 @@
</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>
@ -1565,7 +1591,6 @@
</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>
@ -1605,7 +1630,6 @@
</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>
@ -1613,7 +1637,6 @@
</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>
@ -1829,7 +1852,6 @@
</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>
@ -1851,102 +1873,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testRot2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>timeRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCal3_S2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testHomography2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j1</buildArguments> <buildArguments>-j1</buildArguments>
@ -2148,6 +2074,7 @@
</target> </target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget> <buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2155,6 +2082,7 @@
</target> </target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget> <buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2162,6 +2090,7 @@
</target> </target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget> <buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2169,6 +2098,7 @@
</target> </target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget> <buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2310,34 +2240,98 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSpirit.run" path="build/wrap" 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>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>testSpirit.run</buildTarget> <buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>testWrap.run</buildTarget> <buildTarget>testRot2.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>check.wrap</buildTarget> <buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>wrap</buildTarget> <buildTarget>timeRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCal3_S2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testHomography2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -2381,6 +2375,38 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testWrap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets> </buildTargets>
</storageModule> </storageModule>
</cproject> </cproject>

80186
examples/Data/w10000-odom.graph Normal file
View File

File diff suppressed because it is too large Load Diff

29
gtsam.h
View File

@ -187,12 +187,13 @@ virtual class Point2 : gtsam::Value {
// Lie Group // Lie Group
static gtsam::Point2 Expmap(Vector v); static gtsam::Point2 Expmap(Vector v);
static Vector Logmap(const gtsam::Point2& p); static Vector Logmap(const gtsam::Point2& p);
// Standard Interface // Standard Interface
double x() const; double x() const;
double y() const; double y() const;
Vector vector() const; Vector vector() const;
double dist(const gtsam::Point2& p2) const;
}; };
virtual class StereoPoint2 : gtsam::Value { virtual class StereoPoint2 : gtsam::Value {
@ -450,6 +451,7 @@ virtual class Cal3_S2 : gtsam::Value {
Cal3_S2(); Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0); Cal3_S2(double fx, double fy, double s, double u0, double v0);
Cal3_S2(Vector v); Cal3_S2(Vector v);
Cal3_S2(double fov, int w, int h);
// Testable // Testable
void print(string s) const; void print(string s) const;
@ -1299,6 +1301,26 @@ class ISAM2DoglegParams {
void setVerbose(bool verbose); void setVerbose(bool verbose);
}; };
class ISAM2ThresholdMapValue {
ISAM2ThresholdMapValue(char c, Vector thresholds);
ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other);
};
class ISAM2ThresholdMap {
ISAM2ThresholdMap();
ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other);
// Note: no print function
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
void insert(const gtsam::ISAM2ThresholdMapValue& value) const;
};
class ISAM2Params { class ISAM2Params {
ISAM2Params(); ISAM2Params();
@ -1308,8 +1330,7 @@ class ISAM2Params {
void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params); void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params);
void setOptimizationParams(const gtsam::ISAM2DoglegParams& params); void setOptimizationParams(const gtsam::ISAM2DoglegParams& params);
void setRelinearizeThreshold(double relinearizeThreshold); void setRelinearizeThreshold(double relinearizeThreshold);
// TODO: wrap this void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold);
//void setRelinearizeThreshold(const FastMap<char,Vector>& relinearizeThreshold);
int getRelinearizeSkip() const; int getRelinearizeSkip() const;
void setRelinearizeSkip(int relinearizeSkip); void setRelinearizeSkip(int relinearizeSkip);
bool isEnableRelinearization() const; bool isEnableRelinearization() const;

2
gtsam/inference/BayesTree.h
View File

@ -202,7 +202,7 @@ namespace gtsam {
} }
/** return nodes */ /** return nodes */
Nodes nodes() const { return nodes_; } const Nodes& nodes() const { return nodes_; }
/** return root clique */ /** return root clique */
const sharedClique& root() const { return root_; } const sharedClique& root() const { return root_; }

19
gtsam/inference/BayesTreeCliqueBase-inl.h
View File

@ -268,12 +268,19 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::deleteCachedShorcuts() { void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::deleteCachedShorcuts() {
//Delete CachedShortcut for this clique
this->resetCachedShortcut(); // When a shortcut is requested, all of the shortcuts between it and the
// Recursive call over all child cliques // root are also generated. So, if this clique's cached shortcut is set,
BOOST_FOREACH(derived_ptr& child, children_) { // recursively call over all child cliques. Otherwise, it is unnecessary.
child->deleteCachedShorcuts(); if(cachedShortcut_) {
} BOOST_FOREACH(derived_ptr& child, children_) {
child->deleteCachedShorcuts();
}
//Delete CachedShortcut for this clique
this->resetCachedShortcut();
}
} }
} }

2
gtsam/inference/VariableIndex.cpp
View File

@ -68,7 +68,7 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
/* ************************************************************************* */ /* ************************************************************************* */
void VariableIndex::permuteInPlace(const Permutation& permutation) { void VariableIndex::permuteInPlace(const Permutation& permutation) {
// Create new index and move references to data into it in permuted order // Create new index and move references to data into it in permuted order
vector<VariableIndex::Factors> newIndex(this->size()); deque<VariableIndex::Factors> newIndex(this->size());
for(Index i = 0; i < newIndex.size(); ++i) for(Index i = 0; i < newIndex.size(); ++i)
newIndex[i].swap(this->index_[permutation[i]]); newIndex[i].swap(this->index_[permutation[i]]);

5
gtsam/inference/VariableIndex.h
View File

@ -18,6 +18,7 @@
#pragma once #pragma once
#include <vector> #include <vector>
#include <deque>
#include <stdexcept> #include <stdexcept>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
@ -33,7 +34,7 @@ namespace gtsam {
* factor graph. The factor graph stores a collection of factors, each of * factor graph. The factor graph stores a collection of factors, each of
* which involves a set of variables. In contrast, the VariableIndex is built * which involves a set of variables. In contrast, the VariableIndex is built
* from a factor graph prior to elimination, and stores the list of factors * from a factor graph prior to elimination, and stores the list of factors
* that involve each variable. This information is stored as a vector of * that involve each variable. This information is stored as a deque of
* lists of factor indices. * lists of factor indices.
* \nosubgrouping * \nosubgrouping
*/ */
@ -46,7 +47,7 @@ public:
typedef Factors::const_iterator Factor_const_iterator; typedef Factors::const_iterator Factor_const_iterator;
protected: protected:
std::vector<Factors> index_; std::deque<Factors> index_;
size_t nFactors_; // Number of factors in the original factor graph. size_t nFactors_; // Number of factors in the original factor graph.
size_t nEntries_; // Sum of involved variable counts of each factor. size_t nEntries_; // Sum of involved variable counts of each factor.

2
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -177,7 +177,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Helper functions for Combine // Helper functions for Combine
static boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) { static boost::tuple<vector<size_t>, size_t, size_t> countDims(const FactorGraph<JacobianFactor>& factors, const VariableSlots& variableSlots) {
#ifndef NDEBUG #ifndef NDEBUG
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max()); vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
#else #else

13
gtsam/nonlinear/ISAM2.cpp
View File

@ -747,17 +747,20 @@ ISAM2Result ISAM2::update(
// After the top of the tree has been redone and may have index gaps from // After the top of the tree has been redone and may have index gaps from
// unused keys, condense the indices to remove gaps by rearranging indices // unused keys, condense the indices to remove gaps by rearranging indices
// in all data structures. // in all data structures.
Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, if(!unusedKeys.empty()) {
deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_); tic(10,"remove variables");
Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_);
toc(10,"remove variables");
}
result.cliques = this->nodes().size(); result.cliques = this->nodes().size();
deltaDoglegUptodate_ = false; deltaDoglegUptodate_ = false;
deltaUptodate_ = false; deltaUptodate_ = false;
tic(10,"evaluate error after"); tic(11,"evaluate error after");
if(params_.evaluateNonlinearError) if(params_.evaluateNonlinearError)
result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate()));
toc(10,"evaluate error after"); toc(11,"evaluate error after");
return result; return result;
} }

7
gtsam/nonlinear/ISAM2.h
View File

@ -98,6 +98,8 @@ struct ISAM2DoglegParams {
* @addtogroup ISAM2 * @addtogroup ISAM2
* Parameters for the ISAM2 algorithm. Default parameter values are listed below. * Parameters for the ISAM2 algorithm. Default parameter values are listed below.
*/ */
typedef FastMap<char,Vector> ISAM2ThresholdMap;
typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
struct ISAM2Params { struct ISAM2Params {
typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams> OptimizationParams; ///< Either ISAM2GaussNewtonParams or ISAM2DoglegParams typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams> OptimizationParams; ///< Either ISAM2GaussNewtonParams or ISAM2DoglegParams
typedef boost::variant<double, FastMap<char,Vector> > RelinearizationThreshold; ///< Either a constant relinearization threshold or a per-variable-type set of thresholds typedef boost::variant<double, FastMap<char,Vector> > RelinearizationThreshold; ///< Either a constant relinearization threshold or a per-variable-type set of thresholds
@ -189,7 +191,12 @@ struct ISAM2Params {
if(relinearizeThreshold.type() == typeid(double)) if(relinearizeThreshold.type() == typeid(double))
std::cout << "relinearizeThreshold: " << boost::get<double>(relinearizeThreshold) << "\n"; std::cout << "relinearizeThreshold: " << boost::get<double>(relinearizeThreshold) << "\n";
else else
{
std::cout << "relinearizeThreshold: " << "{mapped}" << "\n"; std::cout << "relinearizeThreshold: " << "{mapped}" << "\n";
BOOST_FOREACH(const ISAM2ThresholdMapValue& value, boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
std::cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n";
}
}
std::cout << "relinearizeSkip: " << relinearizeSkip << "\n"; std::cout << "relinearizeSkip: " << relinearizeSkip << "\n";
std::cout << "enableRelinearization: " << enableRelinearization << "\n"; std::cout << "enableRelinearization: " << enableRelinearization << "\n";
std::cout << "evaluateNonlinearError: " << evaluateNonlinearError << "\n"; std::cout << "evaluateNonlinearError: " << evaluateNonlinearError << "\n";

2
gtsam_unstable/CMakeLists.txt
View File

@ -2,6 +2,7 @@
# and also build tests # and also build tests
set (gtsam_unstable_subdirs set (gtsam_unstable_subdirs
base base
geometry
discrete discrete
# linear # linear
dynamics dynamics
@ -31,6 +32,7 @@ endforeach(subdir)
# assemble gtsam_unstable components # assemble gtsam_unstable components
set(gtsam_unstable_srcs set(gtsam_unstable_srcs
${base_srcs} ${base_srcs}
${geometry_srcs}
${discrete_srcs} ${discrete_srcs}
${dynamics_srcs} ${dynamics_srcs}
#${linear_srcs} #${linear_srcs}

22
gtsam_unstable/geometry/CMakeLists.txt Normal file
View File

@ -0,0 +1,22 @@
# Install headers
file(GLOB geometry_headers "*.h")
install(FILES ${geometry_headers} DESTINATION include/gtsam_unstable/geometry)
# Components to link tests in this subfolder against
set(geometry_local_libs
#geometry_unstable # No sources currently, so no convenience lib
geometry
base
ccolamd
)
set (geometry_full_libs
gtsam-static
gtsam_unstable-static)
# Exclude tests that don't work
set (geometry_excluded_tests "")
# Add all tests
gtsam_add_subdir_tests(geometry_unstable "${geometry_local_libs}" "${geometry_full_libs}" "${geometry_excluded_tests}")
add_dependencies(check.unstable check.geometry_unstable)

179
gtsam_unstable/geometry/InvDepthCamera3.h Normal file
View File

@ -0,0 +1,179 @@
/**
* @file InvDepthCamera3.h
* @brief
* @author Chris Beall
* @date Apr 14, 2012
*/
#pragma once
#include <cmath>
#include <boost/optional.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Vector.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/LieScalar.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/PinholeCamera.h>
namespace gtsam {
/**
* A pinhole camera class that has a Pose3 and a Calibration.
* @ingroup geometry
* \nosubgrouping
*/
template <class CALIBRATION>
class InvDepthCamera3 {
private:
Pose3 pose_;
boost::shared_ptr<CALIBRATION> k_;
public:
/// @name Standard Constructors
/// @{
/** default constructor */
InvDepthCamera3() {}
/** constructor with pose and calibration */
InvDepthCamera3(const Pose3& pose, const boost::shared_ptr<CALIBRATION>& k) :
pose_(pose),k_(k) {}
/// @}
/// @name Standard Interface
/// @{
virtual ~InvDepthCamera3() {}
/// return pose
inline Pose3& pose() { return pose_; }
/// return calibration
inline const boost::shared_ptr<CALIBRATION>& calibration() const { return k_; }
/// print
void print(const std::string& s = "") const {
pose_.print("pose3");
k_.print("calibration");
}
static gtsam::Point3 invDepthTo3D(const gtsam::LieVector& pw, const gtsam::LieScalar& inv) {
gtsam::Point3 ray_base(pw.vector().segment(0,3));
double theta = pw(3), phi = pw(4);
double rho = inv.value(); // inverse depth
gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
return ray_base + m/rho;
}
/** project a point from world InvDepth parameterization to the image
* @param pw is a point in the world coordinate
* @param H1 is the jacobian w.r.t. [pose3 calibration]
* @param H2 is the jacobian w.r.t. inv_depth_landmark
*/
inline gtsam::Point2 project(const gtsam::LieVector& pw,
const gtsam::LieScalar& inv_depth,
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none) const {
gtsam::Point3 ray_base(pw.vector().segment(0,3));
double theta = pw(3), phi = pw(4);
double rho = inv_depth.value(); // inverse depth
gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
const gtsam::Point3 landmark = ray_base + m/rho;
gtsam::PinholeCamera<CALIBRATION> camera(pose_, *k_);
if (!H1 && !H2 && !H3) {
gtsam::Point2 uv= camera.project(landmark);
return uv;
}
else {
gtsam::Matrix J2;
gtsam::Point2 uv= camera.project(landmark,H1, J2);
if (H1) {
*H1 = (*H1) * gtsam::eye(6);
}
double cos_theta = cos(theta);
double sin_theta = sin(theta);
double cos_phi = cos(phi);
double sin_phi = sin(phi);
double rho2 = rho * rho;
if (H2) {
double H11 = 1;
double H12 = 0;
double H13 = 0;
double H14 = -cos_phi*sin_theta/rho;
double H15 = -cos_theta*sin_phi/rho;
double H21 = 0;
double H22 = 1;
double H23 = 0;
double H24 = cos_phi*cos_theta/rho;
double H25 = -sin_phi*sin_theta/rho;
double H31 = 0;
double H32 = 0;
double H33 = 1;
double H34 = 0;
double H35 = cos_phi/rho;
*H2 = J2 * gtsam::Matrix_(3,5,
H11, H12, H13, H14, H15,
H21, H22, H23, H24, H25,
H31, H32, H33, H34, H35);
}
if(H3) {
double H16 = -cos_phi*cos_theta/rho2;
double H26 = -cos_phi*sin_theta/rho2;
double H36 = -sin_phi/rho2;
*H3 = J2 * gtsam::Matrix_(3,1,
H16,
H26,
H36);
}
return uv;
}
}
/**
* backproject a 2-dimensional point to an Inverse Depth landmark
* useful for point initialization
*/
inline std::pair<gtsam::LieVector, gtsam::LieScalar> backproject(const gtsam::Point2& pi, const double depth) const {
const gtsam::Point2 pn = k_->calibrate(pi);
gtsam::Point3 pc(pn.x(), pn.y(), 1.0);
pc = pc/pc.norm();
gtsam::Point3 pw = pose_.transform_from(pc);
const gtsam::Point3& pt = pose_.translation();
gtsam::Point3 ray = pw - pt;
double theta = atan2(ray.y(), ray.x()); // longitude
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
return std::make_pair(gtsam::LieVector(5, pt.x(),pt.y(),pt.z(), theta, phi),
gtsam::LieScalar(1./depth));
}
private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(pose_);
ar & BOOST_SERIALIZATION_NVP(k_);
}
/// @}
}; // \class InvDepthCamera
} // \namesapce gtsam

157
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp Normal file
View File

@ -0,0 +1,157 @@
/*
* testInvDepthFactor.cpp
*
* Created on: Apr 13, 2012
* Author: cbeall3
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam_unstable/geometry/InvDepthCamera3.h>
using namespace std;
using namespace gtsam;
static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K);
/* ************************************************************************* */
TEST( InvDepthFactor, Project1) {
// landmark 5 meters infront of camera
Point3 landmark(5, 0, 1);
Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector inv_landmark(5, 1., 0., 1., 0., 0.);
LieScalar inv_depth(1./4);
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
EXPECT(assert_equal(expected_uv, actual_uv));
EXPECT(assert_equal(Point2(640,480), actual_uv));
}
/* ************************************************************************* */
TEST( InvDepthFactor, Project2) {
// landmark 1m to the left and 1m up from camera
// inv landmark xyz is same as camera xyz, so depth actually doesn't matter
Point3 landmark(1, 1, 2);
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1/sqrt(2)));
LieScalar inv_depth(1/sqrt(3));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( InvDepthFactor, Project3) {
// landmark 1m to the left and 1m up from camera
// inv depth landmark xyz at origion
Point3 landmark(1, 1, 2);
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2)));
LieScalar inv_depth( 1./sqrt(1+1+4));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( InvDepthFactor, Project4) {
// landmark 4m to the left and 1m up from camera
// inv depth landmark xyz at origion
Point3 landmark(1, 4, 2);
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark(5, 0., 0., 0., atan(4/1), atan(2./sqrt(1+16)));
LieScalar inv_depth(1./sqrt(1+16+4));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& inv_depth) {
return InvDepthCamera3<Cal3_S2>(pose,K).project(landmark, inv_depth); }
TEST( InvDepthFactor, Dproject_pose)
{
LieVector landmark(6,0.1,0.2,0.3, 0.1,0.2);
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative31<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none);
EXPECT(assert_equal(expected,actual,1e-6));
}
/* ************************************************************************* */
TEST( InvDepthFactor, Dproject_landmark)
{
LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2);
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative32<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none);
EXPECT(assert_equal(expected,actual,1e-7));
}
/* ************************************************************************* */
TEST( InvDepthFactor, Dproject_inv_depth)
{
LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2);
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative33<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual);
EXPECT(assert_equal(expected,actual,1e-7));
}
/* ************************************************************************* */
TEST(InvDepthFactor, backproject)
{
LieVector expected(5,0.,0.,1., 0.1,0.2);
LieScalar inv_depth(1./4);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Point2 z = inv_camera.project(expected, inv_depth);
LieVector actual_vec;
LieScalar actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4);
EXPECT(assert_equal(expected,actual_vec,1e-7));
EXPECT(assert_equal(inv_depth,actual_inv,1e-7));
}
/* ************************************************************************* */
TEST(InvDepthFactor, backproject2)
{
// backwards facing camera
LieVector expected(5,-5.,-5.,2., 3., -0.1);
LieScalar inv_depth(1./10);
InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
Point2 z = inv_camera.project(expected, inv_depth);
LieVector actual_vec;
LieScalar actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10);
EXPECT(assert_equal(expected,actual_vec,1e-7));
EXPECT(assert_equal(inv_depth,actual_inv,1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

19
gtsam_unstable/gtsam_unstable.h
View File

@ -6,6 +6,7 @@
virtual class gtsam::Value; virtual class gtsam::Value;
virtual class gtsam::Point3; virtual class gtsam::Point3;
virtual class gtsam::Rot3; virtual class gtsam::Rot3;
virtual class gtsam::Pose2;
virtual class gtsam::Pose3; virtual class gtsam::Pose3;
virtual class gtsam::noiseModel::Base; virtual class gtsam::noiseModel::Base;
virtual class gtsam::NonlinearFactor; virtual class gtsam::NonlinearFactor;
@ -165,5 +166,21 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model);
}; };
//*************************************************************************
// General nonlinear factor types
//*************************************************************************
#include <gtsam/geometry/Pose2.h>
}///\namespace gtsam #include <gtsam_unstable/slam/PoseTranslationPrior.h>
template<POSE = {gtsam::Pose2,gtsam::Pose3}>
virtual class PoseTranslationPrior : gtsam::NonlinearFactor {
PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam_unstable/slam/PoseRotationPrior.h>
template<POSE = {gtsam::Pose2,gtsam::Pose3}>
virtual class PoseRotationPrior : gtsam::NonlinearFactor {
PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
};
} //\namespace gtsam

114
gtsam_unstable/slam/InvDepthFactor3.h Normal file
View File

@ -0,0 +1,114 @@
/**
* @file InvDepthFactor3.h
* @brief Inverse Depth Factor
* @author Chris Beall
*/
#pragma once
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/geometry/InvDepthCamera3.h>
namespace gtsam {
/**
* Ternary factor representing a visual measurement that includes inverse depth
*/
template<class POSE, class LANDMARK, class INVDEPTH>
class InvDepthFactor3: public gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
protected:
// Keep a copy of measurement and calibration for I/O
gtsam::Point2 measured_; ///< 2D measurement
boost::shared_ptr<gtsam::Cal3_S2> K_; ///< shared pointer to calibration object
public:
/// shorthand for base class type
typedef gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
/// shorthand for this class
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactor3() : K_(new gtsam::Cal3_S2(444, 555, 666, 777, 888)) {}
/**
* Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param z is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param j_pose is basically the frame number
* @param j_landmark is the index of the landmark
* @param K shared pointer to the constant calibration
*/
InvDepthFactor3(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model,
const gtsam::Key poseKey, gtsam::Key pointKey, gtsam::Key invDepthKey, const gtsam::shared_ptrK& K) :
Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {}
/** Virtual destructor */
virtual ~InvDepthFactor3() {}
/**
* print
* @param s optional string naming the factor
*/
void print(const std::string& s = "InvDepthFactor3",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
gtsam::Vector evaluateError(const POSE& pose, const gtsam::LieVector& point, const INVDEPTH& invDepth,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none,
boost::optional<gtsam::Matrix&> H3=boost::none) const {
try {
InvDepthCamera3<gtsam::Cal3_S2> camera(pose, K_);
gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_);
return reprojectionError.vector();
} catch( CheiralityException& e) {
if (H1) *H1 = gtsam::zeros(2,6);
if (H2) *H2 = gtsam::zeros(2,5);
if (H3) *H2 = gtsam::zeros(2,1);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
}
return gtsam::Vector_(1, 0.0);
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
return measured_;
}
/** return the calibration object */
inline const gtsam::Cal3_S2::shared_ptr calibration() const {
return K_;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
} // \ namespace gtsam

23
gtsam_unstable/slam/PoseRotationPrior.h
View File

@ -29,9 +29,23 @@ public:
/** standard constructor */ /** standard constructor */
PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model) PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model)
: Base(key, model) : Base(key, model) {
{ initialize(rot_z);
assert(rot_z.dim() == model->dim()); }
/** Constructor that pulls the translation from an incoming POSE */
PoseRotationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model)
: Base(key, model) {
initialize(pose_z.rotation());
}
/** get the rotation used to create the measurement */
Rotation priorRotation() const { return Rotation::Expmap(this->prior_); }
protected:
/** loads the underlying partial prior factor */
void initialize(const Rotation& rot_z) {
assert(rot_z.dim() == this->noiseModel_->dim());
// Calculate the prior applied // Calculate the prior applied
this->prior_ = Rotation::Logmap(rot_z); this->prior_ = Rotation::Logmap(rot_z);
@ -52,9 +66,6 @@ public:
this->fillH(); this->fillH();
} }
/** get the rotation used to create the measurement */
Rotation priorRotation() const { return Rotation::Expmap(this->prior_); }
}; };
} // \namespace gtsam } // \namespace gtsam

29
gtsam_unstable/slam/PoseTranslationPrior.h
View File

@ -28,17 +28,31 @@ public:
GTSAM_CONCEPT_LIE_TYPE(Translation) GTSAM_CONCEPT_LIE_TYPE(Translation)
/** standard constructor */ /** standard constructor */
PoseTranslationPrior(Key key, const Translation& rot_z, const SharedNoiseModel& model) PoseTranslationPrior(Key key, const Translation& trans_z, const SharedNoiseModel& model)
: Base(key, model) : Base(key, model) {
{ initialize(trans_z);
assert(rot_z.dim() == model->dim()); }
/** Constructor that pulls the translation from an incoming POSE */
PoseTranslationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model)
: Base(key, model) {
initialize(pose_z.translation());
}
/** get the rotation used to create the measurement */
Translation priorTranslation() const { return Translation::Expmap(this->prior_); }
protected:
/** loads the underlying partial prior factor */
void initialize(const Translation& trans_z) {
assert(trans_z.dim() == this->noiseModel_->dim());
// Calculate the prior applied // Calculate the prior applied
this->prior_ = Translation::Logmap(rot_z); this->prior_ = Translation::Logmap(trans_z);
// Create the mask for partial prior // Create the mask for partial prior
size_t pose_dim = Pose::identity().dim(); size_t pose_dim = Pose::identity().dim();
size_t rot_dim = rot_z.dim(); size_t rot_dim = trans_z.dim();
// get the interval of the lie coordinates corresponding to rotation // get the interval of the lie coordinates corresponding to rotation
std::pair<size_t, size_t> interval = Pose::translationInterval(); std::pair<size_t, size_t> interval = Pose::translationInterval();
@ -52,9 +66,6 @@ public:
this->fillH(); this->fillH();
} }
/** get the rotation used to create the measurement */
Translation priorTranslation() const { return Translation::Expmap(this->prior_); }
}; };
} // \namespace gtsam } // \namespace gtsam

92
gtsam_unstable/slam/tests/testInvDepthFactor3.cpp Normal file
View File

@ -0,0 +1,92 @@
/*
* testInvDepthFactor.cpp
*
* Created on: Apr 13, 2012
* Author: cbeall3
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam_unstable/slam/InvDepthFactor3.h>
using namespace std;
using namespace gtsam;
static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K);
typedef InvDepthFactor3<Pose3, LieVector, LieScalar> InverseDepthFactor;
typedef NonlinearEquality<Pose3> PoseConstraint;
/* ************************************************************************* */
TEST( InvDepthFactor, optimize) {
// landmark 5 meters infront of camera
Point3 landmark(5, 0, 1);
Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector inv_landmark(5, 1., 0., 1., 0., 0.);
LieScalar inv_depth(1./4);
gtsam::NonlinearFactorGraph graph;
Values initial;
InverseDepthFactor::shared_ptr factor(new InverseDepthFactor(expected_uv, sigma,
Symbol('x',1), Symbol('l',1), Symbol('d',1), K));
graph.push_back(factor);
graph.add(PoseConstraint(Symbol('x',1),level_pose));
initial.insert(Symbol('x',1), level_pose);
initial.insert(Symbol('l',1), inv_landmark);
initial.insert(Symbol('d',1), inv_depth);
LevenbergMarquardtParams lmParams;
Values result = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
EXPECT(assert_equal(initial, result, 1e-9));
/// Add a second camera
// add a camera 1 meter to the right
Pose3 right_pose = level_pose * Pose3(Rot3(), Point3(2,0,0));
SimpleCamera right_camera(right_pose, *K);
InvDepthCamera3<Cal3_S2> right_inv_camera(right_pose, K);
Point3 landmark1(6,0,1);
Point2 right_uv = right_camera.project(landmark1);
InverseDepthFactor::shared_ptr factor1(new InverseDepthFactor(right_uv, sigma,
Symbol('x',2), Symbol('l',1),Symbol('d',1),K));
graph.push_back(factor1);
graph.add(PoseConstraint(Symbol('x',2),right_pose));
initial.insert(Symbol('x',2), right_pose);
// TODO: need to add priors to make this work with
// Values result2 = optimize<NonlinearFactorGraph>(graph, initial,
// NonlinearOptimizationParameters(),MULTIFRONTAL, GN);
Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
Point3 l1_result2 = InvDepthCamera3<Cal3_S2>::invDepthTo3D(
result2.at<LieVector>(Symbol('l',1)),
result2.at<LieScalar>(Symbol('d',1)));
EXPECT(assert_equal(landmark1, l1_result2, 1e-9));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */