diff --git a/.cproject b/.cproject index 812108f7c..72285e057 100644 --- a/.cproject +++ b/.cproject @@ -375,14 +375,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -409,6 +401,7 @@ make + tests/testBayesTree.run true false @@ -416,6 +409,7 @@ make + testBinaryBayesNet.run true false @@ -463,6 +457,7 @@ make + testSymbolicBayesNet.run true false @@ -470,6 +465,7 @@ make + tests/testSymbolicFactor.run true false @@ -477,6 +473,7 @@ make + testSymbolicFactorGraph.run true false @@ -492,11 +489,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -523,7 +529,6 @@ make - testGraph.run true false @@ -595,7 +600,6 @@ make - testInference.run true false @@ -603,7 +607,6 @@ make - testGaussianFactor.run true false @@ -611,7 +614,6 @@ make - testJunctionTree.run true false @@ -619,7 +621,6 @@ make - testSymbolicBayesNet.run true false @@ -627,7 +628,6 @@ make - testSymbolicFactorGraph.run true false @@ -681,6 +681,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -721,7 +737,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -729,6 +753,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -769,15 +801,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -785,14 +809,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -1107,6 +1123,7 @@ make + testErrors.run true false @@ -1514,7 +1531,6 @@ make - testSimulated2DOriented.run true false @@ -1554,7 +1570,6 @@ make - testSimulated2D.run true false @@ -1562,7 +1577,6 @@ make - testSimulated3D.run true false @@ -1810,7 +1824,6 @@ make - tests/testGaussianISAM2 true false @@ -1832,6 +1845,46 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + make -j2 @@ -1928,62 +1981,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - make -j2 @@ -2024,6 +2021,22 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + diff --git a/gtsam/base/Lie-inl.h b/gtsam/base/Lie-inl.h index 559a01960..2238958ff 100644 --- a/gtsam/base/Lie-inl.h +++ b/gtsam/base/Lie-inl.h @@ -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; + template bool equal(const T&, const T&); + diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 04bc3eb84..57a5b0f50 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -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 tol || fabs(k1_ - K.k1_) > tol || fabs(k2_ - K.k2_) > tol) return false; return true ; } +/* ************************************************************************* */ Point2 Cal3Bundler::uncalibrate(const Point2& p, boost::optional H1, boost::optional 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(); } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 620dabad6..a238d40e2 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -44,18 +44,18 @@ public: bool equals(const Cal3Bundler& K, double tol = 10e-9) const; Point2 uncalibrate(const Point2& p, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const ; + boost::optional H1 = boost::none, + boost::optional 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; } diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 069ec4efb..e5258f73e 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -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 H1, boost::optional 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(); } } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 595247249..a5c84d2d6 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -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 H1 = boost::none, - boost::optional H2 = boost::none) const ; + Point2 uncalibrate(const Point2& p, + boost::optional H1 = boost::none, + boost::optional 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 - 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 + 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_); + } - }; +}; } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index d2235e699..072010f59 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -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 diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 6b528b51a..7e54843ee 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -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 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 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 D_intrinsic_pose, + boost::optional 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 D_intrinsic_pose, - boost::optional 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()) ; +} /* ************************************************************************* */ } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index a7d11d13e..6a596ce26 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -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 ; } diff --git a/gtsam/geometry/CalibratedCameraT.h b/gtsam/geometry/CalibratedCameraT.h index c432364e6..026eeeb87 100644 --- a/gtsam/geometry/CalibratedCameraT.h +++ b/gtsam/geometry/CalibratedCameraT.h @@ -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 diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am index 820922d32..e8c7510f4 100644 --- a/gtsam/geometry/Makefile.am +++ b/gtsam/geometry/Makefile.am @@ -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 diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 0e9acdbf4..79607b276 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -29,7 +29,7 @@ namespace gtsam { * Functional, so no set functions: once created, a point is constant. * @ingroup geometry */ - class Point2: public Lie { + 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 H1=boost::none, diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 095a2d9fe..d5740f2f2 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -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 H1=boost::none, diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 605163d5d..567e0f7f4 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -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()); } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d69e09874..41fa865d9 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -31,7 +31,7 @@ namespace gtsam { * A 2D pose (Point2,Rot2) * @ingroup geometry */ - class Pose2: public Lie { + 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 Point2Pair; boost::optional align(const std::vector& 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(const Vector& xi) { return Pose2::ExpmapFull(xi); } - template<> - inline Vector LogmapFull(const Pose2& p) { return Pose2::LogmapFull(p); } - - /** binary versions */ - template<> - inline Pose2 expmapFull(const Pose2& t, const Vector& v) { return t.expmapFull(v); } - template<> - inline Vector logmapFull(const Pose2& t, const Pose2& p2) { return t.logmapFull(p2); } - } // namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b1b00582d..dc83247a7 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -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); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8b8659585..46c20061b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -27,7 +27,7 @@ namespace gtsam { * A 3D pose (R,t) : (Rot3,Point3) * @ingroup geometry */ - class Pose3 : public Lie { + 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(const Vector& xi) { return Pose3::ExpmapFull(xi); } - template<> - inline Vector LogmapFull(const Pose3& p) { return Pose3::LogmapFull(p); } - - /** binary versions */ - template<> - inline Pose3 expmapFull(const Pose3& t, const Vector& v) { return t.expmapFull(v); } - template<> - inline Vector logmapFull(const Pose3& t, const Pose3& p2) { return t.logmapFull(p2); } - } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index b2ceafca0..e4ac1582b 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -28,7 +28,7 @@ namespace gtsam { * NOTE: the angle theta is in radians unless explicitly stated * @ingroup geometry */ - class Rot2: public Lie { + 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 H1 = boost::none, boost::optional H2 = boost::none) const { diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index a37c58d86..6ffb1f7d6 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -28,245 +28,261 @@ namespace gtsam { typedef Eigen::Quaterniond Quaternion; - /** - * @brief 3D Rotations represented as rotation matrices - * @ingroup geometry - */ - class Rot3: public Lie { - 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 H1=boost::none, boost::optional H2=boost::none) const; + /** Compose two rotations i.e., R= (*this) * R2 + */ + Rot3 compose(const Rot3& R2, + boost::optional H1=boost::none, boost::optional 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 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 H1=boost::none, - boost::optional 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 H1=boost::none, boost::optional 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 H1=boost::none, boost::optional H2=boost::none) const; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - 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 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 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 H1=boost::none, + boost::optional 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 H1=boost::none, boost::optional H2=boost::none) const; + + /** + * rotate point from world to rotated + * frame = R'*p + */ + Point3 unrotate(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + 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 RQ(const Matrix& A); } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index f64c404a6..c00c5f392 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -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 H1 = boost::none, + boost::optional 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 H1, + boost::optional H1_k, + boost::optional 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 H1 = boost::none, - boost::optional 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 H1, - boost::optional H1_k, - boost::optional 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 H1=boost::none, + boost::optional 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 + 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 H1=boost::none, - boost::optional 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 - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(leftCamPose_); - ar & BOOST_SERIALIZATION_NVP(K_); - } - - }; +}; } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index d6ad8f352..b2cb20cda 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -26,7 +26,7 @@ namespace gtsam { * A 2D stereo point, v will be same for rectified images * @ingroup geometry */ - class StereoPoint2: Lie { + 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); } diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index ef00c6d00..bb1bd861d 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -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); diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index 22bd9c8a1..4c79a447b 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -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); diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index c341f7b54..fd82557a3 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -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); diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 080a23a81..4be47a4c0 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -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., diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 2aac1703b..b4610d4fa 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -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); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 5f2aa32b0..4e55c56c4 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -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); /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 104d47e31..1f3cd097f 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -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(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(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(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(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)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index e0f62bfa3..c1ec39c3e 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -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(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(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(screw::xi),1e-6)); - CHECK(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6)); + EXPECT(assert_equal(screw::expected, expm(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(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(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(txi,30), actual,1e-6)); - CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6)); + actual = Pose3::Retract(txi); + EXPECT(assert_equal(expm(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(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(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(screw::xi),1e-6)); - CHECK(assert_equal(screw::expected, Pose3::ExpmapFull(screw::xi),1e-6)); + EXPECT(assert_equal(screw::expected, expm(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(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(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(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(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(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(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, T2, T2); - CHECK(assert_equal(numericalH1,actualDcompose1,5e-3)); + EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); Matrix numericalH2 = numericalDerivative22(testing::compose, 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, T1, T2); - CHECK(assert_equal(numericalH1,actualDcompose1,5e-3)); + EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); Matrix numericalH2 = numericalDerivative22(testing::compose, 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, 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, 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 , T2, T3); - CHECK(assert_equal(numericalH1,actualDBetween1,5e-3)); + EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3)); Matrix numericalH2 = numericalDerivative22(testing::between , 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)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 15a81d0f4..dc02de196 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -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); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 06ca2747f..36a746dde 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -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; diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index ae4248ce5..2446ff122 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -24,6 +24,9 @@ using namespace std; using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(StereoCamera) +GTSAM_CONCEPT_MANIFOLD_INST(StereoCamera) + /* ************************************************************************* */ TEST( StereoCamera, operators) { diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp new file mode 100644 index 000000000..26c484069 --- /dev/null +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -0,0 +1,28 @@ +/** + * @file testStereoPoint2.cpp + * + * @brief Tests for the StereoPoint2 class + * + * @date Nov 4, 2011 + * @author Alex Cunningham + */ + +#include + +#include +#include + +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); } +/* ************************************************************************* */