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

12
gtsam/geometry/Cal3Bundler.h
View File

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

108
gtsam/geometry/Cal3DS2.h
View File

@ -22,72 +22,72 @@
namespace gtsam {
/**
* @brief Calibration of a camera with radial distortion
* @ingroup geometry
*/
class Cal3DS2 {
private:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double k3_, k4_ ; // tagential distortion
/**
* @brief Calibration of a camera with radial distortion
* @ingroup geometry
*/
class Cal3DS2 {
private:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double k3_, k4_ ; // tagential distortion
// K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
// 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) ;
// k3 (r + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
// pi = K*pn
// K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
// 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) ;
// k3 (r + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
// pi = K*pn
public:
// Default Constructor with only unit focal length
Cal3DS2();
public:
// Default Constructor with only unit focal length
Cal3DS2();
// Construction
Cal3DS2(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double k3, double k4) ;
// Construction
Cal3DS2(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double k3, double k4) ;
Cal3DS2(const Vector &v) ;
Cal3DS2(const Vector &v) ;
Matrix K() const ;
Vector k() const ;
Vector vector() const ;
void print(const std::string& s = "") const ;
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
Matrix K() const ;
Vector k() const ;
Vector vector() const ;
void print(const std::string& s = "") const ;
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const ;
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const ;
Matrix D2d_intrinsic(const Point2& p) const ;
Matrix D2d_calibration(const Point2& p) const ;
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; }
int dim() const { return 9 ; }
static size_t Dim() { return 9; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(k3_);
ar & BOOST_SERIALIZATION_NVP(k4_);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(k3_);
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
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

128
gtsam/geometry/CalibratedCamera.cpp
View File

@ -21,77 +21,83 @@
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) {
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());
/* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point,
boost::optional<Matrix&> D_intrinsic_pose,
boost::optional<Matrix&> D_intrinsic_point) const {
const Pose3& pose = pose_;
const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = pose.transform_to(point);
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());
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);
}
/* ************************************************************************* */
Vector CalibratedCamera::unretract(const CalibratedCamera& T2) const {
return pose().unretract(T2.pose()) ;
}
Point2 CalibratedCamera::project(const Point3& point,
boost::optional<Matrix&> D_intrinsic_pose,
boost::optional<Matrix&> D_intrinsic_point) const {
const Pose3& pose = pose_;
const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = pose.transform_to(point);
/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Retract(const Vector& v) {
return CalibratedCamera(Pose3::Retract(v)) ;
}
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);
}
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()) ;
}
/* ************************************************************************* */
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 {

420
gtsam/geometry/Rot3.h
View File

@ -28,245 +28,261 @@ namespace gtsam {
typedef Eigen::Quaterniond Quaternion;
/**
* @brief 3D Rotations represented as rotation matrices
* @ingroup geometry
*/
class Rot3: public Lie<Rot3> {
public:
static const size_t dimension = 3;
/**
* @brief 3D Rotations represented as rotation matrices
* @ingroup geometry
*/
class Rot3 {
public:
static const size_t dimension = 3;
private:
/** we store columns ! */
Point3 r1_, r2_, r3_;
private:
/** we store columns ! */
Point3 r1_, r2_, r3_;
public:
public:
/** default constructor, unit rotation */
Rot3() :
r1_(Point3(1.0,0.0,0.0)),
r2_(Point3(0.0,1.0,0.0)),
r3_(Point3(0.0,0.0,1.0)) {}
/** default constructor, unit rotation */
Rot3() :
r1_(Point3(1.0,0.0,0.0)),
r2_(Point3(0.0,1.0,0.0)),
r3_(Point3(0.0,0.0,1.0)) {}
/** constructor from columns */
Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
r1_(r1), r2_(r2), r3_(r3) {}
/** constructor from columns */
Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
r1_(r1), r2_(r2), r3_(r3) {}
/** constructor from doubles in *row* order !!! */
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
r1_(Point3(R11, R21, R31)),
r2_(Point3(R12, R22, R32)),
r3_(Point3(R13, R23, R33)) {}
/** constructor from doubles in *row* order !!! */
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
r1_(Point3(R11, R21, R31)),
r2_(Point3(R12, R22, R32)),
r3_(Point3(R13, R23, R33)) {}
/** constructor from matrix */
Rot3(const Matrix& R):
r1_(Point3(R(0,0), R(1,0), R(2,0))),
r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/** constructor from matrix */
Rot3(const Matrix& R):
r1_(Point3(R(0,0), R(1,0), R(2,0))),
r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/** Constructor from a quaternion. This can also be called using a plain
* Vector, due to implicit conversion from Vector to Quaternion
* @param q The quaternion
*/
Rot3(const Quaternion& q) {
Eigen::Matrix3d R = q.toRotationMatrix();
r1_ = Point3(R.col(0));
r2_ = Point3(R.col(1));
r3_ = Point3(R.col(2));
}
/** Constructor from a quaternion. This can also be called using a plain
* Vector, due to implicit conversion from Vector to Quaternion
* @param q The quaternion
*/
Rot3(const Quaternion& q) {
Eigen::Matrix3d R = q.toRotationMatrix();
r1_ = Point3(R.col(0));
r2_ = Point3(R.col(1));
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
* Counterclockwise when looking from unchanging axis.
*/
static Rot3 Rx(double t);
static Rot3 Ry(double t);
static Rot3 Rz(double t);
static Rot3 RzRyRx(double x, double y, double z);
inline static Rot3 RzRyRx(const Vector& xyz) {
assert(xyz.size() == 3);
return RzRyRx(xyz(0), xyz(1), xyz(2));
}
/**
* Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix
* Counterclockwise when looking from unchanging axis.
*/
static Rot3 Rx(double t);
static Rot3 Ry(double t);
static Rot3 Rz(double t);
static Rot3 RzRyRx(double x, double y, double z);
inline static Rot3 RzRyRx(const Vector& xyz) {
assert(xyz.size() == 3);
return RzRyRx(xyz(0), xyz(1), xyz(2));
}
/**
* 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
* 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 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 ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
/**
* 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
* 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 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 ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion parameters */
static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); }
/** Create from Quaternion parameters */
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
* @param w is the rotation axis, unit length
* @param theta rotation angle
* @return incremental rotation matrix
*/
static Rot3 rodriguez(const Vector& w, double theta);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param w is the rotation axis, unit length
* @param theta rotation angle
* @return incremental rotation matrix
*/
static Rot3 rodriguez(const Vector& w, double theta);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param v a vector of incremental roll,pitch,yaw
* @return incremental rotation matrix
*/
static Rot3 rodriguez(const Vector& v);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param v a vector of incremental roll,pitch,yaw
* @return incremental rotation matrix
*/
static Rot3 rodriguez(const Vector& v);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param wx
* @param wy
* @param wz
* @return incremental rotation matrix
*/
static inline Rot3 rodriguez(double wx, double wy, double wz)
{ return rodriguez(Vector_(3,wx,wy,wz));}
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param wx
* @param wy
* @param wz
* @return incremental rotation matrix
*/
static inline Rot3 rodriguez(double wx, double wy, double wz)
{ return rodriguez(Vector_(3,wx,wy,wz));}
/** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** equals with an tolerance */
bool equals(const Rot3& p, double tol = 1e-9) const;
/** equals with an tolerance */
bool equals(const Rot3& p, double tol = 1e-9) const;
/** return 3*3 rotation matrix */
Matrix matrix() const;
/** return 3*3 rotation matrix */
Matrix matrix() const;
/** return 3*3 transpose (inverse) rotation matrix */
Matrix transpose() const;
/** return 3*3 transpose (inverse) rotation matrix */
Matrix transpose() const;
/** returns column vector specified by index */
Point3 column(int index) const;
Point3 r1() const { return r1_; }
Point3 r2() const { return r2_; }
Point3 r3() const { return r3_; }
/** returns column vector specified by index */
Point3 column(int index) const;
Point3 r1() const { return r1_; }
Point3 r2() const { return r2_; }
Point3 r3() const { return r3_; }
/**
* Use RQ to calculate xyz angle representation
* @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z)
*/
Vector xyz() const;
/**
* Use RQ to calculate xyz angle representation
* @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z)
*/
Vector xyz() const;
/**
* Use RQ to calculate yaw-pitch-roll angle representation
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
*/
Vector ypr() const;
/**
* Use RQ to calculate yaw-pitch-roll angle representation
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
*/
Vector ypr() const;
/**
* Use RQ to calculate roll-pitch-yaw angle representation
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
*/
Vector rpy() const;
/**
* Use RQ to calculate roll-pitch-yaw angle representation
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
*/
Vector rpy() const;
/** Compute the quaternion representation of this rotation.
* @return The quaternion
*/
Quaternion toQuaternion() const {
return Quaternion((Eigen::Matrix3d() <<
r1_.x(), r2_.x(), r3_.x(),
r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z()).finished());
}
/** Compute the quaternion representation of this rotation.
* @return The quaternion
*/
Quaternion toQuaternion() const {
return Quaternion((Eigen::Matrix3d() <<
r1_.x(), r2_.x(), r3_.x(),
r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z()).finished());
}
/** dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; }
/** dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; }
/** Lie requirements */
/** Lie requirements */
/** return DOF, dimensionality of tangent space */
inline size_t dim() const { return dimension; }
/** return DOF, dimensionality of tangent space */
inline size_t dim() const { return dimension; }
/** Compose two rotations i.e., R= (*this) * R2
*/
Rot3 compose(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** Compose two rotations i.e., R= (*this) * R2
*/
Rot3 compose(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** Exponential map at identity - create a rotation from canonical coordinates
* using Rodriguez' formula
*/
static Rot3 Expmap(const Vector& v) {
if(zero(v)) return Rot3();
else return rodriguez(v);
}
/** Exponential map at identity - create a rotation from canonical coordinates
* using Rodriguez' formula
*/
static Rot3 Expmap(const Vector& v) {
if(zero(v)) return Rot3();
else return rodriguez(v);
}
// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Rot3& R);
// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Rot3& R);
/** default implementations of binary functions */
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);}
/** default implementations of binary functions */
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);}
// 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();
return Rot3(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
// Manifold requirements
/**
* 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;
inline Rot3 retract(const Vector& v) const { return expmap(v); }
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const {
return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
}
/** expmap around identity */
inline static Rot3 Retract(const Vector& v) { return Expmap(v); }
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
inline Point3 operator*(const Point3& p) const { return rotate(p);}
/**
* Returns inverse retraction
*/
inline Vector unretract(const Rot3& t2) const { return logmap(t2); }
/**
* 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;
/** Unretract around identity */
inline static Vector Unretract(const Rot3& t) { return Logmap(t); }
/**
* 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_);
}
};
// 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();
return Rot3(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
/**
* [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);
/**
* 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 */
Rot3 operator*(const Rot3& R2) const {
return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
}
/**
* 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 {
/**
* A stereo camera class, parameterize by left camera pose and stereo calibration
* @ingroup geometry
/**
* A stereo camera class, parameterize by left camera pose and stereo calibration
* @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_;
Cal3_S2Stereo::shared_ptr K_;
/*
* to accomodate tsam's assumption that K is estimated, too
*/
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 {
return K_;
}
/** Dimensionality of the tangent space */
inline size_t dim() const {
return 6;
}
const Pose3& pose() const {
return leftCamPose_;
}
/** Dimensionality of the tangent space */
static inline size_t Dim() {
return 6;
}
double baseline() const {
return K_->baseline();
}
/** Exponential map around p0 */
inline StereoCamera expmap(const Vector& d) const {
return StereoCamera(pose().retract(d), K_);
}
/*
* project 3D point and compute optional derivatives
*/
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Vector logmap(const StereoCamera &camera) const {
const Vector v1(leftCamPose_.unretract(camera.leftCamPose_));
return v1;
}
/*
* to accomodate tsam's assumption that K is estimated, too
*/
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 StereoCamera Expmap(const Vector& d) {
return StereoCamera().expmap(d);
}
/*
* 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);
}
inline static Vector Logmap(const StereoCamera &camera) {
return StereoCamera().logmap(camera);
}
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;
}
bool equals(const StereoCamera &camera, double tol = 1e-9) const {
return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
*camera.K_, tol);
}
/** Dimensionality of the tangent space */
inline size_t dim() const {
return 6;
}
// 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); }
/** Dimensionality of the tangent space */
static inline size_t Dim() {
return 6;
}
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);
}
/** Exponential map around p0 */
inline StereoCamera expmap(const Vector& d) const {
return StereoCamera(pose().expmap(d), K_);
}
void print(const std::string& s = "") const {
leftCamPose_.print(s + ".camera.");
K_->print(s + ".calibration.");
}
Vector logmap(const StereoCamera &camera) const {
const Vector v1(leftCamPose_.logmap(camera.leftCamPose_));
return v1;
}
private:
/** utility functions */
Matrix Dproject_to_stereo_camera1(const Point3& P) const;
bool equals(const StereoCamera &camera, double tol = 1e-9) const {
return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
*camera.K_, tol);
}
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_);
}
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
* @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); }
/* ************************************************************************* */