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

Geometry passes tests with newer interfaces and more concept checks

release/4.3a0
Alex Cunningham 2011-11-04 21:44:29 +00:00
parent b8c56f9047
commit 07aaf38245
34 changed files with 972 additions and 776 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

197
.cproject
View File

@ -375,14 +375,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -409,6 +401,7 @@
</target> </target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget> <buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -416,6 +409,7 @@
</target> </target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget> <buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -463,6 +457,7 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget> <buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -470,6 +465,7 @@
</target> </target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget> <buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -477,6 +473,7 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget> <buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -492,11 +489,20 @@
</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>
@ -523,7 +529,6 @@
</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>
@ -595,7 +600,6 @@
</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>
@ -603,7 +607,6 @@
</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>
@ -611,7 +614,6 @@
</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>
@ -619,7 +621,6 @@
</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>
@ -627,7 +628,6 @@
</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>
@ -681,6 +681,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="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>
@ -721,7 +737,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>
@ -729,6 +753,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="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>
@ -769,15 +801,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>
@ -785,14 +809,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="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>
@ -1107,6 +1123,7 @@
</target> </target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget> <buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1514,7 +1531,6 @@
</target> </target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget> <buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1554,7 +1570,6 @@
</target> </target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget> <buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1562,7 +1577,6 @@
</target> </target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget> <buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1810,7 +1824,6 @@
</target> </target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget> <buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1832,6 +1845,46 @@
<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>
@ -1928,62 +1981,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="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="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>
@ -2024,6 +2021,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<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>
</buildTargets> </buildTargets>
</storageModule> </storageModule>
</cproject> </cproject>

4
gtsam/base/Lie-inl.h
View File

@ -25,6 +25,6 @@
template Vector logmap_default(const T&, const T&); \ template Vector logmap_default(const T&, const T&); \
template T expmap_default(const T&, const Vector&); \ template T expmap_default(const T&, const Vector&); \
template bool equal(const T&, const T&, double); \ template bool equal(const T&, const T&, double); \
template bool equal(const T&, const T&); \ template bool equal(const T&, const T&);
template class Lie<T>;

28
gtsam/base/numericalDerivative.h
View File

@ -67,8 +67,8 @@ namespace gtsam {
const size_t n = x.dim(); const size_t n = x.dim();
Vector d = zero(n), g = zero(n); Vector d = zero(n), g = zero(n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; double hxplus = h(x.expmap(d)); d(j) += delta; double hxplus = h(x.retract(d));
d(j) -= 2*delta; double hxmin = h(x.expmap(d)); d(j) -= 2*delta; double hxmin = h(x.retract(d));
d(j) += delta; g(j) = (hxplus-hxmin)*factor; d(j) += delta; g(j) = (hxplus-hxmin)*factor;
} }
return g; return g;
@ -97,8 +97,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x.expmap(d))); d(j) += delta; Vector hxplus = hx.unretract(h(x.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x.expmap(d))); d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }
@ -151,8 +151,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1.expmap(d),x2)); d(j) += delta; Vector hxplus = hx.unretract(h(x1.retract(d),x2));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1.expmap(d),x2)); d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x1.retract(d),x2));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }
@ -215,8 +215,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1,x2.expmap(d))); d(j) += delta; Vector hxplus = hx.unretract(h(x1,x2.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1,x2.expmap(d))); d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x1,x2.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }
@ -281,8 +281,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1.expmap(d),x2,x3)); d(j) += delta; Vector hxplus = hx.unretract(h(x1.retract(d),x2,x3));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1.expmap(d),x2,x3)); d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x1.retract(d),x2,x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }
@ -346,8 +346,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1, x2.expmap(d),x3)); d(j) += delta; Vector hxplus = hx.unretract(h(x1, x2.retract(d),x3));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1, x2.expmap(d),x3)); d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x1, x2.retract(d),x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }
@ -411,8 +411,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1, x2, x3.expmap(d))); d(j) += delta; Vector hxplus = hx.unretract(h(x1, x2, x3.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1, x2, x3.expmap(d))); d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x1, x2, x3.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }

31
gtsam/geometry/Cal3Bundler.cpp
View File

@ -23,21 +23,35 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
Cal3Bundler::Cal3Bundler() : f_(1), k1_(0), k2_(0) {} Cal3Bundler::Cal3Bundler() : f_(1), k1_(0), k2_(0) {}
/* ************************************************************************* */
Cal3Bundler::Cal3Bundler(const Vector &v) : f_(v(0)), k1_(v(1)), k2_(v(2)) {} Cal3Bundler::Cal3Bundler(const Vector &v) : f_(v(0)), k1_(v(1)), k2_(v(2)) {}
/* ************************************************************************* */
Cal3Bundler::Cal3Bundler(const double f, const double k1, const double k2) : f_(f), k1_(k1), k2_(k2) {} Cal3Bundler::Cal3Bundler(const double f, const double k1, const double k2) : f_(f), k1_(k1), k2_(k2) {}
/* ************************************************************************* */
Matrix Cal3Bundler::K() const { return Matrix_(3,3, f_, 0.0, 0.0, 0.0, f_, 0.0, 0.0, 0.0, 1.0); } Matrix Cal3Bundler::K() const { return Matrix_(3,3, f_, 0.0, 0.0, 0.0, f_, 0.0, 0.0, 0.0, 1.0); }
/* ************************************************************************* */
Vector Cal3Bundler::k() const { return Vector_(4, k1_, k2_, 0, 0) ; } Vector Cal3Bundler::k() const { return Vector_(4, k1_, k2_, 0, 0) ; }
/* ************************************************************************* */
Vector Cal3Bundler::vector() const { return Vector_(3, f_, k1_, k2_) ; } Vector Cal3Bundler::vector() const { return Vector_(3, f_, k1_, k2_) ; }
/* ************************************************************************* */
void Cal3Bundler::print(const std::string& s) const { gtsam::print(vector(), s + ".K") ; } void Cal3Bundler::print(const std::string& s) const { gtsam::print(vector(), s + ".K") ; }
/* ************************************************************************* */
bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol || fabs(k2_ - K.k2_) > tol) if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol || fabs(k2_ - K.k2_) > tol)
return false; return false;
return true ; return true ;
} }
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, Point2 Cal3Bundler::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
@ -78,6 +92,7 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p,
return Point2(fg * x, fg * y) ; return Point2(fg * x, fg * y) ;
} }
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const { Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
const double r = xx + yy ; const double r = xx + yy ;
@ -95,6 +110,7 @@ Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
return DK * DR; return DK * DR;
} }
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_calibration(const Point2& p) const { Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ; const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ;
@ -109,6 +125,7 @@ Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
g*y, f*y*r , f*y*r2); g*y, f*y*r , f*y*r2);
} }
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
@ -128,11 +145,17 @@ Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
return H ; return H ;
} }
/* ************************************************************************* */
Cal3Bundler Cal3Bundler::retract(const Vector& d) const { return Cal3Bundler(vector() + d) ; }
Cal3Bundler Cal3Bundler::expmap(const Vector& d) const { return Cal3Bundler(vector() + d) ; } /* ************************************************************************* */
Vector Cal3Bundler::logmap(const Cal3Bundler& T2) const { return vector() - T2.vector(); } Vector Cal3Bundler::unretract(const Cal3Bundler& T2) const { return vector() - T2.vector(); }
Cal3Bundler Cal3Bundler::Expmap(const Vector& v) { return Cal3Bundler(v) ; }
Vector Cal3Bundler::Logmap(const Cal3Bundler& p) { return p.vector(); } /* ************************************************************************* */
Cal3Bundler Cal3Bundler::Retract(const Vector& v) { return Cal3Bundler(v) ; }
/* ************************************************************************* */
Vector Cal3Bundler::Unretract(const Cal3Bundler& p) { return p.vector(); }

8
gtsam/geometry/Cal3Bundler.h
View File

@ -51,11 +51,11 @@ public:
Matrix D2d_calibration(const Point2& p) const ; Matrix D2d_calibration(const Point2& p) const ;
Matrix D2d_intrinsic_calibration(const Point2& p) const ; Matrix D2d_intrinsic_calibration(const Point2& p) const ;
Cal3Bundler expmap(const Vector& d) const ; Cal3Bundler retract(const Vector& d) const ;
Vector logmap(const Cal3Bundler& T2) const ; Vector unretract(const Cal3Bundler& T2) const ;
static Cal3Bundler Expmap(const Vector& v) ; static Cal3Bundler Retract(const Vector& v) ;
static Vector Logmap(const Cal3Bundler& p) ; static Vector Unretract(const Cal3Bundler& p) ;
int dim() const { return 3 ; } int dim() const { return 3 ; }
static size_t Dim() { return 3; } static size_t Dim() { return 3; }

31
gtsam/geometry/Cal3DS2.cpp
View File

