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

Significant API change in slam (GTSAM 2.0.1 or 2.1): to eliminate confusion and give the user more freedom in creating their own Keys, the different slam variants no longer create Symbol keys themselves. Instead, all interaction is done via Keys (which are just unordered, unsigned ints). All PoseSLAM unit tests and examples now just use sequential keys. However, a user can still create Keys using the Symbol constructor, which is illustrated in the landmark-based unit tests and examples.

release/4.3a0
Frank Dellaert 2012-06-02 16:18:40 +00:00
parent 8bcd2da2f0
commit 5160c2eb50
46 changed files with 1118 additions and 1344 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

719
.cproject
View File

@ -311,14 +311,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>
@ -345,6 +337,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>
@ -352,6 +345,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>
@ -399,6 +393,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>
@ -406,6 +401,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>
@ -413,6 +409,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>
@ -428,168 +425,17 @@
</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="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGaussianFactor.run" path="linear/tests" 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="testGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianISAM.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testIterative.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testIterative.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearEquality.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearEquality.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearOptimizer.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSubgraphPreconditioner.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSubgraphPreconditioner.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTupleConfig.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testTupleConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeGaussianFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>timeGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="tests" 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="testGaussianJunctionTree" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianJunctionTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerialization.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSerialization.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
@ -665,22 +511,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>
@ -697,6 +527,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>
@ -721,54 +567,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testValues.run</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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testOrdering.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testGeneralSFMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPlanarSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -793,6 +591,94 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPlanarSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose2SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose3SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimulated2D.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimulated2DOriented.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVisualSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVisualSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testProjectionFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testProjectionFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerializationSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSerializationSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testValues.run</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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testOrdering.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="schedulingExample.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -897,22 +783,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeGaussianFactorGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -921,42 +791,26 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTupleConfig.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testMarginals.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testTupleConfig.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testMarginals.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerialization.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGaussianISAM2.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSerialization.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianISAM2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInference.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testSymbolicFactorGraphB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianISAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSymbolicFactorGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>testSymbolicFactorGraphB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -985,14 +839,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2SLAMwSPCG.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose2SLAMwSPCG.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearOptimizer.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1017,14 +863,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests.testBoundingConstraint.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>tests.testBoundingConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearEquality.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1057,6 +895,62 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianJunctionTreeB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianJunctionTreeB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianISAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDoglegOptimizer.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDoglegOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1147,6 +1041,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>
@ -1602,7 +1497,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>
@ -1642,7 +1536,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>
@ -1650,7 +1543,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>
@ -1752,10 +1644,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="CameraResectioning" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="CameraResectioning.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>CameraResectioning</buildTarget>
<buildArguments>-j5</buildArguments>
<buildTarget>CameraResectioning.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -1832,6 +1724,30 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="LocalizationExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>LocalizationExample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="LocalizationExample2.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>LocalizationExample2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMwSPCG_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>Pose2SLAMwSPCG_advanced.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1842,7 +1758,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>
@ -1864,102 +1779,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>-j2</buildArguments>
@ -2161,6 +1980,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>
@ -2168,6 +1988,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>
@ -2175,6 +1996,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>
@ -2182,6 +2004,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>
@ -2235,42 +2058,98 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap.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>wrap.testSpirit.run</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap.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>wrap.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_gtsam" 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_gtsam</buildTarget>
<buildArguments>-j2</buildArguments>
<buildTarget>timeRot3.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="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap</buildTarget>
<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>
@ -2314,6 +2193,46 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap.testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap.testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap.testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap.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_gtsam" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam</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>

6
examples/LocalizationExample.cpp
View File

@ -66,9 +66,9 @@ int main(int argc, char** argv) {
// Query the marginals
Marginals marginals = graph.marginals(result);
cout.precision(2);
cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(2)) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(3)) << endl;
cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl;
return 0;
}

13
examples/LocalizationExample2.cpp
View File

