Moved geometry components from MastSLAM for Pose3Upright, BearingS2, SimWall2D and SimPolygon2D
Alex Cunningham
parent
777a2eb037
commit
3c2e037b16
352
.cproject
352
.cproject
|
|
@ -1,7 +1,5 @@
|
|||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<?fileVersion 4.0.0?>
|
||||
|
||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<storageModule moduleId="org.eclipse.cdt.core.settings">
|
||||
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
|
||||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
|
||||
|
|
@ -309,6 +307,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -335,7 +341,6 @@
|
|||
</target>
|
||||
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -343,7 +348,6 @@
|
|||
</target>
|
||||
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBinaryBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -391,7 +395,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -399,7 +402,6 @@
|
|||
</target>
|
||||
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -407,7 +409,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -423,20 +424,11 @@
|
|||
</target>
|
||||
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testBayesTree</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPoseRTV.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -525,22 +517,6 @@
|
|||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -557,6 +533,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -581,26 +573,34 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testValues.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testOrdering.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testKey.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testLinearContainerFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -685,34 +685,26 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testValues.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testOrdering.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testKey.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testLinearContainerFactor.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1063,7 +1055,6 @@
|
|||
</target>
|
||||
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1071,7 +1062,6 @@
|
|||
</target>
|
||||
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testJunctionTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1079,7 +1069,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1239,7 +1228,6 @@
|
|||
</target>
|
||||
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testErrors.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1285,6 +1273,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="build/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="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1365,14 +1361,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="build/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="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1703,6 +1691,7 @@
|
|||
</target>
|
||||
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1742,6 +1731,7 @@
|
|||
</target>
|
||||
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1749,6 +1739,7 @@
|
|||
</target>
|
||||
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated3D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1948,6 +1939,7 @@
|
|||
</target>
|
||||
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testGaussianISAM2</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1969,6 +1961,102 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>timeRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCal3_S2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testHomography2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j3</buildArguments>
|
||||
|
|
@ -2170,7 +2258,6 @@
|
|||
</target>
|
||||
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G DEB</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2178,7 +2265,6 @@
|
|||
</target>
|
||||
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G RPM</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2186,7 +2272,6 @@
|
|||
</target>
|
||||
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G TGZ</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2194,7 +2279,6 @@
|
|||
</target>
|
||||
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2320,98 +2404,42 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="check.geometry_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check.geometry_unstable</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot2.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSpirit.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testWrap.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>timeRot3.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check.wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCal3_S2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testHomography2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -2455,38 +2483,6 @@
|
|||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSpirit.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testWrap.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check.wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
</buildTargets>
|
||||
</storageModule>
|
||||
</cproject>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,76 @@
|
|||
/**
|
||||
* @file BearingS2.cpp
|
||||
*
|
||||
* @date Jan 26, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <gtsam_unstable/geometry/BearingS2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BearingS2::print(const std::string& s) const {
|
||||
cout << s << " azimuth: " << azimuth_.theta() << " elevation: " << elevation_.theta() << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool BearingS2::equals(const BearingS2& x, double tol) const {
|
||||
return azimuth_.equals(x.azimuth_, tol) && elevation_.equals(x.elevation_, tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) {
|
||||
// Cnb = DCMnb(Att);
|
||||
Matrix Cnb = A.rotation().matrix().transpose();
|
||||
|
||||
// Cbc = [0,0,1;0,1,0;-1,0,0];
|
||||
Matrix Cbc = Matrix_(3,3,
|
||||
0.,0.,1.,
|
||||
0.,1.,0.,
|
||||
-1.,0.,0.);
|
||||
// p_rel_c = Cbc*Cnb*(PosObj - Pos);
|
||||
Vector p_rel_c = Cbc*Cnb*(B.vector() - A.translation().vector());
|
||||
|
||||
// FIXME: the matlab code checks for p_rel_c(0) greater than
|
||||
|
||||
// azi = atan2(p_rel_c(2),p_rel_c(1));
|
||||
double azimuth = atan2(p_rel_c(1),p_rel_c(0));
|
||||
// elev = atan2(p_rel_c(3),sqrt(p_rel_c(1)^2 + p_rel_c(2)^2));
|
||||
double elevation = atan2(p_rel_c(2),sqrt(p_rel_c(0) * p_rel_c(0) + p_rel_c(1) * p_rel_c(1)));
|
||||
return BearingS2(azimuth, elevation);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
BearingS2 BearingS2::fromForwardObservation(const Pose3& A, const Point3& B) {
|
||||
// Cnb = DCMnb(Att);
|
||||
Matrix Cnb = A.rotation().matrix().transpose();
|
||||
|
||||
Vector p_rel_c = Cnb*(B.vector() - A.translation().vector());
|
||||
|
||||
// FIXME: the matlab code checks for p_rel_c(0) greater than
|
||||
|
||||
// azi = atan2(p_rel_c(2),p_rel_c(1));
|
||||
double azimuth = atan2(p_rel_c(1),p_rel_c(0));
|
||||
// elev = atan2(p_rel_c(3),sqrt(p_rel_c(1)^2 + p_rel_c(2)^2));
|
||||
double elevation = atan2(p_rel_c(2),sqrt(p_rel_c(0) * p_rel_c(0) + p_rel_c(1) * p_rel_c(1)));
|
||||
return BearingS2(azimuth, elevation);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
BearingS2 BearingS2::retract(const Vector& v) const {
|
||||
assert(v.size() == 2);
|
||||
return BearingS2(azimuth_.retract(v.head(1)), elevation_.retract(v.tail(1)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector BearingS2::localCoordinates(const BearingS2& x) const {
|
||||
return Vector_(2, azimuth_.localCoordinates(x.azimuth_)(0),
|
||||
elevation_.localCoordinates(x.elevation_)(0));
|
||||
}
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
/**
|
||||
* @file BearingS2.h
|
||||
*
|
||||
* @brief Manifold measurement between two points on a unit sphere
|
||||
*
|
||||
* @date Jan 26, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Rot2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class BearingS2 : public DerivedValue<BearingS2> {
|
||||
protected:
|
||||
Rot2 azimuth_, elevation_;
|
||||
|
||||
public:
|
||||
static const size_t dimension = 2;
|
||||
|
||||
// constructors
|
||||
|
||||
/** Default constructor - straight ahead */
|
||||
BearingS2() {}
|
||||
|
||||
/** Build from components */
|
||||
BearingS2(double azimuth, double elevation)
|
||||
: azimuth_(Rot2::fromAngle(azimuth)), elevation_(Rot2::fromAngle(elevation)) {}
|
||||
|
||||
BearingS2(const Rot2& azimuth, const Rot2& elevation)
|
||||
: azimuth_(azimuth), elevation_(elevation) {}
|
||||
|
||||
// access
|
||||
const Rot2& azimuth() const { return azimuth_; }
|
||||
const Rot2& elevation() const { return elevation_; }
|
||||
|
||||
/// @}
|
||||
/// @name Measurements
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Observation function for downwards-facing camera
|
||||
*/
|
||||
// FIXME: will not work for TARGET = Point3
|
||||
template<class POSE, class TARGET>
|
||||
static BearingS2 fromDownwardsObservation(const POSE& A, const TARGET& B) {
|
||||
return fromDownwardsObservation(A.pose(), B.translation());
|
||||
}
|
||||
|
||||
static BearingS2 fromDownwardsObservation(const Pose3& A, const Point3& B);
|
||||
|
||||
/** Observation function with standard, forwards-facing camera */
|
||||
static BearingS2 fromForwardObservation(const Pose3& A, const Point3& B);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/** assert equality up to a tolerance */
|
||||
bool equals(const BearingS2& x, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// Dimensionality of tangent space = 2 DOF - used to autodetect sizes
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/// Dimensionality of tangent space = 2 DOF
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/// Retraction from R^2 to BearingS2 manifold neighborhood around current pose
|
||||
/// Tangent space parameterization is [azimuth elevation]
|
||||
BearingS2 retract(const Vector& v) const;
|
||||
|
||||
/// Local coordinates of BearingS2 manifold neighborhood around current pose
|
||||
Vector localCoordinates(const BearingS2& p2) const;
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(azimuth_);
|
||||
ar & BOOST_SERIALIZATION_NVP(elevation_);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
@ -4,10 +4,11 @@ install(FILES ${geometry_headers} DESTINATION include/gtsam_unstable/geometry)
|
|||
|
||||
# Components to link tests in this subfolder against
|
||||
set(geometry_local_libs
|
||||
#geometry_unstable # No sources currently, so no convenience lib
|
||||
geometry_unstable
|
||||
geometry
|
||||
base
|
||||
ccolamd
|
||||
linear
|
||||
)
|
||||
|
||||
set (geometry_full_libs
|
||||
|
|
|
|||
|
|
@ -0,0 +1,158 @@
|
|||
/**
|
||||
* @file Pose3Upright.cpp
|
||||
*
|
||||
* @date Jan 24, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <gtsam_unstable/geometry/Pose3Upright.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright::Pose3Upright(const Rot2& bearing, const Point3& t)
|
||||
: T_(bearing, Point2(t.x(), t.y())), z_(t.z())
|
||||
{
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright::Pose3Upright(double x, double y, double z, double theta)
|
||||
: T_(x, y, theta), z_(z)
|
||||
{
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright::Pose3Upright(const Pose2& pose, double z)
|
||||
: T_(pose), z_(z)
|
||||
{
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright::Pose3Upright(const Pose3& x)
|
||||
: T_(x.x(), x.y(), x.rotation().yaw()), z_(x.z())
|
||||
{
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Pose3Upright::print(const std::string& s) const {
|
||||
cout << s << "(" << T_.x() << ", " << T_.y() << ", " << z_ << ", " << T_.theta() << ")" << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Pose3Upright::equals(const Pose3Upright& x, double tol) const {
|
||||
return T_.equals(x.T_, tol) && fabs(z_ - x.z_) < tol;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3Upright::translation() const {
|
||||
return Point3(x(), y(), z());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Pose3Upright::translation2() const {
|
||||
return T_.t();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Pose3Upright::rotation2() const {
|
||||
return T_.r();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Pose3Upright::rotation() const {
|
||||
return Rot3::yaw(theta());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2 Pose3Upright::pose2() const {
|
||||
return T_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3Upright::pose() const {
|
||||
return Pose3(rotation(), translation());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright Pose3Upright::inverse(boost::optional<Matrix&> H1) const {
|
||||
Pose3Upright result(T_.inverse(H1), -z_);
|
||||
if (H1) {
|
||||
Matrix H1_ = -eye(4,4);
|
||||
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
|
||||
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
|
||||
*H1 = H1_;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright Pose3Upright::compose(const Pose3Upright& p2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (!H1 && !H2)
|
||||
return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_);
|
||||
Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_);
|
||||
if (H1) {
|
||||
Matrix H1_ = eye(4,4);
|
||||
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
|
||||
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
|
||||
*H1 = H1_;
|
||||
}
|
||||
if (H2) *H2 = eye(4,4);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright Pose3Upright::between(const Pose3Upright& p2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (!H1 && !H2)
|
||||
return Pose3Upright(T_.between(p2.T_), p2.z_ - z_);
|
||||
Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_);
|
||||
if (H1) {
|
||||
Matrix H1_ = -eye(4,4);
|
||||
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
|
||||
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
|
||||
*H1 = H1_;
|
||||
}
|
||||
if (H2) *H2 = eye(4,4);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright Pose3Upright::retract(const Vector& v) const {
|
||||
assert(v.size() == 4);
|
||||
Vector v1(3); v1 << v(0), v(1), v(3);
|
||||
return Pose3Upright(T_.retract(v1), z_ + v(2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3Upright::localCoordinates(const Pose3Upright& p2) const {
|
||||
Vector pose2 = T_.localCoordinates(p2.pose2());
|
||||
Vector result(4);
|
||||
result << pose2(0), pose2(1), p2.z() - z_, pose2(2);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright Pose3Upright::Expmap(const Vector& xi) {
|
||||
assert(xi.size() == 4);
|
||||
Vector v1(3); v1 << xi(0), xi(1), xi(3);
|
||||
return Pose3Upright(Pose2::Expmap(v1), xi(2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3Upright::Logmap(const Pose3Upright& p) {
|
||||
Vector pose2 = Pose2::Logmap(p.pose2());
|
||||
Vector result(4);
|
||||
result << pose2(0), pose2(1), p.z(), pose2(2);
|
||||
return result;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,144 @@
|
|||
/**
|
||||
* @file Pose3Upright.h
|
||||
*
|
||||
* @brief Variation of a Pose3 in which the rotation is constained to purely yaw
|
||||
* This state is essentially a Pose2 with a z component, with conversions to
|
||||
* higher and lower dimensional states.
|
||||
*
|
||||
* @date Jan 24, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A 3D Pose with fixed pitch and roll
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class Pose3Upright : public DerivedValue<Pose3Upright> {
|
||||
public:
|
||||
static const size_t dimension = 4;
|
||||
|
||||
protected:
|
||||
|
||||
Pose2 T_;
|
||||
double z_;
|
||||
|
||||
public:
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor initializes at origin
|
||||
Pose3Upright() : z_(0.0) {}
|
||||
|
||||
/// Copy constructor
|
||||
Pose3Upright(const Pose3Upright& x) : T_(x.T_), z_(x.z_) {}
|
||||
Pose3Upright(const Rot2& bearing, const Point3& t);
|
||||
Pose3Upright(double x, double y, double z, double theta);
|
||||
Pose3Upright(const Pose2& pose, double z);
|
||||
|
||||
/// Down-converts from a full Pose3
|
||||
Pose3Upright(const Pose3& fullpose);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/** assert equality up to a tolerance */
|
||||
bool equals(const Pose3Upright& pose, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
double x() const { return T_.x(); }
|
||||
double y() const { return T_.y(); }
|
||||
double z() const { return z_; }
|
||||
double theta() const { return T_.theta(); }
|
||||
|
||||
Point2 translation2() const;
|
||||
Point3 translation() const;
|
||||
Rot2 rotation2() const;
|
||||
Rot3 rotation() const;
|
||||
Pose2 pose2() const;
|
||||
Pose3 pose() const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/// Dimensionality of tangent space = 4 DOF
|
||||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/// Retraction from R^4 to Pose3Upright manifold neighborhood around current pose
|
||||
/// Tangent space parameterization is [x y z theta]
|
||||
Pose3Upright retract(const Vector& v) const;
|
||||
|
||||
/// Local 3D coordinates of Pose3Upright manifold neighborhood around current pose
|
||||
Vector localCoordinates(const Pose3Upright& p2) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
static Pose3Upright identity() { return Pose3Upright(); }
|
||||
|
||||
/// inverse transformation with derivatives
|
||||
Pose3Upright inverse(boost::optional<Matrix&> H1=boost::none) const;
|
||||
|
||||
///compose this transformation onto another (first *this and then p2)
|
||||
Pose3Upright compose(const Pose3Upright& p2,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/// compose syntactic sugar
|
||||
inline Pose3Upright operator*(const Pose3Upright& T) const { return compose(T); }
|
||||
|
||||
/**
|
||||
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||
* as well as optionally the derivatives
|
||||
*/
|
||||
Pose3Upright between(const Pose3Upright& p2,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/// Exponential map at identity - create a rotation from canonical coordinates
|
||||
static Pose3Upright Expmap(const Vector& xi);
|
||||
|
||||
/// Log map at identity - return the canonical coordinates of this rotation
|
||||
static Vector Logmap(const Pose3Upright& p);
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(T_);
|
||||
ar & BOOST_SERIALIZATION_NVP(z_);
|
||||
}
|
||||
|
||||
}; // \class Pose3Upright
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
@ -0,0 +1,331 @@
|
|||
/**
|
||||
* @file SimPolygon2D.cpp
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/random/linear_congruential.hpp>
|
||||
#include <boost/random/uniform_real.hpp>
|
||||
#include <boost/random/normal_distribution.hpp>
|
||||
#include <boost/random/variate_generator.hpp>
|
||||
|
||||
#include <gtsam_unstable/geometry/SimPolygon2D.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
const size_t max_it = 100000;
|
||||
boost::minstd_rand SimPolygon2D::rng(42u);
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SimPolygon2D::seedGenerator(unsigned long seed) {
|
||||
rng = boost::minstd_rand(seed);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SimPolygon2D SimPolygon2D::createTriangle(const Point2& pA, const Point2& pB, const Point2& pC) {
|
||||
SimPolygon2D result;
|
||||
result.landmarks_.push_back(pA);
|
||||
result.landmarks_.push_back(pB);
|
||||
result.landmarks_.push_back(pC);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SimPolygon2D SimPolygon2D::createRectangle(const Point2& p, double height, double width) {
|
||||
SimPolygon2D result;
|
||||
result.landmarks_.push_back(p);
|
||||
result.landmarks_.push_back(p + Point2(width, 0.0));
|
||||
result.landmarks_.push_back(p + Point2(width, height));
|
||||
result.landmarks_.push_back(p + Point2(0.0, height));
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const {
|
||||
if (p.size() != size()) return false;
|
||||
for (size_t i=0; i<size(); ++i)
|
||||
if (!landmarks_[i].equals(p.landmarks_[i], tol))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SimPolygon2D::print(const string& s) const {
|
||||
cout << "SimPolygon " << s << ": " << endl;
|
||||
BOOST_FOREACH(const Point2& p, landmarks_)
|
||||
p.print(" ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
vector<SimWall2D> SimPolygon2D::walls() const {
|
||||
vector<SimWall2D> result;
|
||||
for (size_t i=0; i<size()-1; ++i)
|
||||
result.push_back(SimWall2D(landmarks_[i], landmarks_[i+1]));
|
||||
result.push_back(SimWall2D(landmarks_[size()-1], landmarks_[0]));
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool SimPolygon2D::contains(const Point2& c) const {
|
||||
vector<SimWall2D> edges = walls();
|
||||
bool initialized = false;
|
||||
bool lastSide = false;
|
||||
BOOST_FOREACH(const SimWall2D& ab, edges) {
|
||||
// compute cross product of ab and ac
|
||||
Point2 dab = ab.b() - ab.a();
|
||||
Point2 dac = c - ab.a();
|
||||
double cross = dab.x() * dac.y() - dab.y() * dac.x();
|
||||
if (fabs(cross) < 1e-6) // check for on one of the edges
|
||||
return true;
|
||||
bool side = cross > 0;
|
||||
// save the first side found
|
||||
if (!initialized) {
|
||||
lastSide = side;
|
||||
initialized = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
// to be inside the polygon, point must be on the same side of all lines
|
||||
if (lastSide != side)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool SimPolygon2D::overlaps(const SimPolygon2D& p) const {
|
||||
BOOST_FOREACH(const Point2& a, landmarks_)
|
||||
if (p.contains(a))
|
||||
return true;
|
||||
BOOST_FOREACH(const Point2& a, p.landmarks_)
|
||||
if (contains(a))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
bool SimPolygon2D::anyContains(const Point2& p, const vector<SimPolygon2D>& obstacles) {
|
||||
BOOST_FOREACH(const SimPolygon2D& poly, obstacles)
|
||||
if (poly.contains(p))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool SimPolygon2D::anyOverlaps(const SimPolygon2D& p, const vector<SimPolygon2D>& obstacles) {
|
||||
BOOST_FOREACH(const SimPolygon2D& poly, obstacles)
|
||||
if (poly.overlaps(p))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SimPolygon2D SimPolygon2D::randomTriangle(
|
||||
double side_len, double mean_side_len, double sigma_side_len,
|
||||
double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
|
||||
// get the current set of landmarks
|
||||
std::vector<gtsam::Point2> lms;
|
||||
double d2 = side_len/2.0;
|
||||
lms.push_back(Point2( d2, d2));
|
||||
lms.push_back(Point2(-d2, d2));
|
||||
lms.push_back(Point2(-d2,-d2));
|
||||
lms.push_back(Point2( d2,-d2));
|
||||
|
||||
BOOST_FOREACH(const SimPolygon2D& poly, existing_polys)
|
||||
lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());
|
||||
|
||||
for (size_t i=0; i<max_it; ++i) {
|
||||
|
||||
// find a random pose for line AB
|
||||
Pose2 xA(randomAngle(), randomBoundedPoint2(side_len, lms, existing_polys, min_vertex_dist));
|
||||
|
||||
// extend line by random dist and angle to get BC
|
||||
double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len);
|
||||
double tABC = randomAngle().theta();
|
||||
Pose2 xB = xA.retract(Vector_(3, dAB, 0.0, tABC));
|
||||
|
||||
// extend from B to find C
|
||||
double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len);
|
||||
Pose2 xC = xB.retract(gtsam::delta(3, 0, dBC));
|
||||
|
||||
// use triangle equality to verify non-degenerate triangle
|
||||
double dAC = xA.t().dist(xC.t());
|
||||
|
||||
// form a triangle and test if it meets requirements
|
||||
SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t());
|
||||
|
||||
// check inside walls, long enough edges, far away from landmarks
|
||||
const double thresh = mean_side_len / 2.0;
|
||||
if ((dAB + dBC + thresh > dAC) && // triangle inequality
|
||||
(dAB + dAC + thresh > dBC) &&
|
||||
(dAC + dBC + thresh > dAB) &&
|
||||
insideBox(side_len, test_tri.landmark(0)) &&
|
||||
insideBox(side_len, test_tri.landmark(1)) &&
|
||||
insideBox(side_len, test_tri.landmark(2)) &&
|
||||
test_tri.landmark(1).dist(test_tri.landmark(2)) > min_side_len &&
|
||||
!nearExisting(lms, test_tri.landmark(0), min_vertex_dist) &&
|
||||
!nearExisting(lms, test_tri.landmark(1), min_vertex_dist) &&
|
||||
!nearExisting(lms, test_tri.landmark(2), min_vertex_dist) &&
|
||||
!anyOverlaps(test_tri, existing_polys)) {
|
||||
return test_tri;
|
||||
}
|
||||
}
|
||||
throw runtime_error("Could not find space for a triangle");
|
||||
return SimPolygon2D::createTriangle(Point2(99,99), Point2(99,99), Point2(99,99));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SimPolygon2D SimPolygon2D::randomRectangle(
|
||||
double side_len, double mean_side_len, double sigma_side_len,
|
||||
double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
|
||||
// get the current set of landmarks
|
||||
std::vector<gtsam::Point2> lms;
|
||||
double d2 = side_len/2.0;
|
||||
lms.push_back(Point2( d2, d2));
|
||||
lms.push_back(Point2(-d2, d2));
|
||||
lms.push_back(Point2(-d2,-d2));
|
||||
lms.push_back(Point2( d2,-d2));
|
||||
BOOST_FOREACH(const SimPolygon2D& poly, existing_polys)
|
||||
lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());
|
||||
|
||||
const Point2 lower_corner(-side_len,-side_len);
|
||||
const Point2 upper_corner( side_len, side_len);
|
||||
|
||||
for (size_t i=0; i<max_it; ++i) {
|
||||
|
||||
// pick height and width to be viable distances
|
||||
double height = randomDistance(mean_side_len, sigma_side_len, min_side_len);
|
||||
double width = randomDistance(mean_side_len, sigma_side_len, min_side_len);
|
||||
|
||||
// find a starting point - limited to region viable for this height/width
|
||||
Point2 pA = randomBoundedPoint2(lower_corner, upper_corner - Point2(width, height),
|
||||
lms, existing_polys, min_vertex_dist);
|
||||
|
||||
// verify
|
||||
SimPolygon2D rect = SimPolygon2D::createRectangle(pA, height, width);
|
||||
|
||||
// check inside walls, long enough edges, far away from landmarks
|
||||
if (insideBox(side_len, rect.landmark(0)) &&
|
||||
insideBox(side_len, rect.landmark(1)) &&
|
||||
insideBox(side_len, rect.landmark(2)) &&
|
||||
insideBox(side_len, rect.landmark(3)) &&
|
||||
!nearExisting(lms, rect.landmark(0), min_vertex_dist) &&
|
||||
!nearExisting(lms, rect.landmark(1), min_vertex_dist) &&
|
||||
!nearExisting(lms, rect.landmark(2), min_vertex_dist) &&
|
||||
!nearExisting(lms, rect.landmark(3), min_vertex_dist) &&
|
||||
!anyOverlaps(rect, existing_polys)) {
|
||||
return rect;
|
||||
}
|
||||
}
|
||||
throw runtime_error("Could not find space for a rectangle");
|
||||
return SimPolygon2D::createRectangle(Point2(99,99), 100, 100);
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
Point2 SimPolygon2D::randomPoint2(double s) {
|
||||
boost::uniform_real<> gen_t(-s/2.0, s/2.0);
|
||||
return Point2(gen_t(rng), gen_t(rng));
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
Rot2 SimPolygon2D::randomAngle() {
|
||||
boost::uniform_real<> gen_r(-M_PI, M_PI); // modified range to avoid degenerate cases in triangles
|
||||
return Rot2::fromAngle(gen_r(rng));
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
double SimPolygon2D::randomDistance(double mu, double sigma, double min_dist) {
|
||||
boost::normal_distribution<double> norm_dist(mu, sigma);
|
||||
boost::variate_generator<boost::minstd_rand&, boost::normal_distribution<double> > gen_d(rng, norm_dist);
|
||||
double d = -10.0;
|
||||
for (size_t i=0; i<max_it; ++i) {
|
||||
d = fabs(gen_d());
|
||||
if (d > min_dist)
|
||||
return d;
|
||||
}
|
||||
cout << "Non viable distance: " << d << " with mu = " << mu << " sigma = " << sigma
|
||||
<< " min_dist = " << min_dist << endl;
|
||||
throw runtime_error("Failed to find a viable distance");
|
||||
return fabs(norm_dist(rng));
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
|
||||
const vector<SimPolygon2D>& obstacles) {
|
||||
for (size_t i=0; i<max_it; ++i) {
|
||||
Point2 p = randomPoint2(boundary_size);
|
||||
if (!anyContains(p, obstacles))
|
||||
return p;
|
||||
}
|
||||
throw runtime_error("Failed to find a place for a landmark!");
|
||||
return Point2();
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
|
||||
const std::vector<gtsam::Point2>& landmarks, double min_landmark_dist) {
|
||||
for (size_t i=0; i<max_it; ++i) {
|
||||
Point2 p = randomPoint2(boundary_size);
|
||||
if (!nearExisting(landmarks, p, min_landmark_dist))
|
||||
return p;
|
||||
}
|
||||
throw runtime_error("Failed to find a place for a landmark!");
|
||||
return Point2();
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
|
||||
const std::vector<gtsam::Point2>& landmarks,
|
||||
const vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
|
||||
for (size_t i=0; i<max_it; ++i) {
|
||||
Point2 p = randomPoint2(boundary_size);
|
||||
if (!nearExisting(landmarks, p, min_landmark_dist) && !anyContains(p, obstacles))
|
||||
return p;
|
||||
}
|
||||
throw runtime_error("Failed to find a place for a landmark!");
|
||||
return Point2();
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
Point2 SimPolygon2D::randomBoundedPoint2(
|
||||
const gtsam::Point2& LL_corner, const gtsam::Point2& UR_corner,
|
||||
const std::vector<gtsam::Point2>& landmarks,
|
||||
const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
|
||||
|
||||
boost::uniform_real<> gen_x(0.0, UR_corner.x() - LL_corner.x());
|
||||
boost::uniform_real<> gen_y(0.0, UR_corner.y() - LL_corner.y());
|
||||
|
||||
for (size_t i=0; i<max_it; ++i) {
|
||||
Point2 p = Point2(gen_x(rng), gen_y(rng)) + LL_corner;
|
||||
if (!nearExisting(landmarks, p, min_landmark_dist) && !anyContains(p, obstacles))
|
||||
return p;
|
||||
}
|
||||
throw runtime_error("Failed to find a place for a landmark!");
|
||||
return Point2();
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
Pose2 SimPolygon2D::randomFreePose(double boundary_size, const vector<SimPolygon2D>& obstacles) {
|
||||
return Pose2(randomAngle(), randomBoundedPoint2(boundary_size, obstacles));
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
bool SimPolygon2D::insideBox(double s, const Point2& p) {
|
||||
return fabs(p.x()) < s/2.0 && fabs(p.y()) < s/2.0;
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
bool SimPolygon2D::nearExisting(const std::vector<gtsam::Point2>& S,
|
||||
const Point2& p, double threshold) {
|
||||
BOOST_FOREACH(const gtsam::Point2& Sp, S)
|
||||
if (Sp.dist(p) < threshold)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
} //\namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,136 @@
|
|||
/**
|
||||
* @file SimPolygon2D.h
|
||||
* @brief Polygons for simulation use
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam_unstable/geometry/SimWall2D.h>
|
||||
#include <boost/random/linear_congruential.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* General polygon class for convex polygons
|
||||
*/
|
||||
class SimPolygon2D {
|
||||
protected:
|
||||
std::vector<gtsam::Point2> landmarks_;
|
||||
static boost::minstd_rand rng;
|
||||
|
||||
public:
|
||||
|
||||
/** Don't use this constructor, use a named one instead */
|
||||
SimPolygon2D() {}
|
||||
|
||||
/** seed the random number generator - only needs to be done once */
|
||||
static void seedGenerator(unsigned long seed);
|
||||
|
||||
/** Named constructor for creating triangles */
|
||||
static SimPolygon2D createTriangle(const gtsam::Point2& pA, const gtsam::Point2& pB, const gtsam::Point2& pC);
|
||||
|
||||
/**
|
||||
* Named constructor for creating axis-aligned rectangles
|
||||
* @param p is the lower-left corner
|
||||
*/
|
||||
static SimPolygon2D createRectangle(const gtsam::Point2& p, double height, double width);
|
||||
|
||||
/**
|
||||
* Randomly generate a triangle that does not conflict with others
|
||||
* Uniformly distributed over box area, with normally distributed lengths of edges
|
||||
* THROWS: std::runtime_error if can't find a position
|
||||
*/
|
||||
static SimPolygon2D randomTriangle(double side_len, double mean_side_len, double sigma_side_len,
|
||||
double min_vertex_dist, double min_side_len, const std::vector<SimPolygon2D>& existing_polys);
|
||||
|
||||
/**
|
||||
* Randomly generate a rectangle that does not conflict with others
|
||||
* Uniformly distributed over box area, with normally distributed lengths of edges
|
||||
* THROWS: std::runtime_error if can't find a position
|
||||
*/
|
||||
static SimPolygon2D randomRectangle(double side_len, double mean_side_len, double sigma_side_len,
|
||||
double min_vertex_dist, double min_side_len, const std::vector<SimPolygon2D>& existing_polys);
|
||||
|
||||
// access to underlying points
|
||||
const gtsam::Point2& landmark(size_t i) const { return landmarks_[i]; }
|
||||
size_t size() const { return landmarks_.size(); }
|
||||
const std::vector<gtsam::Point2>& vertices() const { return landmarks_; }
|
||||
|
||||
// testable requirements
|
||||
bool equals(const SimPolygon2D& p, double tol=1e-5) const;
|
||||
void print(const std::string& s="") const;
|
||||
|
||||
/**
|
||||
* Get a set of walls along the edges
|
||||
*/
|
||||
std::vector<SimWall2D> walls() const;
|
||||
|
||||
/**
|
||||
* Core function for randomly generating scenarios.
|
||||
* Polygons are closed, convex shapes.
|
||||
* @return true if the given point is contained by this polygon
|
||||
*/
|
||||
bool contains(const gtsam::Point2& p) const;
|
||||
|
||||
/**
|
||||
* Checks two polygons to determine if they overlap
|
||||
* @return true iff at least one vertex of one polygon is contained in the other
|
||||
*/
|
||||
bool overlaps(const SimPolygon2D& p) const;
|
||||
|
||||
/** returns true iff p is contained in any of a set of polygons */
|
||||
static bool anyContains(const gtsam::Point2& p, const std::vector<SimPolygon2D>& obstacles);
|
||||
|
||||
/** returns true iff polygon p overlaps with any of a set of polygons */
|
||||
static bool anyOverlaps(const SimPolygon2D& p, const std::vector<SimPolygon2D>& obstacles);
|
||||
|
||||
/** returns true iff p is inside a square centered at zero with side s */
|
||||
static bool insideBox(double s, const gtsam::Point2& p);
|
||||
|
||||
/** returns true iff p is within threshold of any point in S */
|
||||
static bool nearExisting(const std::vector<gtsam::Point2>& S,
|
||||
const gtsam::Point2& p, double threshold);
|
||||
|
||||
/** pick a random point uniformly over a box of side s */
|
||||
static gtsam::Point2 randomPoint2(double s);
|
||||
|
||||
/** randomly generate a Rot2 with a uniform distribution over theta */
|
||||
static gtsam::Rot2 randomAngle();
|
||||
|
||||
/** generate a distance from a normal distribution given a mean and sigma */
|
||||
static double randomDistance(double mu, double sigma, double min_dist = -1.0);
|
||||
|
||||
/** pick a random point within a box that is further than dist d away from existing landmarks */
|
||||
static gtsam::Point2 randomBoundedPoint2(double boundary_size,
|
||||
const std::vector<gtsam::Point2>& landmarks, double min_landmark_dist);
|
||||
|
||||
/** pick a random point within a box that meets above requirements, as well as staying out of obstacles */
|
||||
static gtsam::Point2 randomBoundedPoint2(double boundary_size,
|
||||
const std::vector<gtsam::Point2>& landmarks,
|
||||
const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);
|
||||
|
||||
/** pick a random point that only avoid obstacles */
|
||||
static gtsam::Point2 randomBoundedPoint2(double boundary_size,
|
||||
const std::vector<SimPolygon2D>& obstacles);
|
||||
|
||||
/** pick a random point in box defined by lower left and upper right corners */
|
||||
static gtsam::Point2 randomBoundedPoint2(
|
||||
const gtsam::Point2& LL_corner, const gtsam::Point2& UR_corner,
|
||||
const std::vector<gtsam::Point2>& landmarks,
|
||||
const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);
|
||||
|
||||
/** pick a random pose in a bounded area that is not in an obstacle */
|
||||
static gtsam::Pose2 randomFreePose(double boundary_size, const std::vector<SimPolygon2D>& obstacles);
|
||||
|
||||
};
|
||||
|
||||
typedef std::vector<SimPolygon2D> SimPolygon2DVector;
|
||||
|
||||
} //\namespace gtsam
|
||||
|
||||
namespace gtsam {
|
||||
typedef std::vector<Point2> Point2Vector;
|
||||
} // \namespace gtsam
|
||||
|
|
@ -0,0 +1,174 @@
|
|||
/**
|
||||
* @file SimWall2D.cpp
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam_unstable/geometry/SimWall2D.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SimWall2D::print(const std::string& s) const {
|
||||
std::cout << "SimWall2D " << s << ":" << std::endl;
|
||||
a_.print(" a");
|
||||
b_.print(" b");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool SimWall2D::equals(const SimWall2D& other, double tol) const {
|
||||
return a_.equals(other.a_, tol) && b_.equals(other.b_, tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool SimWall2D::intersects(const SimWall2D& B, boost::optional<gtsam::Point2&> pt) const {
|
||||
const bool debug = false;
|
||||
|
||||
const SimWall2D& A = *this;
|
||||
|
||||
// translate so A.a is at the origin, then rotate so that A.b is along x axis
|
||||
Pose2 transform(Rot2::relativeBearing(A.b() - A.a()), A.a());
|
||||
|
||||
// normalized points, Aa at origin, Ab at (length, 0.0)
|
||||
double len = A.length();
|
||||
if (debug) cout << "len: " << len << endl;
|
||||
Point2 Ba = transform.transform_to(B.a()),
|
||||
Bb = transform.transform_to(B.b());
|
||||
if (debug) Ba.print("Ba");
|
||||
if (debug) Bb.print("Bb");
|
||||
|
||||
// check sides of line
|
||||
if (Ba.y() * Bb.y() > 0.0 ||
|
||||
(Ba.x() * Bb.x() > 0.0 && Ba.x() < 0.0) ||
|
||||
(Ba.x() > len && Bb.x() > len)) {
|
||||
if (debug) cout << "Failed first check" << endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// check conditions for exactly on the same line
|
||||
if (Ba.y() == 0.0 && Ba.x() > 0.0 && Ba.x() < len) {
|
||||
if (pt) *pt = transform.transform_from(Ba);
|
||||
if (debug) cout << "Ba on the line" << endl;
|
||||
return true;
|
||||
} else if (Bb.y() == 0.0 && Bb.x() > 0.0 && Bb.x() < len) {
|
||||
if (pt) *pt = transform.transform_from(Bb);
|
||||
if (debug) cout << "Bb on the line" << endl;
|
||||
return true;
|
||||
}
|
||||
|
||||
// handle vertical case to avoid calculating slope
|
||||
if (fabs(Ba.x() - Bb.x()) < 1e-5) {
|
||||
if (debug) cout << "vertical line" << endl;
|
||||
if (Ba.x() < len && Ba.x() > 0.0) {
|
||||
if (pt) *pt = transform.transform_from(Point2(Ba.x(), 0.0));
|
||||
if (debug) cout << " within range" << endl;
|
||||
return true;
|
||||
} else {
|
||||
if (debug) cout << " not within range" << endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// find lower point by y
|
||||
Point2 low, high;
|
||||
if (Ba.y() > Bb.y()) {
|
||||
high = Ba;
|
||||
low = Bb;
|
||||
} else {
|
||||
high = Bb;
|
||||
low = Ba;
|
||||
}
|
||||
if (debug) high.print("high");
|
||||
if (debug) low.print("low");
|
||||
|
||||
// find x-intercept
|
||||
double slope = (high.y() - low.y())/(high.x() - low.x());
|
||||
if (debug) cout << "slope " << slope << endl;
|
||||
double xint = (low.x() < high.x()) ? low.x() + fabs(low.y())/slope : high.x() - fabs(high.y())/slope;
|
||||
if (debug) cout << "xintercept " << xint << endl;
|
||||
if (xint > 0.0 && xint < len) {
|
||||
if (pt) *pt = transform.transform_from(Point2(xint, 0.0));
|
||||
return true;
|
||||
} else {
|
||||
if (debug) cout << "xintercept out of range" << endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Point2 SimWall2D::midpoint() const {
|
||||
Point2 vec = b_ - a_;
|
||||
return a_ + vec * 0.5 * vec.norm();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 SimWall2D::norm() const {
|
||||
Point2 vec = b_ - a_;
|
||||
return Point2(vec.y(), -vec.x());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 SimWall2D::reflection(const Point2& init, const Point2& intersection) const {
|
||||
// translate to put the intersection at the origin and the wall along the x axis
|
||||
Rot2 wallAngle = Rot2::relativeBearing(b_ - a_);
|
||||
Pose2 transform(wallAngle, intersection);
|
||||
Point2 t_init = transform.transform_to(init);
|
||||
Point2 t_goal(-t_init.x(), t_init.y());
|
||||
return Rot2::relativeBearing(wallAngle.rotate(t_goal));
|
||||
}
|
||||
|
||||
/* ***************************************************************** */
|
||||
std::pair<gtsam::Pose2, bool> moveWithBounce(const gtsam::Pose2& cur_pose, double step_size,
|
||||
const std::vector<SimWall2D> walls, gtsam::Sampler& angle_drift,
|
||||
gtsam::Sampler& reflect_noise, const gtsam::Rot2& bias) {
|
||||
|
||||
// calculate angle to change by
|
||||
Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta());
|
||||
Pose2 test_pose = cur_pose.retract(Vector_(3, step_size, 0.0, Rot2::Logmap(dtheta)(0)));
|
||||
|
||||
// create a segment to use for intersection checking
|
||||
// find the closest intersection
|
||||
SimWall2D traj(test_pose.t(), cur_pose.t());
|
||||
bool collision = false; Point2 intersection(1e+10, 1e+10);
|
||||
SimWall2D closest_wall;
|
||||
BOOST_FOREACH(const SimWall2D& wall, walls) {
|
||||
Point2 cur_intersection;
|
||||
if (wall.intersects(traj,cur_intersection)) {
|
||||
collision = true;
|
||||
if (cur_pose.t().dist(cur_intersection) < cur_pose.t().dist(intersection)) {
|
||||
intersection = cur_intersection;
|
||||
closest_wall = wall;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// reflect off of wall with some noise
|
||||
Pose2 pose(test_pose);
|
||||
if (collision) {
|
||||
|
||||
// make sure norm is on the robot's side
|
||||
Point2 norm = closest_wall.norm();
|
||||
norm = norm / norm.norm();
|
||||
|
||||
// Simple check to make sure norm is on side closest robot
|
||||
if (cur_pose.t().dist(intersection + norm) > cur_pose.t().dist(intersection - norm))
|
||||
norm = norm.inverse();
|
||||
|
||||
// using the reflection
|
||||
const double inside_bias = 0.05;
|
||||
pose = Pose2(closest_wall.reflection(cur_pose.t(), intersection), intersection + inside_bias * norm);
|
||||
|
||||
// perturb the rotation for better exploration
|
||||
pose = pose.retract(reflect_noise.sample());
|
||||
}
|
||||
|
||||
return make_pair(pose, collision);
|
||||
}
|
||||
/* ***************************************************************** */
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
/**
|
||||
* @file SimWall2D.h
|
||||
* @brief Implementation of walls for use with simulators
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/linear/Sampler.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* General Wall class for walls defined around unordered endpoints
|
||||
* Primarily to handle ray intersections
|
||||
*/
|
||||
class SimWall2D {
|
||||
protected:
|
||||
gtsam::Point2 a_, b_;
|
||||
|
||||
public:
|
||||
/** default constructor makes canonical wall */
|
||||
SimWall2D() : a_(1.0, 0.0), b_(1.0, 1.0) {}
|
||||
|
||||
/** constructors using endpoints */
|
||||
SimWall2D(const gtsam::Point2& a, const gtsam::Point2& b)
|
||||
: a_(a), b_(b) {}
|
||||
|
||||
SimWall2D(double ax, double ay, double bx, double by)
|
||||
: a_(ax, ay), b_(bx, by) {}
|
||||
|
||||
/** required by testable */
|
||||
void print(const std::string& s="") const;
|
||||
bool equals(const SimWall2D& other, double tol=1e-9) const;
|
||||
|
||||
/** access */
|
||||
gtsam::Point2 a() const { return a_; }
|
||||
gtsam::Point2 b() const { return b_; }
|
||||
|
||||
/** scales a wall to produce a new wall */
|
||||
SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); }
|
||||
|
||||
/** geometry */
|
||||
double length() const { return a_.dist(b_); }
|
||||
gtsam::Point2 midpoint() const;
|
||||
|
||||
/**
|
||||
* intersection check between two segments
|
||||
* returns true if they intersect, with the intersection
|
||||
* point in the optional second argument
|
||||
*/
|
||||
bool intersects(const SimWall2D& wall, boost::optional<gtsam::Point2&> pt=boost::none) const;
|
||||
|
||||
/**
|
||||
* norm is a 2D point representing the norm of the wall
|
||||
*/
|
||||
gtsam::Point2 norm() const;
|
||||
|
||||
/**
|
||||
* reflection around a point of impact with a wall from a starting (init) point
|
||||
* at a given impact point (intersection), returning the angle away from the impact
|
||||
*/
|
||||
gtsam::Rot2 reflection(const gtsam::Point2& init, const gtsam::Point2& intersection) const;
|
||||
|
||||
};
|
||||
|
||||
typedef std::vector<SimWall2D> SimWall2DVector;
|
||||
|
||||
/**
|
||||
* Calculates the next pose in a trajectory constrained by walls, with noise on
|
||||
* angular drift and reflection noise
|
||||
* @param cur_pose is the pose of the robot
|
||||
* @param step_size is the size of the forward step the robot tries to take
|
||||
* @param walls is a set of walls to use for bouncing
|
||||
* @param angle_drift is a sampler for angle drift (dim=1)
|
||||
* @param reflect_noise is a sampler for scatter after hitting a wall (dim=3)
|
||||
* @return the next pose for the robot
|
||||
* NOTE: samplers cannot be const
|
||||
*/
|
||||
std::pair<gtsam::Pose2, bool> moveWithBounce(const gtsam::Pose2& cur_pose, double step_size,
|
||||
const std::vector<SimWall2D> walls, gtsam::Sampler& angle_drift,
|
||||
gtsam::Sampler& reflect_noise, const gtsam::Rot2& bias = gtsam::Rot2());
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
/**
|
||||
* @file testBearingS2.cpp
|
||||
*
|
||||
* @brief Tests for a bearing measurement on S2 for 3D bearing measurements
|
||||
*
|
||||
* @date Jan 26, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
|
||||
#include <gtsam_unstable/geometry/BearingS2.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
static const double tol=1e-5;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testBearingS2, basics ) {
|
||||
BearingS2 origin;
|
||||
EXPECT(assert_equal(Rot2(), origin.elevation(), tol));
|
||||
EXPECT(assert_equal(Rot2(), origin.azimuth(), tol));
|
||||
|
||||
double expAzi = 0.2, expEle = 0.3;
|
||||
BearingS2 actual1(expAzi, expEle);
|
||||
EXPECT(assert_equal(Rot2::fromAngle(expEle), actual1.elevation(), tol));
|
||||
EXPECT(assert_equal(Rot2::fromAngle(expAzi), actual1.azimuth(), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testBearingS2, equals ) {
|
||||
BearingS2 origin, b1(0.2, 0.3), b2(b1), b3(0.1, 0.3), b4(0.2, 0.5);
|
||||
EXPECT(assert_equal(origin, origin, tol));
|
||||
EXPECT(assert_equal(b1, b1, tol));
|
||||
EXPECT(assert_equal(b1, b2, tol));
|
||||
EXPECT(assert_equal(b2, b1, tol));
|
||||
|
||||
EXPECT(assert_inequal(b1, b3, tol));
|
||||
EXPECT(assert_inequal(b3, b1, tol));
|
||||
EXPECT(assert_inequal(b1, b4, tol));
|
||||
EXPECT(assert_inequal(b4, b1, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testBearingS2, manifold ) {
|
||||
BearingS2 origin, b1(0.2, 0.3);
|
||||
EXPECT_LONGS_EQUAL(2, origin.dim());
|
||||
|
||||
EXPECT(assert_equal(zero(2), origin.localCoordinates(origin), tol));
|
||||
EXPECT(assert_equal(origin, origin.retract(zero(2)), tol));
|
||||
|
||||
EXPECT(assert_equal(zero(2), b1.localCoordinates(b1), tol));
|
||||
EXPECT(assert_equal(b1, b1.retract(zero(2)), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,137 @@
|
|||
/**
|
||||
* @file testPose3Upright.cpp
|
||||
*
|
||||
* @date Jan 24, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <gtsam_unstable/geometry/Pose3Upright.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
static const double tol = 1e-5;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testPose3Upright, basics ) {
|
||||
Pose3Upright origin;
|
||||
EXPECT_DOUBLES_EQUAL(0.0, origin.x(), tol);
|
||||
EXPECT_DOUBLES_EQUAL(0.0, origin.y(), tol);
|
||||
EXPECT_DOUBLES_EQUAL(0.0, origin.z(), tol);
|
||||
EXPECT_DOUBLES_EQUAL(0.0, origin.theta(), tol);
|
||||
|
||||
Pose3Upright actual1(Rot2::fromAngle(0.1), Point3(1.0, 2.0, 3.0));
|
||||
EXPECT_DOUBLES_EQUAL(1.0, actual1.x(), tol);
|
||||
EXPECT_DOUBLES_EQUAL(2.0, actual1.y(), tol);
|
||||
EXPECT_DOUBLES_EQUAL(3.0, actual1.z(), tol);
|
||||
EXPECT_DOUBLES_EQUAL(0.1, actual1.theta(), tol);
|
||||
|
||||
Pose3Upright actual2(1.0, 2.0, 3.0, 0.1);
|
||||
EXPECT_DOUBLES_EQUAL(1.0, actual2.x(), tol);
|
||||
EXPECT_DOUBLES_EQUAL(2.0, actual2.y(), tol);
|
||||
EXPECT_DOUBLES_EQUAL(3.0, actual2.z(), tol);
|
||||
EXPECT_DOUBLES_EQUAL(0.1, actual2.theta(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testPose3Upright, equals ) {
|
||||
Pose3Upright origin, actual1(1.0, 2.0, 3.0, 0.1),
|
||||
actual2(1.0, 2.0, 3.0, 0.1), actual3(4.0,-7.0, 3.0, 0.3);
|
||||
EXPECT(assert_equal(origin, origin, tol));
|
||||
EXPECT(assert_equal(actual1, actual1, tol));
|
||||
EXPECT(assert_equal(actual1, actual2, tol));
|
||||
EXPECT(assert_equal(actual2, actual1, tol));
|
||||
|
||||
EXPECT(assert_inequal(actual1, actual3, tol));
|
||||
EXPECT(assert_inequal(actual3, actual1, tol));
|
||||
EXPECT(assert_inequal(actual1, origin, tol));
|
||||
EXPECT(assert_inequal(origin, actual1, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testPose3Upright, conversions ) {
|
||||
Pose3Upright pose(1.0, 2.0, 3.0, 0.1);
|
||||
EXPECT(assert_equal(Point3(1.0, 2.0, 3.0), pose.translation(), tol));
|
||||
EXPECT(assert_equal(Point2(1.0, 2.0), pose.translation2(), tol));
|
||||
EXPECT(assert_equal(Rot2::fromAngle(0.1), pose.rotation2(), tol));
|
||||
EXPECT(assert_equal(Rot3::yaw(0.1), pose.rotation(), tol));
|
||||
EXPECT(assert_equal(Pose2(1.0, 2.0, 0.1), pose.pose2(), tol));
|
||||
EXPECT(assert_equal(Pose3(Rot3::yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testPose3Upright, manifold ) {
|
||||
Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.0), x2(4.0, 2.0, 7.0, 0.0);
|
||||
EXPECT_LONGS_EQUAL(4, origin.dim());
|
||||
|
||||
EXPECT(assert_equal(origin, origin.retract(zero(4)), tol));
|
||||
EXPECT(assert_equal(x1, x1.retract(zero(4)), tol));
|
||||
EXPECT(assert_equal(x2, x2.retract(zero(4)), tol));
|
||||
|
||||
Vector delta12 = Vector_(4, 3.0, 0.0, 4.0, 0.0), delta21 = -delta12;
|
||||
EXPECT(assert_equal(x2, x1.retract(delta12), tol));
|
||||
EXPECT(assert_equal(x1, x2.retract(delta21), tol));
|
||||
|
||||
EXPECT(assert_equal(delta12, x1.localCoordinates(x2), tol));
|
||||
EXPECT(assert_equal(delta21, x2.localCoordinates(x1), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testPose3Upright, lie ) {
|
||||
Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.1);
|
||||
EXPECT(assert_equal(zero(4), Pose3Upright::Logmap(origin), tol));
|
||||
EXPECT(assert_equal(origin, Pose3Upright::Expmap(zero(4)), tol));
|
||||
|
||||
EXPECT(assert_equal(x1, Pose3Upright::Expmap(Pose3Upright::Logmap(x1)), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright between_proxy(const Pose3Upright& x1, const Pose3Upright& x2) { return x1.between(x2); }
|
||||
TEST( testPose3Upright, between ) {
|
||||
Pose3Upright x1(1.0, 2.0, 3.0, 0.1), x2(4.0,-2.0, 7.0, 0.3);
|
||||
Pose3Upright expected(x1.pose2().between(x2.pose2()), x2.z() - x1.z());
|
||||
EXPECT(assert_equal(expected, x1.between(x2), tol));
|
||||
|
||||
Matrix actualH1, actualH2, numericH1, numericH2;
|
||||
x1.between(x2, actualH1, actualH2);
|
||||
numericH1 = numericalDerivative21(between_proxy, x1, x2, 1e-5);
|
||||
numericH2 = numericalDerivative22(between_proxy, x1, x2, 1e-5);
|
||||
EXPECT(assert_equal(numericH1, actualH1, tol));
|
||||
EXPECT(assert_equal(numericH2, actualH2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright compose_proxy(const Pose3Upright& x1, const Pose3Upright& x2) { return x1.compose(x2); }
|
||||
TEST( testPose3Upright, compose ) {
|
||||
Pose3Upright x1(1.0, 2.0, 3.0, 0.1), x2(4.0,-2.0, 7.0, 0.3);
|
||||
Pose3Upright expected(x1.pose2().between(x2.pose2()), x2.z() - x1.z());
|
||||
EXPECT(assert_equal(x2, x1.compose(expected), tol));
|
||||
|
||||
Matrix actualH1, actualH2, numericH1, numericH2;
|
||||
x1.compose(expected, actualH1, actualH2);
|
||||
numericH1 = numericalDerivative21(compose_proxy, x1, expected, 1e-5);
|
||||
numericH2 = numericalDerivative22(compose_proxy, x1, expected, 1e-5);
|
||||
EXPECT(assert_equal(numericH1, actualH1, tol));
|
||||
EXPECT(assert_equal(numericH2, actualH2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Upright inverse_proxy(const Pose3Upright& x1) { return x1.inverse(); }
|
||||
TEST( testPose3Upright, inverse ) {
|
||||
Pose3Upright x1(1.0, 2.0, 3.0, 0.1);
|
||||
Pose3Upright expected(x1.pose2().inverse(), - x1.z());
|
||||
EXPECT(assert_equal(expected, x1.inverse(), tol));
|
||||
|
||||
Matrix actualH1, numericH1;
|
||||
x1.inverse(actualH1);
|
||||
numericH1 = numericalDerivative11(inverse_proxy, x1, 1e-5);
|
||||
EXPECT(assert_equal(numericH1, actualH1, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,139 @@
|
|||
/**
|
||||
* @file testSimPolygon
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam_unstable/geometry/SimPolygon2D.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace gtsam;
|
||||
|
||||
const double tol=1e-5;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testPolygon, triangle_basic) {
|
||||
|
||||
// create a triangle from points, extract landmarks/walls, check occupancy
|
||||
Point2 pA, pB(2.0, 0.0), pC(0.0, 1.0);
|
||||
|
||||
// construct and extract data
|
||||
SimPolygon2D actTriangle = SimPolygon2D::createTriangle(pA, pB, pC);
|
||||
LONGS_EQUAL(3, actTriangle.size());
|
||||
EXPECT(assert_equal(pA, actTriangle.landmark(0)));
|
||||
EXPECT(assert_equal(pB, actTriangle.landmark(1)));
|
||||
EXPECT(assert_equal(pC, actTriangle.landmark(2)));
|
||||
|
||||
// get walls out
|
||||
vector<SimWall2D> actWalls = actTriangle.walls();
|
||||
vector<SimWall2D> expWalls;
|
||||
expWalls.push_back(SimWall2D(pA, pB));
|
||||
expWalls.push_back(SimWall2D(pB, pC));
|
||||
expWalls.push_back(SimWall2D(pC, pA));
|
||||
EXPECT(assert_container_equal(expWalls, actWalls, tol));
|
||||
|
||||
// check occupancy - used in sampling more points
|
||||
// treat as closed polygon - points along edges also count
|
||||
EXPECT(actTriangle.contains(Point2(0.25, 0.5)));
|
||||
EXPECT(actTriangle.contains(pA));
|
||||
EXPECT(actTriangle.contains(pB));
|
||||
EXPECT(actTriangle.contains(pC));
|
||||
EXPECT(actTriangle.contains(Point2(1.0, 0.0)));
|
||||
|
||||
EXPECT(!actTriangle.contains(Point2(1.0, 1.0)));
|
||||
EXPECT(!actTriangle.contains(Point2(-1.0, 1.0)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testPolygon, rectangle_basic) {
|
||||
|
||||
// creates an axis-aligned rectangle given a lower left corner and a height and width
|
||||
double height = 3.0, width = 2.0;
|
||||
Point2 pA(1.0, 0.0), pB(3.0, 0.0), pC(3.0, 3.0), pD(1.0, 3.0);
|
||||
|
||||
// construct and extract data
|
||||
SimPolygon2D actRectangle = SimPolygon2D::createRectangle(pA, height, width);
|
||||
LONGS_EQUAL(4, actRectangle.size());
|
||||
EXPECT(assert_equal(pA, actRectangle.landmark(0)));
|
||||
EXPECT(assert_equal(pB, actRectangle.landmark(1)));
|
||||
EXPECT(assert_equal(pC, actRectangle.landmark(2)));
|
||||
EXPECT(assert_equal(pD, actRectangle.landmark(3)));
|
||||
|
||||
// get walls out
|
||||
vector<SimWall2D> actWalls = actRectangle.walls();
|
||||
vector<SimWall2D> expWalls;
|
||||
expWalls.push_back(SimWall2D(pA, pB));
|
||||
expWalls.push_back(SimWall2D(pB, pC));
|
||||
expWalls.push_back(SimWall2D(pC, pD));
|
||||
expWalls.push_back(SimWall2D(pD, pA));
|
||||
EXPECT(assert_container_equal(expWalls, actWalls, tol));
|
||||
|
||||
// check occupancy - used in sampling more points
|
||||
// treat as closed polygon - points along edges also count
|
||||
EXPECT(actRectangle.contains(Point2(2.0, 1.0)));
|
||||
EXPECT(actRectangle.contains(pA));
|
||||
EXPECT(actRectangle.contains(pB));
|
||||
EXPECT(actRectangle.contains(pC));
|
||||
EXPECT(actRectangle.contains(pD));
|
||||
EXPECT(actRectangle.contains(Point2(1.0, 0.0)));
|
||||
|
||||
EXPECT(!actRectangle.contains(Point2(0.9, 0.5)));
|
||||
EXPECT(!actRectangle.contains(Point2(-1.0, 1.0)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testPolygon, triangle_generator) {
|
||||
// generate random triangles in a bounded region with no overlap
|
||||
double side_len = 10.0; // box of length 10, centered on origin
|
||||
double mean_side_len = 2.0; // mean length of sides
|
||||
double sigma_side_len = 0.5; // stddev for length of sides
|
||||
double min_vertex_dist = 0.4; // minimum allowable distance between vertices
|
||||
double min_side_len = 0.1;
|
||||
|
||||
// initialize the random number generator for consistency
|
||||
SimPolygon2D::seedGenerator(42u);
|
||||
|
||||
vector<SimPolygon2D> existing_polys;
|
||||
|
||||
SimPolygon2D actual = SimPolygon2D::randomTriangle(side_len, mean_side_len, sigma_side_len,
|
||||
min_vertex_dist, min_side_len, existing_polys);
|
||||
|
||||
// use a rectangle to check that it is within boundaries
|
||||
SimPolygon2D bounding_rect = SimPolygon2D::createRectangle(Point2(-5.0,-5.0), side_len, side_len);
|
||||
|
||||
EXPECT(bounding_rect.contains(actual.landmark(0)));
|
||||
EXPECT(bounding_rect.contains(actual.landmark(1)));
|
||||
EXPECT(bounding_rect.contains(actual.landmark(2)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testPolygon, rectangle_generator) {
|
||||
// generate random rectangles in a bounded region with no overlap
|
||||
double side_len = 10.0; // box of length 10, centered on origin
|
||||
double mean_side_len = 2.0; // mean length of sides
|
||||
double sigma_side_len = 0.5; // stddev for length of sides
|
||||
double min_vertex_dist = 0.4; // minimum allowable distance between vertices
|
||||
double min_side_len = 0.1;
|
||||
|
||||
// initialize the random number generator for consistency
|
||||
SimPolygon2D::seedGenerator(42u);
|
||||
|
||||
vector<SimPolygon2D> existing_polys;
|
||||
|
||||
SimPolygon2D actual = SimPolygon2D::randomRectangle(side_len, mean_side_len, sigma_side_len,
|
||||
min_vertex_dist, min_side_len, existing_polys);
|
||||
|
||||
// use a rectangle to check that it is within boundaries
|
||||
SimPolygon2D bounding_rect = SimPolygon2D::createRectangle(Point2(-5.0,-5.0), side_len, side_len);
|
||||
|
||||
EXPECT(bounding_rect.contains(actual.landmark(0)));
|
||||
EXPECT(bounding_rect.contains(actual.landmark(1)));
|
||||
EXPECT(bounding_rect.contains(actual.landmark(2)));
|
||||
EXPECT(bounding_rect.contains(actual.landmark(3)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
/**
|
||||
* @file testSimWall2D2D
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
|
||||
#include <gtsam_unstable/geometry/SimWall2D.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace gtsam;
|
||||
|
||||
const double tol = 1e-5;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testSimWall2D2D, construction ) {
|
||||
Point2 a(1.0, 0.0), b(1.0, 2.0);
|
||||
SimWall2D wall1(a, b), wall2(a.x(), a.y(), b.x(), b.y());
|
||||
EXPECT(assert_equal(a, wall1.a(), tol));
|
||||
EXPECT(assert_equal(a, wall2.a(), tol));
|
||||
EXPECT(assert_equal(b, wall1.b(), tol));
|
||||
EXPECT(assert_equal(b, wall2.b(), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testSimWall2D2D, equals ) {
|
||||
Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3;
|
||||
SimWall2D w1(p1, p2), w2(p1, p3);
|
||||
EXPECT(assert_equal(w1, w1));
|
||||
EXPECT(assert_inequal(w1, w2));
|
||||
EXPECT(assert_inequal(w2, w1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testSimWall2D2D, intersection1 ) {
|
||||
SimWall2D w1(2.0, 2.0, 6.0, 2.0), w2(4.0, 4.0, 4.0, 0.0);
|
||||
Point2 pt;
|
||||
EXPECT(w1.intersects(w2));
|
||||
EXPECT(w2.intersects(w1));
|
||||
w1.intersects(w2, pt);
|
||||
EXPECT(assert_equal(Point2(4.0, 2.0), pt, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testSimWall2D2D, intersection2 ) {
|
||||
SimWall2D traj(7.07107, 7.07107, 0, 0);
|
||||
SimWall2D wall(1.5, 3, 1.5, 1);
|
||||
EXPECT(wall.intersects(traj));
|
||||
EXPECT(traj.intersects(wall));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testSimWall2D2D, reflection1 ) {
|
||||
SimWall2D wall1(1.0, 1.0, 7.0, 1.0), wall2(7.0, 1.0, 1.0, 1.0);
|
||||
Point2 init(2.0, 3.0), intersection(4.0, 1.0);
|
||||
Rot2 actual1 = wall1.reflection(init, intersection);
|
||||
Rot2 actual2 = wall2.reflection(init, intersection);
|
||||
Rot2 expected = Rot2::fromDegrees(45);
|
||||
EXPECT(assert_equal(expected, actual1, tol));
|
||||
EXPECT(assert_equal(expected, actual2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -4,9 +4,11 @@
|
|||
|
||||
// specify the classes from gtsam we are using
|
||||
virtual class gtsam::Value;
|
||||
virtual class gtsam::Point2;
|
||||
virtual class gtsam::Rot2;
|
||||
virtual class gtsam::Pose2;
|
||||
virtual class gtsam::Point3;
|
||||
virtual class gtsam::Rot3;
|
||||
virtual class gtsam::Pose2;
|
||||
virtual class gtsam::Pose3;
|
||||
virtual class gtsam::noiseModel::Base;
|
||||
virtual class gtsam::NonlinearFactor;
|
||||
|
|
@ -77,6 +79,62 @@ virtual class PoseRTV : gtsam::Value {
|
|||
Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const;
|
||||
};
|
||||
|
||||
#include <gtsam_unstable/geometry/Pose3Upright.h>
|
||||
virtual class Pose3Upright : gtsam::Value {
|
||||
Pose3Upright();
|
||||
Pose3Upright(const gtsam::Pose3Upright& x);
|
||||
Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t);
|
||||
Pose3Upright(double x, double y, double z, double theta);
|
||||
Pose3Upright(const gtsam::Pose2& pose, double z);
|
||||
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::Pose3Upright& pose, double tol) const;
|
||||
|
||||
double x() const;
|
||||
double y() const;
|
||||
double z() const;
|
||||
double theta() const;
|
||||
|
||||
gtsam::Point2 translation2() const;
|
||||
gtsam::Point3 translation() const;
|
||||
gtsam::Rot2 rotation2() const;
|
||||
gtsam::Rot3 rotation() const;
|
||||
gtsam::Pose2 pose2() const;
|
||||
gtsam::Pose3 pose() const;
|
||||
|
||||
size_t dim() const;
|
||||
gtsam::Pose3Upright retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::Pose3Upright& p2) const;
|
||||
|
||||
static gtsam::Pose3Upright identity();
|
||||
gtsam::Pose3Upright inverse() const;
|
||||
gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const;
|
||||
gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const;
|
||||
|
||||
static gtsam::Pose3Upright Expmap(Vector xi);
|
||||
static Vector Logmap(const gtsam::Pose3Upright& p);
|
||||
}; // \class Pose3Upright
|
||||
|
||||
#include <gtsam_unstable/geometry/BearingS2.h>
|
||||
virtual class BearingS2 : gtsam::Value {
|
||||
BearingS2();
|
||||
BearingS2(double azimuth, double elevation);
|
||||
BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation);
|
||||
|
||||
gtsam::Rot2 azimuth() const;
|
||||
gtsam::Rot2 elevation() const;
|
||||
|
||||
static gtsam::BearingS2 fromDownwardsObservation(const gtsam::Pose3& A, const gtsam::Point3& B);
|
||||
static gtsam::BearingS2 fromForwardObservation(const gtsam::Pose3& A, const gtsam::Point3& B);
|
||||
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::BearingS2& x, double tol) const;
|
||||
|
||||
size_t dim() const;
|
||||
gtsam::BearingS2 retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::BearingS2& p2) const;
|
||||
};
|
||||
|
||||
|
||||
// Nonlinear factors from gtsam, for our Value types
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
|
|
|
|||
Loading…
Reference in New Issue