@ -23,20 +23,30 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
Cal3DS2::Cal3DS2():fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0){} Cal3DS2::Cal3DS2():fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0){}
// Construction /* ************************************************************************* */
Cal3DS2::Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4) : Cal3DS2::Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {}
/* ************************************************************************* */
Cal3DS2::Cal3DS2(const Vector &v): Cal3DS2::Cal3DS2(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){} fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2::K() const { return Matrix_(3,3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } Matrix Cal3DS2::K() const { return Matrix_(3,3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); }
/* ************************************************************************* */
Vector Cal3DS2::k() const { return Vector_(4, k1_, k2_, k3_, k4_); } Vector Cal3DS2::k() const { return Vector_(4, k1_, k2_, k3_, k4_); }
/* ************************************************************************* */
Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_) ; } Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_) ; }
/* ************************************************************************* */
void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K") ; gtsam::print(k(), s + ".k") ; } void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K") ; gtsam::print(k(), s + ".k") ; }
/* ************************************************************************* */
bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
@ -45,6 +55,7 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
return true ; return true ;
} }
/* ************************************************************************* */
Point2 Cal3DS2::uncalibrate(const Point2& p, Point2 Cal3DS2::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
@ -67,6 +78,7 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
return Point2(fx_* x2 + s_ * y2 + u0_, fy_ * y2 + v0_) ; return Point2(fx_* x2 + s_ * y2 + u0_, fy_ * y2 + v0_) ;
} }
/* ************************************************************************* */
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
//const double fx = fx_, fy = fy_, s = s_ ; //const double fx = fx_, fy = fy_, s = s_ ;
const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
@ -94,7 +106,7 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
return DK * DR; return DK * DR;
} }
/* ************************************************************************* */
Matrix Cal3DS2::D2d_calibration(const Point2& p) const { Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y ; const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y ;
const double r = xx + yy ; const double r = xx + yy ;
@ -111,10 +123,17 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
0.0, pny, 0.0, 0.0, 1.0, fy*y*r , fy*y*r2 , fy*(r+2*yy) , fy*(2*xy) ); 0.0, pny, 0.0, 0.0, 1.0, fy*y*r , fy*y*r2 , fy*(r+2*yy) , fy*(2*xy) );
} }
Cal3DS2 Cal3DS2::expmap(const Vector& d) const { return Cal3DS2(vector() + d) ; } /* ************************************************************************* */
Vector Cal3DS2::logmap(const Cal3DS2& T2) const { return vector() - T2.vector(); } Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d) ; }
Cal3DS2 Cal3DS2::Expmap(const Vector& v) { return Cal3DS2(v) ; }
Vector Cal3DS2::Logmap(const Cal3DS2& p) { return p.vector(); } /* ************************************************************************* */
Vector Cal3DS2::unretract(const Cal3DS2& T2) const { return vector() - T2.vector(); }
/* ************************************************************************* */
Cal3DS2 Cal3DS2::Retract(const Vector& v) { return Cal3DS2(v) ; }
/* ************************************************************************* */
Vector Cal3DS2::Unretract(const Cal3DS2& p) { return p.vector(); }
} }
/* ************************************************************************* */ /* ************************************************************************* */

20
gtsam/geometry/Cal3DS2.h
View File

@ -22,12 +22,12 @@
namespace gtsam { namespace gtsam {
/** /**
* @brief Calibration of a camera with radial distortion * @brief Calibration of a camera with radial distortion
* @ingroup geometry * @ingroup geometry
*/ */
class Cal3DS2 { class Cal3DS2 {
private: private:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order double k1_, k2_ ; // radial 2nd-order and 4th-order
double k3_, k4_ ; // tagential distortion double k3_, k4_ ; // tagential distortion
@ -38,7 +38,7 @@ namespace gtsam {
// k3 (r + 2 Pn.y^2) + 2*k4 pn.x pn.y ] // k3 (r + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
// pi = K*pn // pi = K*pn
public: public:
// Default Constructor with only unit focal length // Default Constructor with only unit focal length
Cal3DS2(); Cal3DS2();
@ -62,16 +62,16 @@ namespace gtsam {
Matrix D2d_intrinsic(const Point2& p) const ; Matrix D2d_intrinsic(const Point2& p) const ;
Matrix D2d_calibration(const Point2& p) const ; Matrix D2d_calibration(const Point2& p) const ;
Cal3DS2 expmap(const Vector& d) const ; Cal3DS2 retract(const Vector& d) const ;
Vector logmap(const Cal3DS2& T2) const ; Vector unretract(const Cal3DS2& T2) const ;
static Cal3DS2 Expmap(const Vector& v) ; static Cal3DS2 Retract(const Vector& v) ;
static Vector Logmap(const Cal3DS2& p) ; static Vector Unretract(const Cal3DS2& p) ;
int dim() const { return 9 ; } int dim() const { return 9 ; }
static size_t Dim() { return 9; } static size_t Dim() { return 9; }
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
@ -88,6 +88,6 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(k4_); ar & BOOST_SERIALIZATION_NVP(k4_);
} }
}; };
} }

17
gtsam/geometry/Cal3_S2.h
View File

@ -129,15 +129,26 @@ namespace gtsam {
} }
/// Given 5-dim tangent vector, create new calibration /// Given 5-dim tangent vector, create new calibration
inline Cal3_S2 expmap(const Vector& d) const { inline Cal3_S2 retract(const Vector& d) const {
return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
} }
/// logmap for the calibration /// Retraction from origin
Vector logmap(const Cal3_S2& T2) const { inline static Cal3_S2 Retract(const Vector& d) {
Cal3_S2 c;
return c.retract(d);
}
/// Unretraction for the calibration
Vector unretract(const Cal3_S2& T2) const {
return vector() - T2.vector(); return vector() - T2.vector();
} }
/// Unretraction from origin
inline static Vector Unretract(const Cal3_S2& T2) {
return T2.vector();
}
private: private:
/// Serialization function /// Serialization function

60
gtsam/geometry/CalibratedCamera.cpp
View File

@ -21,38 +21,40 @@
namespace gtsam { namespace gtsam {
CalibratedCamera::CalibratedCamera() {} /* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Pose3& pose) :
CalibratedCamera::CalibratedCamera(const Pose3& pose) :
pose_(pose) { pose_(pose) {
} }
CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {} /* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {}
CalibratedCamera::~CalibratedCamera() {} /* ************************************************************************* */
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());
} }
Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) { /* ************************************************************************* */
Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) {
return Point3(p.x() * scale, p.y() * scale, scale); return Point3(p.x() * scale, p.y() * scale, scale);
} }
CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) { /* ************************************************************************* */
CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) {
double st = sin(pose2.theta()), ct = cos(pose2.theta()); double st = sin(pose2.theta()), ct = cos(pose2.theta());
Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
Rot3 wRc(x, y, z); Rot3 wRc(x, y, z);
Point3 t(pose2.x(), pose2.y(), height); Point3 t(pose2.x(), pose2.y(), height);
Pose3 pose3(wRc, t); Pose3 pose3(wRc, t);
return CalibratedCamera(pose3); return CalibratedCamera(pose3);
} }
Point2 CalibratedCamera::project(const Point3& point, /* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point,
boost::optional<Matrix&> D_intrinsic_pose, boost::optional<Matrix&> D_intrinsic_pose,
boost::optional<Matrix&> D_intrinsic_point) const { boost::optional<Matrix&> D_intrinsic_point) const {
const Pose3& pose = pose_; const Pose3& pose = pose_;
@ -75,23 +77,27 @@ namespace gtsam {
dp21, dp22, dp23); dp21, dp22, dp23);
} }
return project_to_camera(q); return project_to_camera(q);
} }
/* ************************************************************************* */
CalibratedCamera CalibratedCamera::retract(const Vector& d) const {
return CalibratedCamera(pose().retract(d)) ;
}
CalibratedCamera CalibratedCamera::expmap(const Vector& d) const { /* ************************************************************************* */
return CalibratedCamera(pose().expmap(d)) ; Vector CalibratedCamera::unretract(const CalibratedCamera& T2) const {
} return pose().unretract(T2.pose()) ;
Vector CalibratedCamera::logmap(const CalibratedCamera& T2) const { }
return pose().logmap(T2.pose()) ;
}
CalibratedCamera CalibratedCamera::Expmap(const Vector& v) { /* ************************************************************************* */
return CalibratedCamera(Pose3::Expmap(v)) ; CalibratedCamera CalibratedCamera::Retract(const Vector& v) {
} return CalibratedCamera(Pose3::Retract(v)) ;
}
Vector CalibratedCamera::Logmap(const CalibratedCamera& p) { /* ************************************************************************* */
return Pose3::Logmap(p.pose()) ; Vector CalibratedCamera::Unretract(const CalibratedCamera& p) {
} return Pose3::Unretract(p.pose()) ;
}
/* ************************************************************************* */ /* ************************************************************************* */
} }

18
gtsam/geometry/CalibratedCamera.h
View File

@ -34,10 +34,10 @@ namespace gtsam {
Pose3 pose_; // 6DOF pose Pose3 pose_; // 6DOF pose
public: public:
CalibratedCamera(); ///< default constructor CalibratedCamera() {} ///< default constructor
CalibratedCamera(const Pose3& pose); ///< construct with pose CalibratedCamera(const Pose3& pose); ///< construct with pose
CalibratedCamera(const Vector &v) ; ///< construct from vector CalibratedCamera(const Vector &v) ; ///< construct from vector
virtual ~CalibratedCamera(); ///< destructor virtual ~CalibratedCamera() {} ///< destructor
/// return pose /// return pose
inline const Pose3& pose() const { return pose_; } inline const Pose3& pose() const { return pose_; }
@ -58,16 +58,22 @@ namespace gtsam {
} }
/// move a cameras pose according to d /// move a cameras pose according to d
CalibratedCamera expmap(const Vector& d) const; CalibratedCamera retract(const Vector& d) const;
/// Return canonical coordinate /// Return canonical coordinate
Vector logmap(const CalibratedCamera& T2) const; Vector unretract(const CalibratedCamera& T2) const;
/// move a cameras pose according to d /// move a cameras pose according to d
static CalibratedCamera Expmap(const Vector& v); static CalibratedCamera Retract(const Vector& v);
/// Return canonical coordinate /// Return canonical coordinate
static Vector Logmap(const CalibratedCamera& p); static Vector Unretract(const CalibratedCamera& p);
// // Manifold requirements - use existing expmap/logmap
// inline CalibratedCamera retract(const Vector& v) const { return expmap(v); }
// inline static CalibratedCamera Retract(const Vector& v) { return Expmap(v); }
// inline Vector unretract(const CalibratedCamera& t2) const { return logmap(t2); }
// inline static Vector Unretract(const CalibratedCamera& t) { return Logmap(t); }
/// Lie group dimensionality /// Lie group dimensionality
inline size_t dim() const { return 6 ; } inline size_t dim() const { return 6 ; }

