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(); }

12
gtsam/geometry/Cal3Bundler.h
View File

@ -44,18 +44,18 @@ public:
bool equals(const Cal3Bundler& K, double tol = 10e-9) const; bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
Point2 uncalibrate(const Point2& p, Point2 uncalibrate(const Point2& p,
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 ;
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 ;
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(); }
} }
/* ************************************************************************* */ /* ************************************************************************* */

108
gtsam/geometry/Cal3DS2.h
View File

@ -22,72 +22,72 @@
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
// K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
// r = Pn.x^2 + Pn.y^2 // r = Pn.x^2 + Pn.y^2
// \hat{pn} = (1 + k1*r + k2*r^2 ) pn + [ 2*k3 pn.x pn.y + k4 (r + 2 Pn.x^2) ; // \hat{pn} = (1 + k1*r + k2*r^2 ) pn + [ 2*k3 pn.x pn.y + k4 (r + 2 Pn.x^2) ;
// 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();
// Construction // Construction
Cal3DS2(double fx, double fy, double s, double u0, double v0, Cal3DS2(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double k3, double k4) ; double k1, double k2, double k3, double k4) ;
Cal3DS2(const Vector &v) ; Cal3DS2(const Vector &v) ;
Matrix K() const ; Matrix K() const ;
Vector k() const ; Vector k() const ;
Vector vector() const ; Vector vector() const ;
void print(const std::string& s = "") const ; void print(const std::string& s = "") const ;
bool equals(const Cal3DS2& K, double tol = 10e-9) const; bool equals(const Cal3DS2& K, double tol = 10e-9) const;
Point2 uncalibrate(const Point2& p, Point2 uncalibrate(const Point2& p,
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 ;
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>
void serialize(Archive & ar, const unsigned int version) void serialize(Archive & ar, const unsigned int version)
{ {
ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_); ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_); ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_); ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(k3_); ar & BOOST_SERIALIZATION_NVP(k3_);
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

128
gtsam/geometry/CalibratedCamera.cpp
View File

@ -21,77 +21,83 @@
namespace gtsam { namespace gtsam {
CalibratedCamera::CalibratedCamera() {} /* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Pose3& pose) :
pose_(pose) {
}
CalibratedCamera::CalibratedCamera(const Pose3& pose) : /* ************************************************************************* */
pose_(pose) { CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {}
/* ************************************************************************* */
Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional<Matrix&> H1) {
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
*H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
} }
return Point2(P.x() / P.z(), P.y() / P.z());
}
CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {} /* ************************************************************************* */
Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) {
return Point3(p.x() * scale, p.y() * scale, scale);
}
CalibratedCamera::~CalibratedCamera() {} /* ************************************************************************* */
CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) {
double st = sin(pose2.theta()), ct = cos(pose2.theta());
Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
Rot3 wRc(x, y, z);
Point3 t(pose2.x(), pose2.y(), height);
Pose3 pose3(wRc, t);
return CalibratedCamera(pose3);
}
Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional<Matrix&> H1) { /* ************************************************************************* */
if (H1) { Point2 CalibratedCamera::project(const Point3& point,
double d = 1.0 / P.z(), d2 = d * d; boost::optional<Matrix&> D_intrinsic_pose,
*H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); boost::optional<Matrix&> D_intrinsic_point) const {
} const Pose3& pose = pose_;
return Point2(P.x() / P.z(), P.y() / P.z()); const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = pose.transform_to(point);
if (D_intrinsic_pose || D_intrinsic_point) {
double X = q.x(), Y = q.y(), Z = q.z();
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2;
double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2;
if (D_intrinsic_pose)
*D_intrinsic_pose = Matrix_(2,6,
X*Yd2, -Z*d-X*Xd2, d*Y, -dp11, -dp12, -dp13,
d*Z+Y*Yd2, -X*Yd2, -d*X, -dp21, -dp22, -dp23);
if (D_intrinsic_point)
*D_intrinsic_point = Matrix_(2,3,
dp11, dp12, dp13,
dp21, dp22, dp23);
} }
return project_to_camera(q);
}
Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) { /* ************************************************************************* */
return Point3(p.x() * scale, p.y() * scale, scale); CalibratedCamera CalibratedCamera::retract(const Vector& d) const {
} return CalibratedCamera(pose().retract(d)) ;
}
CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) { /* ************************************************************************* */
double st = sin(pose2.theta()), ct = cos(pose2.theta()); Vector CalibratedCamera::unretract(const CalibratedCamera& T2) const {
Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); return pose().unretract(T2.pose()) ;
Rot3 wRc(x, y, z); }
Point3 t(pose2.x(), pose2.y(), height);
Pose3 pose3(wRc, t);
return CalibratedCamera(pose3);
}
Point2 CalibratedCamera::project(const Point3& point, /* ************************************************************************* */
boost::optional<Matrix&> D_intrinsic_pose, CalibratedCamera CalibratedCamera::Retract(const Vector& v) {
boost::optional<Matrix&> D_intrinsic_point) const { return CalibratedCamera(Pose3::Retract(v)) ;
const Pose3& pose = pose_; }
const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = pose.transform_to(point);
if (D_intrinsic_pose || D_intrinsic_point) { /* ************************************************************************* */
double X = q.x(), Y = q.y(), Z = q.z(); Vector CalibratedCamera::Unretract(const CalibratedCamera& p) {
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2; return Pose3::Unretract(p.pose()) ;
double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2; }
double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2;
if (D_intrinsic_pose)
*D_intrinsic_pose = Matrix_(2,6,
X*Yd2, -Z*d-X*Xd2, d*Y, -dp11, -dp12, -dp13,
d*Z+Y*Yd2, -X*Yd2, -d*X, -dp21, -dp22, -dp23);
if (D_intrinsic_point)
*D_intrinsic_point = Matrix_(2,3,
dp11, dp12, dp13,
dp21, dp22, dp23);
}
return project_to_camera(q);
}
CalibratedCamera CalibratedCamera::expmap(const Vector& d) const {
return CalibratedCamera(pose().expmap(d)) ;
}
Vector CalibratedCamera::logmap(const CalibratedCamera& T2) const {
return pose().logmap(T2.pose()) ;
}
CalibratedCamera CalibratedCamera::Expmap(const Vector& v) {
return CalibratedCamera(Pose3::Expmap(v)) ;
}
Vector CalibratedCamera::Logmap(const CalibratedCamera& p) {
return Pose3::Logmap(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 {

420
gtsam/geometry/Rot3.h
View File

@ -28,245 +28,261 @@ 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() :
r1_(Point3(1.0,0.0,0.0)), r1_(Point3(1.0,0.0,0.0)),
r2_(Point3(0.0,1.0,0.0)), r2_(Point3(0.0,1.0,0.0)),
r3_(Point3(0.0,0.0,1.0)) {} r3_(Point3(0.0,0.0,1.0)) {}
/** constructor from columns */ /** constructor from columns */
Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
r1_(r1), r2_(r2), r3_(r3) {} r1_(r1), r2_(r2), r3_(r3) {}
/** constructor from doubles in *row* order !!! */ /** constructor from doubles in *row* order !!! */
Rot3(double R11, double R12, double R13, Rot3(double R11, double R12, double R13,
double R21, double R22, double R23, double R21, double R22, double R23,
double R31, double R32, double R33) : double R31, double R32, double R33) :
r1_(Point3(R11, R21, R31)), r1_(Point3(R11, R21, R31)),
r2_(Point3(R12, R22, R32)), r2_(Point3(R12, R22, R32)),
r3_(Point3(R13, R23, R33)) {} r3_(Point3(R13, R23, R33)) {}
/** constructor from matrix */ /** constructor from matrix */
Rot3(const Matrix& R): Rot3(const Matrix& R):
r1_(Point3(R(0,0), R(1,0), R(2,0))), r1_(Point3(R(0,0), R(1,0), R(2,0))),
r2_(Point3(R(0,1), R(1,1), R(2,1))), r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {} r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/** Constructor from a quaternion. This can also be called using a plain /** Constructor from a quaternion. This can also be called using a plain
* Vector, due to implicit conversion from Vector to Quaternion * Vector, due to implicit conversion from Vector to Quaternion
* @param q The quaternion * @param q The quaternion
*/ */
Rot3(const Quaternion& q) { Rot3(const Quaternion& q) {
Eigen::Matrix3d R = q.toRotationMatrix(); Eigen::Matrix3d R = q.toRotationMatrix();
r1_ = Point3(R.col(0)); r1_ = Point3(R.col(0));
r2_ = Point3(R.col(1)); r2_ = Point3(R.col(1));
r3_ = Point3(R.col(2)); r3_ = Point3(R.col(2));
} }
/** Static member function to generate some well known rotations */ /** Static member function to generate some well known rotations */
/** /**
* Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix * Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix
* Counterclockwise when looking from unchanging axis. * Counterclockwise when looking from unchanging axis.
*/ */
static Rot3 Rx(double t); static Rot3 Rx(double t);
static Rot3 Ry(double t); static Rot3 Ry(double t);
static Rot3 Rz(double t); static Rot3 Rz(double t);
static Rot3 RzRyRx(double x, double y, double z); static Rot3 RzRyRx(double x, double y, double z);
inline static Rot3 RzRyRx(const Vector& xyz) { inline static Rot3 RzRyRx(const Vector& xyz) {
assert(xyz.size() == 3); assert(xyz.size() == 3);
return RzRyRx(xyz(0), xyz(1), xyz(2)); return RzRyRx(xyz(0), xyz(1), xyz(2));
} }
/** /**
* Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw) * Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw)
* as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf
* Assumes vehicle coordinate frame X forward, Y right, Z down * Assumes vehicle coordinate frame X forward, Y right, Z down
*/ */
static Rot3 yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading) static Rot3 yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading)
static Rot3 pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude) static Rot3 pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude)
static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft) static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft)
static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion parameters */ /** Create from Quaternion parameters */
static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); } static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); }
/** /**
* Rodriguez' formula to compute an incremental rotation matrix * Rodriguez' formula to compute an incremental rotation matrix
* @param w is the rotation axis, unit length * @param w is the rotation axis, unit length
* @param theta rotation angle * @param theta rotation angle
* @return incremental rotation matrix * @return incremental rotation matrix
*/ */
static Rot3 rodriguez(const Vector& w, double theta); static Rot3 rodriguez(const Vector& w, double theta);
/** /**
* Rodriguez' formula to compute an incremental rotation matrix * Rodriguez' formula to compute an incremental rotation matrix
* @param v a vector of incremental roll,pitch,yaw * @param v a vector of incremental roll,pitch,yaw
* @return incremental rotation matrix * @return incremental rotation matrix
*/ */
static Rot3 rodriguez(const Vector& v); static Rot3 rodriguez(const Vector& v);
/** /**
* Rodriguez' formula to compute an incremental rotation matrix * Rodriguez' formula to compute an incremental rotation matrix
* @param wx * @param wx
* @param wy * @param wy
* @param wz * @param wz
* @return incremental rotation matrix * @return incremental rotation matrix
*/ */
static inline Rot3 rodriguez(double wx, double wy, double wz) static inline Rot3 rodriguez(double wx, double wy, double wz)
{ return rodriguez(Vector_(3,wx,wy,wz));} { return rodriguez(Vector_(3,wx,wy,wz));}
/** print */ /** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);} void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** equals with an tolerance */ /** equals with an tolerance */
bool equals(const Rot3& p, double tol = 1e-9) const; bool equals(const Rot3& p, double tol = 1e-9) const;
/** return 3*3 rotation matrix */ /** return 3*3 rotation matrix */
Matrix matrix() const; Matrix matrix() const;
/** return 3*3 transpose (inverse) rotation matrix */ /** return 3*3 transpose (inverse) rotation matrix */
Matrix transpose() const; Matrix transpose() const;
/** returns column vector specified by index */ /** returns column vector specified by index */
Point3 column(int index) const; Point3 column(int index) const;
Point3 r1() const { return r1_; } Point3 r1() const { return r1_; }
Point3 r2() const { return r2_; } Point3 r2() const { return r2_; }
Point3 r3() const { return r3_; } Point3 r3() const { return r3_; }
/** /**
* Use RQ to calculate xyz angle representation * Use RQ to calculate xyz angle representation
* @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z) * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z)
*/ */
Vector xyz() const; Vector xyz() const;
/** /**
* Use RQ to calculate yaw-pitch-roll angle representation * Use RQ to calculate yaw-pitch-roll angle representation
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
*/ */
Vector ypr() const; Vector ypr() const;
/** /**
* Use RQ to calculate roll-pitch-yaw angle representation * Use RQ to calculate roll-pitch-yaw angle representation
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
*/ */
Vector rpy() const; Vector rpy() const;
/** Compute the quaternion representation of this rotation. /** Compute the quaternion representation of this rotation.
* @return The quaternion * @return The quaternion
*/ */
Quaternion toQuaternion() const { Quaternion toQuaternion() const {
return Quaternion((Eigen::Matrix3d() << return Quaternion((Eigen::Matrix3d() <<
r1_.x(), r2_.x(), r3_.x(), r1_.x(), r2_.x(), r3_.x(),
r1_.y(), r2_.y(), r3_.y(), r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z()).finished()); r1_.z(), r2_.z(), r3_.z()).finished());
} }
/** dimension of the variable - used to autodetect sizes */ /** dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; } inline static size_t Dim() { return dimension; }
/** Lie requirements */ /** Lie requirements */
/** return DOF, dimensionality of tangent space */ /** return DOF, dimensionality of tangent space */
inline size_t dim() const { return dimension; } inline size_t dim() const { return dimension; }
/** Compose two rotations i.e., R= (*this) * R2 /** Compose two rotations i.e., R= (*this) * R2
*/ */
Rot3 compose(const Rot3& R2, Rot3 compose(const Rot3& R2,
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;
/** Exponential map at identity - create a rotation from canonical coordinates /** Exponential map at identity - create a rotation from canonical coordinates
* using Rodriguez' formula * using Rodriguez' formula
*/ */
static Rot3 Expmap(const Vector& v) { static Rot3 Expmap(const Vector& v) {
if(zero(v)) return Rot3(); if(zero(v)) return Rot3();
else return rodriguez(v); else return rodriguez(v);
} }
// Log map at identity - return the canonical coordinates of this rotation // Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Rot3& R); static Vector Logmap(const Rot3& R);
/** default implementations of binary functions */ /** default implementations of binary functions */
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);}
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3() // Manifold requirements
inline Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const {
if (H1) *H1 = -matrix();
return Rot3(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
/** inline Rot3 retract(const Vector& v) const { return expmap(v); }
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3 between(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** compose two rotations */ /** expmap around identity */
Rot3 operator*(const Rot3& R2) const { inline static Rot3 Retract(const Vector& v) { return Expmap(v); }
return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
}
/** /**
* rotate point from rotated coordinate frame to * Returns inverse retraction
* world = R*p */
*/ inline Vector unretract(const Rot3& t2) const { return logmap(t2); }
inline Point3 operator*(const Point3& p) const { return rotate(p);}
/** /** Unretract around identity */
* rotate point from rotated coordinate frame to inline static Vector Unretract(const Rot3& t) { return Logmap(t); }
* world = R*p
*/
Point3 rotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
private: // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3()
/** Serialization function */ inline Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const {
friend class boost::serialization::access; if (H1) *H1 = -matrix();
template<class ARCHIVE> return Rot3(
void serialize(ARCHIVE & ar, const unsigned int version) r1_.x(), r1_.y(), r1_.z(),
{ r2_.x(), r2_.y(), r2_.z(),
ar & BOOST_SERIALIZATION_NVP(r1_); r3_.x(), r3_.y(), r3_.z());
ar & BOOST_SERIALIZATION_NVP(r2_); }
ar & BOOST_SERIALIZATION_NVP(r3_);
}
};
/** /**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
* 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 Rot3 between(const Rot3& R2,
* be the identity and Q is a yaw-pitch-roll decomposition of A. boost::optional<Matrix&> H1=boost::none,
* The implementation uses Givens rotations and is based on Hartley-Zisserman. boost::optional<Matrix&> H2=boost::none) const;
* @param a 3 by 3 matrix A=RQ
* @return an upper triangular matrix R /** compose two rotations */
* @return a vector [thetax, thetay, thetaz] in radians. Rot3 operator*(const Rot3& R2) const {
*/ return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
std::pair<Matrix,Vector> RQ(const Matrix& A); }
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
inline Point3 operator*(const Point3& p) const { return rotate(p);}
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point3 rotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(r1_);
ar & BOOST_SERIALIZATION_NVP(r2_);
ar & BOOST_SERIALIZATION_NVP(r3_);
}
};
/**
* [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'
* such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will
* be the identity and Q is a yaw-pitch-roll decomposition of A.
* The implementation uses Givens rotations and is based on Hartley-Zisserman.
* @param a 3 by 3 matrix A=RQ
* @return an upper triangular matrix R
* @return a vector [thetax, thetay, thetaz] in radians.
*/
std::pair<Matrix,Vector> RQ(const Matrix& A);
} }

