Using traits in many places (forced by loss of Point2 mojo)

gtsam/nonlinear/ExtendedKalmanFilter-inl.h

@@ -42,7 +42,8 @@ namespace gtsam {
 
     // Extract the current estimate of x1,P1
     VectorValues result = marginal->solve(VectorValues());
-    T x = linearizationPoints.at<T>(lastKey).retract(result[lastKey]);
+    const T& current = linearizationPoints.at<T>(lastKey);
+    T x = traits_x<T>::Retract(current, result[lastKey]);
 
     // Create a Jacobian Factor from the root node of the produced Bayes Net.
     // This will act as a prior for the next iteration.
@@ -70,9 +71,10 @@ namespace gtsam {
     // Create a Jacobian Prior Factor directly P_initial.
     // Since x0 is set to the provided mean, the b vector in the prior will be zero
     // TODO Frank asks: is there a reason why noiseModel is not simply P_initial ?
+    int n = traits_x<T>::GetDimension(x_initial);
     priorFactor_ = JacobianFactor::shared_ptr(
-        new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(x_initial.dim()),
+        new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n),
-            noiseModel::Unit::Create(P_initial->dim())));
+            noiseModel::Unit::Create(n)));
   }
 
   /* ************************************************************************* */

gtsam/nonlinear/ExtendedKalmanFilter.h

