Point3 loses its mojo (superfluous Lie/Manifold stuff)

gtsam/geometry/Point3.h

@@ -76,100 +76,17 @@ namespace gtsam {
     /// @{
 
     /// identity for group operation
-    inline static Point3 identity() {
+    inline static Point3 identity() { return Point3();}
-      return Point3();
-    }
 
-    /// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3()
+    /// inverse
-    inline Point3 inverse(OptionalJacobian<3,3> H=boost::none) const {
-      if (H) *H = -I_3x3;
-      return Point3(-x_, -y_, -z_);
-    }
-
-    /// syntactic sugar for inverse, i.e., -p == inverse(p)
     Point3 operator - () const { return Point3(-x_,-y_,-z_);}
 
-    /// "Compose" - just adds coordinates of two points
+    /// add
-    inline Point3 compose(const Point3& p2,
-        OptionalJacobian<3,3> H1=boost::none,
-        OptionalJacobian<3,3> H2=boost::none) const {
-      if (H1) *H1 << I_3x3;
-      if (H2) *H2 << I_3x3;
-      return *this + p2;
-    }
-
-    ///syntactic sugar for adding two points, i.e., p+q == compose(p,q)
     Point3 operator + (const Point3& q) const;
 
-    /** Between using the default implementation */
+    /// subtract
-    inline Point3 between(const Point3& p2,
-        OptionalJacobian<3,3> H1=boost::none,
-        OptionalJacobian<3,3> H2=boost::none) const {
-      if(H1) *H1 = -I_3x3;
-      if(H2) *H2 = I_3x3;
-      return p2 - *this;
-    }
-
-    /// syntactic sugar for subtracting points, i.e., q-p == between(p,q)
     Point3 operator - (const Point3& q) const;
 
-    /// @}
-    /// @name Manifold
-    /// @{
-
-    /// dimension of the variable - used to autodetect sizes
-    inline static size_t Dim() { return 3; }
-
-    /// return dimensionality of tangent space, DOF = 3
-    inline size_t dim() const { return 3; }
-
-    /// Updates a with tangent space delta
-    inline Point3 retract(const Vector& v) const { return *this + Point3(v); }
-
-    /// Returns inverse retraction
-    inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); }
-
-    /// @}
-    /// @name Lie Group
-    /// @{
-
-    /** Exponential map at identity - just create a Point3 from x,y,z */
-    static inline Point3 Expmap(const Vector& v, OptionalJacobian<3,3> H=boost::none) {
-      if (H) *H = I_3x3;
-      return Point3(v);
-    }
-
-    /** Log map at identity - return the x,y,z of this point */
-    static inline Vector3 Logmap(const Point3& dp, OptionalJacobian<3,3> H=boost::none) {
-      if (H) *H = I_3x3;
-      return Vector3(dp.x(), dp.y(), dp.z());
-    }
-
-    inline Point3 retract(const Vector& v, OptionalJacobian<3, 3> H1,
-        OptionalJacobian<3, 3> H2 = boost::none) const {
-      if (H1) *H1 = I_3x3;
-      if (H2) *H2 = I_3x3;
-      return *this + Point3(v);
-    }
-
-    inline Vector3 localCoordinates(const Point3& q, OptionalJacobian<3, 3> H1,
-        OptionalJacobian<3, 3> H2 = boost::none) const {
-      if (H1) *H1 = - I_3x3;
-      if (H2) *H2 = I_3x3;
-      Point3 dp = q - *this;
-      return Vector3(dp.x(), dp.y(), dp.z());
-    }
-
-    /// Left-trivialized derivative of the exponential map
-    static Matrix3 dexpL(const Vector& v) {
-      return I_3x3;
-    }
-
-    /// Left-trivialized derivative inverse of the exponential map
-    static Matrix3 dexpInvL(const Vector& v) {
-      return I_3x3;
-    }
-
     /// @}
     /// @name Vector Space
     /// @{

gtsam/geometry/Unit3.cpp

