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

Rearranged the inverse depth factor/camera, created a gtsam_unstable/geometry folder - run cmake to find the new/moved files

release/4.3a0
Alex Cunningham 2012-08-02 20:47:16 +00:00
parent 4c836c6e3a
commit b211c1070d
8 changed files with 633 additions and 554 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>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -343,6 +335,7 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -350,6 +343,7 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -397,6 +391,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -404,6 +399,7 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -411,6 +407,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -426,11 +423,20 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -519,22 +525,6 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -551,6 +541,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -575,26 +581,26 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>testValues.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>testOrdering.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>testKey.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -679,26 +685,26 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testValues.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testOrdering.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testKey.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -775,6 +781,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -807,6 +821,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -953,6 +975,7 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -960,6 +983,7 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -967,6 +991,7 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1102,6 +1127,7 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1565,7 +1591,6 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1605,7 +1630,6 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1613,7 +1637,6 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1829,7 +1852,6 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1851,102 +1873,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j1</buildArguments>
@ -2148,6 +2074,7 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2155,6 +2082,7 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2162,6 +2090,7 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2169,6 +2098,7 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2310,34 +2240,98 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>testRot3.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">
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testWrap.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>testRot2.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">
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap</buildTarget>
<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>
@ -2381,6 +2375,38 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
</storageModule>
</cproject>

2
gtsam_unstable/CMakeLists.txt
View File

@ -2,6 +2,7 @@
# and also build tests
set (gtsam_unstable_subdirs
base
geometry
discrete
# linear
dynamics
@ -31,6 +32,7 @@ endforeach(subdir)
# assemble gtsam_unstable components
set(gtsam_unstable_srcs
${base_srcs}
${geometry_srcs}
${discrete_srcs}
${dynamics_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);}
/* ************************************************************************* */

179
gtsam_unstable/slam/InvDepthCamera3.h
View File

@ -1,179 +0,0 @@
/**
* @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_);
}
/// @}
};
}

167
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -9,103 +9,106 @@
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam_unstable/slam/InvDepthCamera3.h>
#include <gtsam_unstable/geometry/InvDepthCamera3.h>
namespace gtsam {
template<class POSE, class LANDMARK, class INVDEPTH>
class InvDepthFactor3: public gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
protected:
/**
* 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
// 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:
public:
/// shorthand for base class type
typedef gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
/// shorthand for base class type
typedef gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
/// shorthand for this class
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
/// 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;
/// 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)) {}
/// 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) {}
/**
* 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() {}
/** 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");
/**
* 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);
}
/// 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);
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
return measured_;
}
/// 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 the calibration object */
inline const gtsam::Cal3_S2::shared_ptr calibration() const {
return K_;
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
return measured_;
}
private:
/** 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_);
}
};
/// 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

147
gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
View File

@ -5,156 +5,25 @@
* Author: cbeall3
*/
#include <gtsam_unstable/slam/InvDepthFactor3.h>
#include <gtsam/geometry/SimpleCamera.h>
#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);
/* ************************************************************************* */
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);
CHECK(assert_equal(expected_uv, actual_uv));
CHECK(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);
CHECK(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);
CHECK(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);
CHECK(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);
CHECK(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);
CHECK(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);
CHECK(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);
CHECK(assert_equal(expected,actual_vec,1e-7));
CHECK(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);
CHECK(assert_equal(expected,actual_vec,1e-7));
CHECK(assert_equal(inv_depth,actual_inv,1e-7));
}
static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
typedef InvDepthFactor3<Pose3, LieVector, LieScalar> InverseDepthFactor;
typedef NonlinearEquality<Pose3> PoseConstraint;
@ -183,7 +52,7 @@ TEST( InvDepthFactor, optimize) {
LevenbergMarquardtParams lmParams;
Values result = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
CHECK(assert_equal(initial, result, 1e-9));
EXPECT(assert_equal(initial, result, 1e-9));
/// Add a second camera
@ -214,7 +83,7 @@ TEST( InvDepthFactor, optimize) {
result2.at<LieVector>(Symbol('l',1)),
result2.at<LieScalar>(Symbol('d',1)));
CHECK(assert_equal(landmark1, l1_result2, 1e-9));
EXPECT(assert_equal(landmark1, l1_result2, 1e-9));
}