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

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

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

28
gtsam/base/numericalDerivative.h
View File

@ -67,8 +67,8 @@ namespace gtsam {
const size_t n = x.dim();
Vector d = zero(n), g = zero(n);
for (size_t j=0;j<n;j++) {
d(j) += delta; double hxplus = h(x.expmap(d));
d(j) -= 2*delta; double hxmin = h(x.expmap(d));
d(j) += delta; double hxplus = h(x.retract(d));
d(j) -= 2*delta; double hxmin = h(x.retract(d));
d(j) += delta; g(j) = (hxplus-hxmin)*factor;
}
return g;
@ -97,8 +97,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x.expmap(d)));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x.expmap(d)));
d(j) += delta; Vector hxplus = hx.unretract(h(x.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -151,8 +151,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1.expmap(d),x2));
d(j) -= 2*delta; Vector hxmin = 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.unretract(h(x1.retract(d),x2));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -215,8 +215,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1,x2.expmap(d)));
d(j) -= 2*delta; Vector hxmin = 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.unretract(h(x1,x2.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -281,8 +281,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1.expmap(d),x2,x3));
d(j) -= 2*delta; Vector hxmin = 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.unretract(h(x1.retract(d),x2,x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -346,8 +346,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1, x2.expmap(d),x3));
d(j) -= 2*delta; Vector hxmin = 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.unretract(h(x1, x2.retract(d),x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -411,8 +411,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.logmap(h(x1, x2, x3.expmap(d)));
d(j) -= 2*delta; Vector hxmin = 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.unretract(h(x1, x2, x3.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
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 {
/* ************************************************************************* */
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 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); }
/* ************************************************************************* */
Vector Cal3Bundler::k() const { return Vector_(4, k1_, k2_, 0, 0) ; }
/* ************************************************************************* */
Vector Cal3Bundler::vector() const { return Vector_(3, f_, k1_, k2_) ; }
/* ************************************************************************* */
void Cal3Bundler::print(const std::string& s) const { gtsam::print(vector(), s + ".K") ; }
/* ************************************************************************* */
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)
return false;
return true ;
}
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
@ -78,6 +92,7 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p,
return Point2(fg * x, fg * y) ;
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
const double r = xx + yy ;
@ -95,6 +110,7 @@ Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
return DK * DR;
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
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);
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
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 ;
}
/* ************************************************************************* */
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(); }
Cal3Bundler Cal3Bundler::Expmap(const Vector& v) { return Cal3Bundler(v) ; }
Vector Cal3Bundler::Logmap(const Cal3Bundler& p) { return p.vector(); }
/* ************************************************************************* */
Vector Cal3Bundler::unretract(const Cal3Bundler& T2) const { return vector() - T2.vector(); }
/* ************************************************************************* */
Cal3Bundler Cal3Bundler::Retract(const Vector& v) { return Cal3Bundler(v) ; }
/* ************************************************************************* */
Vector Cal3Bundler::Unretract(const Cal3Bundler& p) { return p.vector(); }

8
gtsam/geometry/Cal3Bundler.h
View File

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

31
gtsam/geometry/Cal3DS2.cpp
View File

@ -23,20 +23,30 @@
namespace gtsam {
/* ************************************************************************* */
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) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {}
/* ************************************************************************* */
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]){}
/* ************************************************************************* */
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::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") ; }
/* ************************************************************************* */
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 ||
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 ;
}
/* ************************************************************************* */
Point2 Cal3DS2::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
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_) ;
}
/* ************************************************************************* */
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
//const double fx = fx_, fy = fy_, s = s_ ;
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;
}
/* ************************************************************************* */
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 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) );
}
Cal3DS2 Cal3DS2::expmap(const Vector& d) const { return Cal3DS2(vector() + d) ; }
Vector Cal3DS2::logmap(const Cal3DS2& T2) const { return vector() - T2.vector(); }
Cal3DS2 Cal3DS2::Expmap(const Vector& v) { return Cal3DS2(v) ; }
Vector Cal3DS2::Logmap(const Cal3DS2& p) { return p.vector(); }
/* ************************************************************************* */
Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d) ; }
/* ************************************************************************* */
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(); }
}
/* ************************************************************************* */

8
gtsam/geometry/Cal3DS2.h
View File