@@ -131,7 +131,7 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
 Unit3 Unit3::retract(const Vector2& v) const {
 
   // Get the vector form of the point and the basis matrix
-  Vector3 p = Point3::Logmap(p_);
+  Vector3 p = p_.vector();
   Matrix32 B = basis();
 
   // Compute the 3D xi_hat vector
@@ -156,8 +156,7 @@ Unit3 Unit3::retract(const Vector2& v) const {
 /* ************************************************************************* */
 Vector2 Unit3::localCoordinates(const Unit3& y) const {
 
-  Vector3 p = Point3::Logmap(p_);
+  Vector3 p = p_.vector(), q = y.p_.vector();
-  Vector3 q = Point3::Logmap(y.p_);
   double dot = p.dot(q);
 
   // Check for special cases

gtsam/geometry/tests/testPoint3.cpp

@@ -26,22 +26,36 @@ GTSAM_CONCEPT_LIE_INST(Point3)
 
 static Point3 P(0.2, 0.7, -2);
 
+//******************************************************************************
+TEST(Point3 , Concept) {
+  BOOST_CONCEPT_ASSERT((IsGroup<Point3>));
+  BOOST_CONCEPT_ASSERT((IsManifold<Point3>));
+  BOOST_CONCEPT_ASSERT((IsVectorSpace<Point3>));
+}
+
+//******************************************************************************
+TEST(Point3 , Invariants) {
+  Point3 p1(1, 2, 3), p2(4, 5, 6);
+  check_group_invariants(p1, p2);
+  check_manifold_invariants(p1, p2);
+}
+
 /* ************************************************************************* */
 TEST(Point3, Lie) {
   Point3 p1(1, 2, 3);
   Point3 p2(4, 5, 6);
   Matrix H1, H2;
 
-  EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2)));
+  EXPECT(assert_equal(Point3(5, 7, 9), traits_x<Point3>::Compose(p1, p2, H1, H2)));
   EXPECT(assert_equal(eye(3), H1));
   EXPECT(assert_equal(eye(3), H2));
 
-  EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2)));
+  EXPECT(assert_equal(Point3(3, 3, 3), traits_x<Point3>::Between(p1, p2, H1, H2)));
   EXPECT(assert_equal(-eye(3), H1));
   EXPECT(assert_equal(eye(3), H2));
 
-  EXPECT(assert_equal(Point3(5, 7, 9), p1.retract(Vector3(4., 5., 6.))));
+  EXPECT(assert_equal(Point3(5, 7, 9), traits_x<Point3>::Retract(p1, Vector3(4,5,6))));
-  EXPECT(assert_equal((Vector)Vector3(3.,3.,3.), p1.localCoordinates(p2)));
+  EXPECT(assert_equal(Vector3(3, 3, 3), traits_x<Point3>::Local(p1,p2)));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPose3.cpp

@@ -151,11 +151,11 @@ Pose3 Agrawal06iros(const Vector& xi) {
   Vector v = xi.tail(3);
   double t = norm_2(w);
   if (t < 1e-5)
-    return Pose3(Rot3(), Point3::Expmap(v));
+    return Pose3(Rot3(), Point3(v));
   else {
     Matrix W = skewSymmetric(w/t);
     Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
-    return Pose3(Rot3::Expmap (w), Point3::Expmap(A * v));
+    return Pose3(Rot3::Expmap (w), Point3(A * v));
   }
 }
 

gtsam/navigation/GPSFactor.cpp

@@ -24,15 +24,15 @@ namespace gtsam {
 
 //***************************************************************************
 void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
-  cout << s << "GPSFactor on " << keyFormatter(this->key()) << "\n";
+  cout << s << "GPSFactor on " << keyFormatter(key()) << "\n";
   nT_.print("  prior mean: ");
-  this->noiseModel_->print("  noise model: ");
+  noiseModel_->print("  noise model: ");
 }
 
 //***************************************************************************
 bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && this->nT_.equals(e->nT_, tol);
+  return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol);
 }
 
 //***************************************************************************
@@ -43,8 +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();
   }
-  // manifold equivalent of h(x)-z -> log(z,h(x))
+  return (p.translation() -nT_).vector();
-  return nT_.localCoordinates(p.translation());
 }
 
 //***************************************************************************

gtsam/nonlinear/NonlinearEquality.h

@@ -93,8 +93,9 @@ public:
    */
   NonlinearEquality(Key j, const T& feasible,
       bool (*_compare)(const T&, const T&) = compare<T>) :