@ -71,10 +71,9 @@ int main(int argc, char** argv) {
// add unary measurement factors, like GPS, on all three poses
SharedDiagonal noiseModel(Vector_(2, 0.1, 0.1)); // 10cm std on x,y
Symbol x1('x',1), x2('x',2), x3('x',3);
graph.push_back(boost::make_shared<UnaryFactor>(x1, 0, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(x2, 2, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(x3, 4, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(1, 0, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(2, 2, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(3, 4, 0, noiseModel));
// print
graph.print("\nFactor graph:\n");
@ -94,9 +93,9 @@ int main(int argc, char** argv) {
// Query the marginals
Marginals marginals(graph, result);
cout.precision(2);
cout << "\nP1:\n" << marginals.marginalCovariance(x1) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(x2) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(x3) << endl;
cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl;
return 0;
}

26
examples/PlanarSLAMExample_easy.cpp
View File

@ -35,16 +35,20 @@ int main(int argc, char** argv) {
// create the graph (defined in planarSlam.h, derived from NonlinearFactorGraph)
planarSLAM::Graph graph;
// Create some keys
static Symbol i1('x',1), i2('x',2), i3('x',3);
static Symbol j1('l',1), j2('l',2);
// add a Gaussian prior on pose x_1
Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); // add directly to graph
graph.addPrior(i1, priorMean, priorNoise); // add directly to graph
// add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.addOdometry(i1, i2, odometry, odometryNoise);
graph.addOdometry(i2, i3, odometry, odometryNoise);
// create a noise model for the landmark measurements
SharedDiagonal measurementNoise(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
@ -58,20 +62,20 @@ int main(int argc, char** argv) {
range32 = 2.0;
// add bearing/range factors (created by "addBearingRange")
graph.addBearingRange(1, 1, bearing11, range11, measurementNoise);
graph.addBearingRange(2, 1, bearing21, range21, measurementNoise);
graph.addBearingRange(3, 2, bearing32, range32, measurementNoise);
graph.addBearingRange(i1, j1, bearing11, range11, measurementNoise);
graph.addBearingRange(i2, j1, bearing21, range21, measurementNoise);
graph.addBearingRange(i3, j2, bearing32, range32, measurementNoise);
// print
graph.print("Factor graph");
// create (deliberatly inaccurate) initial estimate
planarSLAM::Values initialEstimate;
initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insertPoint(1, Point2(1.8, 2.1));
initialEstimate.insertPoint(2, Point2(4.1, 1.8));
initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insertPoint(j1, Point2(1.8, 2.1));
initialEstimate.insertPoint(j2, Point2(4.1, 1.8));
initialEstimate.print("Initial estimate:\n ");

62
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -15,29 +15,29 @@
* @author Alex Cunningham
*/
#include <cmath>
#include <iostream>
// for all nonlinear keys
#include <gtsam/nonlinear/Symbol.h>
// for points and poses
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
// for modeling measurement uncertainty - all models included here
#include <gtsam/linear/NoiseModel.h>
// add in headers for specific factors
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
// for all nonlinear keys
#include <gtsam/nonlinear/Symbol.h>
// implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
// for modeling measurement uncertainty - all models included here
#include <gtsam/linear/NoiseModel.h>
// for points and poses
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <cmath>
#include <iostream>
using namespace std;
using namespace gtsam;
@ -52,8 +52,8 @@ using namespace gtsam;
*/
int main(int argc, char** argv) {
// create keys for variables
Symbol x1('x',1), x2('x',2), x3('x',3);
Symbol l1('l',1), l2('l',2);
Symbol i1('x',1), i2('x',2), i3('x',3);
Symbol j1('l',1), j2('l',2);
// create graph container and add factors to it
NonlinearFactorGraph graph;
@ -62,7 +62,7 @@ int main(int argc, char** argv) {
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
PriorFactor<Pose2> posePrior(x1, prior_measurement, prior_model); // create the factor
PriorFactor<Pose2> posePrior(i1, prior_measurement, prior_model); // create the factor
graph.add(posePrior); // add the factor to the graph
/* add odometry */
@ -70,8 +70,8 @@ int main(int argc, char** argv) {
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
// create between factors to represent odometry
BetweenFactor<Pose2> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<Pose2> odom23(x2, x3, odom_measurement, odom_model);
BetweenFactor<Pose2> odom12(i1, i2, odom_measurement, odom_model);
BetweenFactor<Pose2> odom23(i2, i3, odom_measurement, odom_model);
graph.add(odom12); // add both to graph
graph.add(odom23);
@ -88,9 +88,9 @@ int main(int argc, char** argv) {
range32 = 2.0;
// create bearing/range factors
BearingRangeFactor<Pose2, Point2> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<Pose2, Point2> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<Pose2, Point2> meas32(x3, l2, bearing32, range32, meas_model);
BearingRangeFactor<Pose2, Point2> meas11(i1, j1, bearing11, range11, meas_model);
BearingRangeFactor<Pose2, Point2> meas21(i2, j1, bearing21, range21, meas_model);
BearingRangeFactor<Pose2, Point2> meas32(i3, j2, bearing32, range32, meas_model);
// add the factors
graph.add(meas11);
@ -101,11 +101,11 @@ int main(int argc, char** argv) {
// initialize to noisy points
Values initial;
initial.insert(x1, Pose2(0.5, 0.0, 0.2));
initial.insert(x2, Pose2(2.3, 0.1,-0.2));
initial.insert(x3, Pose2(4.1, 0.1, 0.1));
initial.insert(l1, Point2(1.8, 2.1));
initial.insert(l2, Point2(4.1, 1.8));
initial.insert(i1, Pose2(0.5, 0.0, 0.2));
initial.insert(i2, Pose2(2.3, 0.1,-0.2));
initial.insert(i3, Pose2(4.1, 0.1, 0.1));
initial.insert(j1, Point2(1.8, 2.1));
initial.insert(j2, Point2(4.1, 1.8));
initial.print("initial estimate");
@ -126,11 +126,11 @@ int main(int argc, char** argv) {
// Print marginals covariances for all variables
Marginals marginals(graph, resultMultifrontal, Marginals::CHOLESKY);
print(marginals.marginalCovariance(x1), "x1 covariance");
print(marginals.marginalCovariance(x2), "x2 covariance");
print(marginals.marginalCovariance(x3), "x3 covariance");
print(marginals.marginalCovariance(l1), "l1 covariance");
print(marginals.marginalCovariance(l2), "l2 covariance");
print(marginals.marginalCovariance(i1), "i1 covariance");
print(marginals.marginalCovariance(i2), "i2 covariance");
print(marginals.marginalCovariance(i3), "i3 covariance");
print(marginals.marginalCovariance(j1), "j1 covariance");
print(marginals.marginalCovariance(j2), "j2 covariance");
return 0;
}

17
examples/Pose2SLAMExample_advanced.cpp
View File

@ -16,26 +16,23 @@
* @author Chris Beall
*/
#include <cmath>
#include <iostream>
#include <boost/shared_ptr.hpp>
// pull in the Pose2 SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <boost/shared_ptr.hpp>
#include <cmath>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace boost;
using namespace pose2SLAM;
int main(int argc, char** argv) {
/* 1. create graph container and add factors to it */
Graph graph;
pose2SLAM::Graph graph;
/* 2.a add prior */
// gaussian for prior
@ -77,8 +74,8 @@ int main(int argc, char** argv) {
/* Get covariances */
Marginals marginals(graph, result, Marginals::CHOLESKY);
Matrix covariance1 = marginals.marginalCovariance(PoseKey(1));
Matrix covariance2 = marginals.marginalCovariance(PoseKey(2));
Matrix covariance1 = marginals.marginalCovariance(1);
Matrix covariance2 = marginals.marginalCovariance(2);
print(covariance1, "Covariance1");
print(covariance2, "Covariance2");

6
examples/matlab/LocalizationExample.m
View File

@ -45,9 +45,9 @@ result.print(sprintf('\nFinal result:\n '));
%% Query the marginals
marginals = graph.marginals(result);
x{1}=gtsamSymbol('x',1); P{1}=marginals.marginalCovariance(x{1}.key)
x{2}=gtsamSymbol('x',2); P{2}=marginals.marginalCovariance(x{2}.key)
x{3}=gtsamSymbol('x',3); P{3}=marginals.marginalCovariance(x{3}.key)
P{1}=marginals.marginalCovariance(1)
P{2}=marginals.marginalCovariance(2)
P{3}=marginals.marginalCovariance(3)
%% Plot Trajectory
figure(1)

2
examples/vSLAMexample/Feature2D.h
View File

@ -24,7 +24,7 @@ struct Feature2D
{
gtsam::Point2 m_p;
int m_idCamera; // id of the camera pose that makes this measurement
int m_idCamera; // id of the camera pose that makes this measurement
int m_idLandmark; // id of the 3D landmark that it is associated with
Feature2D(int idCamera, int idLandmark, gtsam::Point2 p) :

35
examples/vSLAMexample/vISAMexample.cpp
View File

@ -11,14 +11,12 @@
/**
* @file vISAMexample.cpp
* @brief An ISAM example for synthesis sequence
* single camera
* @brief An ISAM example for synthesis sequence, single camera
* @author Duy-Nguyen Ta
*/
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
using namespace boost;
#include "vSLAMutils.h"
#include "Feature2D.h"
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
@ -26,13 +24,12 @@ using namespace boost;
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include "vSLAMutils.h"
#include "Feature2D.h"
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
using namespace boost;
using namespace std;
using namespace gtsam;
using namespace visualSLAM;
using namespace boost;
/* ************************************************************************* */
#define CALIB_FILE "calib.txt"
@ -45,7 +42,7 @@ string g_dataFolder;
// Store groundtruth values, read from files
shared_ptrK g_calib;
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
map<int, Pose3> g_poses; // map: <camera_id, pose>
std::map<int, std::vector<Feature2D> > g_measurements; // feature sets detected at each frame
@ -76,31 +73,31 @@ void readAllDataISAM() {
/**
* Setup newFactors and initialValues for each new pose and set of measurements at each frame.
*/
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
// Create a graph of newFactors with new measurements
newFactors = shared_ptr<Graph> (new Graph());
newFactors = shared_ptr<visualSLAM::Graph> (new visualSLAM::Graph());
for (size_t i = 0; i < measurements.size(); i++) {
newFactors->addMeasurement(
measurements[i].m_p,
measurementSigma,
pose_id,
measurements[i].m_idLandmark,
Symbol('x',pose_id),
Symbol('l',measurements[i].m_idLandmark),
calib);
}
// ... we need priors on the new pose and all new landmarks
newFactors->addPosePrior(pose_id, pose, poseSigma);
for (size_t i = 0; i < measurements.size(); i++) {
newFactors->addPointPrior(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
newFactors->addPointPrior(Symbol('x',measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
}
// Create initial values for all nodes in the newFactors
initialValues = shared_ptr<Values> (new Values());
initialValues->insert(PoseKey(pose_id), pose);
initialValues->insert(Symbol('x',pose_id), pose);
for (size_t i = 0; i < measurements.size(); i++) {
initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
initialValues->insert(Symbol('l',measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
}
}
@ -127,8 +124,8 @@ int main(int argc, char* argv[]) {
typedef std::map<int, std::vector<Feature2D> > FeatureMap;
BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
const int poseId = features.first;
shared_ptr<Graph> newFactors;
shared_ptr<Values> initialValues;
shared_ptr<visualSLAM::Graph> newFactors;
shared_ptr<visualSLAM::Values> initialValues;
createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
features.second, measurementSigma, g_calib);

37
examples/vSLAMexample/vSFMexample.cpp
View File

@ -11,26 +11,23 @@
/**
* @file vSFMexample.cpp
* @brief An vSFM example for synthesis sequence
* single camera
* @brief A visual SLAM example for simulated sequence
* @author Duy-Nguyen Ta
*/
#include <boost/shared_ptr.hpp>
using namespace boost;
#include "vSLAMutils.h"
#include "Feature2D.h"
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/PriorFactor.h>
#include "vSLAMutils.h"
#include "Feature2D.h"
#include <boost/shared_ptr.hpp>
using namespace boost;
using namespace std;
using namespace gtsam;
using namespace visualSLAM;
using namespace boost;
/* ************************************************************************* */
#define CALIB_FILE "calib.txt"
@ -43,9 +40,9 @@ string g_dataFolder;
// Store groundtruth values, read from files
shared_ptrK g_calib;
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
map<int, Pose3> g_poses; // map: <camera_id, pose>
std::vector<Feature2D> g_measurements; // Feature2D: {camera_id, landmark_id, 2d feature_position}
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
map<int, Pose3> g_poses; // map: <camera_id, pose>
std::vector<Feature2D> g_measurements; // Feature2D: {camera_id, landmark_id, 2d feature_position}
// Noise models
SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
@ -75,9 +72,9 @@ void readAllData() {
* by adding and linking 2D features (measurements) detected in each captured image
* with their corresponding landmarks.
*/
Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
visualSLAM::Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
Graph g;
visualSLAM::Graph g;
cout << "Built graph: " << endl;
for (size_t i = 0; i < measurements.size(); i++) {
@ -86,8 +83,8 @@ Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurem
g.addMeasurement(
measurements[i].m_p,
measurementSigma,
measurements[i].m_idCamera,
measurements[i].m_idLandmark,
Symbol('x',measurements[i].m_idCamera),
Symbol('l',measurements[i].m_idLandmark),
calib);
}
@ -101,15 +98,15 @@ Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurem
*/
Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
Values initValues;
visualSLAM::Values initValues;
// Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
initValues.insert(PointKey(lmit->first), lmit->second);
initValues.insert(Symbol('l',lmit->first), lmit->second);
// Initialize camera poses.
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
initValues.insert(PoseKey(poseit->first), poseit->second);
initValues.insert(Symbol('x',poseit->first), poseit->second);
return initValues;
}
@ -129,7 +126,7 @@ int main(int argc, char* argv[]) {
readAllData();
// Create a graph using the 2D measurements (features) and the calibration data
Graph graph(setupGraph(g_measurements, measurementSigma, g_calib));
visualSLAM::Graph graph(setupGraph(g_measurements, measurementSigma, g_calib));
// Create an initial Values structure using groundtruth values as the initial estimates
Values initialEstimates(initialize(g_landmarks, g_poses));
@ -139,7 +136,7 @@ int main(int argc, char* argv[]) {
// Add prior factor for all poses in the graph
map<int, Pose3>::iterator poseit = g_poses.begin();
for (; poseit != g_poses.end(); poseit++)
graph.addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));
graph.addPosePrior(Symbol('x',poseit->first), poseit->second, noiseModel::Unit::Create(1));
// Optimize the graph
cout << "*******************************************************" << endl;

5
examples/vSLAMexample/vSLAMutils.h
View File

@ -17,13 +17,12 @@
#pragma once
#include <map>
#include <vector>
#include "Feature2D.h"
#include "gtsam/geometry/Pose3.h"
#include "gtsam/geometry/Point3.h"
#include "gtsam/geometry/Cal3_S2.h"
#include <vector>
#include <map>
std::map<int, gtsam::Point3> readLandMarks(const std::string& landmarkFile);

4
gtsam/nonlinear/ISAM2.h
View File

@ -94,8 +94,8 @@ struct ISAM2Params {
* entries would be added with:
* \code
FastMap<char,Vector> thresholds;
thresholds[PoseKey::chr()] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
thresholds[PointKey::chr()] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
thresholds['x'] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
params.relinearizeThreshold = thresholds;
\endcode
*/

8
gtsam/slam/dataset.cpp
View File

@ -95,7 +95,7 @@ pair<sharedPose2Graph, Values::shared_ptr> load2D(const string& filename,
is >> id >> x >> y >> yaw;
// optional filter
if (maxID && id >= maxID) continue;
poses->insert(pose2SLAM::PoseKey(id), Pose2(x, y, yaw));
poses->insert(id, Pose2(x, y, yaw));
}
is.ignore(LINESIZE, '\n');
}
@ -132,10 +132,10 @@ pair<sharedPose2Graph, Values::shared_ptr> load2D(const string& filename,
l1Xl2 = l1Xl2.retract((*model)->sample());
// Insert vertices if pure odometry file
if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2());
if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at<Pose2>(pose2SLAM::PoseKey(id1)) * l1Xl2);
if (!poses->exists(id1)) poses->insert(id1, Pose2());
if (!poses->exists(id2)) poses->insert(id2, poses->at<Pose2>(id1) * l1Xl2);
pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(pose2SLAM::PoseKey(id1), pose2SLAM::PoseKey(id2), l1Xl2, *model));
pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model));
graph->push_back(factor);
}
is.ignore(LINESIZE, '\n');

24
gtsam/slam/planarSLAM.cpp
View File

@ -26,39 +26,39 @@ namespace planarSLAM {
NonlinearFactorGraph(graph) {}
/* ************************************************************************* */
void Graph::addPrior(Index i, const Pose2& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(PoseKey(i), p, model));
void Graph::addPrior(Key i, const Pose2& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Index i, const Pose2& p) {
sharedFactor factor(new Constraint(PoseKey(i), p));
void Graph::addPoseConstraint(Key i, const Pose2& p) {
sharedFactor factor(new Constraint(i, p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), odometry, model));
void Graph::addOdometry(Key i1, Key i2, const Pose2& odometry, const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i1, i2, odometry, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addBearing(Index i, Index j, const Rot2& z, const SharedNoiseModel& model) {
sharedFactor factor(new Bearing(PoseKey(i), PointKey(j), z, model));
void Graph::addBearing(Key i, Key j, const Rot2& z, const SharedNoiseModel& model) {
sharedFactor factor(new Bearing(i, j, z, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addRange(Index i, Index j, double z, const SharedNoiseModel& model) {
sharedFactor factor(new Range(PoseKey(i), PointKey(j), z, model));
void Graph::addRange(Key i, Key j, double z, const SharedNoiseModel& model) {
sharedFactor factor(new Range(i, j, z, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addBearingRange(Index i, Index j, const Rot2& z1,
void Graph::addBearingRange(Key i, Key j, const Rot2& z1,
double z2, const SharedNoiseModel& model) {
sharedFactor factor(new BearingRange(PoseKey(i), PointKey(j), z1, z2, model));
sharedFactor factor(new BearingRange(i, j, z1, z2, model));
push_back(factor);
}

56
gtsam/slam/planarSLAM.h
View File

@ -11,7 +11,7 @@
/**
* @file planarSLAM.h
* @brief: bearing/range measurements in 2D plane
* @brief bearing/range measurements in 2D plane
* @author Frank Dellaert
*/
@ -31,29 +31,25 @@ namespace planarSLAM {
using namespace gtsam;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a pose key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/*
* List of typedefs for factors
*/
/// A hard constraint for PoseKeys to enforce particular values
/// A hard constraint for poses to enforce particular values
typedef NonlinearEquality<Pose2> Constraint;
/// A prior factor to bias the value of a PoseKey
/// A prior factor to bias the value of a pose
typedef PriorFactor<Pose2> Prior;
/// A factor between two PoseKeys set with a Pose2
/// A factor between two poses set with a Pose2
typedef BetweenFactor<Pose2> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
/// A factor between a pose and a point to express difference in rotation (set with a Rot2)
typedef BearingFactor<Pose2, Point2> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
/// A factor between a pose and a point to express distance between them (set with a double)
typedef RangeFactor<Pose2, Point2> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location
/// A factor between a pose and a point to express difference in rotation and location
typedef BearingRangeFactor<Pose2, Point2> BearingRange;
/** Values class, using specific PoseKeys and PointKeys
/**
* Values class, using specific poses and points
* Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods
*/
struct Values: public gtsam::Values {
@ -67,16 +63,16 @@ namespace planarSLAM {
}
/// get a pose
Pose2 pose(Index key) const { return at<Pose2>(PoseKey(key)); }
Pose2 pose(Key i) const { return at<Pose2>(i); }
/// get a point
Point2 point(Index key) const { return at<Point2>(PointKey(key)); }
Point2 point(Key j) const { return at<Point2>(j); }
/// insert a pose
void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); }
void insertPose(Key i, const Pose2& pose) { insert(i, pose); }
/// insert a point
void insertPoint(Index key, const Point2& point) { insert(PointKey(key), point); }
void insertPoint(Key j, const Point2& point) { insert(j, point); }
};
/// Creates a NonlinearFactorGraph with the Values type
@ -88,23 +84,23 @@ namespace planarSLAM {
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph);
/// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(Index poseKey, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Biases the value of pose key with Pose2 p given a noise model
void addPrior(Key i, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
void addPoseConstraint(Index poseKey, const Pose2& pose);
/// Creates a hard constraint on the ith pose
void addPoseConstraint(Key i, const Pose2& pose);
/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
void addOdometry(Index poseKey1, Index poseKey2, const Pose2& odometry, const SharedNoiseModel& model);
/// Creates an odometry factor between poses with keys i1 and i2
void addOdometry(Key i1, Key i2, const Pose2& odometry, const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
void addBearing(Index poseKey, Index pointKey, const Rot2& bearing, const SharedNoiseModel& model);
/// Creates a bearing measurement from pose i to point j
void addBearing(Key i, Key j, const Rot2& bearing, const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
void addRange(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model);
/// Creates a range measurement from pose i to point j
void addRange(Key i, Key k, double range, const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
void addBearingRange(Index poseKey, Index pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Creates a range/bearing measurement from pose i to point j
void addBearingRange(Key i, Key j, const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Optimize
Values optimize(const Values& initialEstimate) const;

15
gtsam/slam/pose2SLAM.cpp
View File

@ -27,27 +27,26 @@ namespace pose2SLAM {
Values x;
double theta = 0, dtheta = 2 * M_PI / n;
for (size_t i = 0; i < n; i++, theta += dtheta)
x.insert(PoseKey(i), Pose2(cos(theta), sin(theta), M_PI_2 + theta));
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
return x;
}
/* ************************************************************************* */
void Graph::addPrior(Index i, const Pose2& p,
const SharedNoiseModel& model) {
sharedFactor factor(new Prior(PoseKey(i), p, model));
void Graph::addPrior(Key i, const Pose2& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Index i, const Pose2& p) {
sharedFactor factor(new HardConstraint(PoseKey(i), p));
void Graph::addPoseConstraint(Key i, const Pose2& p) {
sharedFactor factor(new HardConstraint(i, p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addOdometry(Index i, Index j, const Pose2& z,
void Graph::addOdometry(Key i1, Key i2, const Pose2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), z, model));
sharedFactor factor(new Odometry(i1, i2, z, model));
push_back(factor);
}

25
gtsam/slam/pose2SLAM.h
View File

@ -31,10 +31,7 @@ namespace pose2SLAM {
using namespace gtsam;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper
/// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
struct Values: public gtsam::Values {
/// Default constructor
@ -48,10 +45,10 @@ namespace pose2SLAM {
// Convenience for MATLAB wrapper, which does not allow for identically named methods
/// get a pose
Pose2 pose(Index key) const { return at<Pose2>(PoseKey(key)); }
Pose2 pose(Key i) const { return at<Pose2>(i); }
/// insert a pose
void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); }
void insertPose(Key i, const Pose2& pose) { insert(i, pose); }
};
/**
@ -83,20 +80,18 @@ namespace pose2SLAM {
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph);
/// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph
void addPrior(Index i, const Pose2& p, const SharedNoiseModel& model);
/// Adds a Pose2 prior with mean p and given noise model on pose i
void addPrior(Key i, const Pose2& p, const SharedNoiseModel& model);
/// Creates a hard constraint for key i with the given Pose2 p.
void addPoseConstraint(Index i, const Pose2& p);
void addPoseConstraint(Key i, const Pose2& p);
/// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph
void addOdometry(Index i, Index j, const Pose2& z,
const SharedNoiseModel& model);
/// Creates an odometry factor between poses with keys i1 and i2
void addOdometry(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model);
/// AddConstraint adds a soft constraint between factor between keys i and j
void addConstraint(Index i, Index j, const Pose2& z,
const SharedNoiseModel& model) {
addOdometry(i,j,z,model); // same for now
void addConstraint(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model) {
addOdometry(i1,i2,z,model); // same for now
}
/// Optimize

14
gtsam/slam/pose3SLAM.cpp
View File

@ -36,26 +36,26 @@ namespace pose3SLAM {
Point3 gti(radius*cos(theta), radius*sin(theta), 0);
Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down !
Pose3 gTi(gR0 * _0Ri, gti);
x.insert(PoseKey(i), gTi);
x.insert(i, gTi);
}
return x;
}
/* ************************************************************************* */
void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(PoseKey(i), p, model));
void Graph::addPrior(Key i, const Pose3& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) {
sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model));
void Graph::addConstraint(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model) {
sharedFactor factor(new Constraint(i1, i2, z, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addHardConstraint(Index i, const Pose3& p) {
sharedFactor factor(new HardConstraint(PoseKey(i), p));
void Graph::addHardConstraint(Key i, const Pose3& p) {
sharedFactor factor(new HardConstraint(i, p));
push_back(factor);
}

9
gtsam/slam/pose3SLAM.h
View File

@ -30,9 +30,6 @@ namespace pose3SLAM {
using namespace gtsam;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses
@ -52,13 +49,13 @@ namespace pose3SLAM {
struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type
void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model);
void addPrior(Key i, const Pose3& p, const SharedNoiseModel& model);
/// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph
void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model);
void addConstraint(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model);
/// Creates a hard constraint for key i with the given Pose3 p.
void addHardConstraint(Index i, const Pose3& p);
void addHardConstraint(Key i, const Pose3& p);
/// Optimize
Values optimize(const Values& initialEstimate) {

30
gtsam/slam/simulated2D.h
View File

@ -30,12 +30,6 @@ namespace simulated2D {
// Simulated2D robots have no orientation, just a position
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a landmark key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/**
* Custom Values class that holds poses and points, mainly used as a convenience for MATLAB wrapper
*/
@ -57,14 +51,14 @@ namespace simulated2D {
}
/// Insert a pose
void insertPose(Index j, const Point2& p) {
insert(PoseKey(j), p);
void insertPose(Key i, const Point2& p) {
insert(i, p);
nrPoses_++;
}
/// Insert a point
void insertPoint(Index j, const Point2& p) {
insert(PointKey(j), p);
void insertPoint(Key j, const Point2& p) {
insert(j, p);
nrPoints_++;
}
@ -79,13 +73,13 @@ namespace simulated2D {
}
/// Return pose i
Point2 pose(Index j) const {
return at<Point2>(PoseKey(j));
Point2 pose(Key i) const {
return at<Point2>(i);
}
/// Return point j
Point2 point(Index j) const {
return at<Point2>(PointKey(j));
Point2 point(Key j) const {
return at<Point2>(j);
}
};
@ -171,8 +165,8 @@ namespace simulated2D {
Pose measured_; ///< odometry measurement
/// Create odometry
GenericOdometry(const Pose& measured, const SharedNoiseModel& model, Key key1, Key key2) :
Base(model, key1, key2), measured_(measured) {
GenericOdometry(const Pose& measured, const SharedNoiseModel& model, Key i1, Key i2) :
Base(model, i1, i2), measured_(measured) {
}
/// Evaluate error and optionally return derivatives
@ -215,8 +209,8 @@ namespace simulated2D {
Landmark measured_; ///< Measurement
/// Create measurement factor
GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey) :
Base(model, poseKey, landmarkKey), measured_(measured) {
GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, Key i, Key j) :
Base(model, i, j), measured_(measured) {
}
/// Evaluate error and optionally return derivatives

25
gtsam/slam/simulated2DOriented.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -27,12 +28,6 @@ namespace simulated2DOriented {
using namespace gtsam;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a landmark key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/// Specialized Values structure with syntactic sugar for
/// compatibility with matlab
class Values: public gtsam::Values {
@ -40,21 +35,23 @@ namespace simulated2DOriented {
public:
Values() : nrPoses_(0), nrPoints_(0) {}
void insertPose(Index j, const Pose2& p) {
insert(PoseKey(j), p);
/// insert a pose
void insertPose(Key i, const Pose2& p) {
insert(i, p);
nrPoses_++;
}
void insertPoint(Index j, const Point2& p) {
insert(PointKey(j), p);
/// insert a landmark
void insertPoint(Key j, const Point2& p) {
insert(j, p);
nrPoints_++;
}
int nrPoses() const { return nrPoses_; }
int nrPoints() const { return nrPoints_; }
int nrPoses() const { return nrPoses_; } ///< nr of poses
int nrPoints() const { return nrPoints_; } ///< nr of landmarks
Pose2 pose(Index j) const { return at<Pose2>(PoseKey(j)); }
Point2 point(Index j) const { return at<Point2>(PointKey(j)); }
Pose2 pose(Key i) const { return at<Pose2>(i); } ///< get a pose
Point2 point(Key j) const { return at<Point2>(j); } ///< get a landmark
};
//TODO:: point prior is not implemented right now

59
gtsam/slam/smallExample.cpp
View File

@ -17,28 +17,24 @@
* @author Frank dellaert
*/
#include <iostream>
#include <string>
#include <gtsam/slam/smallExample.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/base/Matrix.h>
#include <boost/optional.hpp>
#include <boost/shared_ptr.hpp>
#include <iostream>
#include <string>
using namespace std;
#include <gtsam/base/Matrix.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/slam/smallExample.h>
// template definitions
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
namespace gtsam {
namespace example {
using simulated2D::PoseKey;
using simulated2D::PointKey;
typedef boost::shared_ptr<NonlinearFactor> shared;
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
@ -49,6 +45,7 @@ namespace example {
static const Index _l1_=0, _x1_=1, _x2_=2;
static const Index _x_=0, _y_=1, _z_=2;
// Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); }
Key kl(size_t i) { return Symbol('l',i); }
@ -60,22 +57,22 @@ namespace example {
// prior on x1
Point2 mu;
shared f1(new simulated2D::Prior(mu, sigma0_1, PoseKey(1)));
shared f1(new simulated2D::Prior(mu, sigma0_1, kx(1)));
nlfg->push_back(f1);
// odometry between x1 and x2
Point2 z2(1.5, 0);
shared f2(new simulated2D::Odometry(z2, sigma0_1, PoseKey(1), PoseKey(2)));
shared f2(new simulated2D::Odometry(z2, sigma0_1, kx(1), kx(2)));
nlfg->push_back(f2);
// measurement between x1 and l1
Point2 z3(0, -1);
shared f3(new simulated2D::Measurement(z3, sigma0_2, PoseKey(1), PointKey(1)));
shared f3(new simulated2D::Measurement(z3, sigma0_2, kx(1), kl(1)));
nlfg->push_back(f3);
// measurement between x2 and l1
Point2 z4(-1.5, -1.);
shared f4(new simulated2D::Measurement(z4, sigma0_2, PoseKey(2), PointKey(1)));
shared f4(new simulated2D::Measurement(z4, sigma0_2, kx(2), kl(1)));
nlfg->push_back(f4);
return nlfg;
@ -89,9 +86,9 @@ namespace example {
/* ************************************************************************* */
Values createValues() {
Values c;
c.insert(PoseKey(1), Point2(0.0, 0.0));
c.insert(PoseKey(2), Point2(1.5, 0.0));
c.insert(PointKey(1), Point2(0.0, -1.0));
c.insert(kx(1), Point2(0.0, 0.0));
c.insert(kx(2), Point2(1.5, 0.0));
c.insert(kl(1), Point2(0.0, -1.0));
return c;
}
@ -107,9 +104,9 @@ namespace example {
/* ************************************************************************* */
boost::shared_ptr<const Values> sharedNoisyValues() {
boost::shared_ptr<Values> c(new Values);
c->insert(PoseKey(1), Point2(0.1, 0.1));
c->insert(PoseKey(2), Point2(1.4, 0.2));
c->insert(PointKey(1), Point2(0.1, -1.1));
c->insert(kx(1), Point2(0.1, 0.1));
c->insert(kx(2), Point2(1.4, 0.2));
c->insert(kl(1), Point2(0.1, -1.1));
return c;
}
@ -223,7 +220,7 @@ namespace example {
Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor(
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), PoseKey(1)));
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), kx(1)));
fg->push_back(factor);
return fg;
}
@ -241,23 +238,23 @@ namespace example {
// prior on x1
Point2 x1(1.0, 0.0);
shared prior(new simulated2D::Prior(x1, sigma1_0, PoseKey(1)));
shared prior(new simulated2D::Prior(x1, sigma1_0, kx(1)));
nlfg.push_back(prior);
poses.insert(simulated2D::PoseKey(1), x1);
poses.insert(kx(1), x1);
for (int t = 2; t <= T; t++) {
// odometry between x_t and x_{t-1}
Point2 odo(1.0, 0.0);
shared odometry(new simulated2D::Odometry(odo, sigma1_0, PoseKey(t - 1), PoseKey(t)));
shared odometry(new simulated2D::Odometry(odo, sigma1_0, kx(t - 1), kx(t)));
nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS
Point2 xt(t, 0);
shared measurement(new simulated2D::Prior(xt, sigma1_0, PoseKey(t)));
shared measurement(new simulated2D::Prior(xt, sigma1_0, kx(t)));
nlfg.push_back(measurement);
// initial estimate
poses.insert(PoseKey(t), xt);
poses.insert(kx(t), xt);
}
return make_pair(nlfg, poses);
@ -418,7 +415,7 @@ namespace example {
/* ************************************************************************* */
// Create key for simulated planar graph
Symbol key(int x, int y) {
return PoseKey(1000*x+y);
return kx(1000*x+y);
}
/* ************************************************************************* */

18
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -27,8 +27,6 @@
using namespace std;
using namespace gtsam;
using pose3SLAM::PoseKey;
/* ************************************************************************* */
TEST( AntiFactor, NegativeHessian)
{
@ -42,17 +40,17 @@ TEST( AntiFactor, NegativeHessian)
// Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values());
values->insert(PoseKey(1), pose1);
values->insert(PoseKey(2), pose2);
values->insert(1, pose1);
values->insert(2, pose2);
// Define an elimination ordering
Ordering::shared_ptr ordering(new Ordering());
ordering->insert(PoseKey(1), 0);
ordering->insert(PoseKey(2), 1);
ordering->insert(1, 0);
ordering->insert(2, 1);
// Create a "standard" factor
BetweenFactor<Pose3>::shared_ptr originalFactor(new BetweenFactor<Pose3>(PoseKey(1), PoseKey(2), z, sigma));
BetweenFactor<Pose3>::shared_ptr originalFactor(new BetweenFactor<Pose3>(1, 2, z, sigma));
// Linearize it into a Jacobian Factor
GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering);
@ -103,8 +101,8 @@ TEST( AntiFactor, EquivalentBayesNet)
// Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values());
values->insert(PoseKey(1), pose1);
values->insert(PoseKey(2), pose2);
values->insert(1, pose1);
values->insert(2, pose2);
// Define an elimination ordering
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
@ -117,7 +115,7 @@ TEST( AntiFactor, EquivalentBayesNet)
VectorValues expectedDeltas = optimize(*expectedBayesNet);
// Add an additional factor between Pose1 and Pose2
pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(PoseKey(1), PoseKey(2), z, sigma));
pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(1, 2, z, sigma));
graph->push_back(f1);
// Add the corresponding AntiFactor between Pose1 and Pose2

49
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -28,6 +28,7 @@ using namespace planarSLAM;
// some shared test values
static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
static Symbol i2('x',2), i3('x',3), j3('l',3);
SharedNoiseModel
sigma(noiseModel::Isotropic::Sigma(1,0.1)),
@ -37,7 +38,7 @@ SharedNoiseModel
/* ************************************************************************* */
TEST( planarSLAM, PriorFactor_equals )
{
planarSLAM::Prior factor1(PoseKey(2), x1, I3), factor2(PoseKey(2), x2, I3);
planarSLAM::Prior factor1(i2, x1, I3), factor2(i2, x2, I3);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -48,12 +49,12 @@ TEST( planarSLAM, BearingFactor )
{
// Create factor
Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
planarSLAM::Bearing factor(PoseKey(2), PointKey(3), z, sigma);
planarSLAM::Bearing factor(i2, j3, z, sigma);
// create config
planarSLAM::Values c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
c.insert(i2, x2);
c.insert(j3, l3);
// Check error
Vector actual = factor.unwhitenedError(c);
@ -64,8 +65,8 @@ TEST( planarSLAM, BearingFactor )
TEST( planarSLAM, BearingFactor_equals )
{
planarSLAM::Bearing
factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), sigma),
factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(2.3), sigma);
factor1(i2, j3, Rot2::fromAngle(0.1), sigma),
factor2(i2, j3, Rot2::fromAngle(2.3), sigma);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -76,12 +77,12 @@ TEST( planarSLAM, RangeFactor )
{
// Create factor
double z(sqrt(2) - 0.22); // h(x) - z = 0.22
planarSLAM::Range factor(PoseKey(2), PointKey(3), z, sigma);
planarSLAM::Range factor(i2, j3, z, sigma);
// create config
planarSLAM::Values c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
c.insert(i2, x2);
c.insert(j3, l3);
// Check error
Vector actual = factor.unwhitenedError(c);
@ -91,7 +92,7 @@ TEST( planarSLAM, RangeFactor )
/* ************************************************************************* */
TEST( planarSLAM, RangeFactor_equals )
{
planarSLAM::Range factor1(PoseKey(2), PointKey(3), 1.2, sigma), factor2(PoseKey(2), PointKey(3), 7.2, sigma);
planarSLAM::Range factor1(i2, j3, 1.2, sigma), factor2(i2, j3, 7.2, sigma);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -103,12 +104,12 @@ TEST( planarSLAM, BearingRangeFactor )
// Create factor
Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
double b(sqrt(2) - 0.22); // h(x) - z = 0.22
planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2);
planarSLAM::BearingRange factor(i2, j3, r, b, sigma2);
// create config
planarSLAM::Values c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
c.insert(i2, x2);
c.insert(j3, l3);
// Check error
Vector actual = factor.unwhitenedError(c);
@ -119,8 +120,8 @@ TEST( planarSLAM, BearingRangeFactor )
TEST( planarSLAM, BearingRangeFactor_equals )
{
planarSLAM::BearingRange
factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), 7.3, sigma2),
factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(3), 2.0, sigma2);
factor1(i2, j3, Rot2::fromAngle(0.1), 7.3, sigma2),
factor2(i2, j3, Rot2::fromAngle(3), 2.0, sigma2);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -130,13 +131,13 @@ TEST( planarSLAM, BearingRangeFactor_equals )
TEST( planarSLAM, BearingRangeFactor_poses )
{
typedef BearingRangeFactor<Pose2,Pose2> PoseBearingRange;
PoseBearingRange actual(PoseKey(2), PoseKey(3), Rot2::fromDegrees(60.0), 12.3, sigma2);
PoseBearingRange actual(2, 3, Rot2::fromDegrees(60.0), 12.3, sigma2);
}
/* ************************************************************************* */
TEST( planarSLAM, PoseConstraint_equals )
{
planarSLAM::Constraint factor1(PoseKey(2), x2), factor2(PoseKey(2), x3);
planarSLAM::Constraint factor1(i2, x2), factor2(i2, x3);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -147,26 +148,26 @@ TEST( planarSLAM, constructor )
{
// create config
planarSLAM::Values c;
c.insert(PoseKey(2), x2);
c.insert(PoseKey(3), x3);
c.insert(PointKey(3), l3);
c.insert(i2, x2);
c.insert(i3, x3);
c.insert(j3, l3);
// create graph
planarSLAM::Graph G;
// Add pose constraint
G.addPoseConstraint(2, x2); // make it feasible :-)
G.addPoseConstraint(i2, x2); // make it feasible :-)
// Add odometry
G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), I3);
G.addOdometry(i2, i3, Pose2(0, 0, M_PI_4), I3);
// Create bearing factor
Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
G.addBearing(2, 3, z1, sigma);
G.addBearing(i2, j3, z1, sigma);
// Create range factor
double z2(sqrt(2) - 0.22); // h(x) - z = 0.22
G.addRange(2, 3, z2, sigma);
G.addRange(i2, j3, z2, sigma);
Vector expected0 = Vector_(3, 0.0, 0.0, 0.0);
Vector expected1 = Vector_(3, 0.0, 0.0, 0.0);

133
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -32,7 +32,6 @@ using namespace boost::assign;
using namespace std;
typedef pose2SLAM::Odometry Pose2Factor;
using pose2SLAM::PoseKey;
// common measurement covariance
static double sx=0.5, sy=0.5,st=0.1;
@ -44,21 +43,19 @@ static Matrix cov(Matrix_(3, 3,
static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Gaussian::Covariance(cov));
//static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5), kl1 = Symbol('l',1);
/* ************************************************************************* */
// Test constraint, small displacement
Vector f1(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2.1130913087, 0.468461064817, 0.436332312999);
Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance);
Pose2Factor constraint(1, 2, z, covariance);
return constraint.evaluateError(pose1, pose2);
}
TEST( Pose2SLAM, constraint1 )
TEST_UNSAFE( Pose2SLAM, constraint1 )
{
// create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999);
Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance);
Pose2Factor constraint(1, 2, pose2, covariance);
Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
@ -73,15 +70,15 @@ TEST( Pose2SLAM, constraint1 )
// Test constraint, large displacement
Vector f2(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2,2,M_PI_2);
Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance);
Pose2Factor constraint(1, 2, z, covariance);
return constraint.evaluateError(pose1, pose2);
}
TEST( Pose2SLAM, constraint2 )
TEST_UNSAFE( Pose2SLAM, constraint2 )
{
// create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2,2,M_PI_2);
Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance);
Pose2Factor constraint(1, 2, pose2, covariance);
Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
@ -93,7 +90,7 @@ TEST( Pose2SLAM, constraint2 )
}
/* ************************************************************************* */
TEST( Pose2SLAM, constructor )
TEST_UNSAFE( Pose2SLAM, constructor )
{
// create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2);
@ -107,11 +104,11 @@ TEST( Pose2SLAM, constructor )
}
/* ************************************************************************* */
TEST( Pose2SLAM, linearization )
TEST_UNSAFE( Pose2SLAM, linearization )
{
// create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2);
Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance);
Pose2Factor constraint(1, 2, measured, covariance);
pose2SLAM::Graph graph;
graph.addOdometry(1,2,measured, covariance);
@ -119,8 +116,8 @@ TEST( Pose2SLAM, linearization )
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2)
pose2SLAM::Values config;
config.insert(pose2SLAM::PoseKey(1),p1);
config.insert(pose2SLAM::PoseKey(2),p2);
config.insert(1,p1);
config.insert(2,p2);
// Linearize
Ordering ordering(*config.orderingArbitrary());
boost::shared_ptr<FactorGraph<GaussianFactor> > lfg_linearized = graph.linearize(config, ordering);
@ -140,13 +137,13 @@ TEST( Pose2SLAM, linearization )
Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0);
SharedDiagonal probModel1 = noiseModel::Unit::Create(3);
lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering[kx1], A1, ordering[kx2], A2, b, probModel1)));
lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering[1], A1, ordering[2], A2, b, probModel1)));
CHECK(assert_equal(lfg_expected, *lfg_linearized));
}
/* ************************************************************************* */
TEST(Pose2SLAM, optimize) {
TEST_UNSAFE(Pose2SLAM, optimize) {
// create a Pose graph with one equality constraint and one measurement
pose2SLAM::Graph fg;
@ -155,12 +152,12 @@ TEST(Pose2SLAM, optimize) {
// Create initial config
Values initial;
initial.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
initial.insert(pose2SLAM::PoseKey(1), Pose2(0,0,0));
initial.insert(0, Pose2(0,0,0));
initial.insert(1, Pose2(0,0,0));
// Choose an ordering and optimize
Ordering ordering;
ordering += kx0, kx1;
ordering += 0, 1;
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
@ -169,23 +166,23 @@ TEST(Pose2SLAM, optimize) {
// Check with expected config
Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2));
expected.insert(0, Pose2(0,0,0));
expected.insert(1, Pose2(1,2,M_PI_2));
CHECK(assert_equal(expected, actual));
// Check marginals
Marginals marginals = fg.marginals(actual);
// Matrix expectedP0 = Infinity, as we have a pose constraint !?
// Matrix actualP0 = marginals.marginalCovariance(pose2SLAM::PoseKey(0));
// Matrix actualP0 = marginals.marginalCovariance(0);
// EQUALITY(expectedP0, actualP0);
Matrix expectedP1 = cov; // the second pose really should have just the noise covariance
Matrix actualP1 = marginals.marginalCovariance(pose2SLAM::PoseKey(1));
Matrix actualP1 = marginals.marginalCovariance(1);
EQUALITY(expectedP1, actualP1);
}
/* ************************************************************************* */
// test optimization with 3 poses
TEST(Pose2SLAM, optimizeThreePoses) {
// test optimization with 3 poses, note we use plain Keys here, not symbols
TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) {
// Create a hexagon of poses
pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0);
@ -207,7 +204,7 @@ TEST(Pose2SLAM, optimizeThreePoses) {
// Choose an ordering
Ordering ordering;
ordering += kx0,kx1,kx2;
ordering += 0,1,2;
// optimize
LevenbergMarquardtParams params;
@ -231,12 +228,12 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
pose2SLAM::Graph fg;
fg.addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1);
fg.addOdometry(0, 1, delta, covariance);
fg.addOdometry(0,1, delta, covariance);
fg.addOdometry(1,2, delta, covariance);
fg.addOdometry(2,3, delta, covariance);
fg.addOdometry(3,4, delta, covariance);
fg.addOdometry(4,5, delta, covariance);
fg.addOdometry(5, 0, delta, covariance);
fg.addOdometry(5,0, delta, covariance);
// Create initial config
pose2SLAM::Values initial;
@ -249,7 +246,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
// Choose an ordering
Ordering ordering;
ordering += kx0,kx1,kx2,kx3,kx4,kx5;
ordering += 0,1,2,3,4,5;
// optimize
LevenbergMarquardtParams params;
@ -261,7 +258,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
CHECK(assert_equal((const Values&)hexagon, actual));
// Check loop closure
CHECK(assert_equal(delta, actual.at<Pose2>(PoseKey(5)).between(actual.at<Pose2>(PoseKey(0)))));
CHECK(assert_equal(delta, actual.at<Pose2>(5).between(actual.at<Pose2>(0))));
// Pose2SLAMOptimizer myOptimizer("3");
@ -299,7 +296,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
}
/* ************************************************************************* */
TEST(Pose2SLAM, optimize2) {
TEST_UNSAFE(Pose2SLAM, optimize2) {
// Pose2SLAMOptimizer myOptimizer("100");
// Matrix A1 = myOptimizer.a1();
// Matrix A2 = myOptimizer.a2();
@ -317,7 +314,7 @@ TEST(Pose2SLAM, optimize2) {
}
///* ************************************************************************* */
// SL-NEEDED? TEST(Pose2SLAM, findMinimumSpanningTree) {
// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, findMinimumSpanningTree) {
// pose2SLAM::Graph G, T, C;
// G.addConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
@ -331,7 +328,7 @@ TEST(Pose2SLAM, optimize2) {
//}
//
///* ************************************************************************* */
// SL-NEEDED? TEST(Pose2SLAM, split) {
// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, split) {
// pose2SLAM::Graph G, T, C;
// G.addConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
@ -350,37 +347,37 @@ TEST(Pose2SLAM, optimize2) {
using namespace pose2SLAM;
/* ************************************************************************* */
TEST(Pose2Values, pose2Circle )
TEST_UNSAFE(Pose2Values, pose2Circle )
{
// expected is 4 poses tangent to circle with radius 1m
pose2SLAM::Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2( 1, 0, M_PI_2));
expected.insert(pose2SLAM::PoseKey(1), Pose2( 0, 1, - M_PI ));
expected.insert(pose2SLAM::PoseKey(2), Pose2(-1, 0, - M_PI_2));
expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1, 0 ));
expected.insert(0, Pose2( 1, 0, M_PI_2));
expected.insert(1, Pose2( 0, 1, - M_PI ));
expected.insert(2, Pose2(-1, 0, - M_PI_2));
expected.insert(3, Pose2( 0, -1, 0 ));
pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST(Pose2SLAM, expmap )
TEST_UNSAFE(Pose2SLAM, expmap )
{
// expected is circle shifted to right
pose2SLAM::Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1, 0, M_PI_2));
expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1, 1, - M_PI ));
expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9, 0, - M_PI_2));
expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1, 0 ));
expected.insert(0, Pose2( 1.1, 0, M_PI_2));
expected.insert(1, Pose2( 0.1, 1, - M_PI ));
expected.insert(2, Pose2(-0.9, 0, - M_PI_2));
expected.insert(3, Pose2( 0.1, -1, 0 ));
// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
pose2SLAM::Values circle(pose2SLAM::circle(4,1.0));
Ordering ordering(*circle.orderingArbitrary());
VectorValues delta(circle.dims(ordering));
delta[ordering[pose2SLAM::PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0);
delta[ordering[pose2SLAM::PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0);
delta[ordering[pose2SLAM::PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0);
delta[ordering[pose2SLAM::PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0);
delta[ordering[0]] = Vector_(3, 0.0,-0.1,0.0);
delta[ordering[1]] = Vector_(3, -0.1,0.0,0.0);
delta[ordering[2]] = Vector_(3, 0.0,0.1,0.0);
delta[ordering[3]] = Vector_(3, 0.1,0.0,0.0);
pose2SLAM::Values actual = circle.retract(delta, ordering);
CHECK(assert_equal(expected,actual));
}
@ -390,15 +387,15 @@ static SharedNoiseModel sigmas = sharedSigmas(Vector_(3,sx,sy,st));
/* ************************************************************************* */
// Very simple test establishing Ax-b \approx z-h(x)
TEST( Pose2Prior, error )
TEST_UNSAFE( Pose2Prior, error )
{
// Choose a linearization point
Pose2 p1(1, 0, 0); // robot at (1,0)
pose2SLAM::Values x0;
x0.insert(PoseKey(1), p1);
x0.insert(1, p1);
// Create factor
pose2SLAM::Prior factor(PoseKey(1), p1, sigmas);
pose2SLAM::Prior factor(1, p1, sigmas);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -407,14 +404,14 @@ TEST( Pose2Prior, error )
// Check error at x0, i.e. delta = zero !
VectorValues delta(VectorValues::Zero(x0.dims(ordering)));
delta[ordering[kx1]] = zero(3);
delta[ordering[1]] = zero(3);
Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
CHECK(assert_equal(error_at_zero,factor.whitenedError(x0)));
CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
// Check error after increasing p2
VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
addition[ordering[kx1]] = Vector_(3, 0.1, 0.0, 0.0);
addition[ordering[1]] = Vector_(3, 0.1, 0.0, 0.0);
VectorValues plus = delta + addition;
pose2SLAM::Values x1 = x0.retract(plus, ordering);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
@ -425,7 +422,7 @@ TEST( Pose2Prior, error )
/* ************************************************************************* */
// common Pose2Prior for tests below
static gtsam::Pose2 priorVal(2,2,M_PI_2);
static pose2SLAM::Prior priorFactor(PoseKey(1), priorVal, sigmas);
static pose2SLAM::Prior priorFactor(1, priorVal, sigmas);
/* ************************************************************************* */
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
@ -435,7 +432,7 @@ LieVector hprior(const Pose2& p1) {
}
/* ************************************************************************* */
TEST( Pose2Prior, linearize )
TEST_UNSAFE( Pose2Prior, linearize )
{
// Choose a linearization point at ground truth
pose2SLAM::Values x0;
@ -448,12 +445,12 @@ TEST( Pose2Prior, linearize )
// Test with numerical derivative
Matrix numericalH = numericalDerivative11(hprior, priorVal);
CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering[kx1]))));
CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering[1]))));
}
/* ************************************************************************* */
// Very simple test establishing Ax-b \approx z-h(x)
TEST( Pose2Factor, error )
TEST_UNSAFE( Pose2Factor, error )
{
// Choose a linearization point
Pose2 p1; // robot at origin
@ -464,7 +461,7 @@ TEST( Pose2Factor, error )
// Create factor
Pose2 z = p1.between(p2);
Pose2Factor factor(PoseKey(1), PoseKey(2), z, covariance);
Pose2Factor factor(1, 2, z, covariance);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -473,15 +470,15 @@ TEST( Pose2Factor, error )
// Check error at x0, i.e. delta = zero !
VectorValues delta(x0.dims(ordering));
delta[ordering[kx1]] = zero(3);
delta[ordering[kx2]] = zero(3);
delta[ordering[1]] = zero(3);
delta[ordering[2]] = zero(3);
Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0)));
CHECK(assert_equal(-error_at_zero, linear->error_vector(delta)));
// Check error after increasing p2
VectorValues plus = delta;
plus[ordering[kx2]] = Vector_(3, 0.1, 0.0, 0.0);
plus[ordering[2]] = Vector_(3, 0.1, 0.0, 0.0);
pose2SLAM::Values x1 = x0.retract(plus, ordering);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
@ -491,17 +488,17 @@ TEST( Pose2Factor, error )
/* ************************************************************************* */
// common Pose2Factor for tests below
static Pose2 measured(2,2,M_PI_2);
static Pose2Factor factor(PoseKey(1),PoseKey(2),measured, covariance);
static Pose2Factor factor(1,2,measured, covariance);
/* ************************************************************************* */
TEST( Pose2Factor, rhs )
TEST_UNSAFE( Pose2Factor, rhs )
{
// Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4)
pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1),p1);
x0.insert(pose2SLAM::PoseKey(2),p2);
x0.insert(1,p1);
x0.insert(2,p2);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -524,14 +521,14 @@ LieVector h(const Pose2& p1,const Pose2& p2) {
}
/* ************************************************************************* */
TEST( Pose2Factor, linearize )
TEST_UNSAFE( Pose2Factor, linearize )
{
// Choose a linearization point at ground truth
Pose2 p1(1,2,M_PI_2);
Pose2 p2(-1,4,M_PI);
pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1),p1);
x0.insert(pose2SLAM::PoseKey(2),p2);
x0.insert(1,p1);
x0.insert(2,p2);
// expected linearization
Matrix expectedH1 = covariance->Whiten(Matrix_(3,3,
@ -549,7 +546,7 @@ TEST( Pose2Factor, linearize )
// expected linear factor
Ordering ordering(*x0.orderingArbitrary());
SharedDiagonal probModel1 = noiseModel::Unit::Create(3);
JacobianFactor expected(ordering[kx1], expectedH1, ordering[kx2], expectedH2, expected_b, probModel1);
JacobianFactor expected(ordering[1], expectedH1, ordering[2], expectedH2, expected_b, probModel1);
// Actual linearization
boost::shared_ptr<JacobianFactor> actual =

80
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -10,39 +10,33 @@
* -------------------------------------------------------------------------- */
/**
* @file testPose3Graph.cpp
* @author Frank Dellaert, Viorela Ila
* @file testpose3SLAM.cpp
* @author Frank Dellaert
* @author Viorela Ila
**/
#include <iostream>
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/slam/PartialPriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost;
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// TODO: DANGEROUS, create shared pointers
#define GTSAM_MAGIC_GAUSSIAN 6
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/slam/PartialPriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace pose3SLAM;
// common measurement covariance
static Matrix covariance = eye(6);
static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Unit::Create(6));
const double tol=1e-5;
const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5);
/* ************************************************************************* */
// test optimization with 6 poses arranged in a hexagon and a loop closure
TEST(Pose3Graph, optimizeCircle) {
@ -50,7 +44,7 @@ TEST(Pose3Graph, optimizeCircle) {
// Create a hexagon of poses
double radius = 10;
Values hexagon = pose3SLAM::circle(6,radius);
Pose3 gT0 = hexagon.at<Pose3>(PoseKey(0)), gT1 = hexagon.at<Pose3>(PoseKey(1));
Pose3 gT0 = hexagon.at<Pose3>(0), gT1 = hexagon.at<Pose3>(1);
// create a Pose graph with one equality constraint and one measurement
pose3SLAM::Graph fg;
@ -67,16 +61,16 @@ TEST(Pose3Graph, optimizeCircle) {
// Create initial config
Values initial;
initial.insert(PoseKey(0), gT0);
initial.insert(PoseKey(1), hexagon.at<Pose3>(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial.insert(PoseKey(2), hexagon.at<Pose3>(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial.insert(PoseKey(3), hexagon.at<Pose3>(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial.insert(PoseKey(4), hexagon.at<Pose3>(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial.insert(PoseKey(5), hexagon.at<Pose3>(PoseKey(5)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial.insert(0, gT0);
initial.insert(1, hexagon.at<Pose3>(1).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial.insert(2, hexagon.at<Pose3>(2).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial.insert(3, hexagon.at<Pose3>(3).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial.insert(4, hexagon.at<Pose3>(4).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial.insert(5, hexagon.at<Pose3>(5).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
// Choose an ordering and optimize
Ordering ordering;
ordering += kx0,kx1,kx2,kx3,kx4,kx5;
ordering += 0,1,2,3,4,5;
Values actual = LevenbergMarquardtOptimizer(fg, initial, ordering).optimize();
@ -84,7 +78,7 @@ TEST(Pose3Graph, optimizeCircle) {
CHECK(assert_equal(hexagon, actual,1e-4));
// Check loop closure
CHECK(assert_equal(_0T1, actual.at<Pose3>(PoseKey(5)).between(actual.at<Pose3>(PoseKey(0))),1e-5));
CHECK(assert_equal(_0T1, actual.at<Pose3>(5).between(actual.at<Pose3>(0)),1e-5));
}
/* ************************************************************************* */
@ -94,11 +88,10 @@ TEST(Pose3Graph, partial_prior_height) {
// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
// height prior - single element interface
Symbol key = PoseKey(1);
double exp_height = 5.0;
SharedDiagonal model = noiseModel::Unit::Create(1);
Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height));
Partial height(key, 5, exp_height, model);
Partial height(1, 5, exp_height, model);
Matrix actA;
EXPECT(assert_equal(Vector_(1,-2.0), height.evaluateError(init, actA), tol));
Matrix expA = Matrix_(1, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
@ -108,13 +101,13 @@ TEST(Pose3Graph, partial_prior_height) {
graph.add(height);
Values values;
values.insert(key, init);
values.insert(1, init);
// linearization
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT(assert_equal(expected, actual.at<Pose3>(1), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
@ -128,12 +121,12 @@ TEST( Pose3Factor, error )
// Create factor
SharedNoiseModel I6(noiseModel::Unit::Create(6));
pose3SLAM::Constraint factor(PoseKey(1), PoseKey(2), z, I6);
pose3SLAM::Constraint factor(1, 2, z, I6);
// Create config
Values x;
x.insert(PoseKey(1),t1);
x.insert(PoseKey(2),t2);
x.insert(1,t1);
x.insert(2,t2);
// Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2))
Vector actual = factor.unwhitenedError(x);
@ -146,12 +139,11 @@ TEST(Pose3Graph, partial_prior_xy) {
typedef PartialPriorFactor<Pose3> Partial;
// XY prior - full mask interface
Symbol key = PoseKey(1);
Vector exp_xy = Vector_(2, 3.0, 4.0);
SharedDiagonal model = noiseModel::Unit::Create(2);
Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0));
vector<size_t> mask; mask += 3, 4;
Partial priorXY(key, mask, exp_xy, model);
Partial priorXY(1, mask, exp_xy, model);
Matrix actA;
EXPECT(assert_equal(Vector_(2,-2.0,-6.0), priorXY.evaluateError(init, actA), tol));
Matrix expA = Matrix_(2, 6,
@ -163,10 +155,10 @@ TEST(Pose3Graph, partial_prior_xy) {
graph.add(priorXY);
Values values;
values.insert(key, init);
values.insert(1, init);
Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT(assert_equal(expected, actual.at<Pose3>(1), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
@ -182,10 +174,10 @@ TEST( Values, pose3Circle )
{
// expected is 4 poses tangent to circle with radius 1m
Values expected;
expected.insert(PoseKey(0), Pose3(R1, Point3( 1, 0, 0)));
expected.insert(PoseKey(1), Pose3(R2, Point3( 0, 1, 0)));
expected.insert(PoseKey(2), Pose3(R3, Point3(-1, 0, 0)));
expected.insert(PoseKey(3), Pose3(R4, Point3( 0,-1, 0)));
expected.insert(0, Pose3(R1, Point3( 1, 0, 0)));
expected.insert(1, Pose3(R2, Point3( 0, 1, 0)));
expected.insert(2, Pose3(R3, Point3(-1, 0, 0)));
expected.insert(3, Pose3(R4, Point3( 0,-1, 0)));
Values actual = pose3SLAM::circle(4,1.0);
CHECK(assert_equal(expected,actual));
@ -195,10 +187,10 @@ TEST( Values, pose3Circle )
TEST( Values, expmap )
{
Values expected;
expected.insert(PoseKey(0), Pose3(R1, Point3( 1.0, 0.1, 0)));
expected.insert(PoseKey(1), Pose3(R2, Point3(-0.1, 1.0, 0)));
expected.insert(PoseKey(2), Pose3(R3, Point3(-1.0,-0.1, 0)));
expected.insert(PoseKey(3), Pose3(R4, Point3( 0.1,-1.0, 0)));
expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
Ordering ordering(*expected.orderingArbitrary());
VectorValues delta(expected.dims(ordering));

36
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -16,13 +16,11 @@
* @date Nov 2009
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/visualSLAM.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace visualSLAM;
// make cube
static Point3
@ -37,23 +35,23 @@ static Cal3_S2 K(fov,w,h);
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
static shared_ptrK sK(new Cal3_S2(K));
const Key kx1 = Symbol('x',1), kl1 = Symbol('l',1);
// make cameras
// Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); }
Key kl(size_t i) { return Symbol('l',i); }
/* ************************************************************************* */
TEST( ProjectionFactor, error )
{
// Create the factor with a measurement that is 3 pixels off in x
Point2 z(323.,240.);
int cameraFrameNumber=1, landmarkNumber=1;
int i=1, j=1;
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
factor(new visualSLAM::ProjectionFactor(z, sigma, kx(i), kl(j), sK));
// For the following values structure, the factor predicts 320,240
Values config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(PoseKey(1), x1);
Point3 l1; config.insert(PointKey(1), l1);
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(kx(1), x1);
Point3 l1; config.insert(kl(1), l1);
// Point should project to Point2(320.,240.)
CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config)));
@ -61,12 +59,12 @@ TEST( ProjectionFactor, error )
DOUBLES_EQUAL(4.5,factor->error(config),1e-9);
// Check linearize
Ordering ordering; ordering += kx1,kl1;
Ordering ordering; ordering += kx(1),kl(1);
Matrix Ax1 = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
Matrix Al1 = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.);
Vector b = Vector_(2,3.,0.);
SharedDiagonal probModel1 = noiseModel::Unit::Create(2);
JacobianFactor expected(ordering[kx1], Ax1, ordering[kl1], Al1, b, probModel1);
JacobianFactor expected(ordering[kx(1)], Ax1, ordering[kl(1)], Al1, b, probModel1);
JacobianFactor::shared_ptr actual =
boost::dynamic_pointer_cast<JacobianFactor>(factor->linearize(config, ordering));
CHECK(assert_equal(expected,*actual,1e-3));
@ -81,11 +79,11 @@ TEST( ProjectionFactor, error )
// expmap on a config
Values expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2);
Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2);
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(kx(1), x2);
Point3 l2(1,2,3); expected_config.insert(kl(1), l2);
VectorValues delta(expected_config.dims(ordering));
delta[ordering[kx1]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
delta[ordering[kl1]] = Vector_(3, 1.,2.,3.);
delta[ordering[kx(1)]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
delta[ordering[kl(1)]] = Vector_(3, 1.,2.,3.);
Values actual_config = config.retract(delta, ordering);
CHECK(assert_equal(expected_config,actual_config,1e-9));
}
@ -95,12 +93,12 @@ TEST( ProjectionFactor, equals )
{
// Create two identical factors and make sure they're equal
Vector z = Vector_(2,323.,240.);
int cameraFrameNumber=1, landmarkNumber=1;
int i=1, j=1;
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor1(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
factor1(new visualSLAM::ProjectionFactor(z, sigma, kx(i), kl(j), sK));
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor2(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
factor2(new visualSLAM::ProjectionFactor(z, sigma, kx(i), kl(j), sK));
CHECK(assert_equal(*factor1, *factor2));
}

37
gtsam/slam/tests/testSerializationSLAM.cpp
View File

@ -11,7 +11,7 @@
/**
* @file testSerializationSLAM.cpp
* @brief
* @brief test serialization
* @author Richard Roberts
* @date Feb 7, 2012
*/
@ -109,19 +109,20 @@ BOOST_CLASS_EXPORT(gtsam::Pose2)
TEST (Serialization, planar_system) {
using namespace planarSLAM;
planarSLAM::Values values;
values.insert(PointKey(3), Point2(1.0, 2.0));
values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
Symbol i2('x',2), i3('x',3), i4('x',4), i9('x',9), j3('l',3), j5('l',5), j9('l',9);
values.insert(j3, Point2(1.0, 2.0));
values.insert(i4, Pose2(1.0, 2.0, 0.3));
SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3);
SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1);
Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1);
Range range(PoseKey(2), PointKey(9), 7.0, model1);
BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2);
Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3);
Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2));
Prior prior(i3, Pose2(0.1,-0.3, 0.2), model1);
Bearing bearing(i3, j5, Rot2::fromDegrees(0.5), model1);
Range range(i2, j9, 7.0, model1);
BearingRange bearingRange(i2, j3, Rot2::fromDegrees(0.6), 2.0, model2);
Odometry odometry(i2, i3, Pose2(1.0, 2.0, 0.3), model3);
Constraint constraint(i9, Pose2(2.0,-1.0, 0.2));
Graph graph;
graph.add(prior);
@ -132,8 +133,8 @@ TEST (Serialization, planar_system) {
graph.add(constraint);
// text
EXPECT(equalsObj<Symbol>(PoseKey(2)));
EXPECT(equalsObj<Symbol>(PointKey(3)));
EXPECT(equalsObj<Symbol>(i2));
EXPECT(equalsObj<Symbol>(j3));
EXPECT(equalsObj<planarSLAM::Values>(values));
EXPECT(equalsObj<Prior>(prior));
EXPECT(equalsObj<Bearing>(bearing));
@ -144,8 +145,8 @@ TEST (Serialization, planar_system) {
EXPECT(equalsObj<Graph>(graph));
// xml
EXPECT(equalsXML<Symbol>(PoseKey(2)));
EXPECT(equalsXML<Symbol>(PointKey(3)));
EXPECT(equalsXML<Symbol>(i2));
EXPECT(equalsXML<Symbol>(j3));
EXPECT(equalsXML<planarSLAM::Values>(values));
EXPECT(equalsXML<Prior>(prior));
EXPECT(equalsXML<Bearing>(bearing));
@ -189,11 +190,11 @@ TEST (Serialization, visual_system) {
boost::shared_ptr<Cal3_S2> K(new Cal3_S2(cal1));
Graph graph;
graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K);
graph.addPointConstraint(1, pt1);
graph.addPointPrior(1, pt2, model3);
graph.addPoseConstraint(1, pose1);
graph.addPosePrior(1, pose2, model6);
graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K);
graph.addPointConstraint(l1, pt1);
graph.addPointPrior(l1, pt2, model3);
graph.addPoseConstraint(x1, pose1);
graph.addPosePrior(x1, pose2, model6);
EXPECT(equalsObj(values));
EXPECT(equalsObj(graph));

4
gtsam/slam/tests/testSimulated2D.cpp
View File

@ -31,8 +31,8 @@ using namespace simulated2D;
TEST( simulated2D, Simulated2DValues )
{
simulated2D::Values actual;
actual.insert(simulated2D::PoseKey(1),Point2(1,1));
actual.insert(simulated2D::PointKey(2),Point2(2,2));
actual.insert(1,Point2(1,1));
actual.insert(2,Point2(2,2));
EXPECT(assert_equal(actual,actual,1e-9));
}

10
gtsam/slam/tests/testSimulated2DOriented.cpp
View File

@ -28,6 +28,10 @@ using namespace std;
using namespace gtsam;
using namespace simulated2DOriented;
// Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); }
Key kl(size_t i) { return Symbol('l',i); }
/* ************************************************************************* */
TEST( simulated2DOriented, Dprior )
{
@ -55,11 +59,11 @@ TEST( simulated2DOriented, constructor )
{
Pose2 measurement(0.2, 0.3, 0.1);
SharedDiagonal model(Vector_(3, 1., 1., 1.));
simulated2DOriented::Odometry factor(measurement, model, simulated2DOriented::PoseKey(1), simulated2DOriented::PoseKey(2));
simulated2DOriented::Odometry factor(measurement, model, kx(1), kx(2));
simulated2DOriented::Values config;
config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2));
config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1));
config.insert(kx(1), Pose2(1., 0., 0.2));
config.insert(kx(2), Pose2(2., 0., 0.1));
boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());
}

26
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -12,24 +12,20 @@
/**
* @file testStereoFactor.cpp
* @brief Unit test for StereoFactor
* single camera
* @author Chris Beall
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/visualSLAM.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace boost;
using namespace visualSLAM;
Pose3 camera1(Matrix_(3,3,
1., 0., 0.,
@ -45,6 +41,10 @@ StereoCamera stereoCam(Pose3(), K);
Point3 p(0, 0, 5);
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
// Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); }
Key kl(size_t i) { return Symbol('l',i); }
/* ************************************************************************* */
TEST( StereoFactor, singlePoint)
{
@ -52,18 +52,18 @@ TEST( StereoFactor, singlePoint)
boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
NonlinearFactorGraph graph;
graph.add(visualSLAM::PoseConstraint(PoseKey(1),camera1));
graph.add(visualSLAM::PoseConstraint(kx(1),camera1));
StereoPoint2 z14(320,320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph.add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K));
graph.add(visualSLAM::StereoFactor(z14,sigma, kx(1), kl(1), K));
// Create a configuration corresponding to the ground truth
Values values;
values.insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin
values.insert(kx(1), camera1); // add camera at z=6.25m looking towards origin
Point3 l1(0, 0, 0);
values.insert(PointKey(1), l1); // add point at origin;
values.insert(kl(1), l1); // add point at origin;
GaussNewtonOptimizer optimizer(graph, values);

122
gtsam/slam/tests/testVSLAM.cpp → gtsam/slam/tests/testVisualSLAM.cpp
View File

@ -10,43 +10,45 @@
* -------------------------------------------------------------------------- */
/**
* @file testGraph.cpp
* @brief Unit test for two cameras and four landmarks
* single camera
* @file testVisualSLAM.cpp
* @brief Unit test for two cameras and four landmarks, single camera
* @author Chris Beall
* @author Frank Dellaert
* @author Viorela Ila
*/
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
using namespace boost;
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/visualSLAM.h>
using namespace std;
using namespace gtsam;
using namespace boost;
using namespace visualSLAM;
/* ************************************************************************* */
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
/* ************************************************************************* */
Point3 landmark1(-1.0,-1.0, 0.0);
Point3 landmark2(-1.0, 1.0, 0.0);
Point3 landmark3( 1.0, 1.0, 0.0);
Point3 landmark4( 1.0,-1.0, 0.0);
// Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); }
Key kl(size_t i) { return Symbol('l',i); }
Pose3 camera1(Matrix_(3,3,
static Point3 landmark1(-1.0,-1.0, 0.0);
static Point3 landmark2(-1.0, 1.0, 0.0);
static Point3 landmark3( 1.0, 1.0, 0.0);
static Point3 landmark4( 1.0,-1.0, 0.0);
static Pose3 camera1(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
Point3(0,0,6.25));
Pose3 camera2(Matrix_(3,3,
static Pose3 camera2(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
@ -66,14 +68,14 @@ visualSLAM::Graph testGraph() {
shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
visualSLAM::Graph g;
g.addMeasurement(z11, sigma, 1, 1, sK);
g.addMeasurement(z12, sigma, 1, 2, sK);
g.addMeasurement(z13, sigma, 1, 3, sK);
g.addMeasurement(z14, sigma, 1, 4, sK);
g.addMeasurement(z21, sigma, 2, 1, sK);
g.addMeasurement(z22, sigma, 2, 2, sK);
g.addMeasurement(z23, sigma, 2, 3, sK);
g.addMeasurement(z24, sigma, 2, 4, sK);
g.addMeasurement(z11, sigma, kx(1), kl(1), sK);
g.addMeasurement(z12, sigma, kx(1), kl(2), sK);
g.addMeasurement(z13, sigma, kx(1), kl(3), sK);
g.addMeasurement(z14, sigma, kx(1), kl(4), sK);
g.addMeasurement(z21, sigma, kx(2), kl(1), sK);
g.addMeasurement(z22, sigma, kx(2), kl(2), sK);
g.addMeasurement(z23, sigma, kx(2), kl(3), sK);
g.addMeasurement(z24, sigma, kx(2), kl(4), sK);
return g;
}
@ -83,22 +85,22 @@ TEST( Graph, optimizeLM)
// build a graph
visualSLAM::Graph graph(testGraph());
// add 3 landmark constraints
graph.addPointConstraint(1, landmark1);
graph.addPointConstraint(2, landmark2);
graph.addPointConstraint(3, landmark3);
graph.addPointConstraint(kl(1), landmark1);
graph.addPointConstraint(kl(2), landmark2);
graph.addPointConstraint(kl(3), landmark3);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(PoseKey(1), camera1);
initialEstimate.insert(PoseKey(2), camera2);
initialEstimate.insert(PointKey(1), landmark1);
initialEstimate.insert(PointKey(2), landmark2);
initialEstimate.insert(PointKey(3), landmark3);
initialEstimate.insert(PointKey(4), landmark4);
initialEstimate.insert(kx(1), camera1);
initialEstimate.insert(kx(2), camera2);
initialEstimate.insert(kl(1), landmark1);
initialEstimate.insert(kl(2), landmark2);
initialEstimate.insert(kl(3), landmark3);
initialEstimate.insert(kl(4), landmark4);
// Create an ordering of the variables
Ordering ordering;
ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2);
ordering += kl(1),kl(2),kl(3),kl(4),kx(1),kx(2);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
@ -121,21 +123,21 @@ TEST( Graph, optimizeLM2)
// build a graph
visualSLAM::Graph graph(testGraph());
// add 2 camera constraints
graph.addPoseConstraint(1, camera1);
graph.addPoseConstraint(2, camera2);
graph.addPoseConstraint(kx(1), camera1);
graph.addPoseConstraint(kx(2), camera2);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(PoseKey(1), camera1);
initialEstimate.insert(PoseKey(2), camera2);
initialEstimate.insert(PointKey(1), landmark1);
initialEstimate.insert(PointKey(2), landmark2);
initialEstimate.insert(PointKey(3), landmark3);
initialEstimate.insert(PointKey(4), landmark4);
initialEstimate.insert(kx(1), camera1);
initialEstimate.insert(kx(2), camera2);
initialEstimate.insert(kl(1), landmark1);
initialEstimate.insert(kl(2), landmark2);
initialEstimate.insert(kl(3), landmark3);
initialEstimate.insert(kl(4), landmark4);
// Create an ordering of the variables
Ordering ordering;
ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2);
ordering += kl(1),kl(2),kl(3),kl(4),kx(1),kx(2);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
@ -158,17 +160,17 @@ TEST( Graph, CHECK_ORDERING)
// build a graph
visualSLAM::Graph graph = testGraph();
// add 2 camera constraints
graph.addPoseConstraint(1, camera1);
graph.addPoseConstraint(2, camera2);
graph.addPoseConstraint(kx(1), camera1);
graph.addPoseConstraint(kx(2), camera2);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(PoseKey(1), camera1);
initialEstimate.insert(PoseKey(2), camera2);
initialEstimate.insert(PointKey(1), landmark1);
initialEstimate.insert(PointKey(2), landmark2);
initialEstimate.insert(PointKey(3), landmark3);
initialEstimate.insert(PointKey(4), landmark4);
initialEstimate.insert(kx(1), camera1);
initialEstimate.insert(kx(2), camera2);
initialEstimate.insert(kl(1), landmark1);
initialEstimate.insert(kl(2), landmark2);
initialEstimate.insert(kl(3), landmark3);
initialEstimate.insert(kl(4), landmark4);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
@ -189,21 +191,21 @@ TEST( Values, update_with_large_delta) {
// this test ensures that if the update for delta is larger than
// the size of the config, it only updates existing variables
Values init;
init.insert(PoseKey(1), Pose3());
init.insert(PointKey(1), Point3(1.0, 2.0, 3.0));
init.insert(kx(1), Pose3());
init.insert(kl(1), Point3(1.0, 2.0, 3.0));
Values expected;
expected.insert(PoseKey(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(PointKey(1), Point3(1.1, 2.1, 3.1));
expected.insert(kx(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(kl(1), Point3(1.1, 2.1, 3.1));
Ordering largeOrdering;
Values largeValues = init;
largeValues.insert(PoseKey(2), Pose3());
largeOrdering += PoseKey(1),PointKey(1),PoseKey(2);
largeValues.insert(kx(2), Pose3());
largeOrdering += kx(1),kl(1),kx(2);
VectorValues delta(largeValues.dims(largeOrdering));
delta[largeOrdering[PoseKey(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
delta[largeOrdering[PointKey(1)]] = Vector_(3, 0.1, 0.1, 0.1);
delta[largeOrdering[PoseKey(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
delta[largeOrdering[kx(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
delta[largeOrdering[kl(1)]] = Vector_(3, 0.1, 0.1, 0.1);
delta[largeOrdering[kx(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
Values actual = init.retract(delta, largeOrdering);
CHECK(assert_equal(expected,actual));

24
gtsam/slam/visualSLAM.cpp
View File

@ -22,38 +22,38 @@ namespace visualSLAM {
/* ************************************************************************* */
void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Index poseKey, Index pointKey, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K));
Key poseKey, Key pointKey, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, poseKey, pointKey, K));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Index poseKey, const Pose3& p) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(PoseKey(poseKey), p));
void Graph::addPoseConstraint(Key poseKey, const Pose3& p) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(poseKey, p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPointConstraint(Index pointKey, const Point3& p) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(PointKey(pointKey), p));
void Graph::addPointConstraint(Key pointKey, const Point3& p) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(pointKey, p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPosePrior(Index poseKey, const Pose3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PosePrior> factor(new PosePrior(PoseKey(poseKey), p, model));
void Graph::addPosePrior(Key poseKey, const Pose3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PosePrior> factor(new PosePrior(poseKey, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPointPrior(Index pointKey, const Point3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PointPrior> factor(new PointPrior(PointKey(pointKey), p, model));
void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PointPrior> factor(new PointPrior(pointKey, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model) {
push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model)));
void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) {
push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(poseKey, pointKey, range, model)));
}
/* ************************************************************************* */

18
gtsam/slam/visualSLAM.h
View File

@ -34,12 +34,6 @@ namespace visualSLAM {
using namespace gtsam;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a pose key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/**
* Typedefs that make up the visualSLAM namespace.
*/
@ -85,21 +79,21 @@ namespace visualSLAM {
* @param K shared pointer to calibration object
*/
void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Index poseKey, Index pointKey, const shared_ptrK& K);
Key poseKey, Key pointKey, const shared_ptrK& K);
/**
* Add a constraint on a pose (for now, *must* be satisfied in any Values)
* @param key variable key of the camera pose
* @param p to which pose to constrain it to
*/
void addPoseConstraint(Index poseKey, const Pose3& p = Pose3());
void addPoseConstraint(Key poseKey, const Pose3& p = Pose3());
/**
* Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param key variable key of the landmark
* @param p point around which soft prior is defined
*/
void addPointConstraint(Index pointKey, const Point3& p = Point3());
void addPointConstraint(Key pointKey, const Point3& p = Point3());
/**
* Add a prior on a pose
@ -107,7 +101,7 @@ namespace visualSLAM {
* @param p around which soft prior is defined
* @param model uncertainty model of this prior
*/
void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6));
void addPosePrior(Key poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6));
/**
* Add a prior on a landmark
@ -115,7 +109,7 @@ namespace visualSLAM {
* @param p to which point to constrain it to
* @param model uncertainty model of this prior
*/
void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3));
void addPointPrior(Key pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3));
/**
* Add a range prior to a landmark
@ -124,7 +118,7 @@ namespace visualSLAM {
* @param range approximate range to landmark
* @param model uncertainty model of this prior
*/
void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1));
void addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1));
/**
* Optimize the graph

11
gtsam_unstable/slam/simulated3D.h
View File

@ -36,12 +36,6 @@ namespace simulated3D {
* the simulated2D domain.
*/
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a pose key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/**
* Prior on a single pose
*/
@ -105,9 +99,8 @@ struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
* @param poseKey is the pose key of the robot
* @param pointKey is the point key for the landmark
*/
Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey) :
NoiseModelFactor2<Point3, Point3>(model, poseKey, pointKey), measured_(measured) {}
Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, Key i, Key j) :
NoiseModelFactor2<Point3, Point3>(model, i, j), measured_(measured) {}
/**
* Error function with optional derivatives

4
gtsam_unstable/slam/tests/testSimulated3D.cpp
View File

@ -30,8 +30,8 @@ using namespace simulated3D;
TEST( simulated3D, Values )
{
Values actual;
actual.insert(simulated3D::PointKey(1),Point3(1,1,1));
actual.insert(simulated3D::PoseKey(2),Point3(2,2,2));
actual.insert(Symbol('l',1),Point3(1,1,1));
actual.insert(Symbol('x',2),Point3(2,2,2));
EXPECT(assert_equal(actual,actual,1e-9));
}

2
tests/testBoundingConstraint.cpp
View File

@ -32,7 +32,7 @@ SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1);
SharedDiagonal hard_model1 = noiseModel::Constrained::All(1);
// some simple inequality constraints
Symbol key(simulated2D::PoseKey(1));
Symbol key('x',1);
double mu = 10.0;
// greater than
iq2D::PoseXInequality constraint1(key, 1.0, true, mu);

4
tests/testDoglegOptimizer.cpp
View File

@ -387,11 +387,11 @@ TEST(DoglegOptimizer, Iterate) {
// config far from minimum
Point2 x0(3,0);
boost::shared_ptr<Values> config(new Values);
config->insert(simulated2D::PoseKey(1), x0);
config->insert(Symbol('x',1), x0);
// ordering
shared_ptr<Ordering> ord(new Ordering());
ord->push_back(simulated2D::PoseKey(1));
ord->push_back(Symbol('x',1));
double Delta = 1.0;
for(size_t it=0; it<10; ++it) {

423
tests/testGaussianISAM2.cpp
View File

@ -4,13 +4,6 @@
* @author Michael Kaess
*/
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/nonlinear/Ordering.h>
@ -21,26 +14,32 @@ using namespace boost::assign;
#include <gtsam/slam/smallExample.h>
#include <gtsam/slam/planarSLAM.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
using boost::shared_ptr;
const double tol = 1e-4;
// SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true);
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
ISAM2 createSlamlikeISAM2(
boost::optional<Values&> init_values = boost::none,
boost::optional<planarSLAM::Graph&> full_graph = boost::none,
const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)) {
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
ISAM2 isam(params);
// ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
@ -57,8 +56,8 @@ ISAM2 createSlamlikeISAM2(
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
@ -70,8 +69,8 @@ ISAM2 createSlamlikeISAM2(
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -80,17 +79,17 @@ ISAM2 createSlamlikeISAM2(
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
@ -103,8 +102,8 @@ ISAM2 createSlamlikeISAM2(
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -113,13 +112,13 @@ ISAM2 createSlamlikeISAM2(
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init);
++ i;
@ -139,10 +138,10 @@ TEST_UNSAFE(ISAM2, AddVariables) {
// Create initial state
Values theta;
theta.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3));
theta.insert(planarSLAM::PointKey(0), Point2(.4, .5));
theta.insert((0), Pose2(.1, .2, .3));
theta.insert(100, Point2(.4, .5));
Values newTheta;
newTheta.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
newTheta.insert((1), Pose2(.6, .7, .8));
VectorValues deltaUnpermuted;
deltaUnpermuted.insert(0, Vector_(3, .1, .2, .3));
@ -176,21 +175,21 @@ TEST_UNSAFE(ISAM2, AddVariables) {
vector<bool> replacedKeys(2, false);
Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0);
Ordering ordering; ordering += 100, (0);
ISAM2::Nodes nodes(2);
// Verify initial state
LONGS_EQUAL(0, ordering[planarSLAM::PointKey(0)]);
LONGS_EQUAL(1, ordering[planarSLAM::PoseKey(0)]);
EXPECT(assert_equal(deltaUnpermuted[1], delta[ordering[planarSLAM::PointKey(0)]]));
EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[planarSLAM::PoseKey(0)]]));
LONGS_EQUAL(0, ordering[100]);
LONGS_EQUAL(1, ordering[(0)]);
EXPECT(assert_equal(deltaUnpermuted[1], delta[ordering[100]]));
EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[(0)]]));
// Create expected state
Values thetaExpected;
thetaExpected.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3));
thetaExpected.insert(planarSLAM::PointKey(0), Point2(.4, .5));
thetaExpected.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
thetaExpected.insert((0), Pose2(.1, .2, .3));
thetaExpected.insert(100, Point2(.4, .5));
thetaExpected.insert((1), Pose2(.6, .7, .8));
VectorValues deltaUnpermutedExpected;
deltaUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
@ -230,7 +229,7 @@ TEST_UNSAFE(ISAM2, AddVariables) {
vector<bool> replacedKeysExpected(3, false);
Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1);
Ordering orderingExpected; orderingExpected += 100, (0), (1);
ISAM2::Nodes nodesExpected(
3, ISAM2::sharedClique());
@ -255,10 +254,10 @@ TEST_UNSAFE(ISAM2, AddVariables) {
// using namespace gtsam::planarSLAM;
// typedef GaussianISAM2<Values>::Impl Impl;
//
// Ordering ordering; ordering += PointKey(0), PoseKey(0), PoseKey(1);
// Ordering ordering; ordering += (0), (0), (1);
// planarSLAM::Graph graph;
// graph.addPrior(PoseKey(0), Pose2(), sharedUnit(Pose2::dimension));
// graph.addRange(PoseKey(0), PointKey(0), 1.0, sharedUnit(1));
// graph.addPrior((0), Pose2(), sharedUnit(Pose2::dimension));
// graph.addRange((0), (0), 1.0, sharedUnit(1));
//
// FastSet<Index> expected;
// expected.insert(0);
@ -307,7 +306,7 @@ TEST(ISAM2, optimize2) {
// Create initialization
Values theta;
theta.insert(planarSLAM::PoseKey(0), Pose2(0.01, 0.01, 0.01));
theta.insert((0), Pose2(0.01, 0.01, 0.01));
// Create conditional
Vector d(3); d << -0.1, -0.1, -0.31831;
@ -318,7 +317,7 @@ TEST(ISAM2, optimize2) {
GaussianConditional::shared_ptr conditional(new GaussianConditional(0, d, R, Vector::Ones(3)));
// Create ordering
Ordering ordering; ordering += planarSLAM::PoseKey(0);
Ordering ordering; ordering += (0);
// Expected vector
VectorValues expected(1, 3);
@ -353,18 +352,6 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, cons
TEST(ISAM2, slamlike_solution_gaussnewton)
{
// SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true);
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
Values fullinit;
@ -380,8 +367,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
@ -395,8 +382,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -405,17 +392,17 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
@ -428,8 +415,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -438,13 +425,13 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init);
++ i;
@ -485,19 +472,6 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
/* ************************************************************************* */
TEST(ISAM2, slamlike_solution_dogleg)
{
// SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true);
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
Values fullinit;
@ -513,8 +487,8 @@ TEST(ISAM2, slamlike_solution_dogleg)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
@ -528,8 +502,8 @@ TEST(ISAM2, slamlike_solution_dogleg)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -538,17 +512,17 @@ TEST(ISAM2, slamlike_solution_dogleg)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
@ -561,8 +535,8 @@ TEST(ISAM2, slamlike_solution_dogleg)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -571,13 +545,13 @@ TEST(ISAM2, slamlike_solution_dogleg)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init);
++ i;
@ -618,19 +592,6 @@ TEST(ISAM2, slamlike_solution_dogleg)
/* ************************************************************************* */
TEST(ISAM2, slamlike_solution_gaussnewton_qr)
{
// SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true);
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR));
Values fullinit;
@ -646,8 +607,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
@ -661,8 +622,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -671,17 +632,17 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
@ -694,8 +655,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -704,13 +665,13 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init);
++ i;
@ -751,19 +712,6 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr)
/* ************************************************************************* */
TEST(ISAM2, slamlike_solution_dogleg_qr)
{
// SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true);
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR));
Values fullinit;
@ -779,8 +727,8 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
@ -794,8 +742,8 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -804,17 +752,17 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
@ -827,8 +775,8 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -837,13 +785,13 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init);
++ i;
@ -894,8 +842,8 @@ TEST(ISAM2, clone) {
// Modify original isam
NonlinearFactorGraph factors;
factors.add(BetweenFactor<Pose2>(Symbol('x',0), Symbol('x',10),
isam.calculateEstimate<Pose2>(Symbol('x',0)).between(isam.calculateEstimate<Pose2>(Symbol('x',10))), sharedUnit(3)));
factors.add(BetweenFactor<Pose2>(0, 10,
isam.calculateEstimate<Pose2>(0).between(isam.calculateEstimate<Pose2>(10)), sharedUnit(3)));
isam.update(factors);
CHECK(assert_equal(createSlamlikeISAM2(), clone2));
@ -978,22 +926,9 @@ TEST(ISAM2, permute_cached) {
/* ************************************************************************* */
TEST(ISAM2, removeFactors)
{
// SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true);
// This test builds a graph in the same way as the "slamlike" test above, but
// then removes the 2nd-to-last landmark measurement
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
Values fullinit;
@ -1009,8 +944,8 @@ TEST(ISAM2, removeFactors)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
@ -1024,8 +959,8 @@ TEST(ISAM2, removeFactors)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -1034,17 +969,17 @@ TEST(ISAM2, removeFactors)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init);
++ i;
@ -1057,8 +992,8 @@ TEST(ISAM2, removeFactors)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init);
}
@ -1067,14 +1002,14 @@ TEST(ISAM2, removeFactors)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors[0]);
fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0
Values init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
ISAM2Result result = isam.update(newfactors, init);
++ i;
@ -1119,24 +1054,11 @@ TEST(ISAM2, removeFactors)
}
/* ************************************************************************* */
TEST(ISAM2, swapFactors)
TEST_UNSAFE(ISAM2, swapFactors)
{
// SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true);
// This test builds a graph in the same way as the "slamlike" test above, but
// then swaps the 2nd-to-last landmark measurement with a different one
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
Values fullinit;
planarSLAM::Graph fullgraph;
ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph);
@ -1149,8 +1071,8 @@ TEST(ISAM2, swapFactors)
fullgraph.remove(swap_idx);
planarSLAM::Graph swapfactors;
// swapfactors.addBearingRange(10, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor
swapfactors.addBearingRange(10, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise);
// swapfactors.addBearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor
swapfactors.addBearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise);
fullgraph.push_back(swapfactors);
isam.update(swapfactors, Values(), toRemove);
}
@ -1191,19 +1113,6 @@ TEST(ISAM2, swapFactors)
/* ************************************************************************* */
TEST(ISAM2, constrained_ordering)
{
// SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true);
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
Values fullinit;
@ -1211,8 +1120,8 @@ TEST(ISAM2, constrained_ordering)
// We will constrain x3 and x4 to the end
FastMap<Key, int> constrained;
constrained.insert(make_pair(planarSLAM::PoseKey(3), 1));
constrained.insert(make_pair(planarSLAM::PoseKey(4), 2));
constrained.insert(make_pair((3), 1));
constrained.insert(make_pair((4), 2));
// i keeps track of the time step
size_t i = 0;
@ -1224,8 +1133,8 @@ TEST(ISAM2, constrained_ordering)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
init.insert((0), Pose2(0.01, 0.01, 0.01));
fullinit.insert((0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init);
}
@ -1239,8 +1148,8 @@ TEST(ISAM2, constrained_ordering)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
if(i >= 3)
isam.update(newfactors, init, FastVector<size_t>(), constrained);
@ -1252,17 +1161,17 @@ TEST(ISAM2, constrained_ordering)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
init.insert((i+1), Pose2(1.01, 0.01, 0.01));
init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01));
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init, FastVector<size_t>(), constrained);
++ i;
@ -1275,8 +1184,8 @@ TEST(ISAM2, constrained_ordering)
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init, FastVector<size_t>(), constrained);
}
@ -1285,13 +1194,13 @@ TEST(ISAM2, constrained_ordering)
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
Values init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init, FastVector<size_t>(), constrained);
++ i;
@ -1301,7 +1210,7 @@ TEST(ISAM2, constrained_ordering)
EXPECT(isam_check(fullgraph, fullinit, isam));
// Check that x3 and x4 are last, but either can come before the other
EXPECT(isam.getOrdering()[planarSLAM::PoseKey(3)] == 12 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 13);
EXPECT(isam.getOrdering()[(3)] == 12 && isam.getOrdering()[(4)] == 13);
// Check gradient at each node
typedef ISAM2::sharedClique sharedClique;

43
tests/testGaussianJunctionTreeB.cpp
View File

@ -125,9 +125,6 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2)
/* ************************************************************************* */
TEST(GaussianJunctionTree, slamlike) {
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
Values init;
planarSLAM::Graph newfactors;
planarSLAM::Graph fullgraph;
@ -137,39 +134,39 @@ TEST(GaussianJunctionTree, slamlike) {
size_t i = 0;
newfactors = planarSLAM::Graph();
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
newfactors.addPrior(kx(0), Pose2(0.0, 0.0, 0.0), odoNoise);
init.insert(kx(0), Pose2(0.01, 0.01, 0.01));
fullgraph.push_back(newfactors);
for( ; i<5; ++i) {
newfactors = planarSLAM::Graph();
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
init.insert(kx(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullgraph.push_back(newfactors);
}
newfactors = planarSLAM::Graph();
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(kx(i), kl(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(kx(i), kl(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
init.insert(kx(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(kl(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(kl(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
fullgraph.push_back(newfactors);
++ i;
for( ; i<5; ++i) {
newfactors = planarSLAM::Graph();
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
init.insert(kx(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullgraph.push_back(newfactors);
}
newfactors = planarSLAM::Graph();
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(kx(i), kl(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(kx(i), kl(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
init.insert(kx(i+1), Pose2(6.9, 0.1, 0.01));
fullgraph.push_back(newfactors);
++ i;
@ -195,12 +192,12 @@ TEST(GaussianJunctionTree, simpleMarginal) {
// Create a simple graph
pose2SLAM::Graph fg;
fg.addPrior(0, Pose2(), sharedSigma(3, 10.0));
fg.addOdometry(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0)));
fg.addPrior(kx(0), Pose2(), sharedSigma(3, 10.0));
fg.addOdometry(kx(0), kx(1), Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0)));
Values init;
init.insert(pose2SLAM::PoseKey(0), Pose2());
init.insert(pose2SLAM::PoseKey(1), Pose2(1.0, 0.0, 0.0));
init.insert(kx(0), Pose2());
init.insert(kx(1), Pose2(1.0, 0.0, 0.0));
Ordering ordering;
ordering += kx(1), kx(0);

54
tests/testGraph.cpp
View File

@ -16,38 +16,34 @@
* @brief unit test for graph-inl.h
*/
#include <iostream>
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/inference/graph-inl.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// TODO: DANGEROUS, create shared pointers
#define GTSAM_MAGIC_GAUSSIAN 3
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/inference/graph-inl.h>
#include <iostream>
using namespace std;
using namespace gtsam;
Key kx(size_t i) { return Symbol('x',i); }
/* ************************************************************************* */
// x1 -> x2
// -> x3 -> x4
// -> x5
TEST ( Ordering, predecessorMap2Keys ) {
PredecessorMap<Key> p_map;
p_map.insert(kx(1),kx(1));
p_map.insert(kx(2),kx(1));
p_map.insert(kx(3),kx(1));
p_map.insert(kx(4),kx(3));
p_map.insert(kx(5),kx(1));
p_map.insert(1,1);
p_map.insert(2,1);
p_map.insert(3,1);
p_map.insert(4,3);
p_map.insert(5,1);
list<Key> expected;
expected += kx(4),kx(5),kx(3),kx(2),kx(1);//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1);
expected += 4,5,3,2,1;
list<Key> actual = predecessorMap2Keys<Key>(p_map);
LONGS_EQUAL(expected.size(), actual.size());
@ -67,20 +63,20 @@ TEST( Graph, predecessorMap2Graph )
map<Key, SVertex> key2vertex;
PredecessorMap<Key> p_map;
p_map.insert(kx(1), kx(2));
p_map.insert(kx(2), kx(2));
p_map.insert(kx(3), kx(2));
p_map.insert(1, 2);
p_map.insert(2, 2);
p_map.insert(3, 2);
tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
LONGS_EQUAL(3, boost::num_vertices(graph));
CHECK(root == key2vertex[kx(2)]);
CHECK(root == key2vertex[2]);
}
/* ************************************************************************* */
TEST( Graph, composePoses )
{
pose2SLAM::Graph graph;
Matrix cov = eye(3);
SharedNoiseModel cov = SharedNoiseModel::Unit(3);
Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9);
Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3);
graph.addOdometry(1,2, p12, cov);
@ -88,10 +84,10 @@ TEST( Graph, composePoses )
graph.addOdometry(4,3, p43, cov);
PredecessorMap<Key> tree;
tree.insert(pose2SLAM::PoseKey(1),pose2SLAM::PoseKey(2));
tree.insert(pose2SLAM::PoseKey(2),pose2SLAM::PoseKey(2));
tree.insert(pose2SLAM::PoseKey(3),pose2SLAM::PoseKey(2));
tree.insert(pose2SLAM::PoseKey(4),pose2SLAM::PoseKey(3));
tree.insert(1,2);
tree.insert(2,2);
tree.insert(3,2);
tree.insert(4,3);
Pose2 rootPose = p2;
@ -99,10 +95,10 @@ TEST( Graph, composePoses )
Pose2, Key> (graph, tree, rootPose);
Values expected;
expected.insert(pose2SLAM::PoseKey(1), p1);
expected.insert(pose2SLAM::PoseKey(2), p2);
expected.insert(pose2SLAM::PoseKey(3), p3);
expected.insert(pose2SLAM::PoseKey(4), p4);
expected.insert(1, p1);
expected.insert(2, p2);
expected.insert(3, p3);
expected.insert(4, p4);
LONGS_EQUAL(4, actual->size());
CHECK(assert_equal(expected, *actual));

26
tests/testInferenceB.cpp
View File

@ -25,6 +25,10 @@
using namespace std;
using namespace gtsam;
// Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); }
Key kl(size_t i) { return Symbol('l',i); }
/* ************************************************************************* */
// The tests below test the *generic* inference algorithms. Some of these have
// specialized versions in the derived classes GaussianFactorGraph etc...
@ -52,23 +56,23 @@ TEST( inference, marginals2)
SharedDiagonal poseModel(sharedSigma(3, 0.1));
SharedDiagonal pointModel(sharedSigma(3, 0.1));
fg.addPrior(0, Pose2(), poseModel);
fg.addOdometry(0, 1, Pose2(1.0,0.0,0.0), poseModel);
fg.addOdometry(1, 2, Pose2(1.0,0.0,0.0), poseModel);
fg.addBearingRange(0, 0, Rot2(), 1.0, pointModel);
fg.addBearingRange(1, 0, Rot2(), 1.0, pointModel);
fg.addBearingRange(2, 0, Rot2(), 1.0, pointModel);
fg.addPrior(kx(0), Pose2(), poseModel);
fg.addOdometry(kx(0), kx(1), Pose2(1.0,0.0,0.0), poseModel);
fg.addOdometry(kx(1), kx(2), Pose2(1.0,0.0,0.0), poseModel);
fg.addBearingRange(kx(0), kl(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(kx(1), kl(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(kx(2), kl(0), Rot2(), 1.0, pointModel);
Values init;
init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0));
init.insert(planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0));
init.insert(planarSLAM::PoseKey(2), Pose2(2.0,0.0,0.0));
init.insert(planarSLAM::PointKey(0), Point2(1.0,1.0));
init.insert(kx(0), Pose2(0.0,0.0,0.0));
init.insert(kx(1), Pose2(1.0,0.0,0.0));
init.insert(kx(2), Pose2(2.0,0.0,0.0));
init.insert(kl(0), Point2(1.0,1.0));
Ordering ordering(*fg.orderingCOLAMD(init));
FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
GaussianMultifrontalSolver solver(*gfg);
solver.marginalFactor(ordering[planarSLAM::PointKey(0)]);
solver.marginalFactor(ordering[kl(0)]);
}
/* ************************************************************************* */

8
tests/testNonlinearEquality.cpp
View File

@ -535,13 +535,13 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
VGraph graph;
// create equality constraints for poses
graph.addPoseConstraint(1, camera1.pose());
graph.addPoseConstraint(2, camera2.pose());
graph.addPoseConstraint(x1, camera1.pose());
graph.addPoseConstraint(x2, camera2.pose());
// create factors
SharedDiagonal vmodel = noiseModel::Unit::Create(3);
graph.addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK);
graph.addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK);
graph.addMeasurement(camera1.project(landmark), vmodel, x1, l1, shK);
graph.addMeasurement(camera2.project(landmark), vmodel, x2, l2, shK);
// add equality constraint
graph.add(Point3Equality(l1, l2));

97
tests/testNonlinearFactor.cpp
View File

@ -37,8 +37,9 @@ using namespace std;
using namespace gtsam;
using namespace example;
using simulated2D::PoseKey;
using simulated2D::PointKey;
// Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); }
Key kl(size_t i) { return Symbol('l',i); }
typedef boost::shared_ptr<NonlinearFactor > shared_nlf;
@ -49,11 +50,11 @@ TEST( NonlinearFactor, equals )
// create two nonlinear2 factors
Point2 z3(0.,-1.);
simulated2D::Measurement f0(z3, sigma, PoseKey(1),PointKey(1));
simulated2D::Measurement f0(z3, sigma, kx(1),kl(1));
// measurement between x2 and l1
Point2 z4(-1.5, -1.);
simulated2D::Measurement f1(z4, sigma, PoseKey(2),PointKey(1));
simulated2D::Measurement f1(z4, sigma, kx(2),kl(1));
CHECK(assert_equal(f0,f0));
CHECK(f0.equals(f0));
@ -199,16 +200,16 @@ TEST( NonlinearFactor, linearize_constraint1 )
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
Point2 mu(1., -1.);
Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, PoseKey(1)));
Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, kx(1)));
Values config;
config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
config.insert(kx(1), Point2(1.0, 2.0));
GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary());
// create expected
Ordering ord(*config.orderingArbitrary());
Vector b = Vector_(2, 0., -3.);
JacobianFactor expected(ord[PoseKey(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint);
JacobianFactor expected(ord[kx(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint);
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}
@ -219,18 +220,18 @@ TEST( NonlinearFactor, linearize_constraint2 )
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
Point2 z3(1.,-1.);
simulated2D::Measurement f0(z3, constraint, PoseKey(1),PointKey(1));
simulated2D::Measurement f0(z3, constraint, kx(1),kl(1));
Values config;
config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0));
config.insert(kx(1), Point2(1.0, 2.0));
config.insert(kl(1), Point2(5.0, 4.0));
GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary());
// create expected
Ordering ord(*config.orderingArbitrary());
Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0);
Vector b = Vector_(2, -15., -3.);
JacobianFactor expected(ord[PoseKey(1)], -1*A, ord[PointKey(1)], A, b, constraint);
JacobianFactor expected(ord[kx(1)], -1*A, ord[kl(1)], A, b, constraint);
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}
@ -238,7 +239,7 @@ TEST( NonlinearFactor, linearize_constraint2 )
class TestFactor4 : public NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> {
public:
typedef NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4)) {}
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4)) {}
virtual Vector
evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
@ -262,13 +263,13 @@ public:
TEST(NonlinearFactor, NoiseModelFactor4) {
TestFactor4 tf;
Values tv;
tv.insert(PoseKey(1), LieVector(1, 1.0));
tv.insert(PoseKey(2), LieVector(1, 2.0));
tv.insert(PoseKey(3), LieVector(1, 3.0));
tv.insert(PoseKey(4), LieVector(1, 4.0));
tv.insert(kx(1), LieVector(1, 1.0));
tv.insert(kx(2), LieVector(1, 2.0));
tv.insert(kx(3), LieVector(1, 3.0));
tv.insert(kx(4), LieVector(1, 4.0));
EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4);
Ordering ordering; ordering += kx(1), kx(2), kx(3), kx(4);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
@ -285,7 +286,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
class TestFactor5 : public NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
public:
typedef NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5)) {}
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4), kx(5)) {}
virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
@ -311,14 +312,14 @@ public:
TEST(NonlinearFactor, NoiseModelFactor5) {
TestFactor5 tf;
Values tv;
tv.insert(PoseKey(1), LieVector(1, 1.0));
tv.insert(PoseKey(2), LieVector(1, 2.0));
tv.insert(PoseKey(3), LieVector(1, 3.0));
tv.insert(PoseKey(4), LieVector(1, 4.0));
tv.insert(PoseKey(5), LieVector(1, 5.0));
tv.insert(kx(1), LieVector(1, 1.0));
tv.insert(kx(2), LieVector(1, 2.0));
tv.insert(kx(3), LieVector(1, 3.0));
tv.insert(kx(4), LieVector(1, 4.0));
tv.insert(kx(5), LieVector(1, 5.0));
EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5);
Ordering ordering; ordering += kx(1), kx(2), kx(3), kx(4), kx(5);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
@ -337,7 +338,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
class TestFactor6 : public NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
public:
typedef NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5), PoseKey(6)) {}
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4), kx(5), kx(6)) {}
virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
@ -365,15 +366,15 @@ public:
TEST(NonlinearFactor, NoiseModelFactor6) {
TestFactor6 tf;
Values tv;
tv.insert(PoseKey(1), LieVector(1, 1.0));
tv.insert(PoseKey(2), LieVector(1, 2.0));
tv.insert(PoseKey(3), LieVector(1, 3.0));
tv.insert(PoseKey(4), LieVector(1, 4.0));
tv.insert(PoseKey(5), LieVector(1, 5.0));
tv.insert(PoseKey(6), LieVector(1, 6.0));
tv.insert(kx(1), LieVector(1, 1.0));
tv.insert(kx(2), LieVector(1, 2.0));
tv.insert(kx(3), LieVector(1, 3.0));
tv.insert(kx(4), LieVector(1, 4.0));
tv.insert(kx(5), LieVector(1, 5.0));
tv.insert(kx(6), LieVector(1, 6.0));
EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5), PoseKey(6);
Ordering ordering; ordering += kx(1), kx(2), kx(3), kx(4), kx(5), kx(6);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
@ -395,10 +396,10 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
TEST( NonlinearFactor, clone_rekey )
{
shared_nlf init(new TestFactor4());
EXPECT_LONGS_EQUAL(PoseKey(1), init->keys()[0]);
EXPECT_LONGS_EQUAL(PoseKey(2), init->keys()[1]);
EXPECT_LONGS_EQUAL(PoseKey(3), init->keys()[2]);
EXPECT_LONGS_EQUAL(PoseKey(4), init->keys()[3]);
EXPECT_LONGS_EQUAL(kx(1), init->keys()[0]);
EXPECT_LONGS_EQUAL(kx(2), init->keys()[1]);
EXPECT_LONGS_EQUAL(kx(3), init->keys()[2]);
EXPECT_LONGS_EQUAL(kx(4), init->keys()[3]);
// Standard clone
shared_nlf actClone = init->clone();
@ -407,24 +408,24 @@ TEST( NonlinearFactor, clone_rekey )
// Re-key factor - clones with different keys
std::vector<Key> new_keys(4);
new_keys[0] = PoseKey(5);
new_keys[1] = PoseKey(6);
new_keys[2] = PoseKey(7);
new_keys[3] = PoseKey(8);
new_keys[0] = kx(5);
new_keys[1] = kx(6);
new_keys[2] = kx(7);
new_keys[3] = kx(8);
shared_nlf actRekey = init->rekey(new_keys);
EXPECT(actRekey.get() != init.get()); // Ensure different pointers
// Ensure init is unchanged
EXPECT_LONGS_EQUAL(PoseKey(1), init->keys()[0]);
EXPECT_LONGS_EQUAL(PoseKey(2), init->keys()[1]);
EXPECT_LONGS_EQUAL(PoseKey(3), init->keys()[2]);
EXPECT_LONGS_EQUAL(PoseKey(4), init->keys()[3]);
EXPECT_LONGS_EQUAL(kx(1), init->keys()[0]);
EXPECT_LONGS_EQUAL(kx(2), init->keys()[1]);
EXPECT_LONGS_EQUAL(kx(3), init->keys()[2]);
EXPECT_LONGS_EQUAL(kx(4), init->keys()[3]);
// Check new keys
EXPECT_LONGS_EQUAL(PoseKey(5), actRekey->keys()[0]);
EXPECT_LONGS_EQUAL(PoseKey(6), actRekey->keys()[1]);
EXPECT_LONGS_EQUAL(PoseKey(7), actRekey->keys()[2]);
EXPECT_LONGS_EQUAL(PoseKey(8), actRekey->keys()[3]);
EXPECT_LONGS_EQUAL(kx(5), actRekey->keys()[0]);
EXPECT_LONGS_EQUAL(kx(6), actRekey->keys()[1]);
EXPECT_LONGS_EQUAL(kx(7), actRekey->keys()[2]);
EXPECT_LONGS_EQUAL(kx(8), actRekey->keys()[3]);
}
/* ************************************************************************* */

2
tests/testNonlinearISAM.cpp
View File

@ -54,7 +54,7 @@ TEST(testNonlinearISAM, markov_chain ) {
ordering.push_back(secondLast);
isam.setOrdering(ordering);
Ordering expected; expected += PoseKey(0), PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(3);
Ordering expected; expected += (0), (1), (2), (4), (3);
EXPECT(assert_equal(expected, isam.getOrdering()));
}

34
tests/testNonlinearOptimizer.cpp
View File

@ -50,7 +50,7 @@ TEST( NonlinearOptimizer, iterateLM )
// config far from minimum
Point2 x0(3,0);
Values config;
config.insert(simulated2D::PoseKey(1), x0);
config.insert(kx(1), x0);
// normal iterate
GaussNewtonParams gnParams;
@ -74,13 +74,13 @@ TEST( NonlinearOptimizer, optimize )
// test error at minimum
Point2 xstar(0,0);
Values cstar;
cstar.insert(simulated2D::PoseKey(1), xstar);
cstar.insert(kx(1), xstar);
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Point2 x0(3,3);
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
c0.insert(kx(1), x0);
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// optimize parameters
@ -113,7 +113,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer )
Point2 x0(3,3);
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
c0.insert(kx(1), x0);
Values actual = LevenbergMarquardtOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual),tol);
@ -126,7 +126,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer )
Point2 x0(3,3);
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
c0.insert(kx(1), x0);
Values actual = GaussNewtonOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual),tol);
@ -139,7 +139,7 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer )
Point2 x0(3,3);
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
c0.insert(kx(1), x0);
Values actual = DoglegOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual),tol);
@ -157,7 +157,7 @@ TEST( NonlinearOptimizer, optimization_method )
Point2 x0(3,3);
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
c0.insert(kx(1), x0);
Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize();
DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);
@ -170,23 +170,23 @@ TEST( NonlinearOptimizer, optimization_method )
TEST( NonlinearOptimizer, Factorization )
{
Values config;
config.insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.));
config.insert(pose2SLAM::PoseKey(2), Pose2(1.5,0.,0.));
config.insert(kx(1), Pose2(0.,0.,0.));
config.insert(kx(2), Pose2(1.5,0.,0.));
pose2SLAM::Graph graph;
graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph.addOdometry(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
graph.addPrior(kx(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph.addOdometry(kx(1),kx(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
Ordering ordering;
ordering.push_back(pose2SLAM::PoseKey(1));
ordering.push_back(pose2SLAM::PoseKey(2));
ordering.push_back(kx(1));
ordering.push_back(kx(2));
LevenbergMarquardtOptimizer optimizer(graph, config, ordering);
optimizer.iterate();
Values expected;
expected.insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.));
expected.insert(pose2SLAM::PoseKey(2), Pose2(1.,0.,0.));
expected.insert(kx(1), Pose2(0.,0.,0.));
expected.insert(kx(2), Pose2(1.,0.,0.));
CHECK(assert_equal(expected, optimizer.values(), 1e-5));
}
@ -201,13 +201,13 @@ TEST(NonlinearOptimizer, NullFactor) {
// test error at minimum
Point2 xstar(0,0);
Values cstar;
cstar.insert(simulated2D::PoseKey(1), xstar);
cstar.insert(kx(1), xstar);
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Point2 x0(3,3);
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
c0.insert(kx(1), x0);
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// optimize parameters