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

Added clone() functionality to nonlinear factors

release/4.3a0
Alex Cunningham 2012-05-21 20:54:40 +00:00
parent 75673e28f1
commit 54f6f3629c
26 changed files with 292 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

339
.cproject
View File

@ -311,6 +311,14 @@
<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>
@ -337,7 +345,6 @@
</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>
@ -345,7 +352,6 @@
</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>
@ -393,7 +399,6 @@
</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>
@ -401,7 +406,6 @@
</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>
@ -409,7 +413,6 @@
</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>
@ -425,20 +428,11 @@
</target> </target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget> <buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -465,6 +459,7 @@
</target> </target>
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget> <buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -536,6 +531,7 @@
</target> </target>
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testInference.run</buildTarget> <buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -543,6 +539,7 @@
</target> </target>
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactor.run</buildTarget> <buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -550,6 +547,7 @@
</target> </target>
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget> <buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -557,6 +555,7 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="tests" 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>
@ -564,6 +563,7 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="tests" 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>
@ -665,22 +665,6 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -697,6 +681,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <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,26 +721,18 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>all</buildTarget> <buildTarget>testValues.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>check</buildTarget> <buildTarget>testOrdering.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -777,18 +769,26 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>testValues.run</buildTarget> <buildTarget>all</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>testOrdering.run</buildTarget> <buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -1049,6 +1049,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testNonlinearFactor.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testNonlinearFactor.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>
@ -1139,7 +1147,6 @@
</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>
@ -1595,6 +1602,7 @@
</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>
@ -1634,6 +1642,7 @@
</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>
@ -1641,6 +1650,7 @@
</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>
@ -1832,6 +1842,7 @@
</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>
@ -1853,6 +1864,102 @@
<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>
@ -2054,7 +2161,6 @@
</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>
@ -2062,7 +2168,6 @@
</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>
@ -2070,7 +2175,6 @@
</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>
@ -2078,7 +2182,6 @@
</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>
@ -2092,98 +2195,42 @@
<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"> <target name="wrap.testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testRot3.run</buildTarget> <buildTarget>wrap.testSpirit.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="wrap.testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testRot2.run</buildTarget> <buildTarget>wrap.testWrap.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testPose3.run</buildTarget> <buildTarget>check.wrap</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="wrap_gtsam" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>timeRot3.run</buildTarget> <buildTarget>wrap_gtsam</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testPose2.run</buildTarget> <buildTarget>wrap</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>
@ -2227,46 +2274,6 @@
<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>

2
examples/CameraResectioning.cpp
View File