-      Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(
+      Base(noiseModel::Constrained::All(traits_x<T>::GetDimension(feasible)),
-          feasible), allow_error_(false), error_gain_(0.0), compare_(_compare) {
+          j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
+      compare_(_compare) {
   }
 
   /**

gtsam_unstable/dynamics/PoseRTV.cpp

@@ -59,14 +59,14 @@ void PoseRTV::print(const string& s) const {
 /* ************************************************************************* */
 PoseRTV PoseRTV::Expmap(const Vector9& v) {
   Pose3 newPose = Pose3::Expmap(v.head<6>());
-  Velocity3 newVel = Velocity3::Expmap(v.tail<3>());
+  Velocity3 newVel = Velocity3(v.tail<3>());
   return PoseRTV(newPose, newVel);
 }
 
 /* ************************************************************************* */
 Vector9 PoseRTV::Logmap(const PoseRTV& p) {
   Vector6 Lx = Pose3::Logmap(p.Rt_);
-  Vector3 Lv = Velocity3::Logmap(p.v_);
+  Vector3 Lv = p.v_.vector();
   return (Vector9() << Lx, Lv).finished();
 }
 
@@ -108,7 +108,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1,
 PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
 PoseRTV PoseRTV::inverse(OptionalJacobian<dimension,dimension> H1) const {
   if (H1) *H1 = numericalDerivative11<PoseRTV,PoseRTV>(inverse_, *this, 1e-5);
-  return PoseRTV(Rt_.inverse(), v_.inverse());
+  return PoseRTV(Rt_.inverse(), - v_);
 }
 
 /* ************************************************************************* */
@@ -118,7 +118,7 @@ PoseRTV PoseRTV::compose(const PoseRTV& p,
     OptionalJacobian<dimension,dimension> H2) const {
   if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5);
   if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5);
-  return PoseRTV(Rt_.compose(p.Rt_), v_.compose(p.v_));
+  return PoseRTV(Rt_.compose(p.Rt_), v_+p.v_);
 }
 
 /* ************************************************************************* */
@@ -196,7 +196,7 @@ PoseRTV PoseRTV::generalDynamics(
   Rot3 r2 = rotation().retract(gyro * dt);
 
   //  Integrate Velocity Equations
-  Velocity3 v2 = v_.compose(Velocity3(dt * (r2.matrix() * accel + g)));
+  Velocity3 v2 = v_ + (Velocity3(dt * (r2.matrix() * accel + g)));
 
   //  Integrate Position Equations
   Point3 t2 = translationIntegration(r2, v2, dt);
@@ -213,15 +213,15 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
   Vector6 imu;
 
   // acceleration
-  Vector accel = v1.localCoordinates(v2) / dt;
+  Vector3 accel = (v2-v1).vector() / dt;
   imu.head<3>() = r2.transpose() * (accel - g);
 
   // rotation rates
   // just using euler angles based on matlab code
   // FIXME: this is silly - we shouldn't use differences in Euler angles
   Matrix Enb = RRTMnb(r1);
-  Vector euler1 = r1.xyz(), euler2 = r2.xyz();
+  Vector3 euler1 = r1.xyz(), euler2 = r2.xyz();
-  Vector dR = euler2 - euler1;
+  Vector3 dR = euler2 - euler1;
 
   // normalize yaw in difference (as per Mitch's code)
   dR(2) = Rot2::fromAngle(dR(2)).theta();

gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp

@@ -218,7 +218,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   Vector3 Vel1(Vector3(0.5, -0.5, 0.4));
   Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888,
       -0.422594037, -0.636011287, 0.731761397, 0.244979388);
-  Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt));
+  Point3 t2 = t1 + Point3(Vel1 * measurement_dt);
   Pose3 Pose2(R2, t2);
   Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
   Vector3 Vel2 = Vel1 + dv;
@@ -568,7 +568,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   Vector3 Vel1(Vector3(0.5, -0.5, 0.4));
   Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888,
       -0.422594037, -0.636011287, 0.731761397, 0.244979388);
-  Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt));
+  Point3 t2 = t1+ Point3(Vel1 * measurement_dt);
   Pose3 Pose2(R2, t2);
   Vector dv =
       measurement_dt