From 3394e85ef7b82905cfb4ad6884bef6898009085d Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 17:34:42 -0800 Subject: [PATCH 01/20] Now allows for a flag to compile Point3 as derived from Vector3 --- gtsam/geometry/Point3.cpp | 31 ++++---- gtsam/geometry/Point3.h | 100 +++++++++++++++++++++++-- gtsam/geometry/Pose3.cpp | 4 +- gtsam/geometry/Pose3.h | 6 +- gtsam/geometry/Rot3.h | 37 +++++---- gtsam/geometry/Unit3.cpp | 6 +- gtsam/geometry/Unit3.h | 2 + python/handwritten/geometry/Point3.cpp | 4 + python/handwritten/geometry/Rot3.cpp | 17 +++-- 9 files changed, 159 insertions(+), 48 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 29d59d9e3..7b1a26e2c 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -15,25 +15,25 @@ */ #include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -bool Point3::equals(const Point3 & q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol - && fabs(z_ - q.z()) < tol); +bool Point3::equals(const Point3 &q, double tol) const { + return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && + fabs(z() - q.z()) < tol); } /* ************************************************************************* */ - void Point3::print(const string& s) const { cout << s << *this << endl; } +#ifndef GTSAM_USE_VECTOR3_POINTS /* ************************************************************************* */ - bool Point3::operator==(const Point3& q) const { return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; } @@ -57,18 +57,19 @@ Point3 Point3::operator*(double s) const { Point3 Point3::operator/(double s) const { return Point3(x_ / s, y_ / s, z_ / s); } +#endif /* ************************************************************************* */ double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { double d = (p2 - *this).norm(); if (H1) { - *H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z(); + *H1 << x() - p2.x(), y() - p2.y(), z() - p2.z(); *H1 = *H1 *(1. / d); } if (H2) { - *H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z(); + *H2 << -x() + p2.x(), -y() + p2.y(), -z() + p2.z(); *H2 = *H2 *(1. / d); } return d; @@ -100,8 +101,8 @@ Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobi *H_q << skewSymmetric(vector()); } - return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_, - x_ * q.y_ - y_ * q.x_); + return Point3(y() * q.z() - z() * q.y(), z() * q.x() - x() * q.z(), + x() * q.y() - y() * q.x()); } /* ************************************************************************* */ @@ -113,15 +114,15 @@ double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian *H_q << vector().transpose(); } - return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_); + return (x() * q.x() + y() * q.y() + z() * q.z()); } /* ************************************************************************* */ double Point3::norm(OptionalJacobian<1,3> H) const { - double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_); + double r = std::sqrt(x() * x() + y() * y() + z() * z()); if (H) { if (fabs(r) > 1e-10) - *H << x_ / r, y_ / r, z_ / r; + *H << x() / r, y() / r, z() / r; else *H << 1, 1, 1; // really infinity, why 1 ? } @@ -133,10 +134,10 @@ Point3 Point3::normalize(OptionalJacobian<3,3> H) const { Point3 normalized = *this / norm(); if (H) { // 3*3 Derivative - double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; - double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; + double x2 = x() * x(), y2 = y() * y(), z2 = z() * z(); + double xy = x() * y(), xz = x() * z(), yz = y() * z(); *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; - *H /= pow(x2 + y2 + z2, 1.5); + *H /= std::pow(x2 + y2 + z2, 1.5); } return normalized; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 978d7b963..467703d46 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -22,18 +22,104 @@ #pragma once #include +#include #include #include -#include namespace gtsam { - /** +//#define GTSAM_USE_VECTOR3_POINTS +#ifdef GTSAM_USE_VECTOR3_POINTS +/** + * A 3D point is just a Vector3 with some additional methods + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Point3 : public Vector3 { + + public: + enum { dimension = 3 }; + + /// @name Standard Constructors + /// @{ + + /// Default constructor creates a zero-Point3 + Point3() { setZero(); } + + /// Construct from x, y, and z coordinates + Point3(double x, double y, double z): Vector3(x,y, z) {} + + /// Construct from other vector + template + inline Point3(const Eigen::MatrixBase& v): Vector3(v) {} + + /// @} + /// @name Testable + /// @{ + + /** print with optional string */ + void print(const std::string& s = "") const; + + /** equals with an tolerance */ + bool equals(const Point3& p, double tol = 1e-9) const; + + /// @} + /// @name Group + /// @{ + + /// identity for group operation + inline static Point3 identity() { return Point3();} + + /// @} + /// @name Vector Space + /// @{ + + /** distance between two points */ + double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const; + + /** Distance of the point from the origin, with Jacobian */ + double norm(OptionalJacobian<1,3> H = boost::none) const; + + /** normalize, with optional Jacobian */ + Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; + + /** cross product @return this x q */ + Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // + OptionalJacobian<3, 3> H_q = boost::none) const; + + /** dot product @return this * q*/ + double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, // + OptionalJacobian<1, 3> H_q = boost::none) const; + + /// @} + /// @name Standard Interface + /// @{ + + /** return vectorized form (column-wise)*/ + const Vector3& vector() const { return *this; } + + /// @} + + /// Output stream operator + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3); + } + }; +#else +/** * A 3D point * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Point3 { +class GTSAM_EXPORT Point3 { private: @@ -181,13 +267,15 @@ namespace gtsam { /// @} }; +/// Syntactic sugar for multiplying coordinates by a scalar s*p +inline Point3 operator*(double s, const Point3& p) { return p*s;} + +#endif + // Convenience typedef typedef std::pair Point3Pair; std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); -/// Syntactic sugar for multiplying coordinates by a scalar s*p -inline Point3 operator*(double s, const Point3& p) { return p*s;} - template<> struct traits : public internal::VectorSpace {}; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 9954240fd..44eb5c226 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -297,7 +297,7 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p - t_).vector()); + const Point3 q(Rt*(p.vector() - t_.vector())); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); (*Dpose) << diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index f82e25424..fa4b2e51a 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -65,10 +65,12 @@ public: R_(R), t_(t) { } - /** Construct from R,t, where t \in vector3 */ +#ifndef GTSAM_USE_VECTOR3_POINTS + /** Construct from R,t */ Pose3(const Rot3& R, const Vector3& t) : R_(R), t_(t) { } +#endif /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 264be1537..b26475578 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -93,7 +93,7 @@ namespace gtsam { * See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top */ template - inline Rot3(const Eigen::MatrixBase& R) { + inline explicit Rot3(const Eigen::MatrixBase& R) { #ifdef GTSAM_USE_QUATERNIONS quaternion_=Matrix3(R); #else @@ -105,7 +105,7 @@ namespace gtsam { * Constructor from a rotation matrix * Overload version for Matrix3 to avoid casting in quaternion mode. */ - inline Rot3(const Matrix3& R) { + inline explicit Rot3(const Matrix3& R) { #ifdef GTSAM_USE_QUATERNIONS quaternion_=R; #else @@ -171,6 +171,7 @@ namespace gtsam { return Rot3(q); } +#ifndef GTSAM_USE_VECTOR3_POINTS /** * Convert from axis/angle representation * @param axis is the rotation axis, unit length @@ -181,9 +182,10 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); #else - return SO3::AxisAngle(axis,angle); + return Rot3(SO3::AxisAngle(axis,angle)); #endif } +#endif /** * Convert from axis/angle representation @@ -192,7 +194,11 @@ namespace gtsam { * @return incremental rotation */ static Rot3 AxisAngle(const Point3& axis, double angle) { - return AxisAngle(axis.vector(),angle); +#ifdef GTSAM_USE_QUATERNIONS + return gtsam::Quaternion(Eigen::AngleAxis(angle, axis.vector())); +#else + return Rot3(SO3::AxisAngle(axis.vector(),angle)); +#endif } /** @@ -315,7 +321,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS return traits::Expmap(v); #else - return traits::Expmap(v); + return Rot3(traits::Expmap(v)); #endif } @@ -352,6 +358,14 @@ namespace gtsam { Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, OptionalJacobian<3,3> H2 = boost::none) const; + /// rotate point from rotated coordinate frame to world = R*p + Point3 operator*(const Point3& p) const; + + /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ + Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2=boost::none) const; + +#ifndef GTSAM_USE_VECTOR3_POINTS /// operator* for Vector3 inline Vector3 operator*(const Vector3& v) const { return rotate(Point3(v)).vector(); @@ -362,19 +376,12 @@ namespace gtsam { OptionalJacobian<3, 3> H2 = boost::none) const { return rotate(Point3(v), H1, H2).vector(); } - - /// rotate point from rotated coordinate frame to world = R*p - Point3 operator*(const Point3& p) const; - - /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, - OptionalJacobian<3,3> H2=boost::none) const; - /// unrotate for Vector3 Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const { return unrotate(Point3(v), H1, H2).vector(); } +#endif /// @} /// @name Group Action on Unit3 @@ -539,7 +546,7 @@ namespace gtsam { template<> struct traits : public internal::LieGroup {}; - + template<> struct traits : public internal::LieGroup {}; } diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index aaf0aa953..203a08fb3 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -35,6 +35,7 @@ #include #include #include +#include using namespace std; @@ -252,11 +253,10 @@ Unit3 Unit3::retract(const Vector2& v) const { // Treat case of very small v differently if (theta < std::numeric_limits::epsilon()) { - return Unit3(cos(theta) * p_ + xi_hat); + return Unit3(Vector3(std::cos(theta) * p_ + xi_hat)); } - Vector3 exp_p_xi_hat = - cos(theta) * p_ + xi_hat * (sin(theta) / theta); + Vector3 exp_p_xi_hat = std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 428211c6b..1ce790a1b 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -65,10 +65,12 @@ public: p_(1.0, 0.0, 0.0) { } +#ifndef GTSAM_USE_VECTOR3_POINTS /// Construct from point explicit Unit3(const Point3& p) : p_(p.vector().normalized()) { } +#endif /// Construct from a vector3 explicit Unit3(const Vector3& p) : diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index 5a3ba8fb5..1c4e6f714 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -43,10 +43,14 @@ class_("Point3") .def("norm", &Point3::norm) .def("normalize", &Point3::normalize) .def("print", &Point3::print, print_overloads(args("s"))) +#ifndef GTSAM_USE_VECTOR3_POINTS .def("vector", &Point3::vector) .def("x", &Point3::x) .def("y", &Point3::y) .def("z", &Point3::z) +#else + .def("vector", &Point3::vector, return_value_policy()) +#endif .def(self * other()) .def(other() * self) .def(self + self) diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp index 68643fe2c..f4b868b50 100644 --- a/python/handwritten/geometry/Rot3.cpp +++ b/python/handwritten/geometry/Rot3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -37,8 +37,11 @@ static Rot3 Quaternion_1(double w, double x, double y, double z) // Prototypes used to perform overloading // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html -gtsam::Rot3 (*AxisAngle_0)(const Vector3&, double) = &Rot3::AxisAngle; -gtsam::Rot3 (*AxisAngle_1)(const gtsam::Point3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*AxisAngle_0)(const gtsam::Point3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*AxisAngle_1)(const gtsam::Unit3&, double) = &Rot3::AxisAngle; +#ifndef GTSAM_USE_VECTOR3_POINTS +gtsam::Rot3 (*AxisAngle_2)(const Vector3&, double) = &Rot3::AxisAngle; +#endif gtsam::Rot3 (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues; gtsam::Rot3 (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues; gtsam::Rot3 (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx; @@ -58,8 +61,6 @@ void exportRot3(){ .def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion") .def("Quaternion", Quaternion_1, (arg("w"),arg("x"),arg("y"),arg("z")) ) .staticmethod("Quaternion") - .def("AxisAngle", AxisAngle_0) - .def("AxisAngle", AxisAngle_1) .staticmethod("AxisAngle") .def("Expmap", &Rot3::Expmap) .staticmethod("Expmap") @@ -69,6 +70,12 @@ void exportRot3(){ .staticmethod("Logmap") .def("LogmapDerivative", &Rot3::LogmapDerivative) .staticmethod("LogmapDerivative") +#ifdef GTSAM_USE_VECTOR3_POINTS + .def("AxisAngle", AxisAngle_0) + .def("AxisAngle", AxisAngle_1) +#else + .def("AxisAngle", AxisAngle_2) +#endif .def("Rodrigues", Rodrigues_0) .def("Rodrigues", Rodrigues_1) .staticmethod("Rodrigues") From ae58516e23cdd1d896e38d1196bb33f20e3e6067 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 9 Feb 2016 17:27:50 -0800 Subject: [PATCH 02/20] Fix some more timing scripts --- timing/timeCalibratedCamera.cpp | 9 ++------- timing/timePinholeCamera.cpp | 9 ++------- timing/timeStereoCamera.cpp | 9 ++------- 3 files changed, 6 insertions(+), 21 deletions(-) diff --git a/timing/timeCalibratedCamera.cpp b/timing/timeCalibratedCamera.cpp index 2d0576aea..088ec7b1f 100644 --- a/timing/timeCalibratedCamera.cpp +++ b/timing/timeCalibratedCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -27,12 +27,7 @@ int main() { int n = 100000; - const Pose3 pose1(Matrix3((Matrix(3,3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished()), - Point3(0,0,0.5)); + const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); const CalibratedCamera camera(pose1); const Point3 point1(-0.08,-0.08, 0.0); diff --git a/timing/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp index 6f7e5e972..458f88db1 100644 --- a/timing/timePinholeCamera.cpp +++ b/timing/timePinholeCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -28,12 +28,7 @@ int main() { int n = 1e6; - const Pose3 pose1((Matrix)(Matrix(3,3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); + const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); static Cal3Bundler K(500, 1e-3, 2.0*1e-3); const PinholeCamera camera(pose1,K); diff --git a/timing/timeStereoCamera.cpp b/timing/timeStereoCamera.cpp index ab8e2fff9..c609f8885 100644 --- a/timing/timeStereoCamera.cpp +++ b/timing/timeStereoCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -27,12 +27,7 @@ int main() { int n = 100000; - const Pose3 pose1((Matrix)(Matrix(3,3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); + const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); const StereoCamera camera(pose1, K); From 90e7a9a19440dfecfbd326979953d4c7a977a2f2 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 9 Feb 2016 18:01:47 -0800 Subject: [PATCH 03/20] Made all methods with derivatives available as free functions --- gtsam/geometry/Point3.cpp | 108 +++++++++++++------------ gtsam/geometry/Point3.h | 34 +++++++- gtsam/geometry/Unit3.cpp | 4 +- gtsam/geometry/tests/testPoint3.cpp | 4 +- python/handwritten/geometry/Point3.cpp | 2 +- 5 files changed, 92 insertions(+), 60 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 29d59d9e3..8b44583a6 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -33,45 +33,54 @@ void Point3::print(const string& s) const { } /* ************************************************************************* */ - bool Point3::operator==(const Point3& q) const { return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; } -/* ************************************************************************* */ Point3 Point3::operator+(const Point3& q) const { return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_); } -/* ************************************************************************* */ Point3 Point3::operator-(const Point3& q) const { return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_); } -/* ************************************************************************* */ Point3 Point3::operator*(double s) const { return Point3(x_ * s, y_ * s, z_ * s); } -/* ************************************************************************* */ Point3 Point3::operator/(double s) const { return Point3(x_ / s, y_ / s, z_ / s); } /* ************************************************************************* */ -double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1, +double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { - double d = (p2 - *this).norm(); - if (H1) { - *H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z(); - *H1 = *H1 *(1. / d); - } + return gtsam::distance(*this,q,H1,H2); +} - if (H2) { - *H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z(); - *H2 = *H2 *(1. / d); - } - return d; +double Point3::norm(OptionalJacobian<1,3> H) const { + return gtsam::norm(*this, H); +} + +Point3 Point3::normalized(OptionalJacobian<3,3> H) const { + return gtsam::normalize(*this, H); +} + +Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + return gtsam::cross(*this, q, H1, H2); +} + +double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) const { + return gtsam::dot(*this, q, H1, H2); +} + +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point3& p) { + os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';"; + return os; } /* ************************************************************************* */ @@ -92,59 +101,56 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, #endif /* ************************************************************************* */ -Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const { - if (H_p) { - *H_p << skewSymmetric(-q.vector()); +double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) { + double d = (q - p1).norm(); + if (H1) { + *H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z(); + *H1 = *H1 *(1. / d); } - if (H_q) { - *H_q << skewSymmetric(vector()); + if (H2) { + *H2 << -p1.x() + q.x(), -p1.y() + q.y(), -p1.z() + q.z(); + *H2 = *H2 *(1. / d); } - - return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_, - x_ * q.y_ - y_ * q.x_); + return d; } -/* ************************************************************************* */ -double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const { - if (H_p) { - *H_p << q.vector().transpose(); - } - if (H_q) { - *H_q << vector().transpose(); - } - - return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_); -} - -/* ************************************************************************* */ -double Point3::norm(OptionalJacobian<1,3> H) const { - double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_); +double norm(const Point3 &p, OptionalJacobian<1, 3> H) { + double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z()); if (H) { if (fabs(r) > 1e-10) - *H << x_ / r, y_ / r, z_ / r; + *H << p.x() / r, p.y() / r, p.z() / r; else - *H << 1, 1, 1; // really infinity, why 1 ? + *H << 1, 1, 1; // really infinity, why 1 ? } return r; } -/* ************************************************************************* */ -Point3 Point3::normalize(OptionalJacobian<3,3> H) const { - Point3 normalized = *this / norm(); +Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) { + Point3 normalized = p / norm(p); if (H) { // 3*3 Derivative - double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; - double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; - *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; + double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z(); + double xy = p.x() * p.y(), xz = p.x() * p.z(), yz = p.y() * p.z(); + *H << y2 + z2, -xy, -xz, /**/ -xy, x2 + z2, -yz, /**/ -xz, -yz, x2 + y2; *H /= pow(x2 + y2 + z2, 1.5); } return normalized; } -/* ************************************************************************* */ -ostream &operator<<(ostream &os, const Point3& p) { - os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';"; - return os; +Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) { + if (H1) *H1 << skewSymmetric(-q.x(), -q.y(), -q.z()); + if (H2) *H2 << skewSymmetric(p.x(), p.y(), p.z()); + return Point3(p.y() * q.z() - p.z() * q.y(), p.z() * q.x() - p.x() * q.z(), + p.x() * q.y() - p.y() * q.x()); +} + +double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) { + if (H1) *H1 << q.x(), q.y(), q.z(); + if (H2) *H2 << p.x(), p.y(), p.z(); + return p.x() * q.x() + p.y() * q.y() + p.z() * q.z(); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 978d7b963..90bfb7cea 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -104,14 +104,14 @@ namespace gtsam { Point3 operator / (double s) const; /** distance between two points */ - double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none, + double distance(const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) const; /** Distance of the point from the origin, with Jacobian */ double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ - Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; + Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // @@ -155,7 +155,8 @@ namespace gtsam { Point3 retract(const Vector3& v) const { return compose(Point3(v));} static Vector3 Logmap(const Point3& p) { return p.vector();} static Point3 Expmap(const Vector3& v) { return Point3(v);} - inline double dist(const Point3& p2) const { return (p2 - *this).norm(); } + inline double dist(const Point3& q) const { return (q - *this).norm(); } + Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);} Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, @@ -194,6 +195,31 @@ struct traits : public internal::VectorSpace {}; template<> struct traits : public internal::VectorSpace {}; +/// distance between two points +double distance(const Point3& p1, const Point3& q, + OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none); + +/// Distance of the point from the origin, with Jacobian +double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none); + +/// normalize, with optional Jacobian +Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none); + +/// cross product @return this x q +Point3 cross(const Point3& p, const Point3& q, + OptionalJacobian<3, 3> H_p = boost::none, + OptionalJacobian<3, 3> H_q = boost::none); + +/// dot product +double dot(const Point3& p, const Point3& q, + OptionalJacobian<1, 3> H_p = boost::none, + OptionalJacobian<1, 3> H_q = boost::none); + +// Convenience typedef +typedef std::pair Point3Pair; +std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); + template struct Range; @@ -203,7 +229,7 @@ struct Range { double operator()(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) { - return p.distance(q, H1, H2); + return distance(p, q, H1, H2); } }; diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index aaf0aa953..d80fe2915 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -44,7 +44,7 @@ namespace gtsam { Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { // 3*3 Derivative of representation with respect to point is 3*3: Matrix3 D_p_point; - Point3 normalized = point.normalize(H ? &D_p_point : 0); + Point3 normalized = normalize(point, H ? &D_p_point : 0); Unit3 direction; direction.p_ = normalized.vector(); if (H) @@ -108,7 +108,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { // Normalize result to get a unit vector: b1 = B1 / |B1|. Matrix33 H_b1_B1; - Point3 b1 = B1.normalize(H ? &H_b1_B1 : 0); + Point3 b1 = normalize(B1, H ? &H_b1_B1 : 0); // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. // No need to normalize this, p and b1 are orthogonal unit vectors. diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index d0123af83..37ad105c7 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -96,9 +96,9 @@ TEST (Point3, normalize) { Matrix actualH; Point3 point(1, -2, 3); // arbitrary point Point3 expected(point / sqrt(14.0)); - EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8)); + EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(&Point3::normalize, _1, boost::none), point); + boost::bind(gtsam::normalize, _1, boost::none), point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index 5a3ba8fb5..19f5a4602 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -41,7 +41,7 @@ class_("Point3") .def("dot", &Point3::dot) .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) .def("norm", &Point3::norm) - .def("normalize", &Point3::normalize) + .def("normalized", &Point3::normalized) .def("print", &Point3::print, print_overloads(args("s"))) .def("vector", &Point3::vector) .def("x", &Point3::x) From 0a7fd27f28bc155c198618ea3ebd00067e451b11 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 10 Feb 2016 17:48:52 -0800 Subject: [PATCH 04/20] Working on getting a simple typedef to compile - as well as dealing with Point3() now creating uninitialized memory. --- gtsam/base/serializationTestHelpers.h | 27 +-- gtsam/geometry/EssentialMatrix.cpp | 2 +- gtsam/geometry/OrientedPlane3.cpp | 4 +- gtsam/geometry/Point3.cpp | 27 +-- gtsam/geometry/Point3.h | 173 +++--------------- gtsam/geometry/Pose3.cpp | 52 +++--- gtsam/geometry/Pose3.h | 10 +- gtsam/geometry/Rot3.cpp | 4 +- gtsam/geometry/Rot3.h | 36 +--- gtsam/geometry/Rot3M.cpp | 10 +- gtsam/geometry/Unit3.cpp | 21 +-- gtsam/geometry/tests/testPinholeCamera.cpp | 12 +- gtsam/geometry/tests/testPinholePose.cpp | 13 +- gtsam/geometry/tests/testPoint3.cpp | 18 +- gtsam/geometry/tests/testPose3.cpp | 14 +- gtsam/geometry/tests/testRot3.cpp | 4 +- gtsam/geometry/tests/testSimpleCamera.cpp | 10 +- gtsam/geometry/tests/testStereoCamera.cpp | 2 +- gtsam/geometry/tests/testUnit3.cpp | 2 +- gtsam/geometry/triangulation.h | 2 +- gtsam/navigation/GPSFactor.cpp | 8 +- gtsam/navigation/GPSFactor.h | 5 +- gtsam/navigation/NavState.cpp | 13 +- gtsam/navigation/NavState.h | 7 +- gtsam/navigation/PreintegrationBase.cpp | 4 +- gtsam/navigation/Scenario.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 6 +- gtsam/slam/InitializePose3.cpp | 2 +- gtsam/slam/dataset.cpp | 6 +- gtsam/slam/dataset.h | 3 +- gtsam_unstable/geometry/Event.h | 2 +- gtsam_unstable/geometry/Similarity3.cpp | 4 +- .../geometry/tests/testSimilarity3.cpp | 6 +- 33 files changed, 155 insertions(+), 356 deletions(-) diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index f408427d8..c55e5067b 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file serializationTestHelpers.h - * @brief + * @brief * @author Alex Cunningham * @author Richard Roberts * @date Feb 7, 2012 @@ -30,6 +30,11 @@ const bool verbose = false; namespace gtsam { namespace serializationTestHelpers { +// templated default object creation so we only need to declare one friend (if applicable) +template +T create() { + return T(); +} // Templated round-trip serialization template @@ -44,7 +49,7 @@ void roundtrip(const T& input, T& output) { // This version requires equality operator template bool equality(const T& input = T()) { - T output; + T output = create(); roundtrip(input,output); return input==output; } @@ -52,7 +57,7 @@ bool equality(const T& input = T()) { // This version requires Testable template bool equalsObj(const T& input = T()) { - T output; + T output = create(); roundtrip(input,output); return assert_equal(input, output); } @@ -60,7 +65,7 @@ bool equalsObj(const T& input = T()) { // De-referenced version for pointers, requires equals method template bool equalsDereferenced(const T& input) { - T output; + T output = create(); roundtrip(input,output); return input->equals(*output); } @@ -79,7 +84,7 @@ void roundtripXML(const T& input, T& output) { // This version requires equality operator template bool equalityXML(const T& input = T()) { - T output; + T output = create(); roundtripXML(input,output); return input==output; } @@ -87,7 +92,7 @@ bool equalityXML(const T& input = T()) { // This version requires Testable template bool equalsXML(const T& input = T()) { - T output; + T output = create(); roundtripXML(input,output); return assert_equal(input, output); } @@ -95,7 +100,7 @@ bool equalsXML(const T& input = T()) { // This version is for pointers, requires equals method template bool equalsDereferencedXML(const T& input = T()) { - T output; + T output = create(); roundtripXML(input,output); return input->equals(*output); } @@ -114,7 +119,7 @@ void roundtripBinary(const T& input, T& output) { // This version requires equality operator template bool equalityBinary(const T& input = T()) { - T output; + T output = create(); roundtripBinary(input,output); return input==output; } @@ -122,7 +127,7 @@ bool equalityBinary(const T& input = T()) { // This version requires Testable template bool equalsBinary(const T& input = T()) { - T output; + T output = create(); roundtripBinary(input,output); return assert_equal(input, output); } @@ -130,7 +135,7 @@ bool equalsBinary(const T& input = T()) { // This version is for pointers, requires equals method template bool equalsDereferencedBinary(const T& input = T()) { - T output; + T output = create(); roundtripBinary(input,output); return input->equals(*output); } diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 7b90edc39..699705fa5 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -107,7 +107,7 @@ ostream& operator <<(ostream& os, const EssentialMatrix& E) { Rot3 R = E.rotation(); Unit3 d = E.direction(); os.precision(10); - os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; + os << R.xyz().transpose() << " " << d.point3().transpose() << " "; return os; } diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 95290497d..bb9925e23 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -39,7 +39,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose); Vector3 unit_vec = n_rotated.unitVector(); - double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_; + double pred_d = n_.unitVector().dot(xr.translation()) + d_; if (Hr) { *Hr = zeros(3, 6); @@ -47,7 +47,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hr->block<1, 3>(2, 3) = unit_vec; } if (Hp) { - Vector2 hpp = n_.basis().transpose() * xr.translation().vector(); + Vector2 hpp = n_.basis().transpose() * xr.translation(); *Hp = Z_3x3; Hp->block<2, 2>(0, 0) = D_rotated_pose; Hp->block<1, 2>(2, 0) = hpp; diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 181ff3e97..01b5ed132 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -21,40 +21,16 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ +#ifndef GTSAM_USE_VECTOR3_POINTS bool Point3::equals(const Point3 &q, double tol) const { return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol); } -/* ************************************************************************* */ void Point3::print(const string& s) const { cout << s << *this << endl; } -#ifndef GTSAM_USE_VECTOR3_POINTS -/* ************************************************************************* */ -bool Point3::operator==(const Point3& q) const { - return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; -} - -Point3 Point3::operator+(const Point3& q) const { - return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_); -} - -Point3 Point3::operator-(const Point3& q) const { - return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_); -} - -Point3 Point3::operator*(double s) const { - return Point3(x_ * s, y_ * s, z_ * s); -} - -Point3 Point3::operator/(double s) const { - return Point3(x_ / s, y_ / s, z_ / s); -} -#endif - /* ************************************************************************* */ double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { @@ -102,6 +78,7 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, } #endif +#endif /* ************************************************************************* */ double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 75b8a7829..ba06e1d85 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -30,6 +30,12 @@ namespace gtsam { //#define GTSAM_USE_VECTOR3_POINTS #ifdef GTSAM_USE_VECTOR3_POINTS + + // As of GTSAM4, we just typedef Point3 to Vector3 + typedef Vector3 Point3; + +#else + /** * A 3D point is just a Vector3 with some additional methods * @addtogroup geometry @@ -38,13 +44,17 @@ namespace gtsam { class GTSAM_EXPORT Point3 : public Vector3 { public: + enum { dimension = 3 }; /// @name Standard Constructors /// @{ - /// Default constructor creates a zero-Point3 - Point3() { setZero(); } +#ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// Default constructor now creates *uninitialized* point !!!! + Point3() {} +#endif + /// Construct from x, y, and z coordinates Point3(double x, double y, double z): Vector3(x,y, z) {} @@ -68,7 +78,7 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @{ /// identity for group operation - inline static Point3 identity() { return Point3();} + inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); } /// @} /// @name Vector Space @@ -82,7 +92,7 @@ class GTSAM_EXPORT Point3 : public Vector3 { double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ - Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; + Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // @@ -104,136 +114,10 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3); - } - }; -#else -/** - * A 3D point - * @addtogroup geometry - * \nosubgrouping - */ -class GTSAM_EXPORT Point3 { - - private: - - double x_, y_, z_; - - public: - enum { dimension = 3 }; - - /// @name Standard Constructors - /// @{ - - /// Default constructor creates a zero-Point3 - Point3(): x_(0), y_(0), z_(0) {} - - /// Construct from x, y, and z coordinates - Point3(double x, double y, double z): x_(x), y_(y), z_(z) {} - - /// @} - /// @name Advanced Constructors - /// @{ - - /// Construct from 3-element vector - explicit Point3(const Vector3& v) { - x_ = v(0); - y_ = v(1); - z_ = v(2); - } - - /// @} - /// @name Testable - /// @{ - - /** print with optional string */ - void print(const std::string& s = "") const; - - /** equals with an tolerance */ - bool equals(const Point3& p, double tol = 1e-9) const; - - /// @} - /// @name Group - /// @{ - - /// identity for group operation - inline static Point3 identity() { return Point3();} - - /// inverse - Point3 operator - () const { return Point3(-x_,-y_,-z_);} - - /// add vector on right - inline Point3 operator +(const Vector3& v) const { - return Point3(x_ + v[0], y_ + v[1], z_ + v[2]); - } - - /// add - Point3 operator + (const Point3& q) const; - - /// subtract - Point3 operator - (const Point3& q) const; - - /// @} - /// @name Vector Space - /// @{ - - ///multiply with a scalar - Point3 operator * (double s) const; - - ///divide by a scalar - Point3 operator / (double s) const; - - /** distance between two points */ - double distance(const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) const; - - /** Distance of the point from the origin, with Jacobian */ - double norm(OptionalJacobian<1,3> H = boost::none) const; - - /** normalize, with optional Jacobian */ - Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const; - - /** cross product @return this x q */ - Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // - OptionalJacobian<3, 3> H_q = boost::none) const; - - /** dot product @return this * q*/ - double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, // - OptionalJacobian<1, 3> H_q = boost::none) const; - - /// @} - /// @name Standard Interface - /// @{ - - /// equality - bool operator ==(const Point3& q) const; - - /** return vectorized form (column-wise)*/ - Vector3 vector() const { return Vector3(x_,y_,z_); } - - /// get x - inline double x() const {return x_;} - - /// get y - inline double y() const {return y_;} - - /// get z - inline double z() const {return z_;} - - /// @} - - /// Output stream operator - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ + Point3() { setZero(); } Point3 inverse() const { return -(*this);} Point3 compose(const Point3& q) const { return (*this)+q;} Point3 between(const Point3& q) const { return q-(*this);} @@ -252,37 +136,26 @@ class GTSAM_EXPORT Point3 { private: - /// @name Advanced Interface - /// @{ - /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) - { - ar & BOOST_SERIALIZATION_NVP(x_); - ar & BOOST_SERIALIZATION_NVP(y_); - ar & BOOST_SERIALIZATION_NVP(z_); + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3); } - - /// @} }; -/// Syntactic sugar for multiplying coordinates by a scalar s*p -inline Point3 operator*(double s, const Point3& p) { return p*s;} - -#endif - -// Convenience typedef -typedef std::pair Point3Pair; -std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); - template<> struct traits : public internal::VectorSpace {}; template<> struct traits : public internal::VectorSpace {}; +#endif + +// Convenience typedef +typedef std::pair Point3Pair; +std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); + /// distance between two points double distance(const Point3& p1, const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 44eb5c226..ccf39f153 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -48,8 +48,7 @@ Pose3 Pose3::inverse() const { // Experimental - unit tests of derivatives based on it do not check out yet Matrix6 Pose3::AdjointMap() const { const Matrix3 R = R_.matrix(); - const Vector3 t = t_.vector(); - Matrix3 A = skewSymmetric(t) * R; + Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; Matrix6 adj; adj << R, Z_3x3, A, R; return adj; @@ -101,12 +100,12 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, void Pose3::print(const string& s) const { cout << s; R_.print("R:\n"); - t_.print("t: "); + traits::Print(t_, "t: "); } /* ************************************************************************* */ bool Pose3::equals(const Pose3& pose, double tol) const { - return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol); + return R_.equals(pose.R_, tol) && traits::Equals(t_, pose.t_, tol); } /* ************************************************************************* */ @@ -115,14 +114,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { if (H) *H = ExpmapDerivative(xi); // get angular velocity omega and translational velocity v from twist xi - Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); + Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); - Rot3 R = Rot3::Expmap(omega.vector()); + Rot3 R = Rot3::Expmap(omega); double theta2 = omega.dot(omega); if (theta2 > std::numeric_limits::epsilon()) { - Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis - Point3 omega_cross_v = omega.cross(v); // points towards axis - Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; + Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis + Vector3 omega_cross_v = omega.cross(v); // points towards axis + Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; return Pose3(R, t); } else { return Pose3(R, v); @@ -132,19 +131,20 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { /* ************************************************************************* */ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { if (H) *H = LogmapDerivative(p); - Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); - double t = w.norm(); + const Vector3 w = Rot3::Logmap(p.rotation()); + const Vector3 T = p.translation(); + const double t = w.norm(); if (t < 1e-10) { Vector6 log; log << w, T; return log; } else { - Matrix3 W = skewSymmetric(w / t); + const Matrix3 W = skewSymmetric(w / t); // Formula from Agrawal06iros, equation (14) // simplified with Mathematica, and multiplying in T to avoid matrix math - double Tan = tan(0.5 * t); - Vector3 WT = W * T; - Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); + const double Tan = tan(0.5 * t); + const Vector3 WT = W * T; + const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); Vector6 log; log << w, u; return log; @@ -178,7 +178,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { H->topLeftCorner<3,3>() = DR; } Vector6 xi; - xi << omega, T.translation().vector(); + xi << omega, T.translation(); return xi; #endif } @@ -264,7 +264,7 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { Matrix4 Pose3::matrix() const { static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished(); Matrix4 mat; - mat << R_.matrix(), t_.vector(), A14; + mat << R_.matrix(), t_, A14; return mat; } @@ -288,7 +288,7 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, if (Dpoint) { *Dpoint = R; } - return Point3(R * p.vector()) + t_; + return R_ * p + t_; } /* ************************************************************************* */ @@ -297,7 +297,7 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p.vector() - t_.vector())); + const Point3 q(Rt*(p - t_)); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); (*Dpose) << @@ -321,7 +321,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, return local.norm(); } else { Matrix13 D_r_local; - const double r = local.norm(D_r_local); + const double r = norm(local, D_r_local); if (H1) *H1 = D_r_local * D_local_pose; if (H2) *H2 = D_r_local * D_local_point; return r; @@ -361,10 +361,10 @@ boost::optional align(const vector& pairs) { return boost::none; // we need at least three pairs // calculate centroids - Vector3 cp = Vector3::Zero(), cq = Vector3::Zero(); + Point3 cp(0,0,0), cq(0,0,0); BOOST_FOREACH(const Point3Pair& pair, pairs) { - cp += pair.first.vector(); - cq += pair.second.vector(); + cp += pair.first; + cq += pair.second; } double f = 1.0 / n; cp *= f; @@ -373,10 +373,10 @@ boost::optional align(const vector& pairs) { // Add to form H matrix Matrix3 H = Z_3x3; BOOST_FOREACH(const Point3Pair& pair, pairs) { - Vector3 dp = pair.first.vector() - cp; - Vector3 dq = pair.second.vector() - cq; + Point3 dp = pair.first - cp; + Point3 dq = pair.second - cq; H += dp * dq.transpose(); - } + } // Compute SVD Matrix U, V; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index fa4b2e51a..ba4b9737b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -52,8 +52,7 @@ public: /// @{ /** Default constructor is origin */ - Pose3() { - } + Pose3() : R_(traits::Identity()), t_(traits::Identity()) {} /** Copy constructor */ Pose3(const Pose3& pose) : @@ -65,13 +64,6 @@ public: R_(R), t_(t) { } -#ifndef GTSAM_USE_VECTOR3_POINTS - /** Construct from R,t */ - Pose3(const Rot3& R, const Vector3& t) : - R_(R), t_(t) { - } -#endif - /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index ef384c3ef..696c9df68 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -82,7 +82,7 @@ Unit3 Rot3::operator*(const Unit3& p) const { Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { const Matrix3& Rt = transpose(); - Point3 q(Rt * p.vector()); // q = Rt*p + Point3 q(Rt * p); // q = Rt*p const double wx = q.x(), wy = q.y(), wz = q.z(); if (H1) *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index b26475578..30385bd8c 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -171,22 +171,6 @@ namespace gtsam { return Rot3(q); } -#ifndef GTSAM_USE_VECTOR3_POINTS - /** - * Convert from axis/angle representation - * @param axis is the rotation axis, unit length - * @param angle rotation angle - * @return incremental rotation - */ - static Rot3 AxisAngle(const Vector3& axis, double angle) { -#ifdef GTSAM_USE_QUATERNIONS - return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); -#else - return Rot3(SO3::AxisAngle(axis,angle)); -#endif - } -#endif - /** * Convert from axis/angle representation * @param axisw is the rotation axis, unit length @@ -197,7 +181,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS return gtsam::Quaternion(Eigen::AngleAxis(angle, axis.vector())); #else - return Rot3(SO3::AxisAngle(axis.vector(),angle)); + return Rot3(SO3::AxisAngle(axis,angle)); #endif } @@ -365,24 +349,6 @@ namespace gtsam { Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, OptionalJacobian<3,3> H2=boost::none) const; -#ifndef GTSAM_USE_VECTOR3_POINTS - /// operator* for Vector3 - inline Vector3 operator*(const Vector3& v) const { - return rotate(Point3(v)).vector(); - } - - /// rotate for Vector3 - Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { - return rotate(Point3(v), H1, H2).vector(); - } - /// unrotate for Vector3 - Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { - return unrotate(Point3(v), H1, H2).vector(); - } -#endif - /// @} /// @name Group Action on Unit3 /// @{ diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index c3636000b..03cde1a01 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -36,9 +36,9 @@ Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { - rot_.col(0) = col1.vector(); - rot_.col(1) = col2.vector(); - rot_.col(2) = col3.vector(); + rot_.col(0) = (Vector3)col1; + rot_.col(1) = (Vector3)col2; + rot_.col(2) = (Vector3)col3; } /* ************************************************************************* */ @@ -121,7 +121,7 @@ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = rot_; - return Point3(rot_ * p.vector()); + return rot_ * p; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 69d4276dd..a64f99eb1 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -45,9 +45,8 @@ namespace gtsam { Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { // 3*3 Derivative of representation with respect to point is 3*3: Matrix3 D_p_point; - Point3 normalized = normalize(point, H ? &D_p_point : 0); Unit3 direction; - direction.p_ = normalized.vector(); + direction.p_ = normalize(point, H ? &D_p_point : 0); if (H) *H << direction.basis().transpose() * D_p_point; return direction; @@ -90,22 +89,18 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { const Point3 n(p_); // Get the axis of rotation with the minimum projected length of the point - Point3 axis; + Point3 axis(0, 0, 1); double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); if ((mx <= my) && (mx <= mz)) { axis = Point3(1.0, 0.0, 0.0); } else if ((my <= mx) && (my <= mz)) { axis = Point3(0.0, 1.0, 0.0); - } else if ((mz <= mx) && (mz <= my)) { - axis = Point3(0.0, 0.0, 1.0); - } else { - assert(false); } // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with // the chosen axis. Matrix33 H_B1_n; - Point3 B1 = n.cross(axis, H ? &H_B1_n : 0); + Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : 0); // Normalize result to get a unit vector: b1 = B1 / |B1|. Matrix33 H_b1_B1; @@ -114,7 +109,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. // No need to normalize this, p and b1 are orthogonal unit vectors. Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = n.cross(b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); + Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); // Create the basis by stacking b1 and b2. B_.reset(Matrix32()); @@ -176,7 +171,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1, // Compute the dot product of the Point3s. Matrix13 H_dot_pn, H_dot_qn; - double d = pn.dot(qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); + double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); if (H_p) { (*H_p) << H_dot_pn * H_pn_p; @@ -209,7 +204,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ // 2D error here is projecting q into the tangent plane of this (p). Matrix62 H_B_p; Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose(); - Vector2 xi = Bt * qn.vector(); + Vector2 xi = Bt * qn; if (H_p) { // Derivatives of each basis vector. @@ -217,8 +212,8 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0); // Derivatives of the two entries of xi wrt the basis vectors. - Matrix13 H_xi1_b1 = qn.vector().transpose(); - Matrix13 H_xi2_b2 = qn.vector().transpose(); + Matrix13 H_xi1_b1 = qn.transpose(); + Matrix13 H_xi2_b2 = qn.transpose(); // Assemble dxi/dp = dxi/dB * dB/dp. Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index ce9932879..4682a6507 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -103,16 +103,16 @@ TEST( PinholeCamera, level2) TEST( PinholeCamera, lookat) { // Create a level camera, looking in Y-direction - Point3 C(10.0,0.0,0.0); - Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + Point3 C(10,0,0); + Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1)); // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); - EXPECT(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal(camera.pose(), expected)); - Point3 C2(30.0,0.0,10.0); - Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + Point3 C2(30,0,10); + Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -278,7 +278,7 @@ TEST( PinholeCamera, range0) { double result = camera.range(point1, D1, D2); Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, + EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(Hexpected1, D1, 1e-7)); EXPECT(assert_equal(Hexpected2, D2, 1e-7)); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 3c79a45e3..c5137e3b3 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -74,16 +74,16 @@ TEST(PinholeCamera, Pose) { TEST( PinholePose, lookat) { // Create a level camera, looking in Y-direction - Point3 C(10.0,0.0,0.0); - Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + Point3 C(10,0,0); + Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1)); // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); EXPECT(assert_equal( camera.pose(), expected)); - Point3 C2(30.0,0.0,10.0); - Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + Point3 C2(30,0,10); + Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -120,7 +120,7 @@ TEST( PinholePose, backprojectInfinity) /* ************************************************************************* */ TEST( PinholePose, backproject2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); @@ -212,8 +212,7 @@ TEST( PinholePose, range0) { double result = camera.range(point1, D1, D2); Matrix expectedDcamera = numericalDerivative21(range0, camera, point1); Matrix expectedDpoint = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, - 1e-9); + EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 37ad105c7..c7fb44716 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -71,14 +71,14 @@ TEST( Point3, arithmetic) { /* ************************************************************************* */ TEST( Point3, equals) { - CHECK(P.equals(P)); - Point3 Q; - CHECK(!P.equals(Q)); + CHECK(traits::Equals(P,P)); + Point3 Q(0,0,0); + CHECK(!traits::Equals(P,Q)); } /* ************************************************************************* */ TEST( Point3, dot) { - Point3 origin, ones(1, 1, 1); + Point3 origin(0,0,0), ones(1, 1, 1); CHECK(origin.dot(Point3(1, 1, 0)) == 0); CHECK(ones.dot(Point3(1, 1, 0)) == 2); } @@ -111,20 +111,20 @@ TEST (Point3, norm) { Matrix actualH; Point3 point(3,4,5); // arbitrary point double expected = sqrt(50); - EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8); + EXPECT_DOUBLES_EQUAL(expected, norm(point, actualH), 1e-8); Matrix expectedH = numericalDerivative11(norm_proxy, point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } /* ************************************************************************* */ double testFunc(const Point3& P, const Point3& Q) { - return P.distance(Q); + return distance(P,Q); } TEST (Point3, distance) { Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3); Matrix H1, H2; - double d = P.distance(Q, H1, H2); + double d = distance(P, Q, H1, H2); double expectedDistance = 55.542686; Matrix numH1 = numericalDerivative21(testFunc, P, Q); Matrix numH2 = numericalDerivative22(testFunc, P, Q); @@ -137,9 +137,9 @@ TEST (Point3, distance) { TEST(Point3, cross) { Matrix aH1, aH2; boost::function f = - boost::bind(&Point3::cross, _1, _2, boost::none, boost::none); + boost::bind(>sam::cross, _1, _2, boost::none, boost::none); const Point3 omega(0, 1, 0), theta(4, 6, 8); - omega.cross(theta, aH1, aH2); + cross(omega, theta, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 56e1e022c..61550d540 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -62,7 +62,7 @@ TEST( Pose3, retract_first_order) Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2)); + EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2)); v(3)=0.2;v(4)=0.7;v(5)=-2; EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2)); } @@ -72,7 +72,7 @@ TEST( Pose3, retract_expmap) { Vector v = zero(6); v(0) = 0.3; Pose3 pose = Pose3::Expmap(v); - EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2)); + EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2)); EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2)); } @@ -82,7 +82,7 @@ TEST( Pose3, expmap_a_full) Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3()))); + EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3(0,0,0)))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; EXPECT(assert_equal(Pose3(R, P),expmap_default(id, v),1e-5)); } @@ -93,7 +93,7 @@ TEST( Pose3, expmap_a_full2) Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3()))); + EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3(0,0,0)))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; EXPECT(assert_equal(Pose3(R, P),expmap_default(id, v),1e-5)); } @@ -388,7 +388,7 @@ TEST( Pose3, transform_to_translate) /* ************************************************************************* */ TEST( Pose3, transform_to_rotate) { - Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3()); + Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(0,0,0)); Point3 actual = transform.transform_to(Point3(2,1,10)); Point3 expected(-1,2,10); EXPECT(assert_equal(expected, actual, 0.001)); @@ -406,7 +406,7 @@ TEST( Pose3, transform_to) /* ************************************************************************* */ TEST( Pose3, transform_from) { - Point3 actual = T3.transform_from(Point3()); + Point3 actual = T3.transform_from(Point3(0,0,0)); Point3 expected = Point3(1.,2.,3.); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index ce5d3268c..f96198b94 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -542,8 +542,8 @@ TEST(Rot3, quaternion) { Point3 expected1 = R1*p1; Point3 expected2 = R2*p2; - Point3 actual1 = Point3(q1*p1.vector()); - Point3 actual2 = Point3(q2*p2.vector()); + Point3 actual1 = Point3(q1*p1); + Point3 actual2 = Point3(q2*p2); EXPECT(assert_equal(expected1, actual1)); EXPECT(assert_equal(expected2, actual2)); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 8db8113a1..71dcabd4e 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -63,16 +63,16 @@ TEST( SimpleCamera, level2) TEST( SimpleCamera, lookat) { // Create a level camera, looking in Y-direction - Point3 C(10.0,0.0,0.0); - SimpleCamera camera = SimpleCamera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + Point3 C(10,0,0); + SimpleCamera camera = SimpleCamera::Lookat(C, Point3(0,0,0), Point3(0,0,1)); // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); CHECK(assert_equal( camera.pose(), expected)); - Point3 C2(30.0,0.0,10.0); - SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + Point3 C2(30,0,10); + SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(0,0,0), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -100,7 +100,7 @@ TEST( SimpleCamera, backproject) /* ************************************************************************* */ TEST( SimpleCamera, backproject2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down SimpleCamera camera(Pose3(rot, origin), K); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 6508965ec..497599a6a 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -46,7 +46,7 @@ TEST( StereoCamera, project) // point X Y Z in meters Point3 p(0, 0, 5); StereoPoint2 result = stereoCam.project(p); - CHECK(assert_equal(StereoPoint2(320,320-150,240),result)); + CHECK(assert_equal(StereoPoint2(320, 320 - 150, 240), result)); // point X Y Z in meters Point3 p2(1, 1, 5); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index dbe315807..07789acaa 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -377,7 +377,7 @@ TEST(Unit3, retract_expmap) { TEST(Unit3, Random) { boost::mt19937 rng(42); // Check that means are all zero at least - Point3 expectedMean, actualMean; + Point3 expectedMean(0,0,0), actualMean(0,0,0); for (size_t i = 0; i < 100; i++) actualMean = actualMean + Unit3::Random(rng).point3(); actualMean = actualMean / 100; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index c6e613b14..15029b8ad 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -436,7 +436,7 @@ TriangulationResult triangulateSafe(const std::vector& cameras, BOOST_FOREACH(const CAMERA& camera, cameras) { const Pose3& pose = camera.pose(); if (params.landmarkDistanceThreshold > 0 - && pose.translation().distance(point) + && distance(pose.translation(), point) > params.landmarkDistanceThreshold) return TriangulationResult::Degenerate(); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 2f4cb9bc3..b2cb5ed82 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -25,14 +25,14 @@ namespace gtsam { //*************************************************************************** void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; - nT_.print(" prior mean: "); + cout << " prior mean: " << nT_ << "\n"; noiseModel_->print(" noise model: "); } //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** @@ -43,7 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p, H->block < 3, 3 > (0, 0) << zeros(3, 3); H->block < 3, 3 > (0, 3) << p.rotation().matrix(); } - return p.translation().vector() -nT_.vector(); + return p.translation() -nT_; } //*************************************************************************** @@ -66,7 +66,7 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, // Construct initial pose Pose3 nTb(nRb, nT); // nTb - return make_pair(nTb, nV.vector()); + return make_pair(nTb, nV); } //*************************************************************************** diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index cd5fa71d2..46e194460 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -48,8 +48,7 @@ public: typedef GPSFactor This; /** default constructor - only use for serialization */ - GPSFactor() { - } + GPSFactor(): nT_(0, 0, 0) {} virtual ~GPSFactor() { } diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 860eaa85c..7e5d85cde 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -89,7 +89,7 @@ void NavState::print(const string& s) const { //------------------------------------------------------------------------------ bool NavState::equals(const NavState& other, double tol) const { - return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) + return R_.equals(other.R_, tol) && traits::Equals(t_, other.t_, tol) && equal_with_abs_tol(v_, other.v_, tol); } @@ -121,11 +121,6 @@ Point3 NavState::operator*(const Point3& b_t) const { return Point3(R_ * b_t + t_); } -//------------------------------------------------------------------------------ -Velocity3 NavState::operator*(const Velocity3& b_v) const { - return Velocity3(R_ * b_v + v_); -} - //------------------------------------------------------------------------------ NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, OptionalJacobian<9, 9> H) { @@ -189,7 +184,7 @@ Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) { throw runtime_error("NavState::Logmap derivative not implemented yet"); TIE(nRb, n_p, n_v, nTb); - Vector3 n_t = n_p.vector(); + Vector3 n_t = n_p; // NOTE(frank): See Pose3::Logmap Vector9 xi; @@ -300,7 +295,7 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper dV(xi) << ((-2.0 * dt) * omega_cross_vel); if (secondOrder) { - const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector())); + const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t)); dP(xi) -= (0.5 * dt2) * omega_cross2_t; dV(xi) -= dt * omega_cross2_t; } diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 4c8b50776..02da46317 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -49,7 +49,7 @@ public: /// Default constructor NavState() : - v_(Vector3::Zero()) { + t_(0,0,0), v_(Vector3::Zero()) { } /// Construct from attitude, position, velocity NavState(const Rot3& R, const Point3& t, const Velocity3& v) : @@ -97,7 +97,7 @@ public: } /// Return position as Vector3 Vector3 t() const { - return t_.vector(); + return t_; } /// Return velocity as Vector3. Computation-free. const Vector3& v() const { @@ -147,9 +147,6 @@ public: /// Act on position alone, n_t = nRb * b_t + n_t Point3 operator*(const Point3& b_t) const; - /// Act on velocity alone, n_v = nRb * b_v + n_v - Velocity3 operator*(const Velocity3& b_v) const; - /// @} /// @name Manifold /// @{ diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index c3f203849..7209391f1 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -94,7 +94,7 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs; // Centrifugal acceleration - const Vector3 b_arm = p().body_P_sensor->translation().vector(); + const Vector3 b_arm = p().body_P_sensor->translation(); if (!b_arm.isZero()) { // Subtract out the the centripetal acceleration from the unbiased one // to get linear acceleration vector in the body frame: @@ -188,7 +188,7 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, if (p().body_P_sensor) { // More complicated derivatives in case of non-trivial sensor pose *C *= D_correctedOmega_omega; - if (!p().body_P_sensor->translation().vector().isZero()) + if (!p().body_P_sensor->translation().isZero()) *C += *B* D_correctedAcc_omega; *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last } diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index ad684f5f8..e4b380ff9 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -80,7 +80,7 @@ class AcceleratingScenario : public Scenario { AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0, const Vector3& a_n, const Vector3& omega_b = Vector3::Zero()) - : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {} + : nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(omega_b) {} Pose3 pose(double t) const override { return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index b81332f5e..f05504d6b 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -84,9 +84,9 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } else if (const GenericValue* p = dynamic_cast*>(&value)) { t << p->value().x(), p->value().y(), 0; } else if (const GenericValue* p = dynamic_cast*>(&value)) { - t = p->value().translation().vector(); + t = p->value().translation(); } else if (const GenericValue* p = dynamic_cast*>(&value)) { - t = p->value().vector(); + t = p->value(); } else { return boost::none; } diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 26580f41e..ddc7b18d6 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -327,7 +327,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){ Key key = key_value.key; const Rot3& rot = initialRot.at(key); - Pose3 initializedPose = Pose3(rot, Point3()); + Pose3 initializedPose = Pose3(rot, Point3(0,0,0)); initialPose.insert(key, initializedPose); } // add prior diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 50516afe4..c79fb5823 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1,5 +1,5 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -808,7 +808,7 @@ bool writeBAL(const string& filename, SfM_data &data) { Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation().vector() << endl; + os << poseOpenGL.translation() << endl; os << cameraCalibration.fx() << endl; os << cameraCalibration.k1() << endl; os << cameraCalibration.k2() << endl; @@ -880,7 +880,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, dataValues.tracks[j].r = 1.0; dataValues.tracks[j].g = 0.0; dataValues.tracks[j].b = 0.0; - dataValues.tracks[j].p = Point3(); + dataValues.tracks[j].p = Point3(0,0,0); } } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 54e27229c..dc7804c2d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -147,6 +147,7 @@ typedef std::pair SIFT_Index; /// Define the structure for the 3D points struct SfM_Track { + SfM_Track():p(0,0,0) {} Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 5bd453424..77b02d9fb 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -35,7 +35,7 @@ public: /// Default Constructor Event() : - time_(0) { + time_(0), location_(0,0,0) { } /// Constructor from time and location diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 17d54b04d..516e1fb5b 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -23,11 +23,11 @@ namespace gtsam { Similarity3::Similarity3() : - R_(), t_(), s_(1) { + t_(0,0,0), s_(1) { } Similarity3::Similarity3(double s) : - s_(s) { + t_(0,0,0), s_(s) { } Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) : diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 9c7620d70..713ea3054 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -71,7 +71,7 @@ TEST(Similarity3, Constructors) { TEST(Similarity3, Getters) { Similarity3 sim3_default; EXPECT(assert_equal(Rot3(), sim3_default.rotation())); - EXPECT(assert_equal(Point3(), sim3_default.translation())); + EXPECT(assert_equal(Point3(0,0,0), sim3_default.translation())); EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9); Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); @@ -158,7 +158,7 @@ TEST( Similarity3, retract_first_order) { Similarity3 id; Vector v = zero(7); v(0) = 0.3; - EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v), 1e-2)); + EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2)); // v(3) = 0.2; // v(4) = 0.7; // v(5) = -2; From 94ccf989857964dc7e517b2cea100721b20110fb Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 11 Feb 2016 16:52:02 -0800 Subject: [PATCH 05/20] Avoid default constructor in tests --- gtsam/geometry/tests/testPinholeCamera.cpp | 8 ++++---- gtsam/geometry/tests/testPose3.cpp | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 4 ++-- .../navigation/tests/testImuFactorSerialization.cpp | 2 +- gtsam/slam/tests/testInitializePose3.cpp | 12 ++++++------ gtsam/slam/tests/testSmartProjectionPoseFactor.cpp | 4 ++-- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 2 +- gtsam_unstable/geometry/tests/testEvent.cpp | 4 ++-- .../slam/tests/testInvDepthFactorVariant1.cpp | 6 +++--- .../slam/tests/testInvDepthFactorVariant2.cpp | 8 ++++---- tests/testNonlinearEquality.cpp | 2 +- 11 files changed, 27 insertions(+), 27 deletions(-) diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 4682a6507..4fbdbcbe1 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -112,7 +112,7 @@ TEST( PinholeCamera, lookat) EXPECT(assert_equal(camera.pose(), expected)); Point3 C2(30,0,10); - Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0,0,1)); + Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -149,7 +149,7 @@ TEST( PinholeCamera, backprojectInfinity) /* ************************************************************************* */ TEST( PinholeCamera, backproject2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); @@ -165,7 +165,7 @@ TEST( PinholeCamera, backproject2) /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); @@ -180,7 +180,7 @@ TEST( PinholeCamera, backprojectInfinity2) /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity3) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity Camera camera(Pose3(rot, origin), K); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 61550d540..af8e7c6a0 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -306,7 +306,7 @@ TEST( Pose3, Dtransform_from1_b) TEST( Pose3, Dtransform_from1_c) { - Point3 origin; + Point3 origin(0,0,0); Pose3 T0(R,origin); Matrix actualDtransform_from1; T0.transform_from(P, actualDtransform_from1, boost::none); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index f1d761cb0..28ad037c0 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -590,7 +590,7 @@ TEST(ImuFactor, PredictRotation) { // Predict NavState actual = pim.predict(NavState(), bias); - NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(), Z_3x1); + NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0,0,0), Z_3x1); EXPECT(assert_equal(expected, actual)); } @@ -708,7 +708,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { auto p = testing::Params(); p->n_gravity = Vector3(0, 0, -kGravity); - p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3()); + p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0)); p->accelerometerCovariance = 1e-7 * I_3x3; p->gyroscopeCovariance = 1e-8 * I_3x3; p->integrationCovariance = 1e-9 * I_3x3; diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index a7796d1b2..9f9781d2c 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -43,7 +43,7 @@ TEST(ImuFactor, serialization) { // Create default parameters with Z-down and above noise paramaters auto p = PreintegrationParams::MakeSharedD(9.81); - p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3()); + p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0)); p->accelerometerCovariance = 1e-7 * I_3x3; p->gyroscopeCovariance = 1e-8 * I_3x3; p->integrationCovariance = 1e-9 * I_3x3; diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 9fd8474eb..e6f08286a 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -148,9 +148,9 @@ TEST( InitializePose3, iterationGradient ) { Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); - givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); - givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); - givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); + givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); + givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3(0,0,0)) )); + givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); size_t maxIter = 1; // test gradient at the first iteration bool setRefFrame = false; @@ -189,9 +189,9 @@ TEST( InitializePose3, orientationsGradient ) { Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); - givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); - givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); - givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); + givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); + givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3(0,0,0)) )); + givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); // do 10 gradient iterations bool setRefFrame = false; Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 1c1bc3c03..5430e1088 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -959,8 +959,8 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac views.push_back(x3); // Two different cameras, at the same position, but different rotations - Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); + Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); Camera cam2(pose2, sharedK2); Camera cam3(pose3, sharedK2); diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index db2f8f7f8..855131042 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -29,7 +29,7 @@ TEST( testPoseRTV, constructors ) { EXPECT(assert_equal(Pose3(rot, pt), state1.pose())); PoseRTV state2; - EXPECT(assert_equal(Point3(), state2.t())); + EXPECT(assert_equal(Point3(0,0,0), state2.t())); EXPECT(assert_equal(Rot3(), state2.R())); EXPECT(assert_equal(kZero3, state2.v())); EXPECT(assert_equal(Pose3(), state2.pose())); diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index 0842e2146..ec8ca1f4b 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -34,7 +34,7 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); -static const Point3 microphoneAt0; +static const Point3 microphoneAt0(0,0,0); //***************************************************************************** TEST( Event, Constructor ) { diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 3aa758701..96043fb50 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -48,7 +48,7 @@ TEST( InvDepthFactorVariant1, optimize) { Vector6 expected((Vector(6) << x, y, z, theta, phi, rho).finished()); - + // Create a factor graph with two inverse depth factors and two pose priors Key poseKey1(1); Key poseKey2(2); @@ -89,14 +89,14 @@ TEST( InvDepthFactorVariant1, optimize) { // cout << endl << endl; // Calculate world coordinates of landmark versions - Point3 world_landmarkBefore; + Point3 world_landmarkBefore(0,0,0); { Vector6 landmarkBefore = values.at(landmarkKey); double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2); double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5); world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - Point3 world_landmarkAfter; + Point3 world_landmarkAfter(0,0,0); { Vector6 landmarkAfter = result.at(landmarkKey); double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index d20fc000c..7ac0faa1e 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -46,7 +46,7 @@ TEST( InvDepthFactorVariant2, optimize) { Vector3 expected((Vector(3) << theta, phi, rho).finished()); - + // Create a factor graph with two inverse depth factors and two pose priors Key poseKey1(1); Key poseKey2(2); @@ -72,7 +72,7 @@ TEST( InvDepthFactorVariant2, optimize) { LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); Vector3 actual = result.at(landmarkKey); - + // values.at(poseKey1).print("Pose1 Before:\n"); // result.at(poseKey1).print("Pose1 After:\n"); // pose1.print("Pose1 Truth:\n"); @@ -87,13 +87,13 @@ TEST( InvDepthFactorVariant2, optimize) { // std::cout << std::endl << std::endl; // Calculate world coordinates of landmark versions - Point3 world_landmarkBefore; + Point3 world_landmarkBefore(0,0,0); { Vector3 landmarkBefore = values.at(landmarkKey); double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2); world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - Point3 world_landmarkAfter; + Point3 world_landmarkAfter(0,0,0); { Vector3 landmarkAfter = result.at(landmarkKey); double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index a3c99ece7..130dc1bc8 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -531,7 +531,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { // create initial estimates Rot3 faceDownY( (Matrix) (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0).finished()); - Pose3 pose1(faceDownY, Point3()); // origin, left camera + Pose3 pose1(faceDownY, Point3(0,0,0)); // origin, left camera SimpleCamera camera1(pose1, K); Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left SimpleCamera camera2(pose2, K); From 2060b09a2b391079d4a88975375dcdced5e11510 Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 11 Feb 2016 19:03:11 -0800 Subject: [PATCH 06/20] Avoid calling default constructors and/or vector --- gtsam/geometry/Point3.h | 11 +++++++++-- gtsam/slam/dataset.cpp | 6 ++++-- gtsam_unstable/dynamics/PoseRTV.cpp | 10 +++++----- gtsam_unstable/dynamics/PoseRTV.h | 4 ++-- gtsam_unstable/geometry/BearingS2.cpp | 4 ++-- gtsam_unstable/geometry/Event.h | 7 +++---- gtsam_unstable/geometry/Similarity3.cpp | 15 +++++++-------- gtsam_unstable/slam/SmartStereoProjectionFactor.h | 8 ++++---- wrap/Class.cpp | 2 +- wrap/tests/expected/geometry_wrapper.cpp | 2 +- 10 files changed, 38 insertions(+), 31 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index ba06e1d85..0cf771844 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -52,7 +52,9 @@ class GTSAM_EXPORT Point3 : public Vector3 { #ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// Default constructor now creates *uninitialized* point !!!! - Point3() {} + Point3() { + throw std::runtime_error("Default constructor called!"); + } #endif @@ -106,9 +108,14 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @name Standard Interface /// @{ - /** return vectorized form (column-wise)*/ + /// return as Vector3 const Vector3& vector() const { return *this; } + /// return as transposed vector + Eigen::DenseBase::ConstTransposeReturnType transpose() const { + return this->Vector3::transpose(); + } + /// @} /// Output stream operator diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index c79fb5823..d037f7535 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -779,7 +779,7 @@ bool writeBAL(const string& filename, SfM_data &data) { os << endl; for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j - SfM_Track track = data.tracks[j]; + const SfM_Track& track = data.tracks[j]; for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id @@ -808,7 +808,9 @@ bool writeBAL(const string& filename, SfM_data &data) { Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation() << endl; + os << poseOpenGL.translation().x() << endl; + os << poseOpenGL.translation().y() << endl; + os << poseOpenGL.translation().z() << endl; os << cameraCalibration.fx() << endl; os << cameraCalibration.k1() << endl; os << cameraCalibration.k2() << endl; diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index b3953dd23..b6fc61411 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -37,7 +37,7 @@ PoseRTV::PoseRTV(const Vector& rtv) : Vector PoseRTV::vector() const { Vector rtv(9); rtv.head(3) = rotation().xyz(); - rtv.segment(3,3) = translation().vector(); + rtv.segment(3,3) = translation(); rtv.tail(3) = velocity(); return rtv; } @@ -52,7 +52,7 @@ bool PoseRTV::equals(const PoseRTV& other, double tol) const { void PoseRTV::print(const string& s) const { cout << s << ":" << endl; gtsam::print((Vector)R().xyz(), " R:rpy"); - t().print(" T"); + cout << " T" << t().transpose() << endl; gtsam::print((Vector)velocity(), " V"); } @@ -103,7 +103,7 @@ PoseRTV PoseRTV::flyingDynamics( Rot3 yaw_correction_bn = Rot3::Yaw(yaw2); Point3 forward(forward_accel, 0.0, 0.0); Vector Acc_n = - yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame + yaw_correction_bn.rotate(forward) // applies locally forward force in the global frame - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1 + delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch @@ -172,7 +172,7 @@ double PoseRTV::range(const PoseRTV& other, const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0); const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0); Matrix13 D_d_t1, D_d_t2; - double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); + double d = distance(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0; if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0; return d; @@ -188,7 +188,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, // Note that we rotate the velocity Matrix3 D_newvel_R, D_newvel_v; - Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v).vector(); + Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v); if (Dglobal) { Dglobal->setZero(); diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index b59ea4614..bf92ab9c4 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -66,7 +66,7 @@ public: // Access to vector for ease of use with Matlab // and avoidance of Point3 Vector vector() const; - Vector translationVec() const { return pose().translation().vector(); } + Vector translationVec() const { return pose().translation(); } const Velocity3& velocityVec() const { return velocity(); } // testable @@ -126,7 +126,7 @@ public: /// @return a vector for Matlab compatibility inline Vector translationIntegrationVec(const PoseRTV& x2, double dt) const { - return translationIntegration(x2, dt).vector(); + return translationIntegration(x2, dt); } /** diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index 71278789c..9dfed612c 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -34,7 +34,7 @@ BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) { 0.,1.,0., -1.,0.,0.).finished(); // p_rel_c = Cbc*Cnb*(PosObj - Pos); - Vector p_rel_c = Cbc*Cnb*(B.vector() - A.translation().vector()); + Vector p_rel_c = Cbc*Cnb*(B - A.translation()); // FIXME: the matlab code checks for p_rel_c(0) greater than @@ -50,7 +50,7 @@ BearingS2 BearingS2::fromForwardObservation(const Pose3& A, const Point3& B) { // Cnb = DCMnb(Att); Matrix Cnb = A.rotation().matrix().transpose(); - Vector p_rel_c = Cnb*(B.vector() - A.translation().vector()); + Vector p_rel_c = Cnb*(B - A.translation()); // FIXME: the matlab code checks for p_rel_c(0) greater than diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 77b02d9fb..615b1d467 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -60,14 +60,13 @@ public: /** print with optional string */ void print(const std::string& s = "") const { - std::cout << s << "time = " << time_; - location_.print("; location"); + std::cout << s << "time = " << time_ << "location = " << location_.transpose(); } /** equals with an tolerance */ bool equals(const Event& other, double tol = 1e-9) const { return std::abs(time_ - other.time_) < tol - && location_.equals(other.location_, tol); + && traits::Equals(location_, other.location_, tol); } /// Updates a with tangent space delta @@ -86,7 +85,7 @@ public: OptionalJacobian<1, 3> H2 = boost::none) const { static const double Speed = 330; Matrix13 D1, D2; - double distance = location_.distance(microphone, D1, D2); + double distance = gtsam::distance(location_, microphone, D1, D2); if (H1) // derivative of toa with respect to event *H1 << 1.0, D1 / Speed; diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 516e1fb5b..9768f4fa4 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -43,7 +43,7 @@ Similarity3::Similarity3(const Matrix4& T) : } bool Similarity3::equals(const Similarity3& other, double tol) const { - return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) + return R_.equals(other.R_, tol) && traits::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) && s_ > (other.s_ - tol); } @@ -55,8 +55,7 @@ void Similarity3::print(const std::string& s) const { std::cout << std::endl; std::cout << s; rotation().print("R:\n"); - translation().print("t: "); - std::cout << "s: " << scale() << std::endl; + std::cout << "t: " << translation().transpose() << "s: " << scale() << std::endl; } Similarity3 Similarity3::identity() { @@ -79,7 +78,7 @@ Point3 Similarity3::transform_from(const Point3& p, // // For this derivative, see LieGroups.pdf const Matrix3 sR = s_ * R_.matrix(); const Matrix3 DR = sR * skewSymmetric(-p.x(), -p.y(), -p.z()); - *H1 << DR, sR, sR * p.vector(); + *H1 << DR, sR, sR * p; } if (H2) *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix() @@ -103,7 +102,7 @@ Matrix4 Similarity3::wedge(const Vector7& xi) { Matrix7 Similarity3::AdjointMap() const { // http://www.ethaneade.org/latex2html/lie/node30.html const Matrix3 R = R_.matrix(); - const Vector3 t = t_.vector(); + const Vector3 t = t_; const Matrix3 A = s_ * skewSymmetric(t) * R; Matrix7 adj; adj << R, Z_3x3, Matrix31::Zero(), // 3*7 @@ -153,7 +152,7 @@ Vector7 Similarity3::Logmap(const Similarity3& T, OptionalJacobian<7, 7> Hm) { const Vector3 w = Rot3::Logmap(T.R_); const double lambda = log(T.s_); Vector7 result; - result << w, GetV(w, lambda).inverse() * T.t_.vector(), lambda; + result << w, GetV(w, lambda).inverse() * T.t_, lambda; if (Hm) { throw std::runtime_error("Similarity3::Logmap: derivative not implemented"); } @@ -173,13 +172,13 @@ Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { std::ostream &operator<<(std::ostream &os, const Similarity3& p) { os << "[" << p.rotation().xyz().transpose() << " " - << p.translation().vector().transpose() << " " << p.scale() << "]\';"; + << p.translation().transpose() << " " << p.scale() << "]\';"; return os; } const Matrix4 Similarity3::matrix() const { Matrix4 T; - T.topRows<3>() << R_.matrix(), t_.vector(); + T.topRows<3>() << R_.matrix(), t_; T.bottomRows<1>() << 0, 0, 0, 1.0 / s_; return T; } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 6651c005f..7cbeaae65 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -1,12 +1,12 @@ /* ---------------------------------------------------------------------------- - + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - + * See LICENSE for the license information - + * -------------------------------------------------------------------------- */ /** @@ -253,7 +253,7 @@ public: reprojections.push_back(cameras[i].backproject(measured_[i])); } - Point3 pw_sum; + Point3 pw_sum(0,0,0); BOOST_FOREACH(const Point3& pw, reprojections) { pw_sum = pw_sum + pw; } diff --git a/wrap/Class.cpp b/wrap/Class.cpp index e62e31bc1..d7a3b6ee4 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -567,7 +567,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile, // string serialized = unwrap< string >(in[0]); // istringstream in_archive_stream(serialized); // boost::archive::text_iarchive in_archive(in_archive_stream); - // Shared output(new Point3()); + // Shared output(new Point3(0,0,0)); // in_archive >> *output; // out[0] = wrap_shared_ptr(output,"Point3", false); //} diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 226030a0d..6646394f6 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -326,7 +326,7 @@ void gtsamPoint3_string_deserialize_19(int nargout, mxArray *out[], int nargin, string serialized = unwrap< string >(in[0]); istringstream in_archive_stream(serialized); boost::archive::text_iarchive in_archive(in_archive_stream); - Shared output(new gtsam::Point3()); + Shared output(new gtsam::Point3(0,0,0)); in_archive >> *output; out[0] = wrap_shared_ptr(output,"gtsam.Point3", false); } From 7fd838611e951e01a8e28ce1ee4aa4146d7c3763 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Feb 2016 23:27:09 -0800 Subject: [PATCH 07/20] Fixed typo --- wrap/tests/expected/geometry_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 6646394f6..226030a0d 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -326,7 +326,7 @@ void gtsamPoint3_string_deserialize_19(int nargout, mxArray *out[], int nargin, string serialized = unwrap< string >(in[0]); istringstream in_archive_stream(serialized); boost::archive::text_iarchive in_archive(in_archive_stream); - Shared output(new gtsam::Point3(0,0,0)); + Shared output(new gtsam::Point3()); in_archive >> *output; out[0] = wrap_shared_ptr(output,"gtsam.Point3", false); } From 56dbf487ee70b169a75829448224d9e4757f1944 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Feb 2016 23:27:58 -0800 Subject: [PATCH 08/20] Fixed more default constructor calls --- gtsam/navigation/MagFactor.h | 10 +++++----- gtsam/navigation/tests/testScenarios.cpp | 4 ++-- gtsam/slam/EssentialMatrixFactor.h | 13 ++++--------- gtsam/slam/tests/testGeneralSFMFactor.cpp | 4 ++-- .../slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp | 2 +- .../slam/tests/testSmartProjectionCameraFactor.cpp | 13 ++++++------- gtsam_unstable/dynamics/VelocityConstraint.h | 2 +- gtsam_unstable/slam/tests/testTOAFactor.cpp | 2 +- 8 files changed, 22 insertions(+), 28 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index fc1e69190..bee3e8944 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -76,7 +76,7 @@ public: boost::optional H = boost::none) const { // measured bM = nRb� * nM + b Point3 hx = unrotate(nRb, nM_, H) + bias_; - return (hx - measured_).vector(); + return (hx - measured_); } }; @@ -114,7 +114,7 @@ public: boost::optional H = boost::none) const { // measured bM = nRb� * nM + b Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; - return (hx - measured_).vector(); + return (hx - measured_); } }; @@ -155,7 +155,7 @@ public: Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; if (H2) *H2 = eye(3); - return (hx - measured_).vector(); + return (hx - measured_); } }; @@ -197,7 +197,7 @@ public: Unit3 rotated = bRn_.rotate(direction, boost::none, H2); Point3 hx = scale * rotated.point3() + bias; if (H1) - *H1 = rotated.point3().vector(); + *H1 = rotated.point3(); if (H2) // H2 is 2*2, but we need 3*2 { Matrix H; @@ -206,7 +206,7 @@ public: } if (H3) *H3 = eye(3); - return (hx - measured_).vector(); + return (hx - measured_); } }; diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index c2fdb963d..b5d312a86 100644 --- a/gtsam/navigation/tests/testScenarios.cpp +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -140,7 +140,7 @@ TEST(ScenarioRunner, Loop) { /* ************************************************************************* */ namespace initial { const Rot3 nRb; -const Point3 P0; +const Point3 P0(0,0,0); const Vector3 V0(0, 0, 0); } @@ -259,7 +259,7 @@ namespace initial3 { // Rotation only // Set up body pointing towards y axis. The body itself has Z axis pointing down const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); -const Point3 P0; +const Point3 P0(0,0,0); const Vector3 V0(0, 0, 0); } diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index da22225e5..e9a117745 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -19,7 +19,7 @@ namespace gtsam { */ class EssentialMatrixFactor: public NoiseModelFactor1 { - Vector vA_, vB_; ///< Homogeneous versions, in ideal coordinates + Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates typedef NoiseModelFactor1 Base; typedef EssentialMatrixFactor This; @@ -107,9 +107,7 @@ public: */ EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, const SharedNoiseModel& model) : - Base(model, key1, key2) { - dP1_ = Point3(pA.x(), pA.y(), 1); - pn_ = pB; + Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(pA)), pn_(pB) { f_ = 1.0; } @@ -125,11 +123,8 @@ public: template EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, const SharedNoiseModel& model, boost::shared_ptr K) : - Base(model, key1, key2) { - assert(K); - Point2 p1 = K->calibrate(pA); - dP1_ = Point3(p1.x(), p1.y(), 1); // d*P1 = (x,y,1) - pn_ = K->calibrate(pB); + Base(model, key1, key2), dP1_( + EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) { f_ = 0.5 * (K->fx() + K->fy()); } diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 07abb557f..48e2e8b2f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -154,7 +154,7 @@ TEST( GeneralSFMFactor, error ) { Point3 t1(0, 0, -6); Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; + Point3 l1(0,0,0); values.insert(L(1), l1); EXPECT( assert_equal(((Vector ) Vector2(-3., 0.)), @@ -451,7 +451,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) { Point3 t1(0, 0, -6); Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; + Point3 l1(0,0,0); values.insert(L(1), l1); vector models; diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index a349a4992..9fda7d745 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -107,7 +107,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, error ) { Point3 t1(0, 0, -6); Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; + Point3 l1(0,0,0); values.insert(L(1), l1); EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values))); } diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 467aefe91..e441c37ff 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -610,11 +610,11 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-3); - Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this) + Point3 oldPoint(0,0,0); // this takes the point stored in the factor (we are not interested in this) if (factor1->point()) oldPoint = *(factor1->point()); - Point3 expectedPoint; + Point3 expectedPoint(0,0,0); if (factor1->point(values)) expectedPoint = *(factor1->point(values)); @@ -636,10 +636,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { smartGraph.push_back(factor1); double expectedError = factor1->error(values); double expectedErrorGraph = smartGraph.error(values); - Point3 expectedPoint; + Point3 expectedPoint(0,0,0); if (factor1->point()) expectedPoint = *(factor1->point()); - // cout << "expectedPoint " << expectedPoint.vector() << endl; // COMMENTS: // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as @@ -677,7 +676,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { smartGraph.push_back(factor1); Matrix expectedHessian = smartGraph.linearize(values)->hessian().first; Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second; - Point3 expectedPoint; + Point3 expectedPoint(0,0,0); if (factor1->point()) expectedPoint = *(factor1->point()); @@ -720,7 +719,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { // smartGraph.push_back(factor1); // GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values); // -// Point3 expectedPoint; +// Point3 expectedPoint(0,0,0); // if(factor1->point()) // expectedPoint = *(factor1->point()); // @@ -773,7 +772,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { cameras.push_back(level_camera_right); factor1->error(values); // this is important to have a triangulation of the point - Point3 point; + Point3 point(0,0,0); if (factor1->point()) point = *(factor1->point()); vector Fblocks; diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 4ce4d5758..d9674b415 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -108,7 +108,7 @@ private: const Velocity3& v1 = x1.v(), v2 = x2.v(); const Point3& p1 = x1.t(), p2 = x2.t(); - Point3 hx; + Point3 hx(0,0,0); switch(mode) { case dynamics::TRAPEZOIDAL: hx = p1 + Point3((v1 + v2) * dt *0.5); break; case dynamics::EULER_START: hx = p1 + Point3(v1 * dt); break; diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 879f7095e..042a15108 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -44,7 +44,7 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); -static const Point3 microphoneAt0; +static const Point3 microphoneAt0(0,0,0); //***************************************************************************** TEST( TOAFactor, NewWay ) { From fe1607c46494ed61d7acaf53120fe2e2c40593c7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Feb 2016 00:05:25 -0800 Subject: [PATCH 09/20] Enable typedef for Jenkins testing --- gtsam/geometry/Point3.h | 19 ++++++------------- 1 file changed, 6 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 0cf771844..524bb9bbf 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -28,10 +28,11 @@ namespace gtsam { -//#define GTSAM_USE_VECTOR3_POINTS +#define GTSAM_USE_VECTOR3_POINTS #ifdef GTSAM_USE_VECTOR3_POINTS - // As of GTSAM4, we just typedef Point3 to Vector3 + /// As of GTSAM 4, in order to make GTSAM more lean, + /// it is now possible to just typedef Point3 to Vector3 typedef Vector3 Point3; #else @@ -51,13 +52,10 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @{ #ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - /// Default constructor now creates *uninitialized* point !!!! - Point3() { - throw std::runtime_error("Default constructor called!"); - } + /// Default constructor no longer initializes, just like Vector3 + Point3() {} #endif - /// Construct from x, y, and z coordinates Point3(double x, double y, double z): Vector3(x,y, z) {} @@ -111,11 +109,6 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// return as Vector3 const Vector3& vector() const { return *this; } - /// return as transposed vector - Eigen::DenseBase::ConstTransposeReturnType transpose() const { - return this->Vector3::transpose(); - } - /// @} /// Output stream operator @@ -124,7 +117,7 @@ class GTSAM_EXPORT Point3 : public Vector3 { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ - Point3() { setZero(); } + Point3() { setZero(); } // initializes to zero, in contrast to new behavior Point3 inverse() const { return -(*this);} Point3 compose(const Point3& q) const { return (*this)+q;} Point3 between(const Point3& q) const { return q-(*this);} From 1c920967d968f78238d2562c7f5e883f1bd42b88 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Feb 2016 00:06:07 -0800 Subject: [PATCH 10/20] No more use of vector() or default constructor --- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- gtsam/nonlinear/tests/testExpression.cpp | 4 ++-- gtsam/sam/tests/testRangeFactor.cpp | 12 ++++++------ gtsam/slam/EssentialMatrixFactor.h | 4 ++-- gtsam/slam/PoseTranslationPrior.h | 6 +++--- gtsam/slam/RotateFactor.h | 6 +++--- gtsam_unstable/dynamics/FullIMUFactor.h | 4 ++-- gtsam_unstable/dynamics/VelocityConstraint.h | 2 +- .../dynamics/tests/testSimpleHelicopter.cpp | 3 ++- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 2 +- gtsam_unstable/slam/BiasedGPSFactor.h | 8 ++++---- .../slam/InertialNavFactor_GlobalVelocity.h | 2 +- gtsam_unstable/slam/InvDepthFactorVariant2.h | 2 +- .../tests/testInertialNavFactor_GlobalVelocity.cpp | 12 ++++++------ tests/simulated3D.h | 4 ++-- tests/testExpressionFactor.cpp | 2 +- 16 files changed, 38 insertions(+), 37 deletions(-) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index d210be789..af6954341 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -167,7 +167,7 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0)); Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! Vector9 P = Camera().localCoordinates(camera); -Vector3 X = point.vector(); +Vector3 X = point; Vector2 expectedMeasurement(1.2431567, 1.2525694); Matrix E1 = numericalDerivative21(adapted, P, X); Matrix E2 = numericalDerivative22(adapted, P, X); diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 75af5f634..6c9588d38 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -81,7 +81,7 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } Vector f3(const Point3& p, OptionalJacobian H) { - return p.vector(); + return p; } Expression p(1); set expected = list_of(1); @@ -108,7 +108,7 @@ TEST(Expression, NullaryMethod) { // Create expression Expression p(67); - Expression norm(p, &Point3::norm); + Expression norm(>sam::norm, p); // Create Values Values values; diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 706c20e78..73ff34d2a 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -361,10 +361,10 @@ TEST( RangeFactor, Camera) { namespace gtsam{ template <> -struct Range { +struct Range { typedef double result_type; - double operator()(const Vector3& v1, const Vector3& v2, - OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { + double operator()(const Vector4& v1, const Vector4& v2, + OptionalJacobian<1, 4> H1, OptionalJacobian<1, 4> H2) { return (v2 - v1).norm(); // derivatives not implemented } @@ -376,11 +376,11 @@ TEST(RangeFactor, NonGTSAM) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - RangeFactor factor(poseKey, pointKey, measurement, model); + RangeFactor factor(poseKey, pointKey, measurement, model); // Set the linearization point - Vector3 pose(1.0, 2.0, 00); - Vector3 point(-4.0, 11.0, 0); + Vector4 pose(1.0, 2.0, 00, 0); + Vector4 point(-4.0, 11.0, 0, 0); // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance Vector expectedError = (Vector(1) << 0.295630141).finished(); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index e9a117745..e668d27d3 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -139,7 +139,7 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << dP1_.vector().transpose() << ")' and (" << pn_.vector().transpose() + << dP1_.transpose() << ")' and (" << pn_.vector().transpose() << ")'" << std::endl; } @@ -191,7 +191,7 @@ public: if (Dd) // efficient backwards computation: // (2*3) * (3*3) * (3*1) - *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2.vector())); + *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2)); } Point2 reprojectionError = pn - pn_; diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index e9914955c..a24421b34 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -70,19 +70,19 @@ public: (*H).middleCols(transInterval.first, tDim) = R.matrix(); } - return newTrans.vector() - measured_.vector(); + return traits::Local(measured_, newTrans); } /** equals specialized to this factor */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(measured_, e->measured_, tol); } /** print contents */ void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s + "PoseTranslationPrior", keyFormatter); - measured_.print("Measured Translation"); + traits::Print(measured_, "Measured Translation"); } private: diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index ea8811d17..2ac217ac1 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -44,9 +44,9 @@ public: virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); - std::cout << "RotateFactor:" << std::endl; - p_.print("p"); - z_.print("z"); + std::cout << "RotateFactor:]\n"; + std::cout << "p: " << p_.transpose() << std::endl; + std::cout << "z: " << z_.transpose() << std::endl; } /// vector of errors returns 2D vector diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 66538e802..1aa85b6fe 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -87,7 +87,7 @@ public: Vector9 z; z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang - z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang + z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang if (H1) *H1 = numericalDerivative21( boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); if (H2) *H2 = numericalDerivative22( @@ -109,7 +109,7 @@ private: static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) { Vector9 hx; hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang - hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang + hx.tail(3).operator=(x1.translationIntegration(x2, dt)); // Strange syntax to work around ambiguous operator error with clang return hx; } }; diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index d9674b415..ed3797015 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -115,7 +115,7 @@ private: case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break; default: assert(false); break; } - return p2.vector() - hx.vector(); + return p2 - hx; } }; diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 1fb2cf39e..8b224f510 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -89,7 +89,8 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { TEST( DiscreteEulerPoincareHelicopter, evaluateError) { Vector Fu = computeFu(gamma2, u2); - Vector fGravity_g1 = zero(6); subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)).vector(), 3); // gravity force in g1 frame + Vector fGravity_g1 = zero(6); + subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)), 3); // gravity force in g1 frame Vector Fb = Fu+fGravity_g1; Vector dV = newtonEuler(V1_g1, Fb, Inertia); diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 713ea3054..e2f2a2a86 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -64,7 +64,7 @@ TEST(Similarity3, Constructors) { Similarity3 sim3_Construct1; Similarity3 sim3_Construct2(s); Similarity3 sim3_Construct3(R, P, s); - Similarity3 sim4_Construct4(R.matrix(), P.vector(), s); + Similarity3 sim4_Construct4(R.matrix(), P, s); } //****************************************************************************** diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index 6f39ce1b6..df1873765 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -58,15 +58,15 @@ namespace gtsam { virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "BiasedGPSFactor(" << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; - measured_.print(" measured: "); + << keyFormatter(this->key2()) << ")\n" + << " measured: " << measured_.transpose() << std::endl; this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ @@ -82,7 +82,7 @@ namespace gtsam { H2->resize(3,3); // jacobian wrt bias (*H2) << Matrix3::Identity(); } - return pose.translation().vector() + bias.vector() - measured_.vector(); + return pose.translation() + bias - measured_; } /** return the measured */ diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index d8fceeb68..30d3a216d 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -187,7 +187,7 @@ public: Vector GyroCorrected(Bias1.correctGyroscope(measurement_gyro_)); body_omega_body = body_R_sensor * GyroCorrected; Matrix body_omega_body__cross = skewSymmetric(body_omega_body); - body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation().vector(); + body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation(); } else { body_a_body = AccCorrected; } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 879e1e1dd..fdba78445 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -79,7 +79,7 @@ public: && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol) - && this->referencePoint_.equals(e->referencePoint_, tol); + && traits::Equals(this->referencePoint_, e->referencePoint_, tol); } Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const { diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index efe1df640..75535eebc 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -233,7 +233,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, ///* VADIM - START ************************************************************************* */ //Vector3 predictionRq(const Vector3 angles, const Vector3 q) { -// return (Rot3().RzRyRx(angles) * q).vector(); +// return (Rot3().RzRyRx(angles) * q); //} // //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) { @@ -435,7 +435,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -474,7 +474,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -512,7 +512,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector measurement_acc = Vector3(0.0, 0.0, 0.0 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -554,7 +554,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -605,7 +605,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity factor( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, diff --git a/tests/simulated3D.h b/tests/simulated3D.h index cf69a8fa3..84d9ec8cd 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -91,7 +91,7 @@ struct PointPrior3D: public NoiseModelFactor1 { */ Vector evaluateError(const Point3& x, boost::optional H = boost::none) const { - return prior(x, H).vector() - measured_.vector(); + return prior(x, H) - measured_; } }; @@ -122,7 +122,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2 { */ Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return mea(x1, x2, H1, H2).vector() - measured_.vector(); + return mea(x1, x2, H1, H2) - measured_; } }; diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 57f03cca6..bda23b3f6 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -139,7 +139,7 @@ TEST(ExpressionFactor, Unary) { typedef Eigen::Matrix Matrix93; Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { Vector9 v; - v << p.vector(), p.vector(), p.vector(); + v << p, p, p; if (H) *H << eye(3), eye(3), eye(3); return v; } From 1a69a71c2c228dc1ebbd6df7a12db66689546e01 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Feb 2016 00:15:49 -0800 Subject: [PATCH 11/20] Fixed print issues --- gtsam/geometry/Pose3.cpp | 6 ++++-- gtsam/geometry/tests/testPoint3.cpp | 2 ++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index ccf39f153..f1cb482bb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -100,7 +100,7 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, void Pose3::print(const string& s) const { cout << s; R_.print("R:\n"); - traits::Print(t_, "t: "); + cout << '[' << t_.x() << ", " << t_.y() << ", " << t_.z() << "]\';"; } /* ************************************************************************* */ @@ -394,7 +394,9 @@ boost::optional align(const vector& pairs) { /* ************************************************************************* */ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { - os << pose.rotation() << "\n" << pose.translation() << endl; + os << pose.rotation() << "\n"; + const Point3& t = pose.translation(); + os << '[' << t.x() << ", " << t.y() << ", " << t.z() << "]\';\n"; return os; } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index c7fb44716..a4e2b139e 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -84,12 +84,14 @@ TEST( Point3, dot) { } /* ************************************************************************* */ +#ifndef GTSAM_USE_VECTOR3_POINTS TEST( Point3, stream) { Point3 p(1, 2, -3); std::ostringstream os; os << p; EXPECT(os.str() == "[1, 2, -3]';"); } +#endif //************************************************************************* TEST (Point3, normalize) { From 8dd42c8ce5016e74f28bf2b0902966c1d3bf4b71 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Feb 2016 00:53:05 -0800 Subject: [PATCH 12/20] Fixed compilation error in deprecated path --- gtsam/geometry/Rot3.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 30385bd8c..057fdf558 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -456,7 +456,9 @@ namespace gtsam { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ +#ifndef GTSAM_USE_VECTOR3_POINTS static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); } +#endif static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } From 3a7c8542b0b10d01e60c3ae60662b411271584a5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Feb 2016 13:14:44 -0800 Subject: [PATCH 13/20] Fixed compile error --- matlab.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/matlab.h b/matlab.h index 4a9ac2309..521e7a469 100644 --- a/matlab.h +++ b/matlab.h @@ -103,7 +103,7 @@ Matrix extractPoint3(const Values& values) { Matrix result(points.size(), 3); size_t j = 0; BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); + result.row(j++) = key_value.value; return result; } @@ -131,7 +131,7 @@ Matrix extractPose3(const Values& values) { result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); - result.row(j).tail(3) = key_value.value.translation().vector(); + result.row(j).tail(3) = key_value.value.translation(); j++; } return result; From 52fff13b6fa802890d4e3b11f423c85f410e975c Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 17 Feb 2016 12:19:03 -0800 Subject: [PATCH 14/20] Added configuration variable for typedef only --- CMakeLists.txt | 8 +++++++- gtsam/config.h.in | 3 +++ gtsam/geometry/Point3.h | 2 +- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3123dbea2..836acde81 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,6 +66,7 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" ON) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) +option(GTSAM_USE_VECTOR3_POINTS "Symply typdef Point3 to eigen::Vector3d" OFF) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -85,7 +86,11 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY) endif() if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4) - message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.") + message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.") +endif() + +if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_USE_VECTOR3_POINTS) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_USE_VECTOR3_POINTS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.") endif() # Flags for choosing default packaging tools @@ -477,6 +482,7 @@ print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency chec print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") +print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") diff --git a/gtsam/config.h.in b/gtsam/config.h.in index a408b254c..cb195dc03 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -62,3 +62,6 @@ // Make sure dependent projects that want it can see deprecated functions #cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4 + +// Publish flag about Eigen typedef +#cmakedefine GTSAM_USE_VECTOR3_POINTS diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 524bb9bbf..9c660f083 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -21,6 +21,7 @@ #pragma once +#include #include #include #include @@ -28,7 +29,6 @@ namespace gtsam { -#define GTSAM_USE_VECTOR3_POINTS #ifdef GTSAM_USE_VECTOR3_POINTS /// As of GTSAM 4, in order to make GTSAM more lean, From a10f462fef3c279bbb807958d7224c8a4a84890b Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 17 Feb 2016 12:36:57 -0800 Subject: [PATCH 15/20] Fixed warning --- gtsam/linear/NoiseModel.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index cb77902d0..584f758a1 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -82,13 +82,13 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { size_t m = R.rows(), n = R.cols(); if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square"); - boost::optional diagonal = boost::none; - if (smart) - diagonal = checkIfDiagonal(R); - if (diagonal) - return Diagonal::Sigmas(diagonal->array().inverse(), true); - else - return shared_ptr(new Gaussian(R.rows(), R)); + if (smart) { + boost::optional diagonal = checkIfDiagonal(R); + if (diagonal) + return Diagonal::Sigmas(diagonal->array().inverse(), true); + } + // NOTE(frank): only reaches here if !smart && !diagonal + return shared_ptr(new Gaussian(R.rows(), R)); } /* ************************************************************************* */ From 7bcdcbd8053609174d3100e7128631dba9714a5b Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 17 Feb 2016 16:05:50 -0800 Subject: [PATCH 16/20] Fixed compilation problems (and used some c++11 for loops) --- matlab.h | 28 +++++++++++++--------------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/matlab.h b/matlab.h index 521e7a469..72889dc4b 100644 --- a/matlab.h +++ b/matlab.h @@ -28,8 +28,6 @@ #include #include -#include - #include namespace gtsam { @@ -92,7 +90,7 @@ Matrix extractPoint2(const Values& values) { size_t j = 0; Values::ConstFiltered points = values.filter(); Matrix result(points.size(), 2); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + for(const auto& key_value: points) result.row(j++) = key_value.value.vector(); return result; } @@ -102,7 +100,7 @@ Matrix extractPoint3(const Values& values) { Values::ConstFiltered points = values.filter(); Matrix result(points.size(), 3); size_t j = 0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + for(const auto& key_value: points) result.row(j++) = key_value.value; return result; } @@ -112,7 +110,7 @@ Matrix extractPose2(const Values& values) { Values::ConstFiltered poses = values.filter(); Matrix result(poses.size(), 3); size_t j = 0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) + for(const auto& key_value: poses) result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); return result; } @@ -127,7 +125,7 @@ Matrix extractPose3(const Values& values) { Values::ConstFiltered poses = values.filter(); Matrix result(poses.size(), 12); size_t j = 0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { + for(const auto& key_value: poses) { result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); @@ -142,8 +140,8 @@ void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, sigma); Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value + Point2(sampler.sample())); + for(const auto& key_value: values.filter()) { + values.update(key_value.key, key_value.value + Point2(sampler.sample())); } } @@ -153,8 +151,8 @@ void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( Vector3(sigmaT, sigmaT, sigmaR)); Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); + for(const auto& key_value: values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); } } @@ -163,8 +161,8 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, sigma); Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value + Point3(sampler.sample())); + for(const auto& key_value: values.filter()) { + values.update(key_value.key, key_value.value + Point3(sampler.sample())); } } @@ -204,13 +202,13 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { // first count size_t K = 0, k = 0; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) + for(const NonlinearFactor::shared_ptr& f: graph) if (boost::dynamic_pointer_cast >( f)) ++K; // now fill Matrix errors(2, K); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { + for(const NonlinearFactor::shared_ptr& f: graph) { boost::shared_ptr > p = boost::dynamic_pointer_cast >( f); @@ -232,7 +230,7 @@ Values localToWorld(const Values& local, const Pose2& base, keys = local.keys(); // Loop over all keys - BOOST_FOREACH(Key key, keys) { + for(Key key: keys) { try { // if value is a Pose2, compose it with base pose Pose2 pose = local.at(key); From df4efbf2d761f145a9f4cc8e6803a1220639034b Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 17 Feb 2016 17:42:20 -0800 Subject: [PATCH 17/20] Fixed python wrapper issue --- python/handwritten/geometry/Rot3.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp index f4b868b50..685a37ca9 100644 --- a/python/handwritten/geometry/Rot3.cpp +++ b/python/handwritten/geometry/Rot3.cpp @@ -39,9 +39,6 @@ static Rot3 Quaternion_1(double w, double x, double y, double z) // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html gtsam::Rot3 (*AxisAngle_0)(const gtsam::Point3&, double) = &Rot3::AxisAngle; gtsam::Rot3 (*AxisAngle_1)(const gtsam::Unit3&, double) = &Rot3::AxisAngle; -#ifndef GTSAM_USE_VECTOR3_POINTS -gtsam::Rot3 (*AxisAngle_2)(const Vector3&, double) = &Rot3::AxisAngle; -#endif gtsam::Rot3 (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues; gtsam::Rot3 (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues; gtsam::Rot3 (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx; @@ -70,12 +67,8 @@ void exportRot3(){ .staticmethod("Logmap") .def("LogmapDerivative", &Rot3::LogmapDerivative) .staticmethod("LogmapDerivative") -#ifdef GTSAM_USE_VECTOR3_POINTS .def("AxisAngle", AxisAngle_0) .def("AxisAngle", AxisAngle_1) -#else - .def("AxisAngle", AxisAngle_2) -#endif .def("Rodrigues", Rodrigues_0) .def("Rodrigues", Rodrigues_1) .staticmethod("Rodrigues") From a9b0d81be47f28d871e7f648700d4af0012af4b4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 17 Feb 2016 18:56:44 -0800 Subject: [PATCH 18/20] Yet another overload fix in python wrapper --- gtsam/geometry/Point3.h | 9 +++++++++ python/handwritten/geometry/Point3.cpp | 6 +++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 9c660f083..8b31316c7 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -109,6 +109,15 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// return as Vector3 const Vector3& vector() const { return *this; } + /// get x + inline double x() const {return (*this)[0];} + + /// get y + inline double y() const {return (*this)[1];} + + /// get z + inline double z() const {return (*this)[2];} + /// @} /// Output stream operator diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index f83b0c516..c87421075 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -43,13 +43,13 @@ class_("Point3") .def("norm", &Point3::norm) .def("normalized", &Point3::normalized) .def("print", &Point3::print, print_overloads(args("s"))) -#ifndef GTSAM_USE_VECTOR3_POINTS +#ifdef GTSAM_USE_VECTOR3_POINTS + .def("vector", &Point3::vector, return_value_policy()) +#else .def("vector", &Point3::vector) .def("x", &Point3::x) .def("y", &Point3::y) .def("z", &Point3::z) -#else - .def("vector", &Point3::vector, return_value_policy()) #endif .def(self * other()) .def(other() * self) From 3a50a0e19eda38ce41837d8f787517c2597e67b8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 18 Feb 2016 08:50:11 -0800 Subject: [PATCH 19/20] spurious vector --- gtsam/navigation/tests/testPoseVelocityBias.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testPoseVelocityBias.cpp b/gtsam/navigation/tests/testPoseVelocityBias.cpp index cc2a47498..0b897bc6e 100644 --- a/gtsam/navigation/tests/testPoseVelocityBias.cpp +++ b/gtsam/navigation/tests/testPoseVelocityBias.cpp @@ -31,7 +31,7 @@ using namespace gtsam; Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) { Matrix3 R1 = pvb1.pose.rotation().matrix(); // Ri.transpose() translate the error from the global frame into pose1's frame - const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()).vector(); + const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()); const Vector3 fv = R1.transpose() * (pvb2.velocity - pvb1.velocity); const Rot3 R1BetweenR2 = pvb1.pose.rotation().between(pvb2.pose.rotation()); const Vector3 fR = Rot3::Logmap(R1BetweenR2); From 6d4cf0c1156c8fc1990dc76b52b5ecd2d388b914 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 18 Feb 2016 09:55:59 -0800 Subject: [PATCH 20/20] Fix compile issue --- python/handwritten/geometry/Point3.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index c87421075..57f0fb75e 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -43,10 +43,8 @@ class_("Point3") .def("norm", &Point3::norm) .def("normalized", &Point3::normalized) .def("print", &Point3::print, print_overloads(args("s"))) -#ifdef GTSAM_USE_VECTOR3_POINTS +#ifndef GTSAM_USE_VECTOR3_POINTS .def("vector", &Point3::vector, return_value_policy()) -#else - .def("vector", &Point3::vector) .def("x", &Point3::x) .def("y", &Point3::y) .def("z", &Point3::z)