@ -44,6 +44,8 @@ public:
virtual ~ResectioningFactor() {} virtual ~ResectioningFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(ResectioningFactor)
virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H = boost::none) const { virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H = boost::none) const {
SimpleCamera camera(*K_, X); SimpleCamera camera(*K_, X);
Point2 reprojectionError(camera.project(P_, H) - p_); Point2 reprojectionError(camera.project(P_, H) - p_);

4
examples/LocalizationExample2.cpp
View File

@ -39,12 +39,16 @@ public:
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {} NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
virtual ~UnaryFactor() {}
Vector evaluateError(const Pose2& q, Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const boost::optional<Matrix&> H = boost::none) const
{ {
if (H) (*H) = Matrix_(2,3, 1.0,0.0,0.0, 0.0,1.0,0.0); if (H) (*H) = Matrix_(2,3, 1.0,0.0,0.0, 0.0,1.0,0.0);
return Vector_(2, q.x() - mx_, q.y() - my_); return Vector_(2, q.x() - mx_, q.y() - my_);
} }
ADD_CLONE_NONLINEAR_FACTOR(UnaryFactor)
}; };
/** /**

8
gtsam/nonlinear/NonlinearEquality.h
View File

@ -161,6 +161,8 @@ namespace gtsam {
return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model)); return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model));
} }
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @} /// @}
private: private:
@ -190,6 +192,7 @@ namespace gtsam {
protected: protected:
typedef NoiseModelFactor1<VALUE> Base; typedef NoiseModelFactor1<VALUE> Base;
typedef NonlinearEquality1<VALUE> This;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
NonlinearEquality1() {} NonlinearEquality1() {}
@ -209,6 +212,8 @@ namespace gtsam {
virtual ~NonlinearEquality1() {} virtual ~NonlinearEquality1() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** g(x) with optional derivative */ /** g(x) with optional derivative */
Vector evaluateError(const X& x1, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const X& x1, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = eye(x1.dim()); if (H) (*H) = eye(x1.dim());
@ -248,6 +253,7 @@ namespace gtsam {
protected: protected:
typedef NoiseModelFactor2<VALUE, VALUE> Base; typedef NoiseModelFactor2<VALUE, VALUE> Base;
typedef NonlinearEquality2<VALUE> This;
GTSAM_CONCEPT_MANIFOLD_TYPE(X); GTSAM_CONCEPT_MANIFOLD_TYPE(X);
@ -263,6 +269,8 @@ namespace gtsam {
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
virtual ~NonlinearEquality2() {} virtual ~NonlinearEquality2() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** g(x) with optional derivative2 */ /** g(x) with optional derivative2 */
Vector evaluateError(const X& x1, const X& x2, Vector evaluateError(const X& x1, const X& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,

16
gtsam/nonlinear/NonlinearFactor.h
View File

@ -35,6 +35,14 @@
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
/**
* Macro to add a standard clone function to a derived factor
*/
#define ADD_CLONE_NONLINEAR_FACTOR(Derived) \
virtual gtsam::NonlinearFactor::shared_ptr clone() const { \
return boost::static_pointer_cast<gtsam::NonlinearFactor>( \
gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); }
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
@ -146,6 +154,14 @@ public:
return IndexFactor::shared_ptr(new IndexFactor(indices)); return IndexFactor::shared_ptr(new IndexFactor(indices));
} }
/**
* Creates a shared_ptr clone of the factor - needs to be specialized to allow
* for subclasses
*
* Default implementation will slice the factor
*/
virtual shared_ptr clone() const =0;
}; // \class NonlinearFactor }; // \class NonlinearFactor
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -163,6 +163,8 @@ namespace gtsam {
return linearize(z_, u, p, j1, j2); return linearize(z_, u, p, j1, j2);
} }
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @} /// @}
}; };

2
gtsam/slam/AntiFactor.h
View File