1
gtsam/geometry/CalibratedCameraT.h
View File

@ -17,6 +17,7 @@ namespace gtsam {
* A Calibrated camera class [R|-R't], calibration K. * A Calibrated camera class [R|-R't], calibration K.
* If calibration is known, it is more computationally efficient * If calibration is known, it is more computationally efficient
* to calibrate the measurements rather than try to predict in pixels. * to calibrate the measurements rather than try to predict in pixels.
* AGC: Is this used or tested anywhere?
* @ingroup geometry * @ingroup geometry
*/ */
template <typename Calibration> template <typename Calibration>

2
gtsam/geometry/Makefile.am
View File

@ -24,7 +24,7 @@ check_PROGRAMS += tests/testCal3DS2 tests/testCal3_S2 tests/testCal3Bundler test
# Stereo # Stereo
sources += StereoPoint2.cpp StereoCamera.cpp sources += StereoPoint2.cpp StereoCamera.cpp
check_PROGRAMS += tests/testStereoCamera check_PROGRAMS += tests/testStereoCamera tests/testStereoPoint2
# Tensors # Tensors
headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h

17
gtsam/geometry/Point2.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
* Functional, so no set functions: once created, a point is constant. * Functional, so no set functions: once created, a point is constant.
* @ingroup geometry * @ingroup geometry
*/ */
class Point2: public Lie<Point2> { class Point2 {
public: public:
/// dimension of the variable - used to autodetect sizes /// dimension of the variable - used to autodetect sizes
static const size_t dimension = 2; static const size_t dimension = 2;
@ -80,6 +80,21 @@ namespace gtsam {
/** Log map around identity - just return the Point2 as a vector */ /** Log map around identity - just return the Point2 as a vector */
static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
// Manifold requirements
inline Point2 retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static Point2 Retract(const Vector& v) { return Expmap(v); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const Point2& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const Point2& t) { return Logmap(t); }
/** "Between", subtracts point coordinates */ /** "Between", subtracts point coordinates */
inline Point2 between(const Point2& p2, inline Point2 between(const Point2& p2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,

15
gtsam/geometry/Point3.h
View File

@ -82,6 +82,21 @@ namespace gtsam {
inline Point3 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); } inline Point3 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); }
inline Vector logmap(const Point3& p2) const { return gtsam::logmap_default(*this, p2);} inline Vector logmap(const Point3& p2) const { return gtsam::logmap_default(*this, p2);}
// Manifold requirements
inline Point3 retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static Point3 Retract(const Vector& v) { return Expmap(v); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const Point3& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const Point3& t) { return Logmap(t); }
/** Between using the default implementation */ /** Between using the default implementation */
inline Point3 between(const Point3& p2, inline Point3 between(const Point3& p2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,

16
gtsam/geometry/Pose2.cpp
View File

@ -53,7 +53,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::ExpmapFull(const Vector& xi) { Pose2 Pose2::Expmap(const Vector& xi) {
assert(xi.size() == 3); assert(xi.size() == 3);
Point2 v(xi(0),xi(1)); Point2 v(xi(0),xi(1));
double w = xi(2); double w = xi(2);
@ -68,7 +68,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Pose2::LogmapFull(const Pose2& p) { Vector Pose2::Logmap(const Pose2& p) {
const Rot2& R = p.r(); const Rot2& R = p.r();
const Point2& t = p.t(); const Point2& t = p.t();
double w = R.theta(); double w = R.theta();
@ -88,25 +88,25 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Changes default to use the full verions of expmap/logmap // Changes default to use the full verions of expmap/logmap
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& xi) { Pose2 Pose2::Retract(const Vector& xi) {
return ExpmapFull(xi); return Expmap(xi);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) { Vector Pose2::Unretract(const Pose2& p) {
return LogmapFull(p); return Logmap(p);
} }
#else #else
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& v) { Pose2 Pose2::Retract(const Vector& v) {
assert(v.size() == 3); assert(v.size() == 3);
return Pose2(v[0], v[1], v[2]); return Pose2(v[0], v[1], v[2]);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) { Vector Pose2::Unretract(const Pose2& p) {
return Vector_(3, p.x(), p.y(), p.theta()); return Vector_(3, p.x(), p.y(), p.theta());
} }

42
gtsam/geometry/Pose2.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* A 2D pose (Point2,Rot2) * A 2D pose (Point2,Rot2)
* @ingroup geometry * @ingroup geometry
*/ */
class Pose2: public Lie<Pose2> { class Pose2 {
public: public:
static const size_t dimension = 3; static const size_t dimension = 3;
@ -120,26 +120,26 @@ namespace gtsam {
inline Point2 operator*(const Point2& point) const { return transform_from(point);} inline Point2 operator*(const Point2& point) const { return transform_from(point);}
/** /**
* Exponential map from se(2) to SE(2) * Retraction from se(2) to SE(2)
*/ */
static Pose2 Expmap(const Vector& v); static Pose2 Retract(const Vector& v);
/** /**
* Inverse of exponential map, from SE(2) to se(2) * Inverse of retraction, from SE(2) to se(2)
*/ */
static Vector Unretract(const Pose2& p);
/** Real versions of Expmap/Logmap */
static Pose2 Expmap(const Vector& xi);
static Vector Logmap(const Pose2& p); static Vector Logmap(const Pose2& p);
/** non-approximated versions of Expmap/Logmap */
static Pose2 ExpmapFull(const Vector& xi);
static Vector LogmapFull(const Pose2& p);
/** default implementations of binary functions */ /** default implementations of binary functions */
inline Pose2 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); } inline Pose2 retract(const Vector& v) const { return compose(Retract(v)); }
inline Vector logmap(const Pose2& p2) const { return gtsam::logmap_default(*this, p2);} inline Vector unretract(const Pose2& p2) const { return Unretract(between(p2));}
/** non-approximated versions of expmap/logmap */ /** non-approximated versions of expmap/logmap */
inline Pose2 expmapFull(const Vector& v) const { return compose(ExpmapFull(v)); } inline Pose2 expmap(const Vector& v) const { return compose(Expmap(v)); }
inline Vector logmapFull(const Pose2& p2) const { return LogmapFull(between(p2));} inline Vector logmap(const Pose2& p2) const { return Logmap(between(p2));}
/** /**
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame
@ -266,23 +266,5 @@ namespace gtsam {
typedef std::pair<Point2,Point2> Point2Pair; typedef std::pair<Point2,Point2> Point2Pair;
boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs); boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
/**
* Specializations for access to full expmap/logmap in templated functions
*
* NOTE: apparently, these *must* be indicated as inline to prevent compile error
*/
/** unary versions */
template<>
inline Pose2 ExpmapFull<Pose2>(const Vector& xi) { return Pose2::ExpmapFull(xi); }
template<>
inline Vector LogmapFull<Pose2>(const Pose2& p) { return Pose2::LogmapFull(p); }
/** binary versions */
template<>
inline Pose2 expmapFull<Pose2>(const Pose2& t, const Vector& v) { return t.expmapFull(v); }
template<>
inline Vector logmapFull<Pose2>(const Pose2& t, const Pose2& p2) { return t.logmapFull(p2); }
} // namespace gtsam } // namespace gtsam

40
gtsam/geometry/Pose3.cpp
View File

@ -60,7 +60,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */ /** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::ExpmapFull(const Vector& xi) { Pose3 Pose3::Expmap(const Vector& xi) {
// get angular velocity omega and translational velocity v from twist xi // get angular velocity omega and translational velocity v from twist xi
Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5)); Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5));
@ -81,7 +81,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Pose3::LogmapFull(const Pose3& p) { Vector Pose3::Logmap(const Pose3& p) {
Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
double t = w.norm(); double t = w.norm();
if (t < 1e-10) if (t < 1e-10)
@ -98,40 +98,40 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Changes default to use the full verions of expmap/logmap // Changes default to use the full verions of expmap/logmap
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Expmap(const Vector& xi) { Pose3 Retract(const Vector& xi) {
return Pose3::ExpmapFull(xi); return Pose3::Expmap(xi);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Logmap(const Pose3& p) { Vector Unretract(const Pose3& p) {
return Pose3::LogmapFull(p); return Pose3::Logmap(p);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 expmap(const Vector& d) { Pose3 retract(const Vector& d) {
return expmapFull(d); return expmap(d);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector logmap(const Pose3& T1, const Pose3& T2) { Vector unretract(const Pose3& T1, const Pose3& T2) {
return logmapFull(T2); return logmap(T2);
} }
#else #else
/* ************************************************************************* */ /* ************************************************************************* */
/* incorrect versions for which we know how to compute derivatives */ /* incorrect versions for which we know how to compute derivatives */
Pose3 Pose3::Expmap(const Vector& d) { Pose3 Pose3::Retract(const Vector& d) {
Vector w = sub(d, 0,3); Vector w = sub(d, 0,3);
Vector u = sub(d, 3,6); Vector u = sub(d, 3,6);
return Pose3(Rot3::Expmap(w), Point3::Expmap(u)); return Pose3(Rot3::Retract(w), Point3::Retract(u));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Log map at identity - return the translation and canonical rotation // Log map at identity - return the translation and canonical rotation
// coordinates of a pose. // coordinates of a pose.
Vector Pose3::Logmap(const Pose3& p) { Vector Pose3::Unretract(const Pose3& p) {
const Vector w = Rot3::Logmap(p.rotation()), u = Point3::Logmap(p.translation()); const Vector w = Rot3::Unretract(p.rotation()), u = Point3::Unretract(p.translation());
return concatVectors(2, &w, &u); return concatVectors(2, &w, &u);
} }
@ -139,15 +139,15 @@ namespace gtsam {
* pose. Increments the offset and rotation independently given a translation and * pose. Increments the offset and rotation independently given a translation and
* canonical rotation coordinates. Created to match ML derivatives, but * canonical rotation coordinates. Created to match ML derivatives, but
* superseded by the correct exponential map story in .cpp */ * superseded by the correct exponential map story in .cpp */
Pose3 Pose3::expmap(const Vector& d) const { Pose3 Pose3::retract(const Vector& d) const {
return Pose3(R_.expmap(sub(d, 0, 3)), return Pose3(R_.retract(sub(d, 0, 3)),
t_.expmap(sub(d, 3, 6))); t_.retract(sub(d, 3, 6)));
} }
/** Independently computes the logmap of the translation and rotation. */ /** Independently computes the logmap of the translation and rotation. */
Vector Pose3::logmap(const Pose3& pp) const { Vector Pose3::unretract(const Pose3& pp) const {
const Vector r(R_.logmap(pp.rotation())), const Vector r(R_.unretract(pp.rotation())),
t(t_.logmap(pp.translation())); t(t_.unretract(pp.translation()));
return concatVectors(2, &r, &t); return concatVectors(2, &r, &t);
} }

36
gtsam/geometry/Pose3.h
View File

@ -27,7 +27,7 @@ namespace gtsam {
* A 3D pose (R,t) : (Rot3,Point3) * A 3D pose (R,t) : (Rot3,Point3)
* @ingroup geometry * @ingroup geometry
*/ */
class Pose3 : public Lie<Pose3> { class Pose3 {
public: public:
static const size_t dimension = 6; static const size_t dimension = 6;
@ -117,25 +117,25 @@ namespace gtsam {
/** Exponential map at identity - create a pose with a translation and /** Exponential map at identity - create a pose with a translation and
* rotation (in canonical coordinates). */ * rotation (in canonical coordinates). */
static Pose3 Expmap(const Vector& v); static Pose3 Retract(const Vector& v);
/** Log map at identity - return the translation and canonical rotation /** Log map at identity - return the translation and canonical rotation
* coordinates of a pose. */ * coordinates of a pose. */
static Vector Logmap(const Pose3& p); static Vector Unretract(const Pose3& p);
/** Exponential map around another pose */ /** Exponential map around another pose */
Pose3 expmap(const Vector& d) const; Pose3 retract(const Vector& d) const;
/** Logarithm map around another pose T1 */ /** Logarithm map around another pose T1 */
Vector logmap(const Pose3& T2) const; Vector unretract(const Pose3& T2) const;
/** non-approximated versions of Expmap/Logmap */ /** non-approximated versions of Expmap/Logmap */
static Pose3 ExpmapFull(const Vector& xi); static Pose3 Expmap(const Vector& xi);
static Vector LogmapFull(const Pose3& p); static Vector Logmap(const Pose3& p);
/** non-approximated versions of expmap/logmap */ /** non-approximated versions of expmap/logmap */
inline Pose3 expmapFull(const Vector& v) const { return compose(Pose3::ExpmapFull(v)); } inline Pose3 expmap(const Vector& v) const { return compose(Pose3::Expmap(v)); }
inline Vector logmapFull(const Pose3& p2) const { return Pose3::LogmapFull(between(p2));} inline Vector logmap(const Pose3& p2) const { return Pose3::Logmap(between(p2));}
/** /**
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame
@ -207,22 +207,4 @@ namespace gtsam {
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
} }
/**
* Specializations for access to full expmap/logmap in templated functions
*
* NOTE: apparently, these *must* be indicated as inline to prevent compile error
*/
/** unary versions */
template<>
inline Pose3 ExpmapFull<Pose3>(const Vector& xi) { return Pose3::ExpmapFull(xi); }
template<>
inline Vector LogmapFull<Pose3>(const Pose3& p) { return Pose3::LogmapFull(p); }
/** binary versions */
template<>
inline Pose3 expmapFull<Pose3>(const Pose3& t, const Vector& v) { return t.expmapFull(v); }
template<>
inline Vector logmapFull<Pose3>(const Pose3& t, const Pose3& p2) { return t.logmapFull(p2); }
} // namespace gtsam } // namespace gtsam

17
gtsam/geometry/Rot2.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
* NOTE: the angle theta is in radians unless explicitly stated * NOTE: the angle theta is in radians unless explicitly stated
* @ingroup geometry * @ingroup geometry
*/ */
class Rot2: public Lie<Rot2> { class Rot2 {
public: public:
/** get the dimension by the type */ /** get the dimension by the type */
@ -158,6 +158,21 @@ namespace gtsam {
return Logmap(between(p2)); return Logmap(between(p2));
} }
// Manifold requirements
inline Rot2 retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static Rot2 Retract(const Vector& v) { return Expmap(v); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const Rot2& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const Rot2& t) { return Logmap(t); }
/** Between using the default implementation */ /** Between using the default implementation */
inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 = inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::none, boost::optional<Matrix&> H2 = boost::none) const {

34
gtsam/geometry/Rot3.h
View File

@ -28,19 +28,19 @@ namespace gtsam {
typedef Eigen::Quaterniond Quaternion; typedef Eigen::Quaterniond Quaternion;
/** /**
* @brief 3D Rotations represented as rotation matrices * @brief 3D Rotations represented as rotation matrices
* @ingroup geometry * @ingroup geometry
*/ */
class Rot3: public Lie<Rot3> { class Rot3 {
public: public:
static const size_t dimension = 3; static const size_t dimension = 3;
private: private:
/** we store columns ! */ /** we store columns ! */
Point3 r1_, r2_, r3_; Point3 r1_, r2_, r3_;
public: public:
/** default constructor, unit rotation */ /** default constructor, unit rotation */
Rot3() : Rot3() :
@ -204,6 +204,22 @@ typedef Eigen::Quaterniond Quaternion;
inline Rot3 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); } inline Rot3 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); }
inline Vector logmap(const Rot3& p2) const { return gtsam::logmap_default(*this, p2);} inline Vector logmap(const Rot3& p2) const { return gtsam::logmap_default(*this, p2);}
// Manifold requirements
inline Rot3 retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static Rot3 Retract(const Vector& v) { return Expmap(v); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const Rot3& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const Rot3& t) { return Logmap(t); }
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3() // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3()
inline Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const { inline Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const {
if (H1) *H1 = -matrix(); if (H1) *H1 = -matrix();
@ -245,7 +261,7 @@ typedef Eigen::Quaterniond Quaternion;
Point3 unrotate(const Point3& p, Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const; boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -255,9 +271,9 @@ typedef Eigen::Quaterniond Quaternion;
ar & BOOST_SERIALIZATION_NVP(r2_); ar & BOOST_SERIALIZATION_NVP(r2_);
ar & BOOST_SERIALIZATION_NVP(r3_); ar & BOOST_SERIALIZATION_NVP(r3_);
} }
}; };
/** /**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
* and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
* such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will * such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will
@ -267,6 +283,6 @@ typedef Eigen::Quaterniond Quaternion;
* @return an upper triangular matrix R * @return an upper triangular matrix R
* @return a vector [thetax, thetay, thetaz] in radians. * @return a vector [thetax, thetay, thetaz] in radians.
*/ */
std::pair<Matrix,Vector> RQ(const Matrix& A); std::pair<Matrix,Vector> RQ(const Matrix& A);
} }

30
gtsam/geometry/StereoCamera.h
View File

@ -24,17 +24,17 @@
namespace gtsam { namespace gtsam {
/** /**
* A stereo camera class, parameterize by left camera pose and stereo calibration * A stereo camera class, parameterize by left camera pose and stereo calibration
* @ingroup geometry * @ingroup geometry
*/ */
class StereoCamera { class StereoCamera {
private: private:
Pose3 leftCamPose_; Pose3 leftCamPose_;
Cal3_S2Stereo::shared_ptr K_; Cal3_S2Stereo::shared_ptr K_;
public: public:
StereoCamera() { StereoCamera() {
} }
@ -100,19 +100,33 @@ namespace gtsam {
/** Exponential map around p0 */ /** Exponential map around p0 */
inline StereoCamera expmap(const Vector& d) const { inline StereoCamera expmap(const Vector& d) const {
return StereoCamera(pose().expmap(d), K_); return StereoCamera(pose().retract(d), K_);
} }
Vector logmap(const StereoCamera &camera) const { Vector logmap(const StereoCamera &camera) const {
const Vector v1(leftCamPose_.logmap(camera.leftCamPose_)); const Vector v1(leftCamPose_.unretract(camera.leftCamPose_));
return v1; return v1;
} }
inline static StereoCamera Expmap(const Vector& d) {
return StereoCamera().expmap(d);
}
inline static Vector Logmap(const StereoCamera &camera) {
return StereoCamera().logmap(camera);
}
bool equals(const StereoCamera &camera, double tol = 1e-9) const { bool equals(const StereoCamera &camera, double tol = 1e-9) const {
return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
*camera.K_, tol); *camera.K_, tol);
} }
// Manifold requirements - use existing expmap/logmap
inline StereoCamera retract(const Vector& v) const { return expmap(v); }
inline static StereoCamera Retract(const Vector& v) { return Expmap(v); }
inline Vector unretract(const StereoCamera& t2) const { return logmap(t2); }
inline static Vector Unretract(const StereoCamera& t) { return Logmap(t); }
Pose3 between(const StereoCamera &camera, Pose3 between(const StereoCamera &camera,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const { boost::optional<Matrix&> H2=boost::none) const {
@ -124,7 +138,7 @@ namespace gtsam {
K_->print(s + ".calibration."); K_->print(s + ".calibration.");
} }
private: private:
/** utility functions */ /** utility functions */
Matrix Dproject_to_stereo_camera1(const Point3& P) const; Matrix Dproject_to_stereo_camera1(const Point3& P) const;
@ -135,6 +149,6 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(K_);
} }
}; };
} }

17
gtsam/geometry/StereoPoint2.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
* A 2D stereo point, v will be same for rectified images * A 2D stereo point, v will be same for rectified images
* @ingroup geometry * @ingroup geometry
*/ */
class StereoPoint2: Lie<StereoPoint2> { class StereoPoint2 {
public: public:
static const size_t dimension = 3; static const size_t dimension = 3;
private: private:
@ -110,6 +110,21 @@ namespace gtsam {
return gtsam::logmap_default(*this, p2); return gtsam::logmap_default(*this, p2);
} }
// Manifold requirements
inline StereoPoint2 retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static StereoPoint2 Retract(const Vector& v) { return Expmap(v); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const StereoPoint2& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const StereoPoint2& t) { return Logmap(t); }
inline StereoPoint2 between(const StereoPoint2& p2) const { inline StereoPoint2 between(const StereoPoint2& p2) const {
return gtsam::between_default(*this, p2); return gtsam::between_default(*this, p2);
} }

3
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -22,6 +22,9 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler)
Cal3Bundler K(500, 1e-3, 1e-3); Cal3Bundler K(500, 1e-3, 1e-3);
Point2 p(2,3); Point2 p(2,3);

3
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -22,6 +22,9 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2)
Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
Point2 p(2,3); Point2 p(2,3);

1
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -22,6 +22,7 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2) GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2)
Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2); Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
Point2 p(1, -2); Point2 p(1, -2);

2
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -25,6 +25,8 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
const Pose3 pose1(Matrix_(3,3, const Pose3 pose1(Matrix_(3,3,
1., 0., 0., 1., 0., 0.,
0.,-1., 0., 0.,-1., 0.,

4
gtsam/geometry/tests/testPoint2.cpp
View File

@ -22,6 +22,10 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point2)
GTSAM_CONCEPT_MANIFOLD_INST(Point2)
GTSAM_CONCEPT_LIE_INST(Point2)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Point2, Lie) { TEST(Point2, Lie) {
Point2 p1(1,2); Point2 p1(1,2);

4
gtsam/geometry/tests/testPoint3.cpp
View File

@ -20,6 +20,10 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point3)
GTSAM_CONCEPT_MANIFOLD_INST(Point3)
GTSAM_CONCEPT_LIE_INST(Point3)
Point3 P(0.2,0.7,-2); Point3 P(0.2,0.7,-2);
/* ************************************************************************* */ /* ************************************************************************* */

69
gtsam/geometry/tests/testPose2.cpp
View File

@ -35,8 +35,11 @@ using namespace std;
// #define SLOW_BUT_CORRECT_EXPMAP // #define SLOW_BUT_CORRECT_EXPMAP
// concept checks for testable
GTSAM_CONCEPT_TESTABLE_INST(Pose2) GTSAM_CONCEPT_TESTABLE_INST(Pose2)
GTSAM_CONCEPT_MANIFOLD_INST(Pose2)
GTSAM_CONCEPT_LIE_INST(Pose2)
// concept checks for testable
GTSAM_CONCEPT_TESTABLE_INST(Point2) GTSAM_CONCEPT_TESTABLE_INST(Point2)
GTSAM_CONCEPT_TESTABLE_INST(Rot2) GTSAM_CONCEPT_TESTABLE_INST(Rot2)
GTSAM_CONCEPT_TESTABLE_INST(LieVector) GTSAM_CONCEPT_TESTABLE_INST(LieVector)
@ -56,44 +59,44 @@ TEST(Pose2, manifold) {
Pose2 t1(M_PI_2, Point2(1, 2)); Pose2 t1(M_PI_2, Point2(1, 2));
Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 origin; Pose2 origin;
Vector d12 = t1.logmap(t2); Vector d12 = t1.unretract(t2);
EXPECT(assert_equal(t2, t1.expmap(d12))); EXPECT(assert_equal(t2, t1.retract(d12)));
EXPECT(assert_equal(t2, t1*origin.expmap(d12))); EXPECT(assert_equal(t2, t1*origin.retract(d12)));
Vector d21 = t2.logmap(t1); Vector d21 = t2.unretract(t1);
EXPECT(assert_equal(t1, t2.expmap(d21))); EXPECT(assert_equal(t1, t2.retract(d21)));
EXPECT(assert_equal(t1, t2*origin.expmap(d21))); EXPECT(assert_equal(t1, t2*origin.retract(d21)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap) { TEST(Pose2, retract) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI_2, Point2(1, 2));
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifdef SLOW_BUT_CORRECT_EXPMAP
Pose2 expected(1.00811, 2.01528, 2.5608); Pose2 expected(1.00811, 2.01528, 2.5608);
#else #else
Pose2 expected(M_PI_2+0.99, Point2(1.015, 2.01)); Pose2 expected(M_PI_2+0.99, Point2(1.015, 2.01));
#endif #endif
Pose2 actual = pose.retract(Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
TEST(Pose2, expmap) {
Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = pose.expmap(Vector_(3, 0.01, -0.015, 0.99)); Pose2 actual = pose.expmap(Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
/* ************************************************************************* */
TEST(Pose2, expmap_full) {
Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = pose.expmapFull(Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
TEST(Pose2, expmap_full2) {
Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = expmapFull<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap2) { TEST(Pose2, expmap2) {
Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = pose.expmap(Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
TEST(Pose2, expmap3) {
// do an actual series exponential map // do an actual series exponential map
// see e.g. http://www.cis.upenn.edu/~cis610/cis610lie1.ps // see e.g. http://www.cis.upenn.edu/~cis610/cis610lie1.ps
Matrix A = Matrix_(3,3, Matrix A = Matrix_(3,3,
@ -119,7 +122,7 @@ TEST(Pose2, expmap0) {
#else #else
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
#endif #endif
Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); Pose2 actual = pose * Pose2::Retract(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -127,7 +130,7 @@ TEST(Pose2, expmap0) {
TEST(Pose2, expmap0_full) { TEST(Pose2, expmap0_full) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(1.01491, 2.01013, 1.5888); Pose2 expected(1.01491, 2.01013, 1.5888);
Pose2 actual = pose * Pose2::ExpmapFull(Vector_(3, 0.01, -0.015, 0.018)); Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -135,7 +138,7 @@ TEST(Pose2, expmap0_full) {
TEST(Pose2, expmap0_full2) { TEST(Pose2, expmap0_full2) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(1.01491, 2.01013, 1.5888); Pose2 expected(1.01491, 2.01013, 1.5888);
Pose2 actual = pose * ExpmapFull<Pose2>(Vector_(3, 0.01, -0.015, 0.018)); Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -153,8 +156,8 @@ namespace screw {
TEST(Pose3, expmap_c) TEST(Pose3, expmap_c)
{ {
EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6)); EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6)); EXPECT(assert_equal(screw::expected, Pose2::Retract(screw::xi),1e-6));
EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6)); EXPECT(assert_equal(screw::xi, Pose2::Unretract(screw::expected),1e-6));
} }
#endif #endif
@ -167,8 +170,8 @@ TEST(Pose3, expmap_c_full)
Point2 expectedT(-0.0446635, 0.29552); Point2 expectedT(-0.0446635, 0.29552);
Pose2 expected(expectedR, expectedT); Pose2 expected(expectedR, expectedT);
EXPECT(assert_equal(expected, expm<Pose2>(xi),1e-6)); EXPECT(assert_equal(expected, expm<Pose2>(xi),1e-6));
EXPECT(assert_equal(expected, Pose2::ExpmapFull(xi),1e-6)); EXPECT(assert_equal(expected, Pose2::Expmap(xi),1e-6));
EXPECT(assert_equal(xi, Pose2::LogmapFull(expected),1e-6)); EXPECT(assert_equal(xi, Pose2::Logmap(expected),1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -180,7 +183,7 @@ TEST(Pose2, logmap) {
#else #else
Vector expected = Vector_(3, 0.01, -0.015, 0.018); Vector expected = Vector_(3, 0.01, -0.015, 0.018);
#endif #endif
Vector actual = pose0.logmap(pose); Vector actual = pose0.unretract(pose);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -189,7 +192,7 @@ TEST(Pose2, logmap_full) {
Pose2 pose0(M_PI_2, Point2(1, 2)); Pose2 pose0(M_PI_2, Point2(1, 2));
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
Vector actual = pose0.logmapFull(pose); Vector actual = pose0.logmap(pose);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }

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

@ -24,6 +24,10 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Pose3)
GTSAM_CONCEPT_MANIFOLD_INST(Pose3)
GTSAM_CONCEPT_LIE_INST(Pose3)
static Point3 P(0.2,0.7,-2); static Point3 P(0.2,0.7,-2);
static Rot3 R = Rot3::rodriguez(0.3,0,0); static Rot3 R = Rot3::rodriguez(0.3,0,0);
static Pose3 T(R,Point3(3.5,-8.2,4.2)); static Pose3 T(R,Point3(3.5,-8.2,4.2));
@ -35,9 +39,9 @@ const double tol=1e-5;
TEST( Pose3, equals) TEST( Pose3, equals)
{ {
Pose3 pose2 = T3; Pose3 pose2 = T3;
CHECK(T3.equals(pose2)); EXPECT(T3.equals(pose2));
Pose3 origin; Pose3 origin;
CHECK(!T3.equals(origin)); EXPECT(!T3.equals(origin));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -46,14 +50,14 @@ TEST( Pose3, expmap_a)
Pose3 id; Pose3 id;
Vector v = zero(6); Vector v = zero(6);
v(0) = 0.3; v(0) = 0.3;
CHECK(assert_equal(id.expmap(v), Pose3(R, Point3()))); EXPECT(assert_equal(id.retract(v), Pose3(R, Point3())));
#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
v(3)=0.2;v(4)=0.7;v(5)=-2; v(3)=0.2;v(4)=0.7;v(5)=-2;
#endif #endif
CHECK(assert_equal(Pose3(R, P),id.expmap(v),1e-5)); EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -62,9 +66,9 @@ TEST( Pose3, expmap_a_full)
Pose3 id; Pose3 id;
Vector v = zero(6); Vector v = zero(6);
v(0) = 0.3; v(0) = 0.3;
CHECK(assert_equal(id.expmapFull(v), Pose3(R, Point3()))); EXPECT(assert_equal(id.expmap(v), Pose3(R, Point3())));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
CHECK(assert_equal(Pose3(R, P),id.expmapFull(v),1e-5)); EXPECT(assert_equal(Pose3(R, P),id.expmap(v),1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -73,18 +77,18 @@ TEST( Pose3, expmap_a_full2)
Pose3 id; Pose3 id;
Vector v = zero(6); Vector v = zero(6);
v(0) = 0.3; v(0) = 0.3;
CHECK(assert_equal(expmapFull<Pose3>(id,v), Pose3(R, Point3()))); EXPECT(assert_equal(id.expmap(v), Pose3(R, Point3())));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
CHECK(assert_equal(Pose3(R, P),expmapFull<Pose3>(id,v),1e-5)); EXPECT(assert_equal(Pose3(R, P),id.expmap(v),1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, expmap_b) TEST(Pose3, expmap_b)
{ {
Pose3 p1(Rot3(), Point3(100, 0, 0)); Pose3 p1(Rot3(), Point3(100, 0, 0));
Pose3 p2 = p1.expmap(Vector_(6,0.0, 0.0, 0.1, 0.0, 0.0, 0.0)); Pose3 p2 = p1.retract(Vector_(6,0.0, 0.0, 0.1, 0.0, 0.0, 0.0));
Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0)); Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
CHECK(assert_equal(expected, p2)); EXPECT(assert_equal(expected, p2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -101,25 +105,25 @@ namespace screw {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, expmap_c) TEST(Pose3, expmap_c)
{ {
CHECK(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6)); EXPECT(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
CHECK(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6)); EXPECT(assert_equal(screw::expected, Pose3::Retract(screw::xi),1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
TEST(Pose3, Adjoint) TEST(Pose3, Adjoint)
{ {
Pose3 expected = T * Pose3::Expmap(screw::xi) * inverse(T); Pose3 expected = T * Pose3::Retract(screw::xi) * inverse(T);
Vector xiprime = Adjoint(T, screw::xi); Vector xiprime = Adjoint(T, screw::xi);
CHECK(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); EXPECT(assert_equal(expected, Pose3::Retract(xiprime), 1e-6));
Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * inverse(T2); Pose3 expected2 = T2 * Pose3::Retract(screw::xi) * inverse(T2);
Vector xiprime2 = Adjoint(T2, screw::xi); Vector xiprime2 = Adjoint(T2, screw::xi);
CHECK(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); EXPECT(assert_equal(expected2, Pose3::Retract(xiprime2), 1e-6));
Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * inverse(T3); Pose3 expected3 = T3 * Pose3::Retract(screw::xi) * inverse(T3);
Vector xiprime3 = Adjoint(T3, screw::xi); Vector xiprime3 = Adjoint(T3, screw::xi);
CHECK(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); EXPECT(assert_equal(expected3, Pose3::Retract(xiprime3), 1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -142,28 +146,28 @@ TEST(Pose3, expmaps_galore)
{ {
Vector xi; Pose3 actual; Vector xi; Pose3 actual;
xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
actual = Pose3::Expmap(xi); actual = Pose3::Retract(xi);
CHECK(assert_equal(expm<Pose3>(xi), actual,1e-6)); EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6));
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
CHECK(assert_equal(xi, logmap(actual),1e-6)); EXPECT(assert_equal(xi, logmap(actual),1e-6));
xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6);
for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
Vector txi = xi*theta; Vector txi = xi*theta;
actual = Pose3::Expmap(txi); actual = Pose3::Retract(txi);
CHECK(assert_equal(expm<Pose3>(txi,30), actual,1e-6)); EXPECT(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6));
Vector log = logmap(actual); Vector log = logmap(actual);
CHECK(assert_equal(actual, Pose3::Expmap(log),1e-6)); EXPECT(assert_equal(actual, Pose3::Retract(log),1e-6));
CHECK(assert_equal(txi,log,1e-6)); // not true once wraps EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps
} }
// Works with large v as well, but expm needs 10 iterations! // Works with large v as well, but expm needs 10 iterations!
xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0); xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0);
actual = Pose3::Expmap(xi); actual = Pose3::Retract(xi);
CHECK(assert_equal(expm<Pose3>(xi,10), actual,1e-5)); EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
CHECK(assert_equal(xi, logmap(actual),1e-6)); EXPECT(assert_equal(xi, logmap(actual),1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -173,35 +177,35 @@ TEST(Pose3, Adjoint_compose)
// T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
const Pose3& T1 = T; const Pose3& T1 = T;
Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8);
Pose3 expected = T1 * Pose3::Expmap(x) * T2; Pose3 expected = T1 * Pose3::Retract(x) * T2;
Vector y = Adjoint(inverse(T2), x); Vector y = Adjoint(inverse(T2), x);
Pose3 actual = T1 * T2 * Pose3::Expmap(y); Pose3 actual = T1 * T2 * Pose3::Retract(y);
CHECK(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(expected, actual, 1e-6));
} }
#endif // SLOW_BUT_CORRECT_EXMAP #endif // SLOW_BUT_CORRECT_EXMAP
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, expmap_c_full) TEST(Pose3, expmap_c_full)
{ {
CHECK(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6)); EXPECT(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
CHECK(assert_equal(screw::expected, Pose3::ExpmapFull(screw::xi),1e-6)); EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
TEST(Pose3, Adjoint_full) TEST(Pose3, Adjoint_full)
{ {
Pose3 expected = T * Pose3::ExpmapFull(screw::xi) * T.inverse(); Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse();
Vector xiprime = T.Adjoint(screw::xi); Vector xiprime = T.Adjoint(screw::xi);
CHECK(assert_equal(expected, Pose3::ExpmapFull(xiprime), 1e-6)); EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));
Pose3 expected2 = T2 * Pose3::ExpmapFull(screw::xi) * T2.inverse(); Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse();
Vector xiprime2 = T2.Adjoint(screw::xi); Vector xiprime2 = T2.Adjoint(screw::xi);
CHECK(assert_equal(expected2, Pose3::ExpmapFull(xiprime2), 1e-6)); EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));
Pose3 expected3 = T3 * Pose3::ExpmapFull(screw::xi) * T3.inverse(); Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse();
Vector xiprime3 = T3.Adjoint(screw::xi); Vector xiprime3 = T3.Adjoint(screw::xi);
CHECK(assert_equal(expected3, Pose3::ExpmapFull(xiprime3), 1e-6)); EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -211,11 +215,11 @@ Pose3 Agrawal06iros(const Vector& xi) {
Vector v = xi.tail(3); Vector v = xi.tail(3);
double t = norm_2(w); double t = norm_2(w);
if (t < 1e-5) if (t < 1e-5)
return Pose3(Rot3(), Point3::Expmap(v)); return Pose3(Rot3(), Point3::Retract(v));
else { else {
Matrix W = skewSymmetric(w/t); Matrix W = skewSymmetric(w/t);
Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
return Pose3(Rot3::Expmap (w), Point3::Expmap(A * v)); return Pose3(Rot3::Expmap (w), Point3::Retract(A * v));
} }
} }
@ -224,28 +228,28 @@ TEST(Pose3, expmaps_galore_full)
{ {
Vector xi; Pose3 actual; Vector xi; Pose3 actual;
xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
actual = Pose3::ExpmapFull(xi); actual = Pose3::Expmap(xi);
CHECK(assert_equal(expm<Pose3>(xi), actual,1e-6)); EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6));
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
CHECK(assert_equal(xi, Pose3::LogmapFull(actual),1e-6)); EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6));
xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6);
for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
Vector txi = xi*theta; Vector txi = xi*theta;
actual = Pose3::ExpmapFull(txi); actual = Pose3::Expmap(txi);
CHECK(assert_equal(expm<Pose3>(txi,30), actual,1e-6)); EXPECT(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6));
Vector log = Pose3::LogmapFull(actual); Vector log = Pose3::Logmap(actual);
CHECK(assert_equal(actual, Pose3::ExpmapFull(log),1e-6)); EXPECT(assert_equal(actual, Pose3::Expmap(log),1e-6));
CHECK(assert_equal(txi,log,1e-6)); // not true once wraps EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps
} }
// Works with large v as well, but expm needs 10 iterations! // Works with large v as well, but expm needs 10 iterations!
xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0); xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0);
actual = Pose3::ExpmapFull(xi); actual = Pose3::Expmap(xi);
CHECK(assert_equal(expm<Pose3>(xi,10), actual,1e-5)); EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
CHECK(assert_equal(xi, Pose3::LogmapFull(actual),1e-6)); EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -255,10 +259,10 @@ TEST(Pose3, Adjoint_compose_full)
// T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
const Pose3& T1 = T; const Pose3& T1 = T;
Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8);
Pose3 expected = T1 * Pose3::ExpmapFull(x) * T2; Pose3 expected = T1 * Pose3::Expmap(x) * T2;
Vector y = T2.inverse().Adjoint(x); Vector y = T2.inverse().Adjoint(x);
Pose3 actual = T1 * T2 * Pose3::ExpmapFull(y); Pose3 actual = T1 * T2 * Pose3::Expmap(y);
CHECK(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(expected, actual, 1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -266,16 +270,16 @@ TEST( Pose3, compose )
{ {
Matrix actual = (T2*T2).matrix(); Matrix actual = (T2*T2).matrix();
Matrix expected = T2.matrix()*T2.matrix(); Matrix expected = T2.matrix()*T2.matrix();
CHECK(assert_equal(actual,expected,1e-8)); EXPECT(assert_equal(actual,expected,1e-8));
Matrix actualDcompose1, actualDcompose2; Matrix actualDcompose1, actualDcompose2;
T2.compose(T2, actualDcompose1, actualDcompose2); T2.compose(T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2); Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-3)); EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2); Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2);
CHECK(assert_equal(numericalH2,actualDcompose2)); EXPECT(assert_equal(numericalH2,actualDcompose2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -284,16 +288,16 @@ TEST( Pose3, compose2 )
const Pose3& T1 = T; const Pose3& T1 = T;
Matrix actual = (T1*T2).matrix(); Matrix actual = (T1*T2).matrix();
Matrix expected = T1.matrix()*T2.matrix(); Matrix expected = T1.matrix()*T2.matrix();
CHECK(assert_equal(actual,expected,1e-8)); EXPECT(assert_equal(actual,expected,1e-8));
Matrix actualDcompose1, actualDcompose2; Matrix actualDcompose1, actualDcompose2;
T1.compose(T2, actualDcompose1, actualDcompose2); T1.compose(T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2); Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-3)); EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2); Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2);
CHECK(assert_equal(numericalH2,actualDcompose2)); EXPECT(assert_equal(numericalH2,actualDcompose2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -302,10 +306,10 @@ TEST( Pose3, inverse)
Matrix actualDinverse; Matrix actualDinverse;
Matrix actual = T.inverse(actualDinverse).matrix(); Matrix actual = T.inverse(actualDinverse).matrix();
Matrix expected = inverse(T.matrix()); Matrix expected = inverse(T.matrix());
CHECK(assert_equal(actual,expected,1e-8)); EXPECT(assert_equal(actual,expected,1e-8));
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T); Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
CHECK(assert_equal(numericalH,actualDinverse,5e-3)); EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -318,7 +322,7 @@ TEST( Pose3, inverseDerivatives2)
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T); Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
Matrix actualDinverse; Matrix actualDinverse;
T.inverse(actualDinverse); T.inverse(actualDinverse);
CHECK(assert_equal(numericalH,actualDinverse,5e-3)); EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -326,7 +330,7 @@ TEST( Pose3, compose_inverse)
{ {
Matrix actual = (T*T.inverse()).matrix(); Matrix actual = (T*T.inverse()).matrix();
Matrix expected = eye(4,4); Matrix expected = eye(4,4);
CHECK(assert_equal(actual,expected,1e-8)); EXPECT(assert_equal(actual,expected,1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -336,7 +340,7 @@ TEST( Pose3, Dtransform_from1_a)
Matrix actualDtransform_from1; Matrix actualDtransform_from1;
T.transform_from(P, actualDtransform_from1, boost::none); T.transform_from(P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from_,T,P); Matrix numerical = numericalDerivative21(transform_from_,T,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
TEST( Pose3, Dtransform_from1_b) TEST( Pose3, Dtransform_from1_b)
@ -345,7 +349,7 @@ TEST( Pose3, Dtransform_from1_b)
Matrix actualDtransform_from1; Matrix actualDtransform_from1;
origin.transform_from(P, actualDtransform_from1, boost::none); origin.transform_from(P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from_,origin,P); Matrix numerical = numericalDerivative21(transform_from_,origin,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
TEST( Pose3, Dtransform_from1_c) TEST( Pose3, Dtransform_from1_c)
@ -355,7 +359,7 @@ TEST( Pose3, Dtransform_from1_c)
Matrix actualDtransform_from1; Matrix actualDtransform_from1;
T0.transform_from(P, actualDtransform_from1, boost::none); T0.transform_from(P, actualDtransform_from1, boost::none);
Matrix numerical = numericalDerivative21(transform_from_,T0,P); Matrix numerical = numericalDerivative21(transform_from_,T0,P);
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
TEST( Pose3, Dtransform_from1_d) TEST( Pose3, Dtransform_from1_d)
@ -368,7 +372,7 @@ TEST( Pose3, Dtransform_from1_d)
//print(computed, "Dtransform_from1_d computed:"); //print(computed, "Dtransform_from1_d computed:");
Matrix numerical = numericalDerivative21(transform_from_,T0,P); Matrix numerical = numericalDerivative21(transform_from_,T0,P);
//print(numerical, "Dtransform_from1_d numerical:"); //print(numerical, "Dtransform_from1_d numerical:");
CHECK(assert_equal(numerical,actualDtransform_from1,1e-8)); EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -377,7 +381,7 @@ TEST( Pose3, Dtransform_from2)
Matrix actualDtransform_from2; Matrix actualDtransform_from2;
T.transform_from(P, boost::none, actualDtransform_from2); T.transform_from(P, boost::none, actualDtransform_from2);
Matrix numerical = numericalDerivative22(transform_from_,T,P); Matrix numerical = numericalDerivative22(transform_from_,T,P);
CHECK(assert_equal(numerical,actualDtransform_from2,1e-8)); EXPECT(assert_equal(numerical,actualDtransform_from2,1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -387,7 +391,7 @@ TEST( Pose3, Dtransform_to1)
Matrix computed; Matrix computed;
T.transform_to(P, computed, boost::none); T.transform_to(P, computed, boost::none);
Matrix numerical = numericalDerivative21(transform_to_,T,P); Matrix numerical = numericalDerivative21(transform_to_,T,P);
CHECK(assert_equal(numerical,computed,1e-8)); EXPECT(assert_equal(numerical,computed,1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -396,7 +400,7 @@ TEST( Pose3, Dtransform_to2)
Matrix computed; Matrix computed;
T.transform_to(P, boost::none, computed); T.transform_to(P, boost::none, computed);
Matrix numerical = numericalDerivative22(transform_to_,T,P); Matrix numerical = numericalDerivative22(transform_to_,T,P);
CHECK(assert_equal(numerical,computed,1e-8)); EXPECT(assert_equal(numerical,computed,1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -406,8 +410,8 @@ TEST( Pose3, transform_to_with_derivatives)
T.transform_to(P,actH1,actH2); T.transform_to(P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_to_, T,P), Matrix expH1 = numericalDerivative21(transform_to_, T,P),
expH2 = numericalDerivative22(transform_to_, T,P); expH2 = numericalDerivative22(transform_to_, T,P);
CHECK(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH1, actH1, 1e-8));
CHECK(assert_equal(expH2, actH2, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -417,8 +421,8 @@ TEST( Pose3, transform_from_with_derivatives)
T.transform_from(P,actH1,actH2); T.transform_from(P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_from_, T,P), Matrix expH1 = numericalDerivative21(transform_from_, T,P),
expH2 = numericalDerivative22(transform_from_, T,P); expH2 = numericalDerivative22(transform_from_, T,P);
CHECK(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH1, actH1, 1e-8));
CHECK(assert_equal(expH2, actH2, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -426,7 +430,7 @@ TEST( Pose3, transform_to_translate)
{ {
Point3 actual = Pose3(Rot3(), Point3(1, 2, 3)).transform_to(Point3(10.,20.,30.)); Point3 actual = Pose3(Rot3(), Point3(1, 2, 3)).transform_to(Point3(10.,20.,30.));
Point3 expected(9.,18.,27.); Point3 expected(9.,18.,27.);
CHECK(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -435,7 +439,7 @@ TEST( Pose3, transform_to_rotate)
Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3()); Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3());
Point3 actual = transform.transform_to(Point3(2,1,10)); Point3 actual = transform.transform_to(Point3(2,1,10));
Point3 expected(-1,2,10); Point3 expected(-1,2,10);
CHECK(assert_equal(expected, actual, 0.001)); EXPECT(assert_equal(expected, actual, 0.001));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -444,7 +448,7 @@ TEST( Pose3, transform_to)
Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3(2,4, 0)); Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3(2,4, 0));
Point3 actual = transform.transform_to(Point3(3,2,10)); Point3 actual = transform.transform_to(Point3(3,2,10));
Point3 expected(2,1,10); Point3 expected(2,1,10);
CHECK(assert_equal(expected, actual, 0.001)); EXPECT(assert_equal(expected, actual, 0.001));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -452,7 +456,7 @@ TEST( Pose3, transform_from)
{ {
Point3 actual = T3.transform_from(Point3()); Point3 actual = T3.transform_from(Point3());
Point3 expected = Point3(1.,2.,3.); Point3 expected = Point3(1.,2.,3.);
CHECK(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -460,7 +464,7 @@ TEST( Pose3, transform_roundtrip)
{ {
Point3 actual = T3.transform_from(T3.transform_to(Point3(12., -0.11,7.0))); Point3 actual = T3.transform_from(T3.transform_to(Point3(12., -0.11,7.0)));
Point3 expected(12., -0.11,7.0); Point3 expected(12., -0.11,7.0);
CHECK(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -468,7 +472,7 @@ TEST( Pose3, transformPose_to_origin)
{ {
// transform to origin // transform to origin
Pose3 actual = T3.transform_to(Pose3()); Pose3 actual = T3.transform_to(Pose3());
CHECK(assert_equal(T3, actual, 1e-8)); EXPECT(assert_equal(T3, actual, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -476,7 +480,7 @@ TEST( Pose3, transformPose_to_itself)
{ {
// transform to itself // transform to itself
Pose3 actual = T3.transform_to(T3); Pose3 actual = T3.transform_to(T3);
CHECK(assert_equal(Pose3(), actual, 1e-8)); EXPECT(assert_equal(Pose3(), actual, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -487,7 +491,7 @@ TEST( Pose3, transformPose_to_translation)
Pose3 pose2(r, Point3(21.,32.,13.)); Pose3 pose2(r, Point3(21.,32.,13.));
Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3))); Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3)));
Pose3 expected(r, Point3(20.,30.,10.)); Pose3 expected(r, Point3(20.,30.,10.));
CHECK(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -499,7 +503,7 @@ TEST( Pose3, transformPose_to_simple_rotate)
Pose3 transform(r, Point3(1,2,3)); Pose3 transform(r, Point3(1,2,3));
Pose3 actual = pose2.transform_to(transform); Pose3 actual = pose2.transform_to(transform);
Pose3 expected(Rot3(), Point3(-30.,20.,10.)); Pose3 expected(Rot3(), Point3(-30.,20.,10.));
CHECK(assert_equal(expected, actual, 0.001)); EXPECT(assert_equal(expected, actual, 0.001));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -512,7 +516,7 @@ TEST( Pose3, transformPose_to)
Pose3 transform(r, Point3(1,2,3)); Pose3 transform(r, Point3(1,2,3));
Pose3 actual = pose2.transform_to(transform); Pose3 actual = pose2.transform_to(transform);
Pose3 expected(Rot3::rodriguez(0,0,2.26892803), Point3(-30.,20.,10.)); Pose3 expected(Rot3::rodriguez(0,0,2.26892803), Point3(-30.,20.,10.));
CHECK(assert_equal(expected, actual, 0.001)); EXPECT(assert_equal(expected, actual, 0.001));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -523,16 +527,16 @@ TEST(Pose3, manifold)
Pose3 t2 = T3; Pose3 t2 = T3;
Pose3 origin; Pose3 origin;
Vector d12 = t1.logmap(t2); Vector d12 = t1.logmap(t2);
CHECK(assert_equal(t2, t1.expmap(d12))); EXPECT(assert_equal(t2, t1.expmap(d12)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap // todo: richard - commented out because this tests for "compose-style" (new) expmap
// CHECK(assert_equal(t2, expmap(origin,d12)*t1)); // EXPECT(assert_equal(t2, retract(origin,d12)*t1));
Vector d21 = t2.logmap(t1); Vector d21 = t2.logmap(t1);
CHECK(assert_equal(t1, t2.expmap(d21))); EXPECT(assert_equal(t1, t2.expmap(d21)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap // todo: richard - commented out because this tests for "compose-style" (new) expmap
// CHECK(assert_equal(t1, expmap(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) - this holds even for incorrect expmap :-)
CHECK(assert_equal(d12,-d21)); EXPECT(assert_equal(d12,-d21));
#ifdef CORRECT_POSE3_EXMAP #ifdef CORRECT_POSE3_EXMAP
@ -541,13 +545,13 @@ TEST(Pose3, manifold)
// 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))
CHECK(assert_equal(Pose3::Expmap(-d),inverse(Pose3::Expmap(d)))); EXPECT(assert_equal(Pose3::Retract(-d),inverse(Pose3::Retract(d))));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Pose3 T2 = Pose3::Expmap(2*d); Pose3 T2 = Pose3::Retract(2*d);
Pose3 T3 = Pose3::Expmap(3*d); Pose3 T3 = Pose3::Retract(3*d);
Pose3 T5 = Pose3::Expmap(5*d); Pose3 T5 = Pose3::Retract(5*d);
CHECK(assert_equal(T5,T2*T3)); EXPECT(assert_equal(T5,T2*T3));
CHECK(assert_equal(T5,T3*T2)); EXPECT(assert_equal(T5,T3*T2));
#endif #endif
} }
@ -558,13 +562,13 @@ TEST( Pose3, between )
Pose3 expected = T2.inverse() * T3; Pose3 expected = T2.inverse() * T3;
Matrix actualDBetween1,actualDBetween2; Matrix actualDBetween1,actualDBetween2;
Pose3 actual = T2.between(T3, actualDBetween1,actualDBetween2); Pose3 actual = T2.between(T3, actualDBetween1,actualDBetween2);
CHECK(assert_equal(expected,actual)); EXPECT(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::between<Pose3> , T2, T3); Matrix numericalH1 = numericalDerivative21(testing::between<Pose3> , T2, T3);
CHECK(assert_equal(numericalH1,actualDBetween1,5e-3)); EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::between<Pose3> , T2, T3); Matrix numericalH2 = numericalDerivative22(testing::between<Pose3> , T2, T3);
CHECK(assert_equal(numericalH2,actualDBetween2)); EXPECT(assert_equal(numericalH2,actualDBetween2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -652,9 +656,9 @@ TEST( Pose3, unicycle )
{ {
// velocity in X should be X in inertial frame, rather than global frame // velocity in X should be X in inertial frame, rather than global frame
Vector x_step = delta(6,3,1.0); Vector x_step = delta(6,3,1.0);
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), x1.expmapFull(x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), x1.expmap(x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), x2.expmapFull(x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), x2.expmap(x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), x3.expmapFull(sqrt(2) * x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), x3.expmap(sqrt(2) * x_step), tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
gtsam/geometry/tests/testRot2.cpp
View File

@ -22,6 +22,10 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Rot2)
GTSAM_CONCEPT_MANIFOLD_INST(Rot2)
GTSAM_CONCEPT_LIE_INST(Rot2)
Rot2 R(Rot2::fromAngle(0.1)); Rot2 R(Rot2::fromAngle(0.1));
Point2 P(0.2, 0.7); Point2 P(0.2, 0.7);

4
gtsam/geometry/tests/testRot3.cpp
View File

@ -25,6 +25,10 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Rot3)
GTSAM_CONCEPT_MANIFOLD_INST(Rot3)
GTSAM_CONCEPT_LIE_INST(Rot3)
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
Point3 P(0.2, 0.7, -2.0); Point3 P(0.2, 0.7, -2.0);
double error = 1e-9, epsilon = 0.001; double error = 1e-9, epsilon = 0.001;

3
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -24,6 +24,9 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(StereoCamera)
GTSAM_CONCEPT_MANIFOLD_INST(StereoCamera)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( StereoCamera, operators) TEST( StereoCamera, operators)
{ {

28
gtsam/geometry/tests/testStereoPoint2.cpp Normal file
View File

@ -0,0 +1,28 @@
/**
* @file testStereoPoint2.cpp
*
* @brief Tests for the StereoPoint2 class
*
* @date Nov 4, 2011
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/StereoPoint2.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
GTSAM_CONCEPT_MANIFOLD_INST(StereoPoint2)
GTSAM_CONCEPT_LIE_INST(StereoPoint2)
/* ************************************************************************* */
TEST(testStereoPoint2, test) {
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */