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

Fixed major problem with Pose3 retract. All retractions have to have exactly the same derivatives as the exponential map. Hence, it should never make sense to have to match derivaties with a retract version. I deleted all "wrong" derivatives and pushforwards. Finally, CalibratedCamera had to be modified as well as it hardcoded the wrong derivative of transform_to (for efficiency). It now simply implements the chain rule.

release/4.3a0
Frank Dellaert 2012-01-05 16:45:52 +00:00
parent 5e2f4c0958
commit e6382c7ec0
5 changed files with 247 additions and 278 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

293
.cproject
View File

@ -378,6 +378,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -404,7 +412,6 @@
</target> </target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget> <buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -412,7 +419,6 @@
</target> </target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget> <buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -460,7 +466,6 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget> <buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -468,7 +473,6 @@
</target> </target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget> <buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -476,7 +480,6 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget> <buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -492,20 +495,11 @@
</target> </target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget> <buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -532,6 +526,7 @@
</target> </target>
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget> <buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -603,6 +598,7 @@
</target> </target>
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testInference.run</buildTarget> <buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -610,6 +606,7 @@
</target> </target>
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactor.run</buildTarget> <buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -617,6 +614,7 @@
</target> </target>
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget> <buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -624,6 +622,7 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget> <buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -631,6 +630,7 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget> <buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -700,22 +700,6 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -732,6 +716,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -756,15 +756,7 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget> <buildTarget>check</buildTarget>
@ -772,14 +764,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -820,7 +804,15 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="check" 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>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget> <buildTarget>check</buildTarget>
@ -828,6 +820,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1150,7 +1150,6 @@
</target> </target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget> <buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1476,6 +1475,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="tests/testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPoint3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1558,6 +1573,7 @@
</target> </target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget> <buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1597,6 +1613,7 @@
</target> </target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget> <buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1604,6 +1621,7 @@
</target> </target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget> <buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1946,6 +1964,7 @@
</target> </target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget> <buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1967,46 +1986,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="" 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="check" path="" 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="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>dist</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2103,6 +2082,94 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="" 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="check" path="" 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="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>dist</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/wrap" 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="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testWrap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2159,54 +2226,6 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="check" path="build/wrap" 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="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testWrap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets> </buildTargets>
</storageModule> </storageModule>
</cproject> </cproject>

42
gtsam/geometry/CalibratedCamera.cpp
View File

@ -30,10 +30,13 @@ CalibratedCamera::CalibratedCamera(const Pose3& pose) :
CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {} CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional<Matrix&> H1) { Point2 CalibratedCamera::project_to_camera(const Point3& P,
boost::optional<Matrix&> H1) {
if (H1) { if (H1) {
double d = 1.0 / P.z(), d2 = d * d; double d = 1.0 / P.z(), d2 = d * d;
*H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); *H1 = Matrix_(2, 3,
d, 0.0, -P.x() * d2,
0.0, d, -P.y() * d2);
} }
return Point2(P.x() / P.z(), P.y() / P.z()); return Point2(P.x() / P.z(), P.y() / P.z());
} }
@ -55,30 +58,25 @@ CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) {
/* ************************************************************************* */ /* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point, Point2 CalibratedCamera::project(const Point3& point,
boost::optional<Matrix&> D_intrinsic_pose, boost::optional<Matrix&> H1,
boost::optional<Matrix&> D_intrinsic_point) const { boost::optional<Matrix&> H2) const {
const Pose3& pose = pose_;
const Rot3& R = pose.rotation(); Point3 q = pose_.transform_to(point, H1, H2);
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = pose.transform_to(point); // Check if point is in front of camera
if(q.z() <= 0) if(q.z() <= 0)
throw CheiralityException(); throw CheiralityException();
if (D_intrinsic_pose || D_intrinsic_point) { if (H1 || H2) {
double X = q.x(), Y = q.y(), Z = q.z(); Matrix H;
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2; Point2 intrinsic = project_to_camera(q,H);
double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2; // just implement chain rule
double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2; if (H1) *H1 = H * (*H1);
if (D_intrinsic_pose) if (H1) *H2 = H * (*H2);
*D_intrinsic_pose = Matrix_(2,6, return intrinsic;
X*Yd2, -Z*d-X*Xd2, d*Y, -dp11, -dp12, -dp13,
d*Z+Y*Yd2, -X*Yd2, -d*X, -dp21, -dp22, -dp23);
if (D_intrinsic_point)
*D_intrinsic_point = Matrix_(2,3,
dp11, dp12, dp13,
dp21, dp22, dp23);
} }
return project_to_camera(q); else
return project_to_camera(q);
} }
/* ************************************************************************* */ /* ************************************************************************* */

142
gtsam/geometry/Pose3.cpp
View File

@ -29,10 +29,7 @@ namespace gtsam {
/** instantiate concept checks */ /** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3); GTSAM_CONCEPT_POSE_INST(Pose3);
static const Matrix I3 = eye(3), Z3 = zeros(3, 3); static const Matrix I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3, I6 = eye(6);
#ifdef CORRECT_POSE3_EXMAP
static const Matrix _I3=-I3, I6 = eye(6);
#endif
/* ************************************************************************* */ /* ************************************************************************* */
// Calculate Adjoint map // Calculate Adjoint map
@ -94,42 +91,60 @@ namespace gtsam {
} }
} }
#ifdef CORRECT_POSE3_EXMAP
/* ************************************************************************* */ /* ************************************************************************* */
// Changes default to use the full verions of expmap/logmap // Changes default to use the full verions of expmap/logmap
/* ************************************************************************* */ /* ************************************************************************* */
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::retract(const Vector& d) const { // Different versions of retract
return compose(Expmap(d)); Pose3 Pose3::retract(const Vector& xi) const {
} #ifdef CORRECT_POSE3_EXMAP
// Lie group exponential map, traces out geodesic
/* ************************************************************************* */ return compose(Expmap(xi));
Vector Pose3::localCoordinates(const Pose3& T2) const {
return Logmap(between(T2));
}
#else #else
Vector omega(sub(xi, 0, 3));
Point3 v(sub(xi, 3, 6));
// R is always done exactly in all three retract versions below
Rot3 R = R_.retract(omega);
// Incorrect version
// Retracts R and t independently
// Point3 t = t_.retract(v.vector());
// First order t approximation
Point3 t = t_ + R_ * v;
// Second order t approximation
// Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2);
return Pose3(R, t);
#endif
}
/* ************************************************************************* */ /* ************************************************************************* */
/** These are the "old-style" expmap and logmap about the specified // different versions of localCoordinates
* pose. Increments the offset and rotation independently given a translation and Vector Pose3::localCoordinates(const Pose3& T) const {
* canonical rotation coordinates. Created to match ML derivatives, but #ifdef CORRECT_POSE3_EXMAP
* superseded by the correct exponential map story in .cpp */ // Lie group logarithm map, exact inverse of exponential map
Pose3 Pose3::retract(const Vector& d) const { return Logmap(between(T));
return Pose3(R_.retract(sub(d, 0, 3)), #else
t_.retract(sub(d, 3, 6))); // R is always done exactly in all three retract versions below
} Vector omega = R_.localCoordinates(T.rotation());
/* ************************************************************************* */ // Incorrect version
/** Independently computes the logmap of the translation and rotation. */ // Independently computes the logmap of the translation and rotation
Vector Pose3::localCoordinates(const Pose3& pp) const { // Vector v = t_.localCoordinates(T.translation());
const Vector r(R_.localCoordinates(pp.rotation())),
t(t_.localCoordinates(pp.translation()));
return concatVectors(2, &r, &t);
}
#endif // Correct first order t inverse
Point3 d = R_.unrotate(T.translation() - t_);
Vector v = d.vector();
// TODO: correct second order t inverse
return concatVectors(2, &omega, &v);
#endif
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Pose3::matrix() const { Matrix Pose3::matrix() const {
@ -150,15 +165,9 @@ namespace gtsam {
Point3 Pose3::transform_from(const Point3& p, Point3 Pose3::transform_from(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) { if (H1) {
#ifdef CORRECT_POSE3_EXMAP
const Matrix R = R_.matrix(); const Matrix R = R_.matrix();
Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z()); Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
*H1 = collect(2,&DR,&R); *H1 = collect(2,&DR,&R);
#else
Matrix DR;
R_.rotate(p, DR, boost::none);
*H1 = collect(2,&DR,&I3);
#endif
} }
if (H2) *H2 = R_.matrix(); if (H2) *H2 = R_.matrix();
return R_ * p + t_; return R_ * p + t_;
@ -168,17 +177,11 @@ namespace gtsam {
Point3 Pose3::transform_to(const Point3& p, Point3 Pose3::transform_to(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point3 result = R_.unrotate(p - t_); const Point3 result = R_.unrotate(p - t_);
if (H1) { if (H1) {
const Point3& q = result; const Point3& q = result;
Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
#ifdef CORRECT_POSE3_EXMAP *H1 = collect(2, &DR, &_I3);
*H1 = collect(2, &DR, &_I3); }
#else
Matrix DT = - R_.transpose(); // negative because of sub
*H1 = collect(2,&DR,&DT);
#endif
}
if (H2) *H2 = R_.transpose(); if (H2) *H2 = R_.transpose();
return result; return result;
} }
@ -186,47 +189,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, Pose3 Pose3::compose(const Pose3& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) { if (H1) *H1 = p2.inverse().adjointMap();
#ifdef CORRECT_POSE3_EXMAP if (H2) *H2 = I6;
*H1 = p2.inverse().adjointMap(); return (*this) * p2;
#else
const Rot3& R2 = p2.rotation();
const Point3& t2 = p2.translation();
Matrix DR_R1 = R2.transpose(), DR_t1 = Z3;
Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt;
transform_from(t2, Dt, boost::none);
*H1 = gtsam::stack(2, &DR, &Dt);
#endif
}
if (H2) {
#ifdef CORRECT_POSE3_EXMAP
*H2 = I6;
#else
Matrix R1 = rotation().matrix();
Matrix DR = collect(2, &I3, &Z3);
Matrix Dt = collect(2, &Z3, &R1);
*H2 = gtsam::stack(2, &DR, &Dt);
#endif
}
return (*this) * p2;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const { Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
if (H1) if (H1) *H1 = -adjointMap();
#ifdef CORRECT_POSE3_EXMAP
{ *H1 = - adjointMap(); }
#else
{
Matrix Rt = R_.transpose();
Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3;
Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt;
Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
*H1 = gtsam::stack(2, &DR, &Dt);
}
#endif
Rot3 Rt = R_.inverse(); Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt*(-t_)); return Pose3(Rt, Rt*(-t_));
} }
@ -262,11 +232,7 @@ namespace gtsam {
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double r = range(point.translation(), H1, H2); double r = range(point.translation(), H1, H2);
if (H2) { if (H2) {
#ifdef CORRECT_POSE3_EXMAP
Matrix H2_ = *H2 * point.rotation().matrix(); Matrix H2_ = *H2 * point.rotation().matrix();
#else
Matrix H2_ = *H2;
#endif
*H2 = zeros(1, 6); *H2 = zeros(1, 6);
insertSub(*H2, H2_, 0, 3); insertSub(*H2, H2_, 0, 3);
} }

37
gtsam/geometry/tests/testPose3.cpp
View File

@ -44,12 +44,12 @@ TEST( Pose3, equals)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, expmap_a) TEST( Pose3, retract)
{ {
Pose3 id; Pose3 id;
Vector v = zero(6); Vector v = zero(6);
v(0) = 0.3; v(0) = 0.3;
EXPECT(assert_equal(id.retract(v), Pose3(R, Point3()))); EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v)));
#ifdef CORRECT_POSE3_EXMAP #ifdef CORRECT_POSE3_EXMAP
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
#else #else
@ -195,9 +195,7 @@ TEST( Pose3, compose )
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2); Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2);
EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
#ifdef CORRECT_POSE3_EXMAP
EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3)); EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3));
#endif
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2); Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2);
EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4)); EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4));
@ -217,9 +215,7 @@ TEST( Pose3, compose2 )
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2); Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2);
EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
#ifdef CORRECT_POSE3_EXMAP
EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3)); EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3));
#endif
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2); Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2);
EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5)); EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5));
@ -235,9 +231,7 @@ TEST( Pose3, inverse)
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T); Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
#ifdef CORRECT_POSE3_EXMAP
EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3)); EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3));
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -251,9 +245,7 @@ TEST( Pose3, inverseDerivatives2)
Matrix actualDinverse; Matrix actualDinverse;
T.inverse(actualDinverse); T.inverse(actualDinverse);
EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
#ifdef CORRECT_POSE3_EXMAP
EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3)); EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3));
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -450,10 +442,17 @@ TEST( Pose3, transformPose_to)
EXPECT(assert_equal(expected, actual, 0.001)); EXPECT(assert_equal(expected, actual, 0.001));
} }
/* ************************************************************************* */
TEST(Pose3, localCoordinates)
{
Vector d12 = repeat(6,0.1);
Pose3 t1 = T, t2 = t1.retract(d12);
EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, manifold) TEST(Pose3, manifold)
{ {
//cout << "manifold" << endl;
Pose3 t1 = T; Pose3 t1 = T;
Pose3 t2 = T3; Pose3 t2 = T3;
Pose3 origin; Pose3 origin;
@ -466,13 +465,13 @@ TEST(Pose3, manifold)
// todo: richard - commented out because this tests for "compose-style" (new) expmap // todo: richard - commented out because this tests for "compose-style" (new) expmap
// EXPECT(assert_equal(t1, retract(origin,d21)*t2)); // EXPECT(assert_equal(t1, retract(origin,d21)*t2));
// Check that log(t1,t2)=-log(t2,t1) - this holds even for incorrect expmap :-) // Check that log(t1,t2)=-log(t2,t1) TODO: only holds for exp map
EXPECT(assert_equal(d12,-d21)); // EXPECT(assert_equal(d12,-d21));
}
#ifdef CORRECT_POSE3_EXMAP /* ************************************************************************* */
TEST(Pose3, subgroups)
{
// todo: Frank - Below only works for correct "Agrawal06iros style expmap // Frank - Below only works for correct "Agrawal06iros style expmap
// lines in canonical coordinates correspond to Abelian subgroups in SE(3) // lines in canonical coordinates correspond to Abelian subgroups in SE(3)
Vector d = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); Vector d = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
// exp(-d)=inverse(exp(d)) // exp(-d)=inverse(exp(d))
@ -483,8 +482,6 @@ TEST(Pose3, manifold)
Pose3 T5 = Pose3::Expmap(5*d); Pose3 T5 = Pose3::Expmap(5*d);
EXPECT(assert_equal(T5,T2*T3)); EXPECT(assert_equal(T5,T2*T3));
EXPECT(assert_equal(T5,T3*T2)); EXPECT(assert_equal(T5,T3*T2));
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -197,21 +197,10 @@ TEST( Pose3Values, pose3Circle )
TEST( Pose3Values, expmap ) TEST( Pose3Values, expmap )
{ {
Pose3Values expected; Pose3Values expected;
#ifdef CORRECT_POSE3_EXMAP
expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0))); expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0))); expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0))); expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0))); expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
#else
// expected is circle shifted to East
expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0)));
expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0)));
expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0)));
expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0)));
#endif
// Note expmap coordinates are in global coordinates with non-compose expmap
// so shifting to East requires little thought, different from with Pose2 !!!
Ordering ordering(*expected.orderingArbitrary()); Ordering ordering(*expected.orderingArbitrary());
VectorValues delta(expected.dims(ordering)); VectorValues delta(expected.dims(ordering));