Now allows for a flag to compile Point3 as derived from Vector3

gtsam/geometry/Point3.cpp

@@ -15,25 +15,25 @@
  */
 
 #include <gtsam/geometry/Point3.h>
+#include <cmath>
 
 using namespace std;
 
 namespace gtsam {
 
 /* ************************************************************************* */
-bool Point3::equals(const Point3 & q, double tol) const {
+bool Point3::equals(const Point3 &q, double tol) const {
-  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol
+  return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol &&
-      && fabs(z_ - q.z()) < 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_,
+  return Point3(y() * q.z() - z() * q.y(), z() * q.x() - x() * q.z(),
-      x_ * q.y_ - y_ * q.x_);
+      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 x2 = x() * x(), y2 = y() * y(), z2 = z() * z();
-    double xy = x_ * y_, xz = x_ * z_, yz = y_ * 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;
 }

gtsam/geometry/Point3.h

@@ -22,18 +22,104 @@
 #pragma once
 
 #include <gtsam/base/VectorSpace.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/dllexport.h>
 #include <boost/serialization/nvp.hpp>
-#include <cmath>
 
 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<typename Derived>
+    inline Point3(const Eigen::MatrixBase<Derived>& 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<class ARCHIVE>
+    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<Point3, Point3> 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<Point3> : public internal::VectorSpace<Point3> {};
 

gtsam/geometry/Pose3.cpp

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

gtsam/geometry/Pose3.h

@@ -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);

gtsam/geometry/Rot3.h

@@ -93,7 +93,7 @@ namespace gtsam {
      * See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top
      */
     template<typename Derived>
-    inline Rot3(const Eigen::MatrixBase<Derived>& R) {
+    inline explicit Rot3(const Eigen::MatrixBase<Derived>& 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<double>(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<double>(angle, axis.vector()));
+#else
+      return Rot3(SO3::AxisAngle(axis.vector(),angle));
+#endif
     }
 
     /**
@@ -315,7 +321,7 @@ namespace gtsam {
 #ifdef GTSAM_USE_QUATERNIONS
       return traits<gtsam::Quaternion>::Expmap(v);
 #else
-      return traits<SO3>::Expmap(v);
+      return Rot3(traits<SO3>::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

gtsam/geometry/Unit3.cpp

@@ -35,6 +35,7 @@
 #include <boost/random/variate_generator.hpp>
 #include <iostream>
 #include <limits>
+#include <cmath>
 
 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<double>::epsilon()) {
-    return Unit3(cos(theta) * p_ + xi_hat);
+    return Unit3(Vector3(std::cos(theta) * p_ + xi_hat));
   }
 
-  Vector3 exp_p_xi_hat =
+  Vector3 exp_p_xi_hat = std::cos(theta) * p_ + xi_hat * (sin(theta) / theta);
-      cos(theta) * p_ + xi_hat * (sin(theta) / theta);
   return Unit3(exp_p_xi_hat);
 }
 

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

python/handwritten/geometry/Point3.cpp

@@ -43,10 +43,14 @@ class_<Point3>("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<copy_const_reference>())
+#endif
   .def(self * other<double>())
   .def(other<double>() * self)
   .def(self + self)

python/handwritten/geometry/Rot3.cpp

@@ -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_0)(const gtsam::Point3&, double) = &Rot3::AxisAngle;
-gtsam::Rot3  (*AxisAngle_1)(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")