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")