@ -50,6 +50,8 @@ namespace gtsam {
virtual ~AntiFactor() {} virtual ~AntiFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */

2
gtsam/slam/BearingFactor.h
View File

@ -55,6 +55,8 @@ namespace gtsam {
virtual ~BearingFactor() {} virtual ~BearingFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */ /** h(x)-z -> between(z,h(x)) for Rot2 manifold */
Vector evaluateError(const Pose& pose, const Point& point, Vector evaluateError(const Pose& pose, const Point& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {

2
gtsam/slam/BearingRangeFactor.h
View File

@ -57,6 +57,8 @@ namespace gtsam {
virtual ~BearingRangeFactor() {} virtual ~BearingRangeFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** Print */ /** Print */
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << ": BearingRangeFactor(" std::cout << s << ": BearingRangeFactor("

2
gtsam/slam/BetweenFactor.h
View File

@ -62,6 +62,8 @@ namespace gtsam {
virtual ~BetweenFactor() {} virtual ~BetweenFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */

2
gtsam/slam/GeneralSFMFactor.h
View File

@ -61,6 +61,8 @@ namespace gtsam {
virtual ~GeneralSFMFactor() {} ///< destructor virtual ~GeneralSFMFactor() {} ///< destructor
ADD_CLONE_NONLINEAR_FACTOR(This)
/** /**
* print * print
* @param s optional string naming the factor * @param s optional string naming the factor

2
gtsam/slam/PartialPriorFactor.h
View File

@ -86,6 +86,8 @@ namespace gtsam {
this->fillH(); this->fillH();
} }
ADD_CLONE_NONLINEAR_FACTOR(This)
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */

2
gtsam/slam/PriorFactor.h
View File

@ -56,6 +56,8 @@ namespace gtsam {
Base(model, key), prior_(prior) { Base(model, key), prior_(prior) {
} }
ADD_CLONE_NONLINEAR_FACTOR(This)
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */

2
gtsam/slam/ProjectionFactor.h
View File

@ -70,6 +70,8 @@ namespace gtsam {
/** Virtual destructor */ /** Virtual destructor */
virtual ~GenericProjectionFactor() {} virtual ~GenericProjectionFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** /**
* print * print
* @param s optional string naming the factor * @param s optional string naming the factor

2
gtsam/slam/RangeFactor.h
View File

@ -51,6 +51,8 @@ namespace gtsam {
virtual ~RangeFactor() {} virtual ~RangeFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** h(x)-z */ /** h(x)-z */
Vector evaluateError(const Pose& pose, const Point& point, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { Vector evaluateError(const Pose& pose, const Point& point, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double hx = pose.range(point, H1, H2); double hx = pose.range(point, H1, H2);

3
gtsam/slam/StereoFactor.h
View File

@ -34,6 +34,7 @@ public:
// shorthand for base class type // shorthand for base class type
typedef NoiseModelFactor2<POSE, LANDMARK> Base; ///< typedef for base class typedef NoiseModelFactor2<POSE, LANDMARK> Base; ///< typedef for base class
typedef GenericStereoFactor<POSE, LANDMARK> This; ///< typedef for this class (with templates)
typedef boost::shared_ptr<GenericStereoFactor> shared_ptr; ///< typedef for shared pointer to this object typedef boost::shared_ptr<GenericStereoFactor> shared_ptr; ///< typedef for shared pointer to this object
typedef POSE CamPose; ///< typedef for Pose Lie Value type typedef POSE CamPose; ///< typedef for Pose Lie Value type
@ -56,6 +57,8 @@ public:
virtual ~GenericStereoFactor() {} ///< Virtual destructor virtual ~GenericStereoFactor() {} ///< Virtual destructor
ADD_CLONE_NONLINEAR_FACTOR(This)
/** /**
* print * print
* @param s optional string naming the factor * @param s optional string naming the factor

24
gtsam/slam/simulated2D.h
View File

@ -123,6 +123,7 @@ namespace simulated2D {
class GenericPrior: public NoiseModelFactor1<VALUE> { class GenericPrior: public NoiseModelFactor1<VALUE> {
public: public:
typedef NoiseModelFactor1<VALUE> Base; ///< base class typedef NoiseModelFactor1<VALUE> Base; ///< base class
typedef GenericPrior<VALUE> This;
typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr; typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type typedef VALUE Pose; ///< shortcut to Pose type
@ -138,11 +139,14 @@ namespace simulated2D {
return (prior(x, H) - measured_).vector(); return (prior(x, H) - measured_).vector();
} }
virtual ~GenericPrior() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
private: private:
/// Default constructor /// Default constructor
GenericPrior() { GenericPrior() { }
}
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
@ -160,6 +164,7 @@ namespace simulated2D {
class GenericOdometry: public NoiseModelFactor2<VALUE, VALUE> { class GenericOdometry: public NoiseModelFactor2<VALUE, VALUE> {
public: public:
typedef NoiseModelFactor2<VALUE, VALUE> Base; ///< base class typedef NoiseModelFactor2<VALUE, VALUE> Base; ///< base class
typedef GenericOdometry<VALUE> This;
typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr; typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type typedef VALUE Pose; ///< shortcut to Pose type
@ -177,11 +182,14 @@ namespace simulated2D {
return (odo(x1, x2, H1, H2) - measured_).vector(); return (odo(x1, x2, H1, H2) - measured_).vector();
} }
virtual ~GenericOdometry() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
private: private:
/// Default constructor /// Default constructor
GenericOdometry() { GenericOdometry() { }
}
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
@ -199,6 +207,7 @@ namespace simulated2D {
class GenericMeasurement: public NoiseModelFactor2<POSE, LANDMARK> { class GenericMeasurement: public NoiseModelFactor2<POSE, LANDMARK> {
public: public:
typedef NoiseModelFactor2<POSE, LANDMARK> Base; ///< base class typedef NoiseModelFactor2<POSE, LANDMARK> Base; ///< base class
typedef GenericMeasurement<POSE, LANDMARK> This;
typedef boost::shared_ptr<GenericMeasurement<POSE, LANDMARK> > shared_ptr; typedef boost::shared_ptr<GenericMeasurement<POSE, LANDMARK> > shared_ptr;
typedef POSE Pose; ///< shortcut to Pose type typedef POSE Pose; ///< shortcut to Pose type
typedef LANDMARK Landmark; ///< shortcut to Landmark type typedef LANDMARK Landmark; ///< shortcut to Landmark type
@ -217,11 +226,14 @@ namespace simulated2D {
return (mea(x1, x2, H1, H2) - measured_).vector(); return (mea(x1, x2, H1, H2) - measured_).vector();
} }
virtual ~GenericMeasurement() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
private: private:
/// Default constructor /// Default constructor
GenericMeasurement() { GenericMeasurement() { }
}
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;

9
gtsam/slam/simulated2DConstraints.h
View File

@ -53,11 +53,14 @@ namespace simulated2D {
template<class VALUE, unsigned int IDX> template<class VALUE, unsigned int IDX>
struct ScalarCoordConstraint1: public BoundingConstraint1<VALUE> { struct ScalarCoordConstraint1: public BoundingConstraint1<VALUE> {
typedef BoundingConstraint1<VALUE> Base; ///< Base class convenience typedef typedef BoundingConstraint1<VALUE> Base; ///< Base class convenience typedef
typedef ScalarCoordConstraint1<VALUE, IDX> This; ///< This class convenience typedef
typedef boost::shared_ptr<ScalarCoordConstraint1<VALUE, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef typedef boost::shared_ptr<ScalarCoordConstraint1<VALUE, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
typedef VALUE Point; ///< Constrained variable type typedef VALUE Point; ///< Constrained variable type
virtual ~ScalarCoordConstraint1() {} virtual ~ScalarCoordConstraint1() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** /**
* Constructor for constraint * Constructor for constraint
* @param key is the label for the constrained variable * @param key is the label for the constrained variable
@ -116,10 +119,13 @@ namespace simulated2D {
template<class VALUE> template<class VALUE>
struct MaxDistanceConstraint : public BoundingConstraint2<VALUE, VALUE> { struct MaxDistanceConstraint : public BoundingConstraint2<VALUE, VALUE> {
typedef BoundingConstraint2<VALUE, VALUE> Base; ///< Base class for factor typedef BoundingConstraint2<VALUE, VALUE> Base; ///< Base class for factor
typedef MaxDistanceConstraint<VALUE> This; ///< This class for factor
typedef VALUE Point; ///< Type of variable constrained typedef VALUE Point; ///< Type of variable constrained
virtual ~MaxDistanceConstraint() {} virtual ~MaxDistanceConstraint() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** /**
* Primary constructor for factor * Primary constructor for factor
* @param key1 is the first variable key * @param key1 is the first variable key
@ -158,11 +164,14 @@ namespace simulated2D {
template<class POSE, class POINT> template<class POSE, class POINT>
struct MinDistanceConstraint : public BoundingConstraint2<POSE, POINT> { struct MinDistanceConstraint : public BoundingConstraint2<POSE, POINT> {
typedef BoundingConstraint2<POSE, POINT> Base; ///< Base class for factor typedef BoundingConstraint2<POSE, POINT> Base; ///< Base class for factor
typedef MinDistanceConstraint<POSE, POINT> This; ///< This class for factor
typedef POSE Pose; ///< Type of pose variable constrained typedef POSE Pose; ///< Type of pose variable constrained
typedef POINT Point; ///< Type of point variable constrained typedef POINT Point; ///< Type of point variable constrained
virtual ~MinDistanceConstraint() {} virtual ~MinDistanceConstraint() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** /**
* Primary constructor for factor * Primary constructor for factor
* @param key1 is the first variable key * @param key1 is the first variable key

6
gtsam/slam/simulated2DOriented.h
View File

@ -102,6 +102,8 @@ namespace simulated2DOriented {
struct GenericOdometry: public NoiseModelFactor2<VALUE, VALUE> { struct GenericOdometry: public NoiseModelFactor2<VALUE, VALUE> {
Pose2 measured_; ///< Between measurement for odometry factor Pose2 measured_; ///< Between measurement for odometry factor
typedef GenericOdometry<VALUE> This;
/** /**
* Creates an odometry factor between two poses * Creates an odometry factor between two poses
*/ */
@ -110,6 +112,8 @@ namespace simulated2DOriented {
NoiseModelFactor2<VALUE, VALUE>(model, i1, i2), measured_(measured) { NoiseModelFactor2<VALUE, VALUE>(model, i1, i2), measured_(measured) {
} }
virtual ~GenericOdometry() {}
/// Evaluate error and optionally derivative /// Evaluate error and optionally derivative
Vector evaluateError(const VALUE& x1, const VALUE& x2, Vector evaluateError(const VALUE& x1, const VALUE& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
@ -117,6 +121,8 @@ namespace simulated2DOriented {
return measured_.localCoordinates(odo(x1, x2, H1, H2)); return measured_.localCoordinates(odo(x1, x2, H1, H2));
} }
ADD_CLONE_NONLINEAR_FACTOR(This)
}; };
typedef GenericOdometry<Pose2> Odometry; typedef GenericOdometry<Pose2> Odometry;

2
gtsam/slam/smallExample.cpp
View File

@ -211,6 +211,8 @@ namespace example {
return (h(x) - z_).vector(); return (h(x) - z_).vector();
} }
ADD_CLONE_NONLINEAR_FACTOR(UnaryFactor)
}; };
} }

2
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -51,6 +51,8 @@ public:
virtual ~FullIMUFactor() {} virtual ~FullIMUFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const {
const This* const f = dynamic_cast<const This*>(&e); const This* const f = dynamic_cast<const This*>(&e);

2
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -44,6 +44,8 @@ public:
virtual ~IMUFactor() {} virtual ~IMUFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const {
const This* const f = dynamic_cast<const This*>(&e); const This* const f = dynamic_cast<const This*>(&e);

2
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -72,6 +72,8 @@ public:
virtual ~VelocityConstraint() {} virtual ~VelocityConstraint() {}
ADD_CLONE_NONLINEAR_FACTOR(VelocityConstraint)
/** /**
* Calculates the error for trapezoidal model given * Calculates the error for trapezoidal model given
*/ */

1
tests/CMakeLists.txt
View File

@ -13,6 +13,7 @@ set(tests_local_libs
# note the source dir on each # note the source dir on each
set (tests_exclude set (tests_exclude
"${CMAKE_CURRENT_SOURCE_DIR}/testPose2SLAMwSPCG.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/testPose2SLAMwSPCG.cpp"
#"${CMAKE_CURRENT_SOURCE_DIR}/testOccupancyGrid.cpp"
) )
# Build tests # Build tests

4
tests/testExtendedKalmanFilter.cpp
View File

@ -120,6 +120,8 @@ public:
virtual ~NonlinearMotionModel() {} virtual ~NonlinearMotionModel() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
// Calculate the next state prediction using the nonlinear function f() // Calculate the next state prediction using the nonlinear function f()
T f(const T& x_t0) const { T f(const T& x_t0) const {
@ -262,6 +264,8 @@ public:
virtual ~NonlinearMeasurementModel() {} virtual ~NonlinearMeasurementModel() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
// Calculate the predicted measurement using the nonlinear function h() // Calculate the predicted measurement using the nonlinear function h()
// Byproduct: updates Jacobian H, and noiseModel (R) // Byproduct: updates Jacobian H, and noiseModel (R)
Vector h(const T& x_t1) const { Vector h(const T& x_t1) const {

20
tests/testNonlinearFactor.cpp
View File

@ -254,6 +254,8 @@ public:
} }
return (Vector(1) << x1 + x2 + x3 + x4).finished(); return (Vector(1) << x1 + x2 + x3 + x4).finished();
} }
ADD_CLONE_NONLINEAR_FACTOR(TestFactor4)
}; };
/* ************************************ */ /* ************************************ */
@ -301,6 +303,8 @@ public:
} }
return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished(); return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished();
} }
ADD_CLONE_NONLINEAR_FACTOR(TestFactor5)
}; };
/* ************************************ */ /* ************************************ */
@ -353,6 +357,8 @@ public:
} }
return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished(); return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished();
} }
ADD_CLONE_NONLINEAR_FACTOR(TestFactor6)
}; };
/* ************************************ */ /* ************************************ */
@ -385,6 +391,20 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
} }
/* ************************************************************************* */
TEST( NonlinearFactor, clone )
{
shared_nlf init(new TestFactor4());
EXPECT_LONGS_EQUAL(PoseKey(1), init->keys()[0]);
EXPECT_LONGS_EQUAL(PoseKey(2), init->keys()[1]);
EXPECT_LONGS_EQUAL(PoseKey(3), init->keys()[2]);
EXPECT_LONGS_EQUAL(PoseKey(4), init->keys()[3]);
shared_nlf actClone = init->clone();
EXPECT(actClone.get() != init.get()); // Ensure different pointers
EXPECT(assert_equal(*init, *actClone));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */