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> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -345,6 +337,7 @@
</target> </target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget> <buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -352,6 +345,7 @@
</target> </target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget> <buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -399,6 +393,7 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget> <buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -406,6 +401,7 @@
</target> </target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget> <buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -413,6 +409,7 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget> <buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -428,168 +425,17 @@
</target> </target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget> <buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <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> <buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError> <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> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
@ -665,22 +511,6 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -697,6 +527,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -721,54 +567,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<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"> <target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -793,6 +591,94 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="schedulingExample.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -897,22 +783,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="timeGaussianFactorGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -921,42 +791,26 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testTupleConfig.run</buildTarget> <buildTarget>testMarginals.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testSerialization.run</buildTarget> <buildTarget>testGaussianISAM2.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testInference.run</buildTarget> <buildTarget>testSymbolicFactorGraphB.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>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -985,14 +839,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testNonlinearOptimizer.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1017,14 +863,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testNonlinearEquality.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1057,6 +895,62 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1147,6 +1041,7 @@
</target> </target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget> <buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1602,7 +1497,6 @@
</target> </target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget> <buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1642,7 +1536,6 @@
</target> </target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget> <buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1650,7 +1543,6 @@
</target> </target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget> <buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1752,10 +1644,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>CameraResectioning</buildTarget> <buildTarget>CameraResectioning.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -1832,6 +1724,30 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1842,7 +1758,6 @@
</target> </target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget> <buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1864,102 +1779,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testRot2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>timeRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCal3_S2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testHomography2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2161,6 +1980,7 @@
</target> </target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget> <buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2168,6 +1988,7 @@
</target> </target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget> <buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2175,6 +1996,7 @@
</target> </target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget> <buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2182,6 +2004,7 @@
</target> </target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget> <buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2235,42 +2058,98 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>wrap.testSpirit.run</buildTarget> <buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>wrap.testWrap.run</buildTarget> <buildTarget>testRot2.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>check.wrap</buildTarget> <buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="wrap_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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>wrap_gtsam</buildTarget> <buildTarget>timeRot3.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>wrap</buildTarget> <buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCal3_S2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testHomography2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -2314,6 +2193,46 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> </buildTargets>
</storageModule> </storageModule>
</cproject> </cproject>

6
examples/LocalizationExample.cpp
View File

@ -66,9 +66,9 @@ int main(int argc, char** argv) {
// Query the marginals // Query the marginals
Marginals marginals = graph.marginals(result); Marginals marginals = graph.marginals(result);
cout.precision(2); cout.precision(2);
cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl; cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(2)) << endl; cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(3)) << endl; cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl;
return 0; 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 // add unary measurement factors, like GPS, on all three poses
SharedDiagonal noiseModel(Vector_(2, 0.1, 0.1)); // 10cm std on x,y 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>(1, 0, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(x1, 0, 0, noiseModel)); graph.push_back(boost::make_shared<UnaryFactor>(2, 2, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(x2, 2, 0, noiseModel)); graph.push_back(boost::make_shared<UnaryFactor>(3, 4, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(x3, 4, 0, noiseModel));
// print // print
graph.print("\nFactor graph:\n"); graph.print("\nFactor graph:\n");
@ -94,9 +93,9 @@ int main(int argc, char** argv) {
// Query the marginals // Query the marginals
Marginals marginals(graph, result); Marginals marginals(graph, result);
cout.precision(2); cout.precision(2);
cout << "\nP1:\n" << marginals.marginalCovariance(x1) << endl; cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl;
cout << "\nP2:\n" << marginals.marginalCovariance(x2) << endl; cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl;
cout << "\nP3:\n" << marginals.marginalCovariance(x3) << endl; cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl;
return 0; 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) // create the graph (defined in planarSlam.h, derived from NonlinearFactorGraph)
planarSLAM::Graph graph; 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 // add a Gaussian prior on pose x_1
Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin 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 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 // add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) 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 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(i1, i2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise); graph.addOdometry(i2, i3, odometry, odometryNoise);
// create a noise model for the landmark measurements // 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 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; range32 = 2.0;
// add bearing/range factors (created by "addBearingRange") // add bearing/range factors (created by "addBearingRange")
graph.addBearingRange(1, 1, bearing11, range11, measurementNoise); graph.addBearingRange(i1, j1, bearing11, range11, measurementNoise);
graph.addBearingRange(2, 1, bearing21, range21, measurementNoise); graph.addBearingRange(i2, j1, bearing21, range21, measurementNoise);
graph.addBearingRange(3, 2, bearing32, range32, measurementNoise); graph.addBearingRange(i3, j2, bearing32, range32, measurementNoise);
// print // print
graph.print("Factor graph"); graph.print("Factor graph");
// create (deliberatly inaccurate) initial estimate // create (deliberatly inaccurate) initial estimate
planarSLAM::Values initialEstimate; planarSLAM::Values initialEstimate;
initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insertPoint(1, Point2(1.8, 2.1)); initialEstimate.insertPoint(j1, Point2(1.8, 2.1));
initialEstimate.insertPoint(2, Point2(4.1, 1.8)); initialEstimate.insertPoint(j2, Point2(4.1, 1.8));
initialEstimate.print("Initial estimate:\n "); initialEstimate.print("Initial estimate:\n ");

62
examples/PlanarSLAMSelfContained_advanced.cpp
View File

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

17
examples/Pose2SLAMExample_advanced.cpp
View File

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

6
examples/matlab/LocalizationExample.m
View File

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

2
examples/vSLAMexample/Feature2D.h
View File

@ -24,7 +24,7 @@ struct Feature2D
{ {
gtsam::Point2 m_p; 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 int m_idLandmark; // id of the 3D landmark that it is associated with
Feature2D(int idCamera, int idLandmark, gtsam::Point2 p) : Feature2D(int idCamera, int idLandmark, gtsam::Point2 p) :

35
examples/vSLAMexample/vISAMexample.cpp
View File

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

37
examples/vSLAMexample/vSFMexample.cpp
View File

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

5
examples/vSLAMexample/vSLAMutils.h
View File

@ -17,13 +17,12 @@
#pragma once #pragma once
#include <map>
#include <vector>
#include "Feature2D.h" #include "Feature2D.h"
#include "gtsam/geometry/Pose3.h" #include "gtsam/geometry/Pose3.h"
#include "gtsam/geometry/Point3.h" #include "gtsam/geometry/Point3.h"
#include "gtsam/geometry/Cal3_S2.h" #include "gtsam/geometry/Cal3_S2.h"
#include <vector>
#include <map>
std::map<int, gtsam::Point3> readLandMarks(const std::string& landmarkFile); 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: * entries would be added with:
* \code * \code
FastMap<char,Vector> thresholds; 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['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[PointKey::chr()] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
params.relinearizeThreshold = thresholds; params.relinearizeThreshold = thresholds;
\endcode \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; is >> id >> x >> y >> yaw;
// optional filter // optional filter
if (maxID && id >= maxID) continue; 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'); is.ignore(LINESIZE, '\n');
} }
@ -132,10 +132,10 @@ pair<sharedPose2Graph, Values::shared_ptr> load2D(const string& filename,
l1Xl2 = l1Xl2.retract((*model)->sample()); l1Xl2 = l1Xl2.retract((*model)->sample());
// Insert vertices if pure odometry file // Insert vertices if pure odometry file
if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2()); if (!poses->exists(id1)) poses->insert(id1, Pose2());
if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at<Pose2>(pose2SLAM::PoseKey(id1)) * l1Xl2); 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); graph->push_back(factor);
} }
is.ignore(LINESIZE, '\n'); is.ignore(LINESIZE, '\n');

24
gtsam/slam/planarSLAM.cpp
View File

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

56
gtsam/slam/planarSLAM.h
View File

@ -11,7 +11,7 @@
/** /**
* @file planarSLAM.h * @file planarSLAM.h
* @brief: bearing/range measurements in 2D plane * @brief bearing/range measurements in 2D plane
* @author Frank Dellaert * @author Frank Dellaert
*/ */
@ -31,29 +31,25 @@ namespace planarSLAM {
using namespace gtsam; 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 * 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; 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; 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; 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; 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; 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; 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 * Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods
*/ */
struct Values: public gtsam::Values { struct Values: public gtsam::Values {
@ -67,16 +63,16 @@ namespace planarSLAM {
} }
/// get a pose /// 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 /// 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 /// 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 /// 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 /// Creates a NonlinearFactorGraph with the Values type
@ -88,23 +84,23 @@ namespace planarSLAM {
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph); Graph(const NonlinearFactorGraph& graph);
/// Biases the value of PoseKey key with Pose2 p given a noise model /// Biases the value of pose key with Pose2 p given a noise model
void addPrior(Index poseKey, const Pose2& pose, const SharedNoiseModel& noiseModel); void addPrior(Key i, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value /// Creates a hard constraint on the ith pose
void addPoseConstraint(Index poseKey, const Pose2& pose); void addPoseConstraint(Key i, const Pose2& pose);
/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement) /// Creates an odometry factor between poses with keys i1 and i2
void addOdometry(Index poseKey1, Index poseKey2, const Pose2& odometry, const SharedNoiseModel& model); 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 /// Creates a bearing measurement from pose i to point j
void addBearing(Index poseKey, Index pointKey, const Rot2& bearing, const SharedNoiseModel& model); 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 /// Creates a range measurement from pose i to point j
void addRange(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model); 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 /// Creates a range/bearing measurement from pose i to point j
void addBearingRange(Index poseKey, Index pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model); void addBearingRange(Key i, Key j, const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Optimize /// Optimize
Values optimize(const Values& initialEstimate) const; Values optimize(const Values& initialEstimate) const;

15
gtsam/slam/pose2SLAM.cpp
View File

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

25
gtsam/slam/pose2SLAM.h
View File

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

14
gtsam/slam/pose3SLAM.cpp
View File

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

9
gtsam/slam/pose3SLAM.h
View File

@ -30,9 +30,6 @@ namespace pose3SLAM {
using namespace gtsam; 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) * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses * @param n number of poses
@ -52,13 +49,13 @@ namespace pose3SLAM {
struct Graph: public NonlinearFactorGraph { struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type /// 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 /// 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. /// 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 /// Optimize
Values optimize(const Values& initialEstimate) { 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 // 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 * 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 /// Insert a pose
void insertPose(Index j, const Point2& p) { void insertPose(Key i, const Point2& p) {
insert(PoseKey(j), p); insert(i, p);
nrPoses_++; nrPoses_++;
} }
/// Insert a point /// Insert a point
void insertPoint(Index j, const Point2& p) { void insertPoint(Key j, const Point2& p) {
insert(PointKey(j), p); insert(j, p);
nrPoints_++; nrPoints_++;
} }
@ -79,13 +73,13 @@ namespace simulated2D {
} }
/// Return pose i /// Return pose i
Point2 pose(Index j) const { Point2 pose(Key i) const {
return at<Point2>(PoseKey(j)); return at<Point2>(i);
} }
/// Return point j /// Return point j
Point2 point(Index j) const { Point2 point(Key j) const {
return at<Point2>(PointKey(j)); return at<Point2>(j);
} }
}; };
@ -171,8 +165,8 @@ namespace simulated2D {
Pose measured_; ///< odometry measurement Pose measured_; ///< odometry measurement
/// Create odometry /// Create odometry
GenericOdometry(const Pose& measured, const SharedNoiseModel& model, Key key1, Key key2) : GenericOdometry(const Pose& measured, const SharedNoiseModel& model, Key i1, Key i2) :
Base(model, key1, key2), measured_(measured) { Base(model, i1, i2), measured_(measured) {
} }
/// Evaluate error and optionally return derivatives /// Evaluate error and optionally return derivatives
@ -215,8 +209,8 @@ namespace simulated2D {
Landmark measured_; ///< Measurement Landmark measured_; ///< Measurement
/// Create measurement factor /// Create measurement factor
GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey) : GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, Key i, Key j) :
Base(model, poseKey, landmarkKey), measured_(measured) { Base(model, i, j), measured_(measured) {
} }
/// Evaluate error and optionally return derivatives /// Evaluate error and optionally return derivatives

25
gtsam/slam/simulated2DOriented.h
View File

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

59
gtsam/slam/smallExample.cpp
View File

@ -17,28 +17,24 @@
* @author Frank dellaert * @author Frank dellaert
*/ */
#include <iostream> #include <gtsam/slam/smallExample.h>
#include <string> #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/optional.hpp>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <iostream>
#include <string>
using namespace std; 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 gtsam {
namespace example { namespace example {
using simulated2D::PoseKey;
using simulated2D::PointKey;
typedef boost::shared_ptr<NonlinearFactor> shared; typedef boost::shared_ptr<NonlinearFactor> shared;
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); 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 _l1_=0, _x1_=1, _x2_=2;
static const Index _x_=0, _y_=1, _z_=2; static const Index _x_=0, _y_=1, _z_=2;
// Convenience for named keys
Key kx(size_t i) { return Symbol('x',i); } Key kx(size_t i) { return Symbol('x',i); }
Key kl(size_t i) { return Symbol('l',i); } Key kl(size_t i) { return Symbol('l',i); }
@ -60,22 +57,22 @@ namespace example {
// prior on x1 // prior on x1
Point2 mu; 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); nlfg->push_back(f1);
// odometry between x1 and x2 // odometry between x1 and x2
Point2 z2(1.5, 0); 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); nlfg->push_back(f2);
// measurement between x1 and l1 // measurement between x1 and l1
Point2 z3(0, -1); 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); nlfg->push_back(f3);
// measurement between x2 and l1 // measurement between x2 and l1
Point2 z4(-1.5, -1.); 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); nlfg->push_back(f4);
return nlfg; return nlfg;
@ -89,9 +86,9 @@ namespace example {
/* ************************************************************************* */ /* ************************************************************************* */
Values createValues() { Values createValues() {
Values c; Values c;
c.insert(PoseKey(1), Point2(0.0, 0.0)); c.insert(kx(1), Point2(0.0, 0.0));
c.insert(PoseKey(2), Point2(1.5, 0.0)); c.insert(kx(2), Point2(1.5, 0.0));
c.insert(PointKey(1), Point2(0.0, -1.0)); c.insert(kl(1), Point2(0.0, -1.0));
return c; return c;
} }
@ -107,9 +104,9 @@ namespace example {
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const Values> sharedNoisyValues() { boost::shared_ptr<const Values> sharedNoisyValues() {
boost::shared_ptr<Values> c(new Values); boost::shared_ptr<Values> c(new Values);
c->insert(PoseKey(1), Point2(0.1, 0.1)); c->insert(kx(1), Point2(0.1, 0.1));
c->insert(PoseKey(2), Point2(1.4, 0.2)); c->insert(kx(2), Point2(1.4, 0.2));
c->insert(PointKey(1), Point2(0.1, -1.1)); c->insert(kl(1), Point2(0.1, -1.1));
return c; return c;
} }
@ -223,7 +220,7 @@ namespace example {
Vector z = Vector_(2, 1.0, 0.0); Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1; double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor( 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); fg->push_back(factor);
return fg; return fg;
} }
@ -241,23 +238,23 @@ namespace example {
// prior on x1 // prior on x1
Point2 x1(1.0, 0.0); 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); nlfg.push_back(prior);
poses.insert(simulated2D::PoseKey(1), x1); poses.insert(kx(1), x1);
for (int t = 2; t <= T; t++) { for (int t = 2; t <= T; t++) {
// odometry between x_t and x_{t-1} // odometry between x_t and x_{t-1}
Point2 odo(1.0, 0.0); 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); nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS // measurement on x_t is like perfect GPS
Point2 xt(t, 0); 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); nlfg.push_back(measurement);
// initial estimate // initial estimate
poses.insert(PoseKey(t), xt); poses.insert(kx(t), xt);
} }
return make_pair(nlfg, poses); return make_pair(nlfg, poses);
@ -418,7 +415,7 @@ namespace example {
/* ************************************************************************* */ /* ************************************************************************* */
// Create key for simulated planar graph // Create key for simulated planar graph
Symbol key(int x, int y) { 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 std;
using namespace gtsam; using namespace gtsam;
using pose3SLAM::PoseKey;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( AntiFactor, NegativeHessian) TEST( AntiFactor, NegativeHessian)
{ {
@ -42,17 +40,17 @@ TEST( AntiFactor, NegativeHessian)
// Create a configuration corresponding to the ground truth // Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values()); boost::shared_ptr<Values> values(new Values());
values->insert(PoseKey(1), pose1); values->insert(1, pose1);
values->insert(PoseKey(2), pose2); values->insert(2, pose2);
// Define an elimination ordering // Define an elimination ordering
Ordering::shared_ptr ordering(new Ordering()); Ordering::shared_ptr ordering(new Ordering());
ordering->insert(PoseKey(1), 0); ordering->insert(1, 0);
ordering->insert(PoseKey(2), 1); ordering->insert(2, 1);
// Create a "standard" factor // 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 // Linearize it into a Jacobian Factor
GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering);
@ -103,8 +101,8 @@ TEST( AntiFactor, EquivalentBayesNet)
// Create a configuration corresponding to the ground truth // Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values()); boost::shared_ptr<Values> values(new Values());
values->insert(PoseKey(1), pose1); values->insert(1, pose1);
values->insert(PoseKey(2), pose2); values->insert(2, pose2);
// Define an elimination ordering // Define an elimination ordering
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
@ -117,7 +115,7 @@ TEST( AntiFactor, EquivalentBayesNet)
VectorValues expectedDeltas = optimize(*expectedBayesNet); VectorValues expectedDeltas = optimize(*expectedBayesNet);
// Add an additional factor between Pose1 and Pose2 // 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); graph->push_back(f1);
// Add the corresponding AntiFactor between Pose1 and Pose2 // 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 // some shared test values
static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); 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 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 SharedNoiseModel
sigma(noiseModel::Isotropic::Sigma(1,0.1)), sigma(noiseModel::Isotropic::Sigma(1,0.1)),
@ -37,7 +38,7 @@ SharedNoiseModel
/* ************************************************************************* */ /* ************************************************************************* */
TEST( planarSLAM, PriorFactor_equals ) 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(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -48,12 +49,12 @@ TEST( planarSLAM, BearingFactor )
{ {
// Create factor // Create factor
Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 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 // create config
planarSLAM::Values c; planarSLAM::Values c;
c.insert(PoseKey(2), x2); c.insert(i2, x2);
c.insert(PointKey(3), l3); c.insert(j3, l3);
// Check error // Check error
Vector actual = factor.unwhitenedError(c); Vector actual = factor.unwhitenedError(c);
@ -64,8 +65,8 @@ TEST( planarSLAM, BearingFactor )
TEST( planarSLAM, BearingFactor_equals ) TEST( planarSLAM, BearingFactor_equals )
{ {
planarSLAM::Bearing planarSLAM::Bearing
factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), sigma), factor1(i2, j3, Rot2::fromAngle(0.1), sigma),
factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(2.3), sigma); factor2(i2, j3, Rot2::fromAngle(2.3), sigma);
EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -76,12 +77,12 @@ TEST( planarSLAM, RangeFactor )
{ {
// Create factor // Create factor
double z(sqrt(2) - 0.22); // h(x) - z = 0.22 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 // create config
planarSLAM::Values c; planarSLAM::Values c;
c.insert(PoseKey(2), x2); c.insert(i2, x2);
c.insert(PointKey(3), l3); c.insert(j3, l3);
// Check error // Check error
Vector actual = factor.unwhitenedError(c); Vector actual = factor.unwhitenedError(c);
@ -91,7 +92,7 @@ TEST( planarSLAM, RangeFactor )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( planarSLAM, RangeFactor_equals ) 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(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -103,12 +104,12 @@ TEST( planarSLAM, BearingRangeFactor )
// Create factor // Create factor
Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 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 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 // create config
planarSLAM::Values c; planarSLAM::Values c;
c.insert(PoseKey(2), x2); c.insert(i2, x2);
c.insert(PointKey(3), l3); c.insert(j3, l3);
// Check error // Check error
Vector actual = factor.unwhitenedError(c); Vector actual = factor.unwhitenedError(c);
@ -119,8 +120,8 @@ TEST( planarSLAM, BearingRangeFactor )
TEST( planarSLAM, BearingRangeFactor_equals ) TEST( planarSLAM, BearingRangeFactor_equals )
{ {
planarSLAM::BearingRange planarSLAM::BearingRange
factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), 7.3, sigma2), factor1(i2, j3, Rot2::fromAngle(0.1), 7.3, sigma2),
factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(3), 2.0, sigma2); factor2(i2, j3, Rot2::fromAngle(3), 2.0, sigma2);
EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -130,13 +131,13 @@ TEST( planarSLAM, BearingRangeFactor_equals )
TEST( planarSLAM, BearingRangeFactor_poses ) TEST( planarSLAM, BearingRangeFactor_poses )
{ {
typedef BearingRangeFactor<Pose2,Pose2> PoseBearingRange; 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 ) 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(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -147,26 +148,26 @@ TEST( planarSLAM, constructor )
{ {
// create config // create config
planarSLAM::Values c; planarSLAM::Values c;
c.insert(PoseKey(2), x2); c.insert(i2, x2);
c.insert(PoseKey(3), x3); c.insert(i3, x3);
c.insert(PointKey(3), l3); c.insert(j3, l3);
// create graph // create graph
planarSLAM::Graph G; planarSLAM::Graph G;
// Add pose constraint // Add pose constraint
G.addPoseConstraint(2, x2); // make it feasible :-) G.addPoseConstraint(i2, x2); // make it feasible :-)
// Add odometry // 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 // Create bearing factor
Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 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 // Create range factor
double z2(sqrt(2) - 0.22); // h(x) - z = 0.22 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 expected0 = Vector_(3, 0.0, 0.0, 0.0);
Vector expected1 = 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; using namespace std;
typedef pose2SLAM::Odometry Pose2Factor; typedef pose2SLAM::Odometry Pose2Factor;
using pose2SLAM::PoseKey;
// common measurement covariance // common measurement covariance
static double sx=0.5, sy=0.5,st=0.1; 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 covariance(noiseModel::Gaussian::Covariance(cov));
//static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3)); //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 // Test constraint, small displacement
Vector f1(const Pose2& pose1, const Pose2& pose2) { Vector f1(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2.1130913087, 0.468461064817, 0.436332312999); 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); return constraint.evaluateError(pose1, pose2);
} }
TEST( Pose2SLAM, constraint1 ) TEST_UNSAFE( Pose2SLAM, constraint1 )
{ {
// create a factor between unknown poses p1 and p2 // create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999); 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; Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
@ -73,15 +70,15 @@ TEST( Pose2SLAM, constraint1 )
// Test constraint, large displacement // Test constraint, large displacement
Vector f2(const Pose2& pose1, const Pose2& pose2) { Vector f2(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2,2,M_PI_2); 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); return constraint.evaluateError(pose1, pose2);
} }
TEST( Pose2SLAM, constraint2 ) TEST_UNSAFE( Pose2SLAM, constraint2 )
{ {
// create a factor between unknown poses p1 and p2 // create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2,2,M_PI_2); 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; Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, 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 // create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2); 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 // create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2); Pose2 measured(2,2,M_PI_2);
Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance); Pose2Factor constraint(1, 2, measured, covariance);
pose2SLAM::Graph graph; pose2SLAM::Graph graph;
graph.addOdometry(1,2,measured, covariance); 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 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) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2)
pose2SLAM::Values config; pose2SLAM::Values config;
config.insert(pose2SLAM::PoseKey(1),p1); config.insert(1,p1);
config.insert(pose2SLAM::PoseKey(2),p2); config.insert(2,p2);
// Linearize // Linearize
Ordering ordering(*config.orderingArbitrary()); Ordering ordering(*config.orderingArbitrary());
boost::shared_ptr<FactorGraph<GaussianFactor> > lfg_linearized = graph.linearize(config, ordering); 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); Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0);
SharedDiagonal probModel1 = noiseModel::Unit::Create(3); 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)); 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 // create a Pose graph with one equality constraint and one measurement
pose2SLAM::Graph fg; pose2SLAM::Graph fg;
@ -155,12 +152,12 @@ TEST(Pose2SLAM, optimize) {
// Create initial config // Create initial config
Values initial; Values initial;
initial.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); initial.insert(0, Pose2(0,0,0));
initial.insert(pose2SLAM::PoseKey(1), Pose2(0,0,0)); initial.insert(1, Pose2(0,0,0));
// Choose an ordering and optimize // Choose an ordering and optimize
Ordering ordering; Ordering ordering;
ordering += kx0, kx1; ordering += 0, 1;
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15; params.relativeErrorTol = 1e-15;
@ -169,23 +166,23 @@ TEST(Pose2SLAM, optimize) {
// Check with expected config // Check with expected config
Values expected; Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); expected.insert(0, Pose2(0,0,0));
expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2)); expected.insert(1, Pose2(1,2,M_PI_2));
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
// Check marginals // Check marginals
Marginals marginals = fg.marginals(actual); Marginals marginals = fg.marginals(actual);
// Matrix expectedP0 = Infinity, as we have a pose constraint !? // Matrix expectedP0 = Infinity, as we have a pose constraint !?
// Matrix actualP0 = marginals.marginalCovariance(pose2SLAM::PoseKey(0)); // Matrix actualP0 = marginals.marginalCovariance(0);
// EQUALITY(expectedP0, actualP0); // EQUALITY(expectedP0, actualP0);
Matrix expectedP1 = cov; // the second pose really should have just the noise covariance 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); EQUALITY(expectedP1, actualP1);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// test optimization with 3 poses // test optimization with 3 poses, note we use plain Keys here, not symbols
TEST(Pose2SLAM, optimizeThreePoses) { TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) {
// Create a hexagon of poses // Create a hexagon of poses
pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0); pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0);
@ -207,7 +204,7 @@ TEST(Pose2SLAM, optimizeThreePoses) {
// Choose an ordering // Choose an ordering
Ordering ordering; Ordering ordering;
ordering += kx0,kx1,kx2; ordering += 0,1,2;
// optimize // optimize
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
@ -231,12 +228,12 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
pose2SLAM::Graph fg; pose2SLAM::Graph fg;
fg.addPoseConstraint(0, p0); fg.addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1); 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(1,2, delta, covariance);
fg.addOdometry(2,3, delta, covariance); fg.addOdometry(2,3, delta, covariance);
fg.addOdometry(3,4, delta, covariance); fg.addOdometry(3,4, delta, covariance);
fg.addOdometry(4,5, delta, covariance); fg.addOdometry(4,5, delta, covariance);
fg.addOdometry(5, 0, delta, covariance); fg.addOdometry(5,0, delta, covariance);
// Create initial config // Create initial config
pose2SLAM::Values initial; pose2SLAM::Values initial;
@ -249,7 +246,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
// Choose an ordering // Choose an ordering
Ordering ordering; Ordering ordering;
ordering += kx0,kx1,kx2,kx3,kx4,kx5; ordering += 0,1,2,3,4,5;
// optimize // optimize
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
@ -261,7 +258,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
CHECK(assert_equal((const Values&)hexagon, actual)); CHECK(assert_equal((const Values&)hexagon, actual));
// Check loop closure // 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"); // Pose2SLAMOptimizer myOptimizer("3");
@ -299,7 +296,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2SLAM, optimize2) { TEST_UNSAFE(Pose2SLAM, optimize2) {
// Pose2SLAMOptimizer myOptimizer("100"); // Pose2SLAMOptimizer myOptimizer("100");
// Matrix A1 = myOptimizer.a1(); // Matrix A1 = myOptimizer.a1();
// Matrix A2 = myOptimizer.a2(); // 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; // pose2SLAM::Graph G, T, C;
// G.addConstraint(1, 2, Pose2(0.,0.,0.), I3); // G.addConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addConstraint(1, 3, 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; // pose2SLAM::Graph G, T, C;
// G.addConstraint(1, 2, Pose2(0.,0.,0.), I3); // G.addConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addConstraint(1, 3, Pose2(0.,0.,0.), I3); // G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
@ -350,37 +347,37 @@ TEST(Pose2SLAM, optimize2) {
using namespace pose2SLAM; using namespace pose2SLAM;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2Values, pose2Circle ) TEST_UNSAFE(Pose2Values, pose2Circle )
{ {
// expected is 4 poses tangent to circle with radius 1m // expected is 4 poses tangent to circle with radius 1m
pose2SLAM::Values expected; pose2SLAM::Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2( 1, 0, M_PI_2)); expected.insert(0, Pose2( 1, 0, M_PI_2));
expected.insert(pose2SLAM::PoseKey(1), Pose2( 0, 1, - M_PI )); expected.insert(1, Pose2( 0, 1, - M_PI ));
expected.insert(pose2SLAM::PoseKey(2), Pose2(-1, 0, - M_PI_2)); expected.insert(2, Pose2(-1, 0, - M_PI_2));
expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1, 0 )); expected.insert(3, Pose2( 0, -1, 0 ));
pose2SLAM::Values actual = pose2SLAM::circle(4,1.0); pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2SLAM, expmap ) TEST_UNSAFE(Pose2SLAM, expmap )
{ {
// expected is circle shifted to right // expected is circle shifted to right
pose2SLAM::Values expected; pose2SLAM::Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1, 0, M_PI_2)); expected.insert(0, Pose2( 1.1, 0, M_PI_2));
expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1, 1, - M_PI )); expected.insert(1, Pose2( 0.1, 1, - M_PI ));
expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9, 0, - M_PI_2)); expected.insert(2, Pose2(-0.9, 0, - M_PI_2));
expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1, 0 )); expected.insert(3, Pose2( 0.1, -1, 0 ));
// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
pose2SLAM::Values circle(pose2SLAM::circle(4,1.0)); pose2SLAM::Values circle(pose2SLAM::circle(4,1.0));
Ordering ordering(*circle.orderingArbitrary()); Ordering ordering(*circle.orderingArbitrary());
VectorValues delta(circle.dims(ordering)); VectorValues delta(circle.dims(ordering));
delta[ordering[pose2SLAM::PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0); delta[ordering[0]] = Vector_(3, 0.0,-0.1,0.0);
delta[ordering[pose2SLAM::PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0); delta[ordering[1]] = Vector_(3, -0.1,0.0,0.0);
delta[ordering[pose2SLAM::PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0); delta[ordering[2]] = Vector_(3, 0.0,0.1,0.0);
delta[ordering[pose2SLAM::PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0); delta[ordering[3]] = Vector_(3, 0.1,0.0,0.0);
pose2SLAM::Values actual = circle.retract(delta, ordering); pose2SLAM::Values actual = circle.retract(delta, ordering);
CHECK(assert_equal(expected,actual)); 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) // Very simple test establishing Ax-b \approx z-h(x)
TEST( Pose2Prior, error ) TEST_UNSAFE( Pose2Prior, error )
{ {
// Choose a linearization point // Choose a linearization point
Pose2 p1(1, 0, 0); // robot at (1,0) Pose2 p1(1, 0, 0); // robot at (1,0)
pose2SLAM::Values x0; pose2SLAM::Values x0;
x0.insert(PoseKey(1), p1); x0.insert(1, p1);
// Create factor // Create factor
pose2SLAM::Prior factor(PoseKey(1), p1, sigmas); pose2SLAM::Prior factor(1, p1, sigmas);
// Actual linearization // Actual linearization
Ordering ordering(*x0.orderingArbitrary()); Ordering ordering(*x0.orderingArbitrary());
@ -407,14 +404,14 @@ TEST( Pose2Prior, error )
// Check error at x0, i.e. delta = zero ! // Check error at x0, i.e. delta = zero !
VectorValues delta(VectorValues::Zero(x0.dims(ordering))); 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); 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,factor.whitenedError(x0)));
CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
// Check error after increasing p2 // Check error after increasing p2
VectorValues addition(VectorValues::Zero(x0.dims(ordering))); 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; VectorValues plus = delta + addition;
pose2SLAM::Values x1 = x0.retract(plus, ordering); 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 ! 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 // common Pose2Prior for tests below
static gtsam::Pose2 priorVal(2,2,M_PI_2); 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 // 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 // Choose a linearization point at ground truth
pose2SLAM::Values x0; pose2SLAM::Values x0;
@ -448,12 +445,12 @@ TEST( Pose2Prior, linearize )
// Test with numerical derivative // Test with numerical derivative
Matrix numericalH = numericalDerivative11(hprior, priorVal); 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) // Very simple test establishing Ax-b \approx z-h(x)
TEST( Pose2Factor, error ) TEST_UNSAFE( Pose2Factor, error )
{ {
// Choose a linearization point // Choose a linearization point
Pose2 p1; // robot at origin Pose2 p1; // robot at origin
@ -464,7 +461,7 @@ TEST( Pose2Factor, error )
// Create factor // Create factor
Pose2 z = p1.between(p2); Pose2 z = p1.between(p2);
Pose2Factor factor(PoseKey(1), PoseKey(2), z, covariance); Pose2Factor factor(1, 2, z, covariance);
// Actual linearization // Actual linearization
Ordering ordering(*x0.orderingArbitrary()); Ordering ordering(*x0.orderingArbitrary());
@ -473,15 +470,15 @@ TEST( Pose2Factor, error )
// Check error at x0, i.e. delta = zero ! // Check error at x0, i.e. delta = zero !
VectorValues delta(x0.dims(ordering)); VectorValues delta(x0.dims(ordering));
delta[ordering[kx1]] = zero(3); delta[ordering[1]] = zero(3);
delta[ordering[kx2]] = zero(3); delta[ordering[2]] = zero(3);
Vector error_at_zero = Vector_(3,0.0,0.0,0.0); 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,factor.unwhitenedError(x0)));
CHECK(assert_equal(-error_at_zero, linear->error_vector(delta))); CHECK(assert_equal(-error_at_zero, linear->error_vector(delta)));
// Check error after increasing p2 // Check error after increasing p2
VectorValues plus = delta; 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); 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 ! 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))); CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
@ -491,17 +488,17 @@ TEST( Pose2Factor, error )
/* ************************************************************************* */ /* ************************************************************************* */
// common Pose2Factor for tests below // common Pose2Factor for tests below
static Pose2 measured(2,2,M_PI_2); 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 // 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 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) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4)
pose2SLAM::Values x0; pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1),p1); x0.insert(1,p1);
x0.insert(pose2SLAM::PoseKey(2),p2); x0.insert(2,p2);
// Actual linearization // Actual linearization
Ordering ordering(*x0.orderingArbitrary()); 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 // Choose a linearization point at ground truth
Pose2 p1(1,2,M_PI_2); Pose2 p1(1,2,M_PI_2);
Pose2 p2(-1,4,M_PI); Pose2 p2(-1,4,M_PI);
pose2SLAM::Values x0; pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1),p1); x0.insert(1,p1);
x0.insert(pose2SLAM::PoseKey(2),p2); x0.insert(2,p2);
// expected linearization // expected linearization
Matrix expectedH1 = covariance->Whiten(Matrix_(3,3, Matrix expectedH1 = covariance->Whiten(Matrix_(3,3,
@ -549,7 +546,7 @@ TEST( Pose2Factor, linearize )
// expected linear factor // expected linear factor
Ordering ordering(*x0.orderingArbitrary()); Ordering ordering(*x0.orderingArbitrary());
SharedDiagonal probModel1 = noiseModel::Unit::Create(3); 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 // Actual linearization
boost::shared_ptr<JacobianFactor> actual = boost::shared_ptr<JacobianFactor> actual =

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

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

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

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

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

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

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

@ -31,8 +31,8 @@ using namespace simulated2D;
TEST( simulated2D, Simulated2DValues ) TEST( simulated2D, Simulated2DValues )
{ {
simulated2D::Values actual; simulated2D::Values actual;
actual.insert(simulated2D::PoseKey(1),Point2(1,1)); actual.insert(1,Point2(1,1));
actual.insert(simulated2D::PointKey(2),Point2(2,2)); actual.insert(2,Point2(2,2));
EXPECT(assert_equal(actual,actual,1e-9)); 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 gtsam;
using namespace simulated2DOriented; 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 ) TEST( simulated2DOriented, Dprior )
{ {
@ -55,11 +59,11 @@ TEST( simulated2DOriented, constructor )
{ {
Pose2 measurement(0.2, 0.3, 0.1); Pose2 measurement(0.2, 0.3, 0.1);
SharedDiagonal model(Vector_(3, 1., 1., 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; simulated2DOriented::Values config;
config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2)); config.insert(kx(1), Pose2(1., 0., 0.2));
config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1)); config.insert(kx(2), Pose2(2., 0., 0.1));
boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary()); 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 * @file testStereoFactor.cpp
* @brief Unit test for StereoFactor * @brief Unit test for StereoFactor
* single camera
* @author Chris Beall * @author Chris Beall
*/ */
#include <CppUnitLite/TestHarness.h> #include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.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 std;
using namespace gtsam; using namespace gtsam;
using namespace boost;
using namespace visualSLAM;
Pose3 camera1(Matrix_(3,3, Pose3 camera1(Matrix_(3,3,
1., 0., 0., 1., 0., 0.,
@ -45,6 +41,10 @@ StereoCamera stereoCam(Pose3(), K);
Point3 p(0, 0, 5); Point3 p(0, 0, 5);
static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); 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) 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)); boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(visualSLAM::PoseConstraint(PoseKey(1),camera1)); graph.add(visualSLAM::PoseConstraint(kx(1),camera1));
StereoPoint2 z14(320,320.0-50, 240); StereoPoint2 z14(320,320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m) // 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 // Create a configuration corresponding to the ground truth
Values values; 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); 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); GaussNewtonOptimizer optimizer(graph, values);

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

@ -10,43 +10,45 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testGraph.cpp * @file testVisualSLAM.cpp
* @brief Unit test for two cameras and four landmarks * @brief Unit test for two cameras and four landmarks, single camera
* single camera
* @author Chris Beall * @author Chris Beall
* @author Frank Dellaert * @author Frank Dellaert
* @author Viorela Ila * @author Viorela Ila
*/ */
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
using namespace boost; using namespace boost;
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/visualSLAM.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace boost;
using namespace visualSLAM; /* ************************************************************************* */
static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
/* ************************************************************************* */ // Convenience for named keys
Point3 landmark1(-1.0,-1.0, 0.0); Key kx(size_t i) { return Symbol('x',i); }
Point3 landmark2(-1.0, 1.0, 0.0); Key kl(size_t i) { return Symbol('l',i); }
Point3 landmark3( 1.0, 1.0, 0.0);
Point3 landmark4( 1.0,-1.0, 0.0);
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., 1., 0., 0.,
0.,-1., 0., 0.,-1., 0.,
0., 0.,-1. 0., 0.,-1.
), ),
Point3(0,0,6.25)); Point3(0,0,6.25));
Pose3 camera2(Matrix_(3,3, static Pose3 camera2(Matrix_(3,3,
1., 0., 0., 1., 0., 0.,
0.,-1., 0., 0.,-1., 0.,
0., 0.,-1. 0., 0.,-1.
@ -66,14 +68,14 @@ visualSLAM::Graph testGraph() {
shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
visualSLAM::Graph g; visualSLAM::Graph g;
g.addMeasurement(z11, sigma, 1, 1, sK); g.addMeasurement(z11, sigma, kx(1), kl(1), sK);
g.addMeasurement(z12, sigma, 1, 2, sK); g.addMeasurement(z12, sigma, kx(1), kl(2), sK);
g.addMeasurement(z13, sigma, 1, 3, sK); g.addMeasurement(z13, sigma, kx(1), kl(3), sK);
g.addMeasurement(z14, sigma, 1, 4, sK); g.addMeasurement(z14, sigma, kx(1), kl(4), sK);
g.addMeasurement(z21, sigma, 2, 1, sK); g.addMeasurement(z21, sigma, kx(2), kl(1), sK);
g.addMeasurement(z22, sigma, 2, 2, sK); g.addMeasurement(z22, sigma, kx(2), kl(2), sK);
g.addMeasurement(z23, sigma, 2, 3, sK); g.addMeasurement(z23, sigma, kx(2), kl(3), sK);
g.addMeasurement(z24, sigma, 2, 4, sK); g.addMeasurement(z24, sigma, kx(2), kl(4), sK);
return g; return g;
} }
@ -83,22 +85,22 @@ TEST( Graph, optimizeLM)
// build a graph // build a graph
visualSLAM::Graph graph(testGraph()); visualSLAM::Graph graph(testGraph());
// add 3 landmark constraints // add 3 landmark constraints
graph.addPointConstraint(1, landmark1); graph.addPointConstraint(kl(1), landmark1);
graph.addPointConstraint(2, landmark2); graph.addPointConstraint(kl(2), landmark2);
graph.addPointConstraint(3, landmark3); graph.addPointConstraint(kl(3), landmark3);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
Values initialEstimate; Values initialEstimate;
initialEstimate.insert(PoseKey(1), camera1); initialEstimate.insert(kx(1), camera1);
initialEstimate.insert(PoseKey(2), camera2); initialEstimate.insert(kx(2), camera2);
initialEstimate.insert(PointKey(1), landmark1); initialEstimate.insert(kl(1), landmark1);
initialEstimate.insert(PointKey(2), landmark2); initialEstimate.insert(kl(2), landmark2);
initialEstimate.insert(PointKey(3), landmark3); initialEstimate.insert(kl(3), landmark3);
initialEstimate.insert(PointKey(4), landmark4); initialEstimate.insert(kl(4), landmark4);
// Create an ordering of the variables // Create an ordering of the variables
Ordering ordering; 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 // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
@ -121,21 +123,21 @@ TEST( Graph, optimizeLM2)
// build a graph // build a graph
visualSLAM::Graph graph(testGraph()); visualSLAM::Graph graph(testGraph());
// add 2 camera constraints // add 2 camera constraints
graph.addPoseConstraint(1, camera1); graph.addPoseConstraint(kx(1), camera1);
graph.addPoseConstraint(2, camera2); graph.addPoseConstraint(kx(2), camera2);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
Values initialEstimate; Values initialEstimate;
initialEstimate.insert(PoseKey(1), camera1); initialEstimate.insert(kx(1), camera1);
initialEstimate.insert(PoseKey(2), camera2); initialEstimate.insert(kx(2), camera2);
initialEstimate.insert(PointKey(1), landmark1); initialEstimate.insert(kl(1), landmark1);
initialEstimate.insert(PointKey(2), landmark2); initialEstimate.insert(kl(2), landmark2);
initialEstimate.insert(PointKey(3), landmark3); initialEstimate.insert(kl(3), landmark3);
initialEstimate.insert(PointKey(4), landmark4); initialEstimate.insert(kl(4), landmark4);
// Create an ordering of the variables // Create an ordering of the variables
Ordering ordering; 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 // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
@ -158,17 +160,17 @@ TEST( Graph, CHECK_ORDERING)
// build a graph // build a graph
visualSLAM::Graph graph = testGraph(); visualSLAM::Graph graph = testGraph();
// add 2 camera constraints // add 2 camera constraints
graph.addPoseConstraint(1, camera1); graph.addPoseConstraint(kx(1), camera1);
graph.addPoseConstraint(2, camera2); graph.addPoseConstraint(kx(2), camera2);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
Values initialEstimate; Values initialEstimate;
initialEstimate.insert(PoseKey(1), camera1); initialEstimate.insert(kx(1), camera1);
initialEstimate.insert(PoseKey(2), camera2); initialEstimate.insert(kx(2), camera2);
initialEstimate.insert(PointKey(1), landmark1); initialEstimate.insert(kl(1), landmark1);
initialEstimate.insert(PointKey(2), landmark2); initialEstimate.insert(kl(2), landmark2);
initialEstimate.insert(PointKey(3), landmark3); initialEstimate.insert(kl(3), landmark3);
initialEstimate.insert(PointKey(4), landmark4); initialEstimate.insert(kl(4), landmark4);
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // 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 // this test ensures that if the update for delta is larger than
// the size of the config, it only updates existing variables // the size of the config, it only updates existing variables
Values init; Values init;
init.insert(PoseKey(1), Pose3()); init.insert(kx(1), Pose3());
init.insert(PointKey(1), Point3(1.0, 2.0, 3.0)); init.insert(kl(1), Point3(1.0, 2.0, 3.0));
Values expected; Values expected;
expected.insert(PoseKey(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); expected.insert(kx(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(PointKey(1), Point3(1.1, 2.1, 3.1)); expected.insert(kl(1), Point3(1.1, 2.1, 3.1));
Ordering largeOrdering; Ordering largeOrdering;
Values largeValues = init; Values largeValues = init;
largeValues.insert(PoseKey(2), Pose3()); largeValues.insert(kx(2), Pose3());
largeOrdering += PoseKey(1),PointKey(1),PoseKey(2); largeOrdering += kx(1),kl(1),kx(2);
VectorValues delta(largeValues.dims(largeOrdering)); 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[kx(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[kl(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(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
Values actual = init.retract(delta, largeOrdering); Values actual = init.retract(delta, largeOrdering);
CHECK(assert_equal(expected,actual)); 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, void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Index poseKey, Index pointKey, const shared_ptrK& K) { Key poseKey, Key pointKey, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K)); boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, poseKey, pointKey, K));
push_back(factor); push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addPoseConstraint(Index poseKey, const Pose3& p) { void Graph::addPoseConstraint(Key poseKey, const Pose3& p) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(PoseKey(poseKey), p)); boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(poseKey, p));
push_back(factor); push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addPointConstraint(Index pointKey, const Point3& p) { void Graph::addPointConstraint(Key pointKey, const Point3& p) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(PointKey(pointKey), p)); boost::shared_ptr<PointConstraint> factor(new PointConstraint(pointKey, p));
push_back(factor); push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addPosePrior(Index poseKey, const Pose3& p, const SharedNoiseModel& model) { void Graph::addPosePrior(Key poseKey, const Pose3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PosePrior> factor(new PosePrior(PoseKey(poseKey), p, model)); boost::shared_ptr<PosePrior> factor(new PosePrior(poseKey, p, model));
push_back(factor); push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addPointPrior(Index pointKey, const Point3& p, const SharedNoiseModel& model) { void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PointPrior> factor(new PointPrior(PointKey(pointKey), p, model)); boost::shared_ptr<PointPrior> factor(new PointPrior(pointKey, p, model));
push_back(factor); push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model) { void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) {
push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, 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; 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. * Typedefs that make up the visualSLAM namespace.
*/ */
@ -85,21 +79,21 @@ namespace visualSLAM {
* @param K shared pointer to calibration object * @param K shared pointer to calibration object
*/ */
void addMeasurement(const Point2& measured, const SharedNoiseModel& model, 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) * Add a constraint on a pose (for now, *must* be satisfied in any Values)
* @param key variable key of the camera pose * @param key variable key of the camera pose
* @param p to which pose to constrain it to * @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) * Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param key variable key of the landmark * @param key variable key of the landmark
* @param p point around which soft prior is defined * @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 * Add a prior on a pose
@ -107,7 +101,7 @@ namespace visualSLAM {
* @param p around which soft prior is defined * @param p around which soft prior is defined
* @param model uncertainty model of this prior * @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 * Add a prior on a landmark
@ -115,7 +109,7 @@ namespace visualSLAM {
* @param p to which point to constrain it to * @param p to which point to constrain it to
* @param model uncertainty model of this prior * @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 * Add a range prior to a landmark
@ -124,7 +118,7 @@ namespace visualSLAM {
* @param range approximate range to landmark * @param range approximate range to landmark
* @param model uncertainty model of this prior * @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 * Optimize the graph

11
gtsam_unstable/slam/simulated3D.h
View File

@ -36,12 +36,6 @@ namespace simulated3D {
* the simulated2D domain. * 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 * 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 poseKey is the pose key of the robot
* @param pointKey is the point key for the landmark * @param pointKey is the point key for the landmark
*/ */
Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, Key i, Key j) :
Key poseKey, Key pointKey) : NoiseModelFactor2<Point3, Point3>(model, i, j), measured_(measured) {}
NoiseModelFactor2<Point3, Point3>(model, poseKey, pointKey), measured_(measured) {}
/** /**
* Error function with optional derivatives * Error function with optional derivatives

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

@ -30,8 +30,8 @@ using namespace simulated3D;
TEST( simulated3D, Values ) TEST( simulated3D, Values )
{ {
Values actual; Values actual;
actual.insert(simulated3D::PointKey(1),Point3(1,1,1)); actual.insert(Symbol('l',1),Point3(1,1,1));
actual.insert(simulated3D::PoseKey(2),Point3(2,2,2)); actual.insert(Symbol('x',2),Point3(2,2,2));
EXPECT(assert_equal(actual,actual,1e-9)); 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); SharedDiagonal hard_model1 = noiseModel::Constrained::All(1);
// some simple inequality constraints // some simple inequality constraints
Symbol key(simulated2D::PoseKey(1)); Symbol key('x',1);
double mu = 10.0; double mu = 10.0;
// greater than // greater than
iq2D::PoseXInequality constraint1(key, 1.0, true, mu); 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 // config far from minimum
Point2 x0(3,0); Point2 x0(3,0);
boost::shared_ptr<Values> config(new Values); boost::shared_ptr<Values> config(new Values);
config->insert(simulated2D::PoseKey(1), x0); config->insert(Symbol('x',1), x0);
// ordering // ordering
shared_ptr<Ordering> ord(new Ordering()); shared_ptr<Ordering> ord(new Ordering());
ord->push_back(simulated2D::PoseKey(1)); ord->push_back(Symbol('x',1));
double Delta = 1.0; double Delta = 1.0;
for(size_t it=0; it<10; ++it) { for(size_t it=0; it<10; ++it) {

423
tests/testGaussianISAM2.cpp
View File

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

43
tests/testGaussianJunctionTreeB.cpp
View File

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

54
tests/testGraph.cpp
View File

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

26
tests/testInferenceB.cpp
View File

@ -25,6 +25,10 @@
using namespace std; using namespace std;
using namespace gtsam; 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 // The tests below test the *generic* inference algorithms. Some of these have
// specialized versions in the derived classes GaussianFactorGraph etc... // specialized versions in the derived classes GaussianFactorGraph etc...
@ -52,23 +56,23 @@ TEST( inference, marginals2)
SharedDiagonal poseModel(sharedSigma(3, 0.1)); SharedDiagonal poseModel(sharedSigma(3, 0.1));
SharedDiagonal pointModel(sharedSigma(3, 0.1)); SharedDiagonal pointModel(sharedSigma(3, 0.1));
fg.addPrior(0, Pose2(), poseModel); fg.addPrior(kx(0), Pose2(), poseModel);
fg.addOdometry(0, 1, Pose2(1.0,0.0,0.0), poseModel); fg.addOdometry(kx(0), kx(1), Pose2(1.0,0.0,0.0), poseModel);
fg.addOdometry(1, 2, Pose2(1.0,0.0,0.0), poseModel); fg.addOdometry(kx(1), kx(2), Pose2(1.0,0.0,0.0), poseModel);
fg.addBearingRange(0, 0, Rot2(), 1.0, pointModel); fg.addBearingRange(kx(0), kl(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(1, 0, Rot2(), 1.0, pointModel); fg.addBearingRange(kx(1), kl(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(2, 0, Rot2(), 1.0, pointModel); fg.addBearingRange(kx(2), kl(0), Rot2(), 1.0, pointModel);
Values init; Values init;
init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0)); init.insert(kx(0), Pose2(0.0,0.0,0.0));
init.insert(planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0)); init.insert(kx(1), Pose2(1.0,0.0,0.0));
init.insert(planarSLAM::PoseKey(2), Pose2(2.0,0.0,0.0)); init.insert(kx(2), Pose2(2.0,0.0,0.0));
init.insert(planarSLAM::PointKey(0), Point2(1.0,1.0)); init.insert(kl(0), Point2(1.0,1.0));
Ordering ordering(*fg.orderingCOLAMD(init)); Ordering ordering(*fg.orderingCOLAMD(init));
FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering)); FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
GaussianMultifrontalSolver solver(*gfg); 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; VGraph graph;
// create equality constraints for poses // create equality constraints for poses
graph.addPoseConstraint(1, camera1.pose()); graph.addPoseConstraint(x1, camera1.pose());
graph.addPoseConstraint(2, camera2.pose()); graph.addPoseConstraint(x2, camera2.pose());
// create factors // create factors
SharedDiagonal vmodel = noiseModel::Unit::Create(3); SharedDiagonal vmodel = noiseModel::Unit::Create(3);
graph.addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK); graph.addMeasurement(camera1.project(landmark), vmodel, x1, l1, shK);
graph.addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK); graph.addMeasurement(camera2.project(landmark), vmodel, x2, l2, shK);
// add equality constraint // add equality constraint
graph.add(Point3Equality(l1, l2)); graph.add(Point3Equality(l1, l2));

97
tests/testNonlinearFactor.cpp
View File

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

34
tests/testNonlinearOptimizer.cpp
View File

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