@@ -42,6 +42,11 @@ namespace gtsam {
 
   template<class VALUE>
   class ExtendedKalmanFilter {
+
+    // Check that VALUE type is a testable Manifold
+    BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
+    BOOST_CONCEPT_ASSERT((IsManifold<VALUE>));
+
   public:
 
     typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;

gtsam/nonlinear/ISAM2-inl.h

@@ -29,7 +29,7 @@ namespace gtsam {
 template<class VALUE>
 VALUE ISAM2::calculateEstimate(Key key) const {
   const Vector& delta = getDelta()[key];
-  return theta_.at<VALUE>(key).retract(delta);
+  return traits_x<VALUE>::Retract(theta_.at<VALUE>(key), delta);
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/NonlinearEquality.h

@@ -23,6 +23,7 @@
 
 #include <limits>
 #include <iostream>
+#include <cmath>
 
 namespace gtsam {
 
@@ -101,9 +102,9 @@ public:
    */
   NonlinearEquality(Key j, const T& feasible, double error_gain,
       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_(true), error_gain_(error_gain), compare_(
+          j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
-          _compare) {
+      compare_(_compare) {
   }
 
   /// @}
@@ -114,14 +115,14 @@ public:
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
     gtsam::print(feasible_, "Feasible Point:\n");
-    std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
+    std::cout << "Variable Dimension: " << traits_x<T>::GetDimension(feasible_) << std::endl;
   }
 
   /** Check if two factors are equal */
   virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
     const This* e = dynamic_cast<const This*>(&f);
     return e && Base::equals(f) && feasible_.equals(e->feasible_, tol)
-        && fabs(error_gain_ - e->error_gain_) < tol;
+        && std::abs(error_gain_ - e->error_gain_) < tol;
   }
 
   /// @}
@@ -142,11 +143,11 @@ public:
   /** error function */
   Vector evaluateError(const T& xj,
       boost::optional<Matrix&> H = boost::none) const {
-    size_t nj = feasible_.dim();
+    const size_t nj = traits_x<T>::GetDimension(feasible_);
     if (allow_error_) {
       if (H)
         *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
-      return xj.localCoordinates(feasible_);
+      return traits_x<T>::Local(xj,feasible_);
     } else if (compare_(feasible_, xj)) {
       if (H)
         *H = eye(nj);
@@ -235,8 +236,8 @@ public:
    *
    */
   NonlinearEquality1(const X& value, Key key, double mu = 1000.0) :
-      Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key), value_(
+      Base( noiseModel::Constrained::All(traits_x<X>::GetDimension(value),
-          value) {
+              std::abs(mu)), key), value_(value) {
   }
 
   virtual ~NonlinearEquality1() {
@@ -252,9 +253,9 @@ public:
   Vector evaluateError(const X& x1,
       boost::optional<Matrix&> H = boost::none) const {
     if (H)
-      (*H) = eye(x1.dim());
+      (*H) = eye(traits_x<X>::GetDimension(x1));
     // manifold equivalent of h(x)-z -> log(z,h(x))
-    return value_.localCoordinates(x1);
+    return traits_x<X>::Local(value_,x1);
   }
 
   /** Print */
@@ -310,7 +311,7 @@ public:
 
   ///TODO: comment
   NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) :
-      Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {
+      Base(noiseModel::Constrained::All(traits_x<X>::dimension, std::abs(mu)), key1, key2) {
   }
   virtual ~NonlinearEquality2() {
   }
@@ -324,12 +325,10 @@ public:
   /** g(x) with optional derivative2 */
   Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
       boost::none, boost::optional<Matrix&> H2 = boost::none) const {
-    const size_t p = X::Dim();
+    static const size_t p = traits_x<X>::dimension;
-    if (H1)
+    if (H1) *H1 = -eye(p);
-      *H1 = -eye(p);
+    if (H2) *H2 =  eye(p);
-    if (H2)
+    return traits_x<X>::Local(x1,x2);
-      *H2 = eye(p);
-    return x1.localCoordinates(x2);
   }
 
 private:

gtsam/slam/PoseTranslationPrior.h

@@ -62,7 +62,8 @@ public:
   Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const {
     const Translation& newTrans = pose.translation();
     const Rotation& R = pose.rotation();
-    const size_t tDim = newTrans.dim(), xDim = pose.dim();
+    const int tDim = traits_x<Translation>::GetDimension(newTrans);
+    const int xDim = traits_x<Pose>::GetDimension(pose);
     if (H) {
       *H = gtsam::zeros(tDim, xDim);
       std::pair<size_t, size_t> transInterval = POSE::translationInterval();

gtsam/slam/ReferenceFrameFactor.h

@@ -84,7 +84,7 @@ public:
    * each degree of freedom.
    */
   ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2)
-  : Base(noiseModel::Isotropic::Sigma(POINT().dim(), sigma),
+  : Base(noiseModel::Isotropic::Sigma(POINT::dimension, sigma),
       globalKey, transKey, localKey) {}
 
   virtual ~ReferenceFrameFactor(){}
@@ -99,11 +99,9 @@ public:
         boost::optional<Matrix&> Dtrans = boost::none,
         boost::optional<Matrix&> Dlocal = boost::none) const  {
     Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
-    if (Dlocal) {
+    if (Dlocal)
-      Point dummy;
+      *Dlocal = -1* gtsam::eye(Point::dimension);
-      *Dlocal = -1* gtsam::eye(dummy.dim());
+    return traits_x<Point>::Local(local,newlocal);
-    }
-    return local.localCoordinates(newlocal);
   }
 
   virtual void print(const std::string& s="",

gtsam_unstable/geometry/SimWall2D.cpp

@@ -156,7 +156,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
 
     // Simple check to make sure norm is on side closest robot
     if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm))
-      norm = norm.inverse();
+      norm = - norm;
 
     // using the reflection
     const double inside_bias = 0.05;

gtsam_unstable/nonlinear/BatchFixedLagSmoother.h

@@ -67,7 +67,7 @@ public:
   template<class VALUE>
   VALUE calculateEstimate(Key key) const {
     const Vector delta = delta_.at(key);
-    return theta_.at<VALUE>(key).retract(delta);
+    return traits_x<VALUE>::Retract(theta_.at<VALUE>(key), delta);
   }
 
   /** read the current set of optimizer parameters */

gtsam_unstable/slam/GaussMarkov1stOrderFactor.h

@@ -91,8 +91,8 @@ public:
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none) const {
 
-    Vector v1( VALUE::Logmap(p1) );
+    Vector v1( traits_x<VALUE>::Logmap(p1) );
-    Vector v2( VALUE::Logmap(p2) );
+    Vector v2( traits_x<VALUE>::Logmap(p2) );
 
     Vector alpha(tau_.size());
     Vector alpha_v1(tau_.size());

gtsam_unstable/slam/TSAMFactors.h

@@ -49,7 +49,7 @@ public:
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
           boost::none) const {
     Point2 d = pose.transform_to(point, H1, H2);
-    Point2 e = measured_.between(d);
+    Point2 e = d - measured_;
     return e.vector();
   }
 };
@@ -99,12 +99,12 @@ public:
         *H3 = D_e_point_g * D_point_g_base2;
       if (H4)
         *H4 = D_e_point_g * D_point_g_point;
-      return measured_.localCoordinates(d);
+      return (d - measured_).vector();
     } else {
       Pose2 pose_g = base1.compose(pose);
       Point2 point_g = base2.transform_from(point);
       Point2 d = pose_g.transform_to(point_g);
-      return measured_.localCoordinates(d);
+      return (d - measured_).vector();
     }
   }
 };

tests/simulated2DConstraints.h

@@ -90,11 +90,11 @@ namespace simulated2D {
       virtual double value(const Point& x, boost::optional<Matrix&> H =
           boost::none) const {
         if (H) {
-          Matrix D = zeros(1, x.dim());
+          Matrix D = zeros(1, traits_x<Point>::GetDimension(x));
           D(0, IDX) = 1.0;
           *H = D;
         }
-        return Point::Logmap(x)(IDX);
+        return traits_x<Point>::Logmap(x)(IDX);
       }
     };
 

tests/testExtendedKalmanFilter.cpp

@@ -92,10 +92,7 @@ TEST( ExtendedKalmanFilter, linear ) {
 
 // Create Motion Model Factor
 class NonlinearMotionModel : public NoiseModelFactor2<Point2,Point2> {
-public:
-  typedef Point2 T;
 
-private:
   typedef NoiseModelFactor2<Point2, Point2> Base;
   typedef NonlinearMotionModel This;
 
@@ -126,19 +123,19 @@ public:
   virtual ~NonlinearMotionModel() {}
 
   // Calculate the next state prediction using the nonlinear function f()
-  T f(const T& x_t0) const {
+  Point2 f(const Point2& x_t0) const {
 
     // Calculate f(x)
     double x = x_t0.x() * x_t0.x();
     double y = x_t0.x() * x_t0.y();
-    T x_t1(x,y);
+    Point2 x_t1(x,y);
 
     // In this example, the noiseModel remains constant
     return x_t1;
   }
 
   // Calculate the Jacobian of the nonlinear function f()
-  Matrix F(const T& x_t0) const {
+  Matrix F(const Point2& x_t0) const {
     // Calculate Jacobian of f()
     Matrix F = Matrix(2,2);
     F(0,0) = 2*x_t0.x();
@@ -150,7 +147,7 @@ public:
   }
 
   // Calculate the inverse square root of the motion model covariance, Q
-  Matrix QInvSqrt(const T& x_t0) const {
+  Matrix QInvSqrt(const Point2& x_t0) const {
     // This motion model has a constant covariance
     return Q_invsqrt_;
   }
@@ -184,7 +181,7 @@ public:
 
   /** Vector of errors, whitened ! */
   Vector whitenedError(const Values& c) const {
-    return QInvSqrt(c.at<T>(key1()))*unwhitenedError(c);
+    return QInvSqrt(c.at<Point2>(key1()))*unwhitenedError(c);
   }
 
   /**
@@ -213,35 +210,29 @@ public:
   }
 
   /** vector of errors */
-  Vector evaluateError(const T& p1, const T& p2,
+  Vector evaluateError(const Point2& p1, const Point2& p2,
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
           boost::none) const {
 
     // error = p2 - f(p1)
     // H1 = d error / d p1 = -d f/ d p1 = -F
     // H2 = d error / d p2 = I
-    T prediction = f(p1);
+    Point2 prediction = f(p1);
 
-    if(H1){
+    if(H1)
       *H1 = -F(p1);
-    }
 
-    if(H2){
+    if(H2)
       *H2 = eye(dim());
-    }
 
     // Return the error between the prediction and the supplied value of p2
-    return prediction.localCoordinates(p2);
+    return (p2 - prediction).vector();
   }
 
 };
 
 // Create Measurement Model Factor
 class NonlinearMeasurementModel : public NoiseModelFactor1<Point2> {
-public:
-  typedef Point2 T;
-
-private:
 
   typedef NoiseModelFactor1<Point2> Base;
   typedef NonlinearMeasurementModel This;
@@ -268,7 +259,7 @@ public:
 
   // Calculate the predicted measurement using the nonlinear function h()
   // Byproduct: updates Jacobian H, and noiseModel (R)
-  Vector h(const T& x_t1) const {
+  Vector h(const Point2& x_t1) const {
 
     // Calculate prediction
     Vector z_hat(1);
@@ -277,7 +268,7 @@ public:
     return z_hat;
   }
 
-  Matrix H(const T& x_t1) const {
+  Matrix H(const Point2& x_t1) const {
     // Update Jacobian
     Matrix H(1,2);
     H(0,0) =  4*x_t1.x() - x_t1.y() - 2.5;
@@ -287,7 +278,7 @@ public:
   }
 
   // Calculate the inverse square root of the motion model covariance, Q
-  Matrix RInvSqrt(const T& x_t0) const {
+  Matrix RInvSqrt(const Point2& x_t0) const {
     // This motion model has a constant covariance
     return R_invsqrt_;
   }
@@ -320,7 +311,7 @@ public:
 
   /** Vector of errors, whitened ! */
   Vector whitenedError(const Values& c) const {
-    return RInvSqrt(c.at<T>(key()))*unwhitenedError(c);
+    return RInvSqrt(c.at<Point2>(key()))*unwhitenedError(c);
   }
 
   /**
@@ -345,7 +336,7 @@ public:
   }
 
   /** vector of errors */
-  Vector evaluateError(const T& p, boost::optional<Matrix&> H1 = boost::none) const {
+  Vector evaluateError(const Point2& p, boost::optional<Matrix&> H1 = boost::none) const {
     // error = z - h(p)
     // H = d error / d p = -d h/ d p = -H
     Vector z_hat = h(p);

tests/testNonlinearEquality.cpp

@@ -408,8 +408,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
 
   // odometry constraint
   eq2D::OdoEqualityConstraint::shared_ptr constraint2(
-      new eq2D::OdoEqualityConstraint(truth_pt1.between(truth_pt2), key1,
+      new eq2D::OdoEqualityConstraint(truth_pt2-truth_pt1, key1, key2));
-          key2));
 
   NonlinearFactorGraph graph;
   graph.push_back(constraint1);