Merge remote-tracking branch 'svn/trunk' into remove_slam_namespaces
Conflicts: gtsam/nonlinear/SuccessiveLinearizationOptimizer.h matlab/examples/Pose2SLAMExample_circle.m matlab/examples/Pose2SLAMExample_graph.m matlab/examples/StereoVOExample_large.m tests/testGradientDescentOptimizer.cpprelease/4.3a0
Richard Roberts
commit
5177f31a5d
326
.cproject
326
.cproject
|
|
@ -309,6 +309,14 @@
|
|||
<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>
|
||||
|
|
@ -335,7 +343,6 @@
|
|||
</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>
|
||||
|
|
@ -343,7 +350,6 @@
|
|||
</target>
|
||||
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBinaryBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -391,7 +397,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -399,7 +404,6 @@
|
|||
</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>
|
||||
|
|
@ -407,7 +411,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -423,20 +426,11 @@
|
|||
</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>
|
||||
|
|
@ -525,22 +519,6 @@
|
|||
<useDefaultCommand>false</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>
|
||||
<buildTarget>tests/testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -557,6 +535,22 @@
|
|||
<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>
|
||||
<buildTarget>tests/testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -581,26 +575,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>
|
||||
|
|
@ -685,26 +679,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>
|
||||
|
|
@ -959,7 +953,6 @@
|
|||
</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>
|
||||
|
|
@ -967,7 +960,6 @@
|
|||
</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>
|
||||
|
|
@ -975,7 +967,6 @@
|
|||
</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>
|
||||
|
|
@ -1111,7 +1102,6 @@
|
|||
</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>
|
||||
|
|
@ -1575,6 +1565,7 @@
|
|||
</target>
|
||||
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1614,6 +1605,7 @@
|
|||
</target>
|
||||
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1621,6 +1613,7 @@
|
|||
</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>
|
||||
|
|
@ -1836,6 +1829,7 @@
|
|||
</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>
|
||||
|
|
@ -1857,14 +1851,110 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>install</buildTarget>
|
||||
<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>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1875,10 +1965,10 @@
|
|||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildArguments>-j1</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
|
|
@ -2058,7 +2148,6 @@
|
|||
</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>
|
||||
|
|
@ -2066,7 +2155,6 @@
|
|||
</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>
|
||||
|
|
@ -2074,7 +2162,6 @@
|
|||
</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>
|
||||
|
|
@ -2082,7 +2169,6 @@
|
|||
</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>
|
||||
|
|
@ -2224,98 +2310,34 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSpirit.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot2.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testWrap.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check.wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="wrap" path="build/wrap" 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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -2359,38 +2381,6 @@
|
|||
<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>
|
||||
|
|
|
|||
|
|
@ -1,16 +0,0 @@
|
|||
eclipse.preferences.version=1
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/PATH/delimiter=\:
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/PATH/operation=append
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/PATH/value=/home/ydjian/matlab/R2012a/bin
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/append=true
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/appendContributed=true
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/PATH/delimiter=\:
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/PATH/operation=append
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/PATH/value=/home/ydjian/matlab/R2012a/bin
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/append=true
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/appendContributed=true
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/delimiter=\:
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/operation=append
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/value=/home/ydjian/matlab/R2012a/bin
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/append=true
|
||||
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/appendContributed=true
|
||||
|
|
@ -109,7 +109,7 @@ if(CYGWIN OR MSVC OR WIN32)
|
|||
set(Boost_USE_STATIC_LIBS 1)
|
||||
endif()
|
||||
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread date_time regex REQUIRED)
|
||||
set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY})
|
||||
set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY})
|
||||
|
||||
# General build settings
|
||||
include_directories(
|
||||
|
|
|
|||
|
|
@ -0,0 +1,47 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
% Atlanta, Georgia 30332-0415
|
||||
% All Rights Reserved
|
||||
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
%
|
||||
% See LICENSE for the license information
|
||||
%
|
||||
% @brief Read graph from file and perform GraphSLAM
|
||||
% @author Frank Dellaert
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%% Initialize graph, initial estimate, and odometry noise
|
||||
import gtsam.*
|
||||
model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
|
||||
maxID = 0;
|
||||
addNoise = false;
|
||||
smart = true;
|
||||
[graph,initial]=load2D('Data/w100-odom.graph',model,maxID,addNoise,smart);
|
||||
initial.print(sprintf('Initial estimate:\n'));
|
||||
|
||||
%% Add a Gaussian prior on pose x_1
|
||||
import gtsam.*
|
||||
priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
|
||||
priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]);
|
||||
graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph
|
||||
|
||||
%% Plot Initial Estimate
|
||||
figure(1);clf
|
||||
P=initial.poses;
|
||||
plot(P(:,1),P(:,2),'g-*'); axis equal
|
||||
|
||||
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
|
||||
result = graph.optimize(initial,1);
|
||||
P=result.poses;
|
||||
hold on; plot(P(:,1),P(:,2),'b-*')
|
||||
result.print(sprintf('\nFinal result:\n'));
|
||||
|
||||
%% Plot Covariance Ellipses
|
||||
marginals = graph.marginals(result);
|
||||
P={};
|
||||
for i=1:result.size()-1
|
||||
pose_i = result.pose(i);
|
||||
P{i}=marginals.marginalCovariance(i);
|
||||
plotPose2(pose_i,'b',P{i})
|
||||
end
|
||||
fprintf(1,'%.5f %.5f %.5f\n',P{99})
|
||||
4
gtsam.h
4
gtsam.h
|
|
@ -1472,6 +1472,10 @@ virtual class GenericStereoFactor : gtsam::NonlinearFactor {
|
|||
};
|
||||
typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
|
||||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
pair<pose2SLAM::Graph*, pose2SLAM::Values*> load2D(string filename,
|
||||
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
|
||||
|
||||
} //\namespace gtsam
|
||||
|
||||
//*************************************************************************
|
||||
|
|
|
|||
|
|
@ -85,7 +85,7 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
|
|||
if (GTSAM_BUILD_STATIC_LIBRARY)
|
||||
message(STATUS "Building GTSAM - static")
|
||||
add_library(gtsam-static STATIC ${gtsam_srcs})
|
||||
target_link_libraries(gtsam-static ${Boost_SERIALIZATION_LIBRARY})
|
||||
target_link_libraries(gtsam-static ${GTSAM_BOOST_LIBRARIES})
|
||||
set_target_properties(gtsam-static PROPERTIES
|
||||
OUTPUT_NAME gtsam
|
||||
CLEAN_DIRECT_OUTPUT 1
|
||||
|
|
@ -100,7 +100,7 @@ endif (GTSAM_BUILD_STATIC_LIBRARY)
|
|||
if (GTSAM_BUILD_SHARED_LIBRARY)
|
||||
message(STATUS "Building GTSAM - shared")
|
||||
add_library(gtsam-shared SHARED ${gtsam_srcs})
|
||||
target_link_libraries(gtsam-shared ${Boost_SERIALIZATION_LIBRARY})
|
||||
target_link_libraries(gtsam-shared ${GTSAM_BOOST_LIBRARIES})
|
||||
set_target_properties(gtsam-shared PROPERTIES
|
||||
OUTPUT_NAME gtsam
|
||||
CLEAN_DIRECT_OUTPUT 1
|
||||
|
|
|
|||
|
|
@ -78,6 +78,36 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
|
|||
return Point2(fx_* x2 + s_ * y2 + u0_, fy_ * y2 + v0_) ;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
|
||||
// Use the following fixed point iteration to invert the radial distortion.
|
||||
// pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
|
||||
|
||||
const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
|
||||
(1 / fy_) * (pi.y() - v0_));
|
||||
|
||||
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
|
||||
Point2 pn = invKPi;
|
||||
|
||||
// iterate until the uncalibrate is close to the actual pixel coordinate
|
||||
const int maxIterations = 10;
|
||||
int iteration;
|
||||
for ( iteration = 0 ; iteration < maxIterations ; ++iteration ) {
|
||||
if ( uncalibrate(pn).dist(pi) <= tol ) break;
|
||||
const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y ;
|
||||
const double r = xx + yy ;
|
||||
const double g = (1+k1_*r+k2_*r*r) ;
|
||||
const double dx = 2*k3_*xy + k4_*(r+2*xx) ;
|
||||
const double dy = 2*k4_*xy + k3_*(r+2*yy) ;
|
||||
pn = (invKPi - Point2(dx,dy))/g ;
|
||||
}
|
||||
|
||||
if ( iteration >= maxIterations )
|
||||
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
|
||||
|
||||
return pn;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
|
||||
//const double fx = fx_, fy = fy_, s = s_ ;
|
||||
|
|
|
|||
|
|
@ -96,6 +96,9 @@ public:
|
|||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const ;
|
||||
|
||||
/// Conver a pixel coordinate to ideal coordinate
|
||||
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
|
||||
|
||||
/// Derivative of uncalibrate wrpt intrinsic coordinates
|
||||
Matrix D2d_intrinsic(const Point2& p) const ;
|
||||
|
||||
|
|
|
|||
|
|
@ -136,8 +136,8 @@ namespace gtsam {
|
|||
/// convert image coordinates uv to intrinsic coordinates xy
|
||||
Point2 calibrate(const Point2& p) const {
|
||||
const double u = p.x(), v = p.y();
|
||||
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), (1 / fy_)
|
||||
* (v - v0_));
|
||||
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)),
|
||||
(1 / fy_) * (v - v0_));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
|
|||
static Point2 p(2,3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3DS2, calibrate)
|
||||
TEST( Cal3DS2, uncalibrate)
|
||||
{
|
||||
Vector k = K.k() ;
|
||||
double r = p.x()*p.x() + p.y()*p.y() ;
|
||||
|
|
@ -43,6 +43,14 @@ TEST( Cal3DS2, calibrate)
|
|||
CHECK(assert_equal(q,p_i));
|
||||
}
|
||||
|
||||
TEST( Cal3DS2, calibrate )
|
||||
{
|
||||
Point2 pn(0.5, 0.5);
|
||||
Point2 pi = K.uncalibrate(pn);
|
||||
Point2 pn_hat = K.calibrate(pi);
|
||||
CHECK( pn.equals(pn_hat, 1e-5));
|
||||
}
|
||||
|
||||
Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -16,8 +16,6 @@
|
|||
* @date Feb 26, 2012
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/linear/GaussianJunctionTree.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -32,22 +30,8 @@ void GaussNewtonOptimizer::iterate() {
|
|||
// Linearize graph
|
||||
GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values, *params_.ordering);
|
||||
|
||||
// Optimize
|
||||
VectorValues delta;
|
||||
{
|
||||
if ( params_.isMultifrontal() ) {
|
||||
delta = GaussianJunctionTree(*linear).optimize(params_.getEliminationFunction());
|
||||
}
|
||||
else if ( params_.isSequential() ) {
|
||||
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(*linear)->eliminate(params_.getEliminationFunction()));
|
||||
}
|
||||
else if ( params_.isCG() ) {
|
||||
throw runtime_error("todo: ");
|
||||
}
|
||||
else {
|
||||
throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::elimination");
|
||||
}
|
||||
}
|
||||
// Solve Factor Graph
|
||||
const VectorValues delta = solveGaussianFactorGraph(*linear, params_);
|
||||
|
||||
// Maybe show output
|
||||
if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta");
|
||||
|
|
|
|||
|
|
@ -1,138 +0,0 @@
|
|||
/**
|
||||
* @file GradientDescentOptimizer.cpp
|
||||
* @brief
|
||||
* @author ydjian
|
||||
* @date Jun 11, 2012
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/GradientDescentOptimizer.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/linear/JacobianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering
|
||||
* Can be moved to NonlinearFactorGraph.h if desired
|
||||
*/
|
||||
void gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values, const Ordering &ordering, VectorValues &g) {
|
||||
|
||||
// Linearize graph
|
||||
GaussianFactorGraph::shared_ptr linear = nfg.linearize(values, ordering);
|
||||
FactorGraph<JacobianFactor> jfg; jfg.reserve(linear->size());
|
||||
BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, *linear) {
|
||||
if(boost::shared_ptr<JacobianFactor> jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
|
||||
jfg.push_back((jf));
|
||||
else
|
||||
jfg.push_back(boost::make_shared<JacobianFactor>(*factor));
|
||||
}
|
||||
|
||||
// compute the gradient direction
|
||||
gradientAtZero(jfg, g);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GradientDescentOptimizer::iterate() {
|
||||
|
||||
|
||||
// Pull out parameters we'll use
|
||||
const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity;
|
||||
|
||||
// compute the gradient vector
|
||||
gradientInPlace(graph_, state_.values, *ordering_, *gradient_);
|
||||
|
||||
/* normalize it such that it becomes a unit vector */
|
||||
const double g = gradient_->vector().norm();
|
||||
gradient_->vector() /= g;
|
||||
|
||||
// perform the golden section search algorithm to decide the the optimal step size
|
||||
// detail refer to http://en.wikipedia.org/wiki/Golden_section_search
|
||||
VectorValues step = VectorValues::SameStructure(*gradient_);
|
||||
const double phi = 0.5*(1.0+std::sqrt(5.0)), resphi = 2.0 - phi, tau = 1e-5;
|
||||
double minStep = -1.0, maxStep = 0,
|
||||
newStep = minStep + (maxStep-minStep) / (phi+1.0) ;
|
||||
|
||||
step.vector() = newStep * gradient_->vector();
|
||||
Values newValues = state_.values.retract(step, *ordering_);
|
||||
double newError = graph_.error(newValues);
|
||||
|
||||
if ( nloVerbosity ) {
|
||||
std::cout << "minStep = " << minStep << ", maxStep = " << maxStep << ", newStep = " << newStep << ", newError = " << newError << std::endl;
|
||||
}
|
||||
|
||||
while (true) {
|
||||
const bool flag = (maxStep - newStep > newStep - minStep) ? true : false ;
|
||||
const double testStep = flag ?
|
||||
newStep + resphi * (maxStep - newStep) : newStep - resphi * (newStep - minStep);
|
||||
|
||||
if ( (maxStep- minStep) < tau * (std::fabs(testStep) + std::fabs(newStep)) ) {
|
||||
newStep = 0.5*(minStep+maxStep);
|
||||
step.vector() = newStep * gradient_->vector();
|
||||
newValues = state_.values.retract(step, *ordering_);
|
||||
newError = graph_.error(newValues);
|
||||
|
||||
if ( newError < state_.error ) {
|
||||
state_.values = state_.values.retract(step, *ordering_);
|
||||
state_.error = graph_.error(state_.values);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
step.vector() = testStep * gradient_->vector();
|
||||
const Values testValues = state_.values.retract(step, *ordering_);
|
||||
const double testError = graph_.error(testValues);
|
||||
|
||||
// update the working range
|
||||
if ( testError >= newError ) {
|
||||
if ( flag ) maxStep = testStep;
|
||||
else minStep = testStep;
|
||||
}
|
||||
else {
|
||||
if ( flag ) {
|
||||
minStep = newStep;
|
||||
newStep = testStep;
|
||||
newError = testError;
|
||||
}
|
||||
else {
|
||||
maxStep = newStep;
|
||||
newStep = testStep;
|
||||
newError = testError;
|
||||
}
|
||||
}
|
||||
|
||||
if ( nloVerbosity ) {
|
||||
std::cout << "minStep = " << minStep << ", maxStep = " << maxStep << ", newStep = " << newStep << ", newError = " << newError << std::endl;
|
||||
}
|
||||
}
|
||||
// Increment the iteration counter
|
||||
++state_.iterations;
|
||||
}
|
||||
|
||||
double ConjugateGradientOptimizer::System::error(const State &state) const {
|
||||
return graph_.error(state);
|
||||
}
|
||||
|
||||
ConjugateGradientOptimizer::System::Gradient ConjugateGradientOptimizer::System::gradient(const State &state) const {
|
||||
Gradient result = state.zeroVectors(ordering_);
|
||||
gradientInPlace(graph_, state, ordering_, result);
|
||||
return result;
|
||||
}
|
||||
ConjugateGradientOptimizer::System::State ConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const {
|
||||
Gradient step = g;
|
||||
step.vector() *= alpha;
|
||||
return current.retract(step, ordering_);
|
||||
}
|
||||
|
||||
Values ConjugateGradientOptimizer::optimize() {
|
||||
return conjugateGradient<System, Values>(System(graph_, *ordering_), initialEstimate_, params_, !cg_);
|
||||
}
|
||||
|
||||
} /* namespace gtsam */
|
||||
|
|
@ -19,12 +19,9 @@
|
|||
#include <cmath>
|
||||
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
|
||||
|
||||
#include <gtsam/base/cholesky.h> // For NegativeMatrixException
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/linear/GaussianJunctionTree.h>
|
||||
#include <gtsam/linear/SimpleSPCGSolver.h>
|
||||
#include <gtsam/linear/SubgraphSolver.h>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <string>
|
||||
|
||||
|
|
@ -106,34 +103,8 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
|
||||
// Try solving
|
||||
try {
|
||||
|
||||
// Optimize
|
||||
VectorValues delta;
|
||||
if ( params_.isMultifrontal() ) {
|
||||
delta = GaussianJunctionTree(dampedSystem).optimize(params_.getEliminationFunction());
|
||||
}
|
||||
else if ( params_.isSequential() ) {
|
||||
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(params_.getEliminationFunction()));
|
||||
}
|
||||
else if ( params_.isCG() ) {
|
||||
|
||||
if ( !params_.iterativeParams ) throw runtime_error("LMSolver: cg parameter has to be assigned ...");
|
||||
|
||||
if ( boost::dynamic_pointer_cast<SimpleSPCGSolverParameters>(params_.iterativeParams)) {
|
||||
SimpleSPCGSolver solver (dampedSystem, *boost::dynamic_pointer_cast<SimpleSPCGSolverParameters>(params_.iterativeParams));
|
||||
delta = solver.optimize();
|
||||
}
|
||||
else if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params_.iterativeParams) ) {
|
||||
SubgraphSolver solver (dampedSystem, *boost::dynamic_pointer_cast<SubgraphSolverParameters>(params_.iterativeParams));
|
||||
delta = solver.optimize();
|
||||
}
|
||||
else {
|
||||
throw runtime_error("LMSolver: special cg parameter type is not handled in LM solver ...");
|
||||
}
|
||||
}
|
||||
else {
|
||||
throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination");
|
||||
}
|
||||
// Solve Damped Gaussian Factor Graph
|
||||
const VectorValues delta = solveGaussianFactorGraph(dampedSystem, params_);
|
||||
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.vector().norm() << endl;
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta");
|
||||
|
|
|
|||
|
|
@ -0,0 +1,64 @@
|
|||
/**
|
||||
* @file GradientDescentOptimizer.cpp
|
||||
* @brief
|
||||
* @author ydjian
|
||||
* @date Jun 11, 2012
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/linear/JacobianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering
|
||||
* Can be moved to NonlinearFactorGraph.h if desired */
|
||||
void gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values, const Ordering &ordering, VectorValues &g) {
|
||||
// Linearize graph
|
||||
GaussianFactorGraph::shared_ptr linear = nfg.linearize(values, ordering);
|
||||
FactorGraph<JacobianFactor> jfg; jfg.reserve(linear->size());
|
||||
BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, *linear) {
|
||||
if(boost::shared_ptr<JacobianFactor> jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
|
||||
jfg.push_back((jf));
|
||||
else
|
||||
jfg.push_back(boost::make_shared<JacobianFactor>(*factor));
|
||||
}
|
||||
// compute the gradient direction
|
||||
gradientAtZero(jfg, g);
|
||||
}
|
||||
|
||||
double NonlinearConjugateGradientOptimizer::System::error(const State &state) const {
|
||||
return graph_.error(state);
|
||||
}
|
||||
|
||||
NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const {
|
||||
Gradient result = state.zeroVectors(ordering_);
|
||||
gradientInPlace(graph_, state, ordering_, result);
|
||||
return result;
|
||||
}
|
||||
NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const {
|
||||
Gradient step = g;
|
||||
step.vector() *= alpha;
|
||||
return current.retract(step, ordering_);
|
||||
}
|
||||
|
||||
void NonlinearConjugateGradientOptimizer::iterate() {
|
||||
size_t dummy ;
|
||||
boost::tie(state_.values, dummy) = nonlinearConjugateGradient<System, Values>(System(graph_, *ordering_), state_.values, params_, true /* single iterations */);
|
||||
++state_.iterations;
|
||||
state_.error = graph_.error(state_.values);
|
||||
}
|
||||
|
||||
const Values& NonlinearConjugateGradientOptimizer::optimize() {
|
||||
boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient<System, Values>(System(graph_, *ordering_), state_.values, params_, false /* up to convergent */);
|
||||
state_.error = graph_.error(state_.values);
|
||||
return state_.values;
|
||||
}
|
||||
|
||||
} /* namespace gtsam */
|
||||
|
|
@ -1,7 +1,7 @@
|
|||
/**
|
||||
* @file GradientDescentOptimizer.cpp
|
||||
* @brief
|
||||
* @author ydjian
|
||||
* @author Yong-Dian Jian
|
||||
* @date Jun 11, 2012
|
||||
*/
|
||||
|
||||
|
|
@ -9,75 +9,31 @@
|
|||
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* an implementation of gradient-descent method using the NLO interface */
|
||||
|
||||
class GradientDescentState : public NonlinearOptimizerState {
|
||||
|
||||
/** An implementation of the nonlinear cg method using the template below */
|
||||
class NonlinearConjugateGradientState : public NonlinearOptimizerState {
|
||||
public:
|
||||
|
||||
typedef NonlinearOptimizerState Base;
|
||||
|
||||
GradientDescentState(const NonlinearFactorGraph& graph, const Values& values)
|
||||
NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, const Values& values)
|
||||
: Base(graph, values) {}
|
||||
};
|
||||
|
||||
class GradientDescentOptimizer : public NonlinearOptimizer {
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<GradientDescentOptimizer> shared_ptr;
|
||||
typedef NonlinearOptimizer Base;
|
||||
typedef GradientDescentState States;
|
||||
typedef NonlinearOptimizerParams Parameters;
|
||||
|
||||
protected:
|
||||
|
||||
Parameters params_;
|
||||
States state_;
|
||||
Ordering::shared_ptr ordering_;
|
||||
VectorValues::shared_ptr gradient_;
|
||||
|
||||
public:
|
||||
|
||||
GradientDescentOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Parameters& params = Parameters())
|
||||
: Base(graph), params_(params), state_(graph, initialValues),
|
||||
ordering_(initialValues.orderingArbitrary()),
|
||||
gradient_(new VectorValues(initialValues.zeroVectors(*ordering_))) {}
|
||||
|
||||
virtual ~GradientDescentOptimizer() {}
|
||||
|
||||
virtual void iterate();
|
||||
|
||||
protected:
|
||||
|
||||
virtual const NonlinearOptimizerState& _state() const { return state_; }
|
||||
virtual const NonlinearOptimizerParams& _params() const { return params_; }
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* An implementation of the nonlinear cg method using the template below
|
||||
*/
|
||||
|
||||
class ConjugateGradientOptimizer {
|
||||
|
||||
class NonlinearConjugateGradientOptimizer : public NonlinearOptimizer {
|
||||
/* a class for the nonlinearConjugateGradient template */
|
||||
class System {
|
||||
|
||||
public:
|
||||
|
||||
typedef Values State;
|
||||
typedef VectorValues Gradient;
|
||||
typedef NonlinearOptimizerParams Parameters;
|
||||
|
||||
protected:
|
||||
|
||||
NonlinearFactorGraph graph_;
|
||||
Ordering ordering_;
|
||||
const NonlinearFactorGraph &graph_;
|
||||
const Ordering &ordering_;
|
||||
|
||||
public:
|
||||
|
||||
System(const NonlinearFactorGraph &graph, const Ordering &ordering): graph_(graph), ordering_(ordering) {}
|
||||
double error(const State &state) const ;
|
||||
Gradient gradient(const State &state) const ;
|
||||
|
|
@ -85,35 +41,32 @@ class ConjugateGradientOptimizer {
|
|||
};
|
||||
|
||||
public:
|
||||
|
||||
typedef NonlinearOptimizer Base;
|
||||
typedef NonlinearConjugateGradientState States;
|
||||
typedef NonlinearOptimizerParams Parameters;
|
||||
typedef boost::shared_ptr<ConjugateGradientOptimizer> shared_ptr;
|
||||
typedef boost::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;
|
||||
|
||||
protected:
|
||||
|
||||
NonlinearFactorGraph graph_;
|
||||
Values initialEstimate_;
|
||||
States state_;
|
||||
Parameters params_;
|
||||
Ordering::shared_ptr ordering_;
|
||||
VectorValues::shared_ptr gradient_;
|
||||
bool cg_;
|
||||
|
||||
public:
|
||||
|
||||
ConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
|
||||
const Parameters& params = Parameters(), const bool cg = true)
|
||||
: graph_(graph), initialEstimate_(initialValues), params_(params),
|
||||
ordering_(initialValues.orderingArbitrary()),
|
||||
gradient_(new VectorValues(initialValues.zeroVectors(*ordering_))),
|
||||
cg_(cg) {}
|
||||
NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
|
||||
const Parameters& params = Parameters())
|
||||
: Base(graph), state_(graph, initialValues), params_(params), ordering_(initialValues.orderingArbitrary()),
|
||||
gradient_(new VectorValues(initialValues.zeroVectors(*ordering_))){}
|
||||
|
||||
virtual ~ConjugateGradientOptimizer() {}
|
||||
virtual Values optimize () ;
|
||||
virtual ~NonlinearConjugateGradientOptimizer() {}
|
||||
virtual void iterate();
|
||||
virtual const Values& optimize ();
|
||||
virtual const NonlinearOptimizerState& _state() const { return state_; }
|
||||
virtual const NonlinearOptimizerParams& _params() const { return params_; }
|
||||
};
|
||||
|
||||
/**
|
||||
* Implement the golden-section line search algorithm
|
||||
*/
|
||||
/** Implement the golden-section line search algorithm */
|
||||
template <class S, class V, class W>
|
||||
double lineSearch(const S &system, const V currentValues, const W &gradient) {
|
||||
|
||||
|
|
@ -171,18 +124,20 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) {
|
|||
*
|
||||
* The last parameter is a switch between gradient-descent and conjugate gradient
|
||||
*/
|
||||
|
||||
template <class S, class V>
|
||||
V conjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool gradientDescent) {
|
||||
boost::tuple<V, size_t> nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) {
|
||||
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(V);
|
||||
// GTSAM_CONCEPT_MANIFOLD_TYPE(V);
|
||||
|
||||
Index iteration = 0;
|
||||
|
||||
// check if we're already close enough
|
||||
double currentError = system.error(initial);
|
||||
if(currentError <= params.errorTol) {
|
||||
if (params.verbosity >= NonlinearOptimizerParams::ERROR)
|
||||
if (params.verbosity >= NonlinearOptimizerParams::ERROR){
|
||||
std::cout << "Exiting, as error = " << currentError << " < " << params.errorTol << std::endl;
|
||||
return initial;
|
||||
}
|
||||
return boost::tie(initial, iteration);
|
||||
}
|
||||
|
||||
V currentValues = initial;
|
||||
|
|
@ -194,14 +149,12 @@ V conjugateGradient(const S &system, const V &initial, const NonlinearOptimizerP
|
|||
double alpha = lineSearch(system, currentValues, direction);
|
||||
currentValues = system.advance(prevValues, alpha, direction);
|
||||
currentError = system.error(currentValues);
|
||||
Index iteration = 0;
|
||||
|
||||
// Maybe show output
|
||||
if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "Initial error: " << currentError << std::endl;
|
||||
|
||||
// Iterative loop
|
||||
do {
|
||||
|
||||
if ( gradientDescent == true) {
|
||||
direction = system.gradient(currentValues);
|
||||
}
|
||||
|
|
@ -222,13 +175,14 @@ V conjugateGradient(const S &system, const V &initial, const NonlinearOptimizerP
|
|||
// Maybe show output
|
||||
if(params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "currentError: " << currentError << std::endl;
|
||||
} while( ++iteration < params.maxIterations &&
|
||||
!singleIteration &&
|
||||
!checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, prevError, currentError, params.verbosity));
|
||||
|
||||
// Printing if verbose
|
||||
if (params.verbosity >= NonlinearOptimizerParams::ERROR && iteration >= params.maxIterations)
|
||||
std::cout << "Terminating because reached maximum iterations" << std::endl;
|
||||
std::cout << "nonlinearConjugateGradient: Terminating because reached maximum iterations" << std::endl;
|
||||
|
||||
return currentValues;
|
||||
return boost::tie(currentValues, iteration);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
/**
|
||||
* @file SuccessiveLinearizationOptimizer.cpp
|
||||
* @brief
|
||||
* @date Jul 24, 2012
|
||||
* @author Yong-Dian Jian
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/linear/GaussianJunctionTree.h>
|
||||
#include <gtsam/linear/SimpleSPCGSolver.h>
|
||||
#include <gtsam/linear/SubgraphSolver.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
void SuccessiveLinearizationParams::print(const std::string& str) const {
|
||||
NonlinearOptimizerParams::print(str);
|
||||
switch ( linearSolverType ) {
|
||||
case MULTIFRONTAL_CHOLESKY:
|
||||
std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n";
|
||||
break;
|
||||
case MULTIFRONTAL_QR:
|
||||
std::cout << " linear solver type: MULTIFRONTAL QR\n";
|
||||
break;
|
||||
case SEQUENTIAL_CHOLESKY:
|
||||
std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n";
|
||||
break;
|
||||
case SEQUENTIAL_QR:
|
||||
std::cout << " linear solver type: SEQUENTIAL QR\n";
|
||||
break;
|
||||
case CHOLMOD:
|
||||
std::cout << " linear solver type: CHOLMOD\n";
|
||||
break;
|
||||
case CG:
|
||||
std::cout << " linear solver type: CG\n";
|
||||
break;
|
||||
default:
|
||||
std::cout << " linear solver type: (invalid)\n";
|
||||
break;
|
||||
}
|
||||
|
||||
if(ordering)
|
||||
std::cout << " ordering: custom\n";
|
||||
else
|
||||
std::cout << " ordering: COLAMD\n";
|
||||
|
||||
std::cout.flush();
|
||||
}
|
||||
|
||||
VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams ¶ms) {
|
||||
VectorValues delta;
|
||||
if ( params.isMultifrontal() ) {
|
||||
delta = GaussianJunctionTree(gfg).optimize(params.getEliminationFunction());
|
||||
}
|
||||
else if ( params.isSequential() ) {
|
||||
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(gfg)->eliminate(params.getEliminationFunction()));
|
||||
}
|
||||
else if ( params.isCG() ) {
|
||||
if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ...");
|
||||
if ( boost::dynamic_pointer_cast<SimpleSPCGSolverParameters>(params.iterativeParams)) {
|
||||
SimpleSPCGSolver solver (gfg, *boost::dynamic_pointer_cast<SimpleSPCGSolverParameters>(params.iterativeParams));
|
||||
delta = solver.optimize();
|
||||
}
|
||||
else if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
|
||||
SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams));
|
||||
delta = solver.optimize();
|
||||
}
|
||||
else {
|
||||
throw std::runtime_error("solveGaussianFactorGraph: special cg parameter type is not handled in LM solver ...");
|
||||
}
|
||||
}
|
||||
else {
|
||||
throw std::runtime_error("solveGaussianFactorGraph: Optimization parameter is invalid");
|
||||
}
|
||||
return delta;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -40,7 +40,6 @@ public:
|
|||
IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers.
|
||||
|
||||
SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {}
|
||||
|
||||
virtual ~SuccessiveLinearizationParams() {}
|
||||
|
||||
virtual void print(const std::string& str = "") const {
|
||||
|
|
@ -62,7 +61,7 @@ public:
|
|||
std::cout << " linear solver type: CHOLMOD\n";
|
||||
break;
|
||||
case CONJUGATE_GRADIENT:
|
||||
std::cout << " linear solver type: CG\n";
|
||||
std::cout << " linear solver type: CONJUGATE GRADIENT\n";
|
||||
break;
|
||||
default:
|
||||
std::cout << " linear solver type: (invalid)\n";
|
||||
|
|
@ -77,7 +76,7 @@ public:
|
|||
std::cout.flush();
|
||||
}
|
||||
|
||||
inline bool isMultifrontal() const {
|
||||
inline bool isMultifrontal() const {
|
||||
return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); }
|
||||
|
||||
inline bool isSequential() const {
|
||||
|
|
@ -87,7 +86,9 @@ public:
|
|||
|
||||
inline bool isCG() const { return (linearSolverType == CONJUGATE_GRADIENT); }
|
||||
|
||||
GaussianFactorGraph::Eliminate getEliminationFunction() {
|
||||
virtual void print(const std::string& str) const;
|
||||
|
||||
GaussianFactorGraph::Eliminate getEliminationFunction() const {
|
||||
switch (linearSolverType) {
|
||||
case MULTIFRONTAL_CHOLESKY:
|
||||
case SEQUENTIAL_CHOLESKY:
|
||||
|
|
@ -132,4 +133,7 @@ private:
|
|||
}
|
||||
};
|
||||
|
||||
/* a wrapper for solving a GaussianFactorGraph according to the parameters */
|
||||
VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams ¶ms) ;
|
||||
|
||||
} /* namespace gtsam */
|
||||
|
|
|
|||
|
|
@ -90,10 +90,8 @@ pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
|
|||
bool addNoise, bool smart) {
|
||||
cout << "Will try to read " << filename << endl;
|
||||
ifstream is(filename.c_str());
|
||||
if (!is) {
|
||||
cout << "load2D: can not find the file!";
|
||||
exit(-1);
|
||||
}
|
||||
if (!is)
|
||||
throw std::invalid_argument("load2D: can not find the file!");
|
||||
|
||||
pose2SLAM::Values::shared_ptr poses(new pose2SLAM::Values);
|
||||
pose2SLAM::Graph::shared_ptr graph(new pose2SLAM::Graph);
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@
|
|||
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/GradientDescentOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearGradientDescentOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
|
@ -55,7 +55,6 @@ boost::tuple<NonlinearFactorGraph, Values> generateProblem() {
|
|||
return boost::tie(graph, initialEstimate);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(optimize, GradientDescentOptimizer) {
|
||||
|
||||
|
|
@ -94,8 +93,7 @@ TEST(optimize, ConjugateGradientOptimizer) {
|
|||
param.maxIterations = 500; /* requires a larger number of iterations to converge */
|
||||
param.verbosity = NonlinearOptimizerParams::SILENT;
|
||||
|
||||
|
||||
ConjugateGradientOptimizer optimizer(graph, initialEstimate, param, true);
|
||||
NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param);
|
||||
Values result = optimizer.optimize();
|
||||
// cout << "cg final error = " << graph.error(result) << endl;
|
||||
|
||||
|
|
@ -103,32 +101,6 @@ TEST(optimize, ConjugateGradientOptimizer) {
|
|||
DOUBLES_EQUAL(0.0, graph.error(result), 1e-2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(optimize, GradientDescentOptimizer2) {
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
Values initialEstimate;
|
||||
|
||||
boost::tie(graph, initialEstimate) = generateProblem();
|
||||
// cout << "initial error = " << graph.error(initialEstimate) << endl ;
|
||||
|
||||
// Single Step Optimization using Levenberg-Marquardt
|
||||
NonlinearOptimizerParams param;
|
||||
param.maxIterations = 500; /* requires a larger number of iterations to converge */
|
||||
param.verbosity = NonlinearOptimizerParams::SILENT;
|
||||
|
||||
|
||||
ConjugateGradientOptimizer optimizer(graph, initialEstimate, param, false);
|
||||
Values result = optimizer.optimize();
|
||||
// cout << "gd2 solver final error = " << graph.error(result) << endl;
|
||||
|
||||
/* the optimality of the solution is not comparable to the */
|
||||
DOUBLES_EQUAL(0.0, graph.error(result), 1e-2);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -19,8 +19,8 @@ using namespace wrap;
|
|||
string ReturnValue::return_type(bool add_ptr, pairing p) const {
|
||||
if (p==pair && isPair) {
|
||||
string str = "pair< " +
|
||||
maybe_shared_ptr(add_ptr && isPtr1, qualifiedType1("::"), type1) + ", " +
|
||||
maybe_shared_ptr(add_ptr && isPtr2, qualifiedType2("::"), type2) + " >";
|
||||
maybe_shared_ptr(add_ptr || isPtr1, qualifiedType1("::"), type1) + ", " +
|
||||
maybe_shared_ptr(add_ptr || isPtr2, qualifiedType2("::"), type2) + " >";
|
||||
return str;
|
||||
} else
|
||||
return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::"), (p==arg2)? type2 : type1);
|
||||
|
|
@ -52,6 +52,9 @@ void ReturnValue::wrap_result(const string& result, FileWriter& file, const Type
|
|||
string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2(".");
|
||||
|
||||
if (isPair) {
|
||||
// For a pair, store the returned pair so we do not evaluate the function twice
|
||||
file.oss << " " << return_type(false, pair) << " pairResult = " << result << ";\n";
|
||||
|
||||
// first return value in pair
|
||||
if (category1 == ReturnValue::CLASS) { // if we are going to make one
|
||||
string objCopy, ptrType;
|
||||
|
|
@ -59,21 +62,21 @@ void ReturnValue::wrap_result(const string& result, FileWriter& file, const Type
|
|||
const bool isVirtual = typeAttributes.at(cppType1).isVirtual;
|
||||
if(isVirtual) {
|
||||
if(isPtr1)
|
||||
objCopy = result + ".first";
|
||||
objCopy = "pairResult.first";
|
||||
else
|
||||
objCopy = result + ".first.clone()";
|
||||
objCopy = "pairResult.first.clone()";
|
||||
} else {
|
||||
if(isPtr1)
|
||||
objCopy = result + ".first";
|
||||
objCopy = "pairResult.first";
|
||||
else
|
||||
objCopy = ptrType + "(new " + cppType1 + "(" + result + ".first))";
|
||||
objCopy = ptrType + "(new " + cppType1 + "(pairResult.first))";
|
||||
}
|
||||
file.oss << " out[0] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType1 << "\", " << (isVirtual ? "true" : "false") << ");\n";
|
||||
} else if(isPtr1) {
|
||||
file.oss << " Shared" << type1 <<"* ret = new Shared" << type1 << "(" << result << ".first);" << endl;
|
||||
file.oss << " Shared" << type1 <<"* ret = new Shared" << type1 << "(pairResult.first);" << endl;
|
||||
file.oss << " out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\", false);\n";
|
||||
} else // if basis type
|
||||
file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(" << result << ".first);\n";
|
||||
file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(pairResult.first);\n";
|
||||
|
||||
// second return value in pair
|
||||
if (category2 == ReturnValue::CLASS) { // if we are going to make one
|
||||
|
|
@ -82,21 +85,21 @@ void ReturnValue::wrap_result(const string& result, FileWriter& file, const Type
|
|||
const bool isVirtual = typeAttributes.at(cppType2).isVirtual;
|
||||
if(isVirtual) {
|
||||
if(isPtr2)
|
||||
objCopy = result + ".second";
|
||||
objCopy = "pairResult.second";
|
||||
else
|
||||
objCopy = result + ".second.clone()";
|
||||
objCopy = "pairResult.second.clone()";
|
||||
} else {
|
||||
if(isPtr2)
|
||||
objCopy = result + ".second";
|
||||
objCopy = "pairResult.second";
|
||||
else
|
||||
objCopy = ptrType + "(new " + cppType2 + "(" + result + ".second))";
|
||||
objCopy = ptrType + "(new " + cppType2 + "(pairResult.second))";
|
||||
}
|
||||
file.oss << " out[0] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType2 << "\", " << (isVirtual ? "true" : "false") << ");\n";
|
||||
file.oss << " out[1] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType2 << "\", " << (isVirtual ? "true" : "false") << ");\n";
|
||||
} else if(isPtr2) {
|
||||
file.oss << " Shared" << type2 <<"* ret = new Shared" << type2 << "(" << result << ".second);" << endl;
|
||||
file.oss << " Shared" << type2 <<"* ret = new Shared" << type2 << "(pairResult.second);" << endl;
|
||||
file.oss << " out[1] = wrap_shared_ptr(ret,\"" << matlabType2 << "\");\n";
|
||||
} else
|
||||
file.oss << " out[1] = wrap< " << return_type(true,arg2) << " >(" << result << ".second);\n";
|
||||
file.oss << " out[1] = wrap< " << return_type(true,arg2) << " >(pairResult.second);\n";
|
||||
}
|
||||
else if (category1 == ReturnValue::CLASS){
|
||||
string objCopy, ptrType;
|
||||
|
|
|
|||
Loading…
Reference in New Issue