@ -62,11 +62,11 @@ namespace gtsam {
Matrix D2d_intrinsic(const Point2& p) const ;
Matrix D2d_calibration(const Point2& p) const ;
Cal3DS2 expmap(const Vector& d) const ;
Vector logmap(const Cal3DS2& T2) const ;
Cal3DS2 retract(const Vector& d) const ;
Vector unretract(const Cal3DS2& T2) const ;
static Cal3DS2 Expmap(const Vector& v) ;
static Vector Logmap(const Cal3DS2& p) ;
static Cal3DS2 Retract(const Vector& v) ;
static Vector Unretract(const Cal3DS2& p) ;
int dim() const { return 9 ; }
static size_t Dim() { return 9; }

17
gtsam/geometry/Cal3_S2.h
View File

@ -129,15 +129,26 @@ namespace gtsam {
}
/// 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));
}
/// logmap for the calibration
Vector logmap(const Cal3_S2& T2) const {
/// Retraction from origin
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();
}
/// Unretraction from origin
inline static Vector Unretract(const Cal3_S2& T2) {
return T2.vector();
}
private:
/// Serialization function

34
gtsam/geometry/CalibratedCamera.cpp
View File

@ -21,16 +21,15 @@
namespace gtsam {
CalibratedCamera::CalibratedCamera() {}
/* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Pose3& pose) :
pose_(pose) {
}
/* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {}
CalibratedCamera::~CalibratedCamera() {}
/* ************************************************************************* */
Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional<Matrix&> H1) {
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
@ -39,10 +38,12 @@ namespace gtsam {
return Point2(P.x() / P.z(), P.y() / P.z());
}
/* ************************************************************************* */
Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) {
return Point3(p.x() * scale, p.y() * scale, scale);
}
/* ************************************************************************* */
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);
@ -52,6 +53,7 @@ namespace gtsam {
return CalibratedCamera(pose3);
}
/* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point,
boost::optional<Matrix&> D_intrinsic_pose,
boost::optional<Matrix&> D_intrinsic_point) const {
@ -77,20 +79,24 @@ namespace gtsam {
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::retract(const Vector& d) const {
return CalibratedCamera(pose().retract(d)) ;
}
CalibratedCamera CalibratedCamera::Expmap(const Vector& v) {
return CalibratedCamera(Pose3::Expmap(v)) ;
/* ************************************************************************* */
Vector CalibratedCamera::unretract(const CalibratedCamera& T2) const {
return pose().unretract(T2.pose()) ;
}
Vector CalibratedCamera::Logmap(const CalibratedCamera& p) {
return Pose3::Logmap(p.pose()) ;
/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Retract(const Vector& v) {
return CalibratedCamera(Pose3::Retract(v)) ;
}
/* ************************************************************************* */
Vector CalibratedCamera::Unretract(const CalibratedCamera& p) {
return Pose3::Unretract(p.pose()) ;
}
/* ************************************************************************* */

18
gtsam/geometry/CalibratedCamera.h
View File

@ -34,10 +34,10 @@ namespace gtsam {
Pose3 pose_; // 6DOF pose
public:
CalibratedCamera(); ///< default constructor
CalibratedCamera() {} ///< default constructor
CalibratedCamera(const Pose3& pose); ///< construct with pose
CalibratedCamera(const Vector &v) ; ///< construct from vector
virtual ~CalibratedCamera(); ///< destructor
virtual ~CalibratedCamera() {} ///< destructor
/// return pose
inline const Pose3& pose() const { return pose_; }
@ -58,16 +58,22 @@ namespace gtsam {
}
/// move a cameras pose according to d
CalibratedCamera expmap(const Vector& d) const;
CalibratedCamera retract(const Vector& d) const;
/// Return canonical coordinate
Vector logmap(const CalibratedCamera& T2) const;
Vector unretract(const CalibratedCamera& T2) const;
/// move a cameras pose according to d
static CalibratedCamera Expmap(const Vector& v);
static CalibratedCamera Retract(const Vector& v);
/// 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
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.
* If calibration is known, it is more computationally efficient
* to calibrate the measurements rather than try to predict in pixels.
* AGC: Is this used or tested anywhere?
* @ingroup geometry
*/
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
sources += StereoPoint2.cpp StereoCamera.cpp
check_PROGRAMS += tests/testStereoCamera
check_PROGRAMS += tests/testStereoCamera tests/testStereoPoint2
# Tensors
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.
* @ingroup geometry
*/
class Point2: public Lie<Point2> {
class Point2 {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 2;
@ -80,6 +80,21 @@ namespace gtsam {
/** 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()); }
// 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 */
inline Point2 between(const Point2& p2,
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 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 */
inline Point3 between(const Point3& p2,
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);
Point2 v(xi(0),xi(1));
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 Point2& t = p.t();
double w = R.theta();
@ -88,25 +88,25 @@ namespace gtsam {
/* ************************************************************************* */
// Changes default to use the full verions of expmap/logmap
/* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& xi) {
return ExpmapFull(xi);
Pose2 Pose2::Retract(const Vector& xi) {
return Expmap(xi);
}
/* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) {
return LogmapFull(p);
Vector Pose2::Unretract(const Pose2& p) {
return Logmap(p);
}
#else
/* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& v) {
Pose2 Pose2::Retract(const Vector& v) {
assert(v.size() == 3);
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());
}

42
gtsam/geometry/Pose2.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* A 2D pose (Point2,Rot2)
* @ingroup geometry
*/
class Pose2: public Lie<Pose2> {
class Pose2 {
public:
static const size_t dimension = 3;
@ -120,26 +120,26 @@ namespace gtsam {
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);
/** non-approximated versions of Expmap/Logmap */
static Pose2 ExpmapFull(const Vector& xi);
static Vector LogmapFull(const Pose2& p);
/** default implementations of binary functions */
inline Pose2 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); }
inline Vector logmap(const Pose2& p2) const { return gtsam::logmap_default(*this, p2);}
inline Pose2 retract(const Vector& v) const { return compose(Retract(v)); }
inline Vector unretract(const Pose2& p2) const { return Unretract(between(p2));}
/** non-approximated versions of expmap/logmap */
inline Pose2 expmapFull(const Vector& v) const { return compose(ExpmapFull(v)); }
inline Vector logmapFull(const Pose2& p2) const { return LogmapFull(between(p2));}
inline Pose2 expmap(const Vector& v) const { return compose(Expmap(v)); }
inline Vector logmap(const Pose2& p2) const { return Logmap(between(p2));}
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
@ -266,23 +266,5 @@ namespace gtsam {
typedef std::pair<Point2,Point2> Point2Pair;
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

40
gtsam/geometry/Pose3.cpp
View File

@ -60,7 +60,7 @@ namespace gtsam {
/* ************************************************************************* */
/** 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
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();
double t = w.norm();
if (t < 1e-10)
@ -98,40 +98,40 @@ namespace gtsam {
/* ************************************************************************* */
// Changes default to use the full verions of expmap/logmap
/* ************************************************************************* */
Pose3 Expmap(const Vector& xi) {
return Pose3::ExpmapFull(xi);
Pose3 Retract(const Vector& xi) {
return Pose3::Expmap(xi);
}
/* ************************************************************************* */
Vector Logmap(const Pose3& p) {
return Pose3::LogmapFull(p);
Vector Unretract(const Pose3& p) {
return Pose3::Logmap(p);
}
/* ************************************************************************* */
Pose3 expmap(const Vector& d) {
return expmapFull(d);
Pose3 retract(const Vector& d) {
return expmap(d);
}
/* ************************************************************************* */
Vector logmap(const Pose3& T1, const Pose3& T2) {
return logmapFull(T2);
Vector unretract(const Pose3& T1, const Pose3& T2) {
return logmap(T2);
}
#else
/* ************************************************************************* */
/* 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 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
// coordinates of a pose.
Vector Pose3::Logmap(const Pose3& p) {
const Vector w = Rot3::Logmap(p.rotation()), u = Point3::Logmap(p.translation());
Vector Pose3::Unretract(const Pose3& p) {
const Vector w = Rot3::Unretract(p.rotation()), u = Point3::Unretract(p.translation());
return concatVectors(2, &w, &u);
}
@ -139,15 +139,15 @@ namespace gtsam {
* pose. Increments the offset and rotation independently given a translation and
* canonical rotation coordinates. Created to match ML derivatives, but
* superseded by the correct exponential map story in .cpp */
Pose3 Pose3::expmap(const Vector& d) const {
return Pose3(R_.expmap(sub(d, 0, 3)),
t_.expmap(sub(d, 3, 6)));
Pose3 Pose3::retract(const Vector& d) const {
return Pose3(R_.retract(sub(d, 0, 3)),
t_.retract(sub(d, 3, 6)));
}
/** Independently computes the logmap of the translation and rotation. */
Vector Pose3::logmap(const Pose3& pp) const {
const Vector r(R_.logmap(pp.rotation())),
t(t_.logmap(pp.translation()));
Vector Pose3::unretract(const Pose3& pp) const {
const Vector r(R_.unretract(pp.rotation())),
t(t_.unretract(pp.translation()));
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)
* @ingroup geometry
*/
class Pose3 : public Lie<Pose3> {
class Pose3 {
public:
static const size_t dimension = 6;
@ -117,25 +117,25 @@ namespace gtsam {
/** Exponential map at identity - create a pose with a translation and
* 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
* coordinates of a pose. */
static Vector Logmap(const Pose3& p);
static Vector Unretract(const Pose3& p);
/** Exponential map around another pose */
Pose3 expmap(const Vector& d) const;
Pose3 retract(const Vector& d) const;
/** Logarithm map around another pose T1 */
Vector logmap(const Pose3& T2) const;
Vector unretract(const Pose3& T2) const;
/** non-approximated versions of Expmap/Logmap */
static Pose3 ExpmapFull(const Vector& xi);
static Vector LogmapFull(const Pose3& p);
static Pose3 Expmap(const Vector& xi);
static Vector Logmap(const Pose3& p);
/** non-approximated versions of expmap/logmap */
inline Pose3 expmapFull(const Vector& v) const { return compose(Pose3::ExpmapFull(v)); }
inline Vector logmapFull(const Pose3& p2) const { return Pose3::LogmapFull(between(p2));}
inline Pose3 expmap(const Vector& v) const { return compose(Pose3::Expmap(v)); }
inline Vector logmap(const Pose3& p2) const { return Pose3::Logmap(between(p2));}
/**
* 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));
}
/**
* 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

17
gtsam/geometry/Rot2.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
* NOTE: the angle theta is in radians unless explicitly stated
* @ingroup geometry
*/
class Rot2: public Lie<Rot2> {
class Rot2 {
public:
/** get the dimension by the type */
@ -158,6 +158,21 @@ namespace gtsam {
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 */
inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {

18
gtsam/geometry/Rot3.h
View File

@ -32,7 +32,7 @@ typedef Eigen::Quaterniond Quaternion;
* @brief 3D Rotations represented as rotation matrices
* @ingroup geometry
*/
class Rot3: public Lie<Rot3> {
class Rot3 {
public:
static const size_t dimension = 3;
@ -204,6 +204,22 @@ typedef Eigen::Quaterniond Quaternion;
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);}
// Manifold requirements
inline Rot3 retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static Rot3 Retract(const Vector& v) { return Expmap(v); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const Rot3& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const Rot3& t) { return Logmap(t); }
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3()
inline Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const {
if (H1) *H1 = -matrix();

18
gtsam/geometry/StereoCamera.h
View File

@ -100,19 +100,33 @@ namespace gtsam {
/** Exponential map around p0 */
inline StereoCamera expmap(const Vector& d) const {
return StereoCamera(pose().expmap(d), K_);
return StereoCamera(pose().retract(d), K_);
}
Vector logmap(const StereoCamera &camera) const {
const Vector v1(leftCamPose_.logmap(camera.leftCamPose_));
const Vector v1(leftCamPose_.unretract(camera.leftCamPose_));
return v1;
}
inline static StereoCamera Expmap(const Vector& d) {
return StereoCamera().expmap(d);
}
inline static Vector Logmap(const StereoCamera &camera) {
return StereoCamera().logmap(camera);
}
bool equals(const StereoCamera &camera, double tol = 1e-9) const {
return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
*camera.K_, tol);
}
// Manifold requirements - use existing expmap/logmap
inline StereoCamera retract(const Vector& v) const { return expmap(v); }
inline static StereoCamera Retract(const Vector& v) { return Expmap(v); }
inline Vector unretract(const StereoCamera& t2) const { return logmap(t2); }
inline static Vector Unretract(const StereoCamera& t) { return Logmap(t); }
Pose3 between(const StereoCamera &camera,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {

17
gtsam/geometry/StereoPoint2.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
* A 2D stereo point, v will be same for rectified images
* @ingroup geometry
*/
class StereoPoint2: Lie<StereoPoint2> {
class StereoPoint2 {
public:
static const size_t dimension = 3;
private:
@ -110,6 +110,21 @@ namespace gtsam {
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 {
return gtsam::between_default(*this, p2);
}

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

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

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

@ -22,6 +22,9 @@
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);
Point2 p(2,3);

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

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

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

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

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

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

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

@ -20,6 +20,10 @@
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);
/* ************************************************************************* */

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

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

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

@ -24,6 +24,10 @@
using namespace std;
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 Rot3 R = Rot3::rodriguez(0.3,0,0);
static Pose3 T(R,Point3(3.5,-8.2,4.2));
@ -35,9 +39,9 @@ const double tol=1e-5;
TEST( Pose3, equals)
{
Pose3 pose2 = T3;
CHECK(T3.equals(pose2));
EXPECT(T3.equals(pose2));
Pose3 origin;
CHECK(!T3.equals(origin));
EXPECT(!T3.equals(origin));
}
/* ************************************************************************* */
@ -46,14 +50,14 @@ TEST( Pose3, expmap_a)
Pose3 id;
Vector v = zero(6);
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
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
#else
v(3)=0.2;v(4)=0.7;v(5)=-2;
#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;
Vector v = zero(6);
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;
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;
Vector v = zero(6);
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;
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)
{
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));
CHECK(assert_equal(expected, p2));
EXPECT(assert_equal(expected, p2));
}
/* ************************************************************************* */
@ -101,25 +105,25 @@ namespace screw {
/* ************************************************************************* */
TEST(Pose3, expmap_c)
{
CHECK(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, expm<Pose3>(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))
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);
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);
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);
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;
xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
actual = Pose3::Expmap(xi);
CHECK(assert_equal(expm<Pose3>(xi), actual,1e-6));
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6));
CHECK(assert_equal(xi, logmap(actual),1e-6));
actual = Pose3::Retract(xi);
EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6));
EXPECT(assert_equal(Agrawal06iros(xi), 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);
for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
Vector txi = xi*theta;
actual = Pose3::Expmap(txi);
CHECK(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6));
actual = Pose3::Retract(txi);
EXPECT(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6));
Vector log = logmap(actual);
CHECK(assert_equal(actual, Pose3::Expmap(log),1e-6));
CHECK(assert_equal(txi,log,1e-6)); // not true once wraps
EXPECT(assert_equal(actual, Pose3::Retract(log),1e-6));
EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps
}
// 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);
actual = Pose3::Expmap(xi);
CHECK(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6));
CHECK(assert_equal(xi, logmap(actual),1e-6));
actual = Pose3::Retract(xi);
EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
EXPECT(assert_equal(Agrawal06iros(xi), 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
const Pose3& T1 = T;
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);
Pose3 actual = T1 * T2 * Pose3::Expmap(y);
CHECK(assert_equal(expected, actual, 1e-6));
Pose3 actual = T1 * T2 * Pose3::Retract(y);
EXPECT(assert_equal(expected, actual, 1e-6));
}
#endif // SLOW_BUT_CORRECT_EXMAP
/* ************************************************************************* */
TEST(Pose3, expmap_c_full)
{
CHECK(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, expm<Pose3>(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))
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);
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);
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);
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);
double t = norm_2(w);
if (t < 1e-5)
return Pose3(Rot3(), Point3::Expmap(v));
return Pose3(Rot3(), Point3::Retract(v));
else {
Matrix W = skewSymmetric(w/t);
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;
xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
actual = Pose3::ExpmapFull(xi);
CHECK(assert_equal(expm<Pose3>(xi), actual,1e-6));
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6));
CHECK(assert_equal(xi, Pose3::LogmapFull(actual),1e-6));
actual = Pose3::Expmap(xi);
EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6));
EXPECT(assert_equal(Agrawal06iros(xi), 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);
for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
Vector txi = xi*theta;
actual = Pose3::ExpmapFull(txi);
CHECK(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6));
Vector log = Pose3::LogmapFull(actual);
CHECK(assert_equal(actual, Pose3::ExpmapFull(log),1e-6));
CHECK(assert_equal(txi,log,1e-6)); // not true once wraps
actual = Pose3::Expmap(txi);
EXPECT(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6));
Vector log = Pose3::Logmap(actual);
EXPECT(assert_equal(actual, Pose3::Expmap(log),1e-6));
EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps
}
// 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);
actual = Pose3::ExpmapFull(xi);
CHECK(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6));
CHECK(assert_equal(xi, Pose3::LogmapFull(actual),1e-6));
actual = Pose3::Expmap(xi);
EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
EXPECT(assert_equal(Agrawal06iros(xi), 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
const Pose3& T1 = T;
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);
Pose3 actual = T1 * T2 * Pose3::ExpmapFull(y);
CHECK(assert_equal(expected, actual, 1e-6));
Pose3 actual = T1 * T2 * Pose3::Expmap(y);
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* ************************************************************************* */
@ -266,16 +270,16 @@ TEST( Pose3, compose )
{
Matrix actual = (T2*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;
T2.compose(T2, actualDcompose1, actualDcompose2);
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);
CHECK(assert_equal(numericalH2,actualDcompose2));
EXPECT(assert_equal(numericalH2,actualDcompose2));
}
/* ************************************************************************* */
@ -284,16 +288,16 @@ TEST( Pose3, compose2 )
const Pose3& T1 = T;
Matrix actual = (T1*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;
T1.compose(T2, actualDcompose1, actualDcompose2);
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);
CHECK(assert_equal(numericalH2,actualDcompose2));
EXPECT(assert_equal(numericalH2,actualDcompose2));
}
/* ************************************************************************* */
@ -302,10 +306,10 @@ TEST( Pose3, inverse)
Matrix actualDinverse;
Matrix actual = T.inverse(actualDinverse).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);
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 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 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;
T.transform_from(P, actualDtransform_from1, boost::none);
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)
@ -345,7 +349,7 @@ TEST( Pose3, Dtransform_from1_b)
Matrix actualDtransform_from1;
origin.transform_from(P, actualDtransform_from1, boost::none);
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)
@ -355,7 +359,7 @@ TEST( Pose3, Dtransform_from1_c)
Matrix actualDtransform_from1;
T0.transform_from(P, actualDtransform_from1, boost::none);
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)
@ -368,7 +372,7 @@ TEST( Pose3, Dtransform_from1_d)
//print(computed, "Dtransform_from1_d computed:");
Matrix numerical = numericalDerivative21(transform_from_,T0,P);
//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;
T.transform_from(P, boost::none, actualDtransform_from2);
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;
T.transform_to(P, computed, boost::none);
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;
T.transform_to(P, boost::none, computed);
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);
Matrix expH1 = numericalDerivative21(transform_to_, T,P),
expH2 = numericalDerivative22(transform_to_, T,P);
CHECK(assert_equal(expH1, actH1, 1e-8));
CHECK(assert_equal(expH2, actH2, 1e-8));
EXPECT(assert_equal(expH1, actH1, 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);
Matrix expH1 = numericalDerivative21(transform_from_, T,P),
expH2 = numericalDerivative22(transform_from_, T,P);
CHECK(assert_equal(expH1, actH1, 1e-8));
CHECK(assert_equal(expH2, actH2, 1e-8));
EXPECT(assert_equal(expH1, actH1, 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 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());
Point3 actual = transform.transform_to(Point3(2,1,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));
Point3 actual = transform.transform_to(Point3(3,2,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 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 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
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
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 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3)));
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 actual = pose2.transform_to(transform);
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 actual = pose2.transform_to(transform);
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 origin;
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
// CHECK(assert_equal(t2, expmap(origin,d12)*t1));
// EXPECT(assert_equal(t2, retract(origin,d12)*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
// 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(assert_equal(d12,-d21));
EXPECT(assert_equal(d12,-d21));
#ifdef CORRECT_POSE3_EXMAP
@ -541,13 +545,13 @@ TEST(Pose3, manifold)
// lines in canonical coordinates correspond to Abelian subgroups in SE(3)
Vector d = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
// exp(-d)=inverse(exp(d))
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)
Pose3 T2 = Pose3::Expmap(2*d);
Pose3 T3 = Pose3::Expmap(3*d);
Pose3 T5 = Pose3::Expmap(5*d);
CHECK(assert_equal(T5,T2*T3));
CHECK(assert_equal(T5,T3*T2));
Pose3 T2 = Pose3::Retract(2*d);
Pose3 T3 = Pose3::Retract(3*d);
Pose3 T5 = Pose3::Retract(5*d);
EXPECT(assert_equal(T5,T2*T3));
EXPECT(assert_equal(T5,T3*T2));
#endif
}
@ -558,13 +562,13 @@ TEST( Pose3, between )
Pose3 expected = T2.inverse() * T3;
Matrix 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);
CHECK(assert_equal(numericalH1,actualDBetween1,5e-3));
EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3));
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
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), Point3(2,1,0)), x2.expmapFull(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(0,0,0), l1), x1.expmap(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.expmap(sqrt(2) * x_step), tol));
}
/* ************************************************************************* */

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

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

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

@ -25,6 +25,10 @@
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);
Point3 P(0.2, 0.7, -2.0);
double error = 1e-9, epsilon = 0.001;

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

@ -24,6 +24,9 @@
using namespace std;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(StereoCamera)
GTSAM_CONCEPT_MANIFOLD_INST(StereoCamera)
/* ************************************************************************* */
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); }
/* ************************************************************************* */