204
gtsam/geometry/StereoCamera.h
View File

@ -24,117 +24,131 @@
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 {
private:
Pose3 leftCamPose_;
Cal3_S2Stereo::shared_ptr K_;
public:
StereoCamera() {
}
StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
const Cal3_S2Stereo::shared_ptr calibration() const {
return K_;
}
const Pose3& pose() const {
return leftCamPose_;
}
double baseline() const {
return K_->baseline();
}
/*
* project 3D point and compute optional derivatives
*/ */
class StereoCamera { StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
private: /*
Pose3 leftCamPose_; * to accomodate tsam's assumption that K is estimated, too
Cal3_S2Stereo::shared_ptr K_; */
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1_k,
boost::optional<Matrix&> H2) const {
return project(point, H1, H2);
}
public: /*
StereoCamera() { * backproject using left camera calibration, up to scale only
} * i.e. does not rely on baseline
*/
Point3 backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = K_->calibrate(projection);
Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);;
return pose().transform_from(cameraPoint);
}
StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); Point3 backproject(const StereoPoint2& z) const {
Vector measured = z.vector();
double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);
double X = Z *(measured[0]- K_->px()) / K_->fx();
double Y = Z *(measured[2]- K_->py()) / K_->fy();
Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point;
}
const Cal3_S2Stereo::shared_ptr calibration() const { /** Dimensionality of the tangent space */
return K_; inline size_t dim() const {
} return 6;
}
const Pose3& pose() const { /** Dimensionality of the tangent space */
return leftCamPose_; static inline size_t Dim() {
} return 6;
}
double baseline() const { /** Exponential map around p0 */
return K_->baseline(); inline StereoCamera expmap(const Vector& d) const {
} return StereoCamera(pose().retract(d), K_);
}
/* Vector logmap(const StereoCamera &camera) const {
* project 3D point and compute optional derivatives const Vector v1(leftCamPose_.unretract(camera.leftCamPose_));
*/ return v1;
StereoPoint2 project(const Point3& point, }
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
/* inline static StereoCamera Expmap(const Vector& d) {
* to accomodate tsam's assumption that K is estimated, too return StereoCamera().expmap(d);
*/ }
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1_k,
boost::optional<Matrix&> H2) const {
return project(point, H1, H2);
}
/* inline static Vector Logmap(const StereoCamera &camera) {
* backproject using left camera calibration, up to scale only return StereoCamera().logmap(camera);
* i.e. does not rely on baseline }
*/
Point3 backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = K_->calibrate(projection);
Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);;
return pose().transform_from(cameraPoint);
}
Point3 backproject(const StereoPoint2& z) const { bool equals(const StereoCamera &camera, double tol = 1e-9) const {
Vector measured = z.vector(); return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); *camera.K_, tol);
double X = Z *(measured[0]- K_->px()) / K_->fx(); }
double Y = Z *(measured[2]- K_->py()) / K_->fy();
Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point;
}
/** Dimensionality of the tangent space */ // Manifold requirements - use existing expmap/logmap
inline size_t dim() const { inline StereoCamera retract(const Vector& v) const { return expmap(v); }
return 6; 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); }
/** Dimensionality of the tangent space */ Pose3 between(const StereoCamera &camera,
static inline size_t Dim() { boost::optional<Matrix&> H1=boost::none,
return 6; boost::optional<Matrix&> H2=boost::none) const {
} return leftCamPose_.between(camera.pose(), H1, H2);
}
/** Exponential map around p0 */ void print(const std::string& s = "") const {
inline StereoCamera expmap(const Vector& d) const { leftCamPose_.print(s + ".camera.");
return StereoCamera(pose().expmap(d), K_); K_->print(s + ".calibration.");
} }
Vector logmap(const StereoCamera &camera) const { private:
const Vector v1(leftCamPose_.logmap(camera.leftCamPose_)); /** utility functions */
return v1; Matrix Dproject_to_stereo_camera1(const Point3& P) const;
}
bool equals(const StereoCamera &camera, double tol = 1e-9) const { friend class boost::serialization::access;
return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( template<class Archive>
*camera.K_, tol); void serialize(Archive & ar, const unsigned int version) {
} ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
Pose3 between(const StereoCamera &camera, };
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
return leftCamPose_.between(camera.pose(), H1, H2);
}
void print(const std::string& s = "") const {
leftCamPose_.print(s + ".camera.");
K_->print(s + ".calibration.");
}
private:
/** utility functions */
Matrix Dproject_to_stereo_camera1(const Point3& P) const;
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
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); }
/* ************************************************************************* */