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.
Frank Dellaert
parent
5e2f4c0958
commit
e6382c7ec0
293
.cproject
293
.cproject
|
|
@ -378,6 +378,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>
|
||||
|
|
@ -404,7 +412,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>
|
||||
|
|
@ -412,7 +419,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>
|
||||
|
|
@ -460,7 +466,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>
|
||||
|
|
@ -468,7 +473,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>
|
||||
|
|
@ -476,7 +480,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>
|
||||
|
|
@ -492,20 +495,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="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -532,6 +526,7 @@
|
|||
</target>
|
||||
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -603,6 +598,7 @@
|
|||
</target>
|
||||
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testInference.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -610,6 +606,7 @@
|
|||
</target>
|
||||
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -617,6 +614,7 @@
|
|||
</target>
|
||||
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testJunctionTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -624,6 +622,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -631,6 +630,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -700,22 +700,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>
|
||||
|
|
@ -732,6 +716,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>
|
||||
|
|
@ -756,15 +756,7 @@
|
|||
<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_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>check</buildTarget>
|
||||
|
|
@ -772,14 +764,6 @@
|
|||
<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="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -820,7 +804,15 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
|
|
@ -828,6 +820,14 @@
|
|||
<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="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1150,7 +1150,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>
|
||||
|
|
@ -1476,6 +1475,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1558,6 +1573,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>
|
||||
|
|
@ -1597,6 +1613,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>
|
||||
|
|
@ -1604,6 +1621,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>
|
||||
|
|
@ -1946,6 +1964,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>
|
||||
|
|
@ -1967,46 +1986,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -2103,6 +2082,94 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -2159,54 +2226,6 @@
|
|||
<useDefaultCommand>false</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>
|
||||
</buildTargets>
|
||||
</storageModule>
|
||||
</cproject>
|
||||
|
|
|
|||
|
|
@ -30,10 +30,13 @@ CalibratedCamera::CalibratedCamera(const Pose3& pose) :
|
|||
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) {
|
||||
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());
|
||||
}
|
||||
|
|
@ -55,30 +58,25 @@ CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Point2 CalibratedCamera::project(const Point3& point,
|
||||
boost::optional<Matrix&> D_intrinsic_pose,
|
||||
boost::optional<Matrix&> D_intrinsic_point) const {
|
||||
const Pose3& pose = pose_;
|
||||
const Rot3& R = pose.rotation();
|
||||
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
|
||||
Point3 q = pose.transform_to(point);
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
|
||||
Point3 q = pose_.transform_to(point, H1, H2);
|
||||
|
||||
// Check if point is in front of camera
|
||||
if(q.z() <= 0)
|
||||
throw CheiralityException();
|
||||
|
||||
if (D_intrinsic_pose || D_intrinsic_point) {
|
||||
double X = q.x(), Y = q.y(), Z = q.z();
|
||||
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
|
||||
double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2;
|
||||
double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2;
|
||||
if (D_intrinsic_pose)
|
||||
*D_intrinsic_pose = Matrix_(2,6,
|
||||
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);
|
||||
if (H1 || H2) {
|
||||
Matrix H;
|
||||
Point2 intrinsic = project_to_camera(q,H);
|
||||
// just implement chain rule
|
||||
if (H1) *H1 = H * (*H1);
|
||||
if (H1) *H2 = H * (*H2);
|
||||
return intrinsic;
|
||||
}
|
||||
return project_to_camera(q);
|
||||
else
|
||||
return project_to_camera(q);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -29,10 +29,7 @@ namespace gtsam {
|
|||
/** instantiate concept checks */
|
||||
GTSAM_CONCEPT_POSE_INST(Pose3);
|
||||
|
||||
static const Matrix I3 = eye(3), Z3 = zeros(3, 3);
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
static const Matrix _I3=-I3, I6 = eye(6);
|
||||
#endif
|
||||
static const Matrix I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3, I6 = eye(6);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Calculate Adjoint map
|
||||
|
|
@ -94,42 +91,60 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
/* ************************************************************************* */
|
||||
// Changes default to use the full verions of expmap/logmap
|
||||
/* ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::retract(const Vector& d) const {
|
||||
return compose(Expmap(d));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::localCoordinates(const Pose3& T2) const {
|
||||
return Logmap(between(T2));
|
||||
}
|
||||
|
||||
// Different versions of retract
|
||||
Pose3 Pose3::retract(const Vector& xi) const {
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
// Lie group exponential map, traces out geodesic
|
||||
return compose(Expmap(xi));
|
||||
#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
|
||||
* pose. Increments the offset and rotation independently given a translation and
|
||||
* canonical rotation coordinates. Created to match ML derivatives, but
|
||||
* superseded by the correct exponential map story in .cpp */
|
||||
Pose3 Pose3::retract(const Vector& d) const {
|
||||
return Pose3(R_.retract(sub(d, 0, 3)),
|
||||
t_.retract(sub(d, 3, 6)));
|
||||
}
|
||||
// different versions of localCoordinates
|
||||
Vector Pose3::localCoordinates(const Pose3& T) const {
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
// Lie group logarithm map, exact inverse of exponential map
|
||||
return Logmap(between(T));
|
||||
#else
|
||||
// R is always done exactly in all three retract versions below
|
||||
Vector omega = R_.localCoordinates(T.rotation());
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** Independently computes the logmap of the translation and rotation. */
|
||||
Vector Pose3::localCoordinates(const Pose3& pp) const {
|
||||
const Vector r(R_.localCoordinates(pp.rotation())),
|
||||
t(t_.localCoordinates(pp.translation()));
|
||||
return concatVectors(2, &r, &t);
|
||||
}
|
||||
// Incorrect version
|
||||
// Independently computes the logmap of the translation and rotation
|
||||
// Vector v = t_.localCoordinates(T.translation());
|
||||
|
||||
#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 {
|
||||
|
|
@ -150,15 +165,9 @@ namespace gtsam {
|
|||
Point3 Pose3::transform_from(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) {
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
const Matrix R = R_.matrix();
|
||||
Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
*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();
|
||||
return R_ * p + t_;
|
||||
|
|
@ -169,16 +178,10 @@ namespace gtsam {
|
|||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
const Point3 result = R_.unrotate(p - t_);
|
||||
if (H1) {
|
||||
const Point3& q = result;
|
||||
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
*H1 = collect(2, &DR, &_I3);
|
||||
#else
|
||||
Matrix DT = - R_.transpose(); // negative because of sub
|
||||
*H1 = collect(2,&DR,&DT);
|
||||
#endif
|
||||
}
|
||||
|
||||
const Point3& q = result;
|
||||
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
|
||||
*H1 = collect(2, &DR, &_I3);
|
||||
}
|
||||
if (H2) *H2 = R_.transpose();
|
||||
return result;
|
||||
}
|
||||
|
|
@ -186,47 +189,14 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Pose3 Pose3::compose(const Pose3& p2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) {
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
*H1 = p2.inverse().adjointMap();
|
||||
#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;
|
||||
if (H1) *H1 = p2.inverse().adjointMap();
|
||||
if (H2) *H2 = I6;
|
||||
return (*this) * p2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
|
||||
if (H1)
|
||||
#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
|
||||
if (H1) *H1 = -adjointMap();
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt, Rt*(-t_));
|
||||
}
|
||||
|
|
@ -262,11 +232,7 @@ namespace gtsam {
|
|||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
double r = range(point.translation(), H1, H2);
|
||||
if (H2) {
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
Matrix H2_ = *H2 * point.rotation().matrix();
|
||||
#else
|
||||
Matrix H2_ = *H2;
|
||||
#endif
|
||||
*H2 = zeros(1, 6);
|
||||
insertSub(*H2, H2_, 0, 3);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -44,12 +44,12 @@ TEST( Pose3, equals)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, expmap_a)
|
||||
TEST( Pose3, retract)
|
||||
{
|
||||
Pose3 id;
|
||||
Vector v = zero(6);
|
||||
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
|
||||
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
|
||||
#else
|
||||
|
|
@ -195,9 +195,7 @@ TEST( Pose3, compose )
|
|||
|
||||
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2);
|
||||
EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3));
|
||||
#endif
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2);
|
||||
EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4));
|
||||
|
|
@ -217,9 +215,7 @@ TEST( Pose3, compose2 )
|
|||
|
||||
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2);
|
||||
EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3));
|
||||
#endif
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2);
|
||||
EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5));
|
||||
|
|
@ -235,9 +231,7 @@ TEST( Pose3, inverse)
|
|||
|
||||
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
|
||||
EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3));
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -251,9 +245,7 @@ TEST( Pose3, inverseDerivatives2)
|
|||
Matrix actualDinverse;
|
||||
T.inverse(actualDinverse);
|
||||
EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3));
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -450,10 +442,17 @@ TEST( Pose3, transformPose_to)
|
|||
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)
|
||||
{
|
||||
//cout << "manifold" << endl;
|
||||
Pose3 t1 = T;
|
||||
Pose3 t2 = T3;
|
||||
Pose3 origin;
|
||||
|
|
@ -466,13 +465,13 @@ TEST(Pose3, manifold)
|
|||
// todo: richard - commented out because this tests for "compose-style" (new) expmap
|
||||
// EXPECT(assert_equal(t1, retract(origin,d21)*t2));
|
||||
|
||||
// Check that log(t1,t2)=-log(t2,t1) - this holds even for incorrect expmap :-)
|
||||
EXPECT(assert_equal(d12,-d21));
|
||||
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
|
||||
|
||||
// todo: Frank - Below only works for correct "Agrawal06iros style expmap
|
||||
// Check that log(t1,t2)=-log(t2,t1) TODO: only holds for exp map
|
||||
// EXPECT(assert_equal(d12,-d21));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, subgroups)
|
||||
{
|
||||
// Frank - Below only works for correct "Agrawal06iros style expmap
|
||||
// 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);
|
||||
// exp(-d)=inverse(exp(d))
|
||||
|
|
@ -483,8 +482,6 @@ TEST(Pose3, manifold)
|
|||
Pose3 T5 = Pose3::Expmap(5*d);
|
||||
EXPECT(assert_equal(T5,T2*T3));
|
||||
EXPECT(assert_equal(T5,T3*T2));
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -197,21 +197,10 @@ TEST( Pose3Values, pose3Circle )
|
|||
TEST( Pose3Values, expmap )
|
||||
{
|
||||
Pose3Values expected;
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
|
||||
expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
|
||||
expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
|
||||
expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
|
||||
#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());
|
||||
VectorValues delta(expected.dims(ordering));
|
||||
|
|
|
|||
Loading…
Reference in New Issue