more fixing of Lie*

gtsam/base/LieMatrix.h

@@ -44,6 +44,9 @@ struct LieMatrix : public Matrix {
   /** initialize from a normal matrix */
   LieMatrix(const Matrix& v) : Matrix(v) {}
 
+  template <class M>
+  LieMatrix(const M& v) : Matrix(v) {}
+
 // Currently TMP constructor causes ICE on MSVS 2013
 #if (_MSC_VER < 1800)
   /** initialize from a fixed size normal vector */
@@ -94,7 +97,10 @@ struct LieMatrix : public Matrix {
       Eigen::Map<const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
       &v(0), this->rows(), this->cols()));
   }
-
+  inline LieMatrix retract(const Vector& v, OptionalJacobian<-1,-1> Horigin, OptionalJacobian<-1,-1> Hv) const {
+    CONCEPT_NOT_IMPLEMENTED;
+    return retract(v);
+  }
   /** @return the local coordinates of another object.  The elements of the
    * tangent space vector correspond to the matrix entries arranged in
    * *row-major* order. */
@@ -104,6 +110,10 @@ struct LieMatrix : public Matrix {
       &result(0), this->rows(), this->cols()) = t2 - *this;
     return result;
   }
+  Vector localCoordinates(const LieMatrix& ts, OptionalJacobian<-1,-1> Horigin, OptionalJacobian<-1,-1> Hother) const {
+    CONCEPT_NOT_IMPLEMENTED;
+    return localCoordinates(ts);
+  }
 
   /// @}
   /// @name Group interface
@@ -121,8 +131,8 @@ struct LieMatrix : public Matrix {
   // as the other geometry objects (Point3, Rot3, etc.) have this problem
   /** compose with another object */
   inline LieMatrix compose(const LieMatrix& p,
-      boost::optional<gtsam::Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      OptionalJacobian<-1,-1> H1 = boost::none,
+      OptionalJacobian<-1,-1> H2 = boost::none) const {
     if(H1) *H1 = eye(dim());
     if(H2) *H2 = eye(p.dim());
 
@@ -131,15 +141,15 @@ struct LieMatrix : public Matrix {
 
   /** between operation */
   inline LieMatrix between(const LieMatrix& l2,
-      boost::optional<gtsam::Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      OptionalJacobian<-1,-1> H1 = boost::none,
+      OptionalJacobian<-1,-1> H2 = boost::none) const {
     if(H1) *H1 = -eye(dim());
     if(H2) *H2 = eye(l2.dim());
     return LieMatrix(l2 - *this);
   }
 
   /** invert the object and yield a new one */
-  inline LieMatrix inverse(boost::optional<gtsam::Matrix&> H=boost::none) const {
+  inline LieMatrix inverse(OptionalJacobian<-1,-1> H = boost::none) const {
     if(H) *H = -eye(dim());
 
     return LieMatrix(-(*this));
@@ -150,12 +160,13 @@ struct LieMatrix : public Matrix {
   /// @{
 
   /** Expmap around identity */
-  static inline LieMatrix Expmap(const Vector& v) {
+  static inline LieMatrix Expmap(const Vector& v, OptionalJacobian<-1,-1> H = boost::none) {
     throw std::runtime_error("LieMatrix::Expmap(): Don't use this function");
     return LieMatrix(v); }
 
   /** Logmap around identity */
-  static inline Vector Logmap(const LieMatrix& p) {
+  static inline Vector Logmap(const LieMatrix& p, OptionalJacobian<-1,-1> H = boost::none) {
+    if (H) { CONCEPT_NOT_IMPLEMENTED; }
     Vector result(p.size());
     Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >(
         result.data(), p.rows(), p.cols()) = p;

gtsam/base/LieScalar.h

@@ -30,6 +30,7 @@ namespace gtsam {
    * we can directly add double, Vector, and Matrix into values now, because of
    * gtsam::traits.
    */
+#if 0
   struct GTSAM_EXPORT LieScalar {
 
     enum { dimension = 1 };
@@ -117,9 +118,37 @@ namespace gtsam {
   private:
       double d_;
   };
+#endif
 
+  struct GTSAM_EXPORT LieScalar {
+
+    enum { dimension = 1 };
+
+    /** default constructor */
+    LieScalar() : d_(0.0) {}
+
+    /** wrap a double */
+    /*explicit*/ LieScalar(double d) : d_(d) {}
+
+    /** access the underlying value */
+    double value() const { return d_; }
+
+    /** Automatic conversion to underlying value */
+    operator double() const { return d_; }
+
+    /** print @param name optional string naming the object */
+    void print(const std::string& name="") const;
+
+    /** equality up to tolerance */
+    bool equals(const LieScalar& expected, double tol=1e-5) const {
+      return fabs(expected.d_ - d_) <= tol;
+    }
+
+   private:
+       double d_;
+   };
 
   template<>
-  struct traits_x<LieScalar> : public internal::LieGroup<LieScalar> {};
+  struct traits_x<LieScalar> : public internal::ScalarTraits<LieScalar> {};
 
 } // \namespace gtsam

gtsam/base/LieVector.h

@@ -90,8 +90,8 @@ struct LieVector : public Vector {
   // as the other geometry objects (Point3, Rot3, etc.) have this problem
   /** compose with another object */
   LieVector compose(const LieVector& p,
-      boost::optional<gtsam::Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      OptionalJacobian<-1,-1> H1 = boost::none,
+      OptionalJacobian<-1,-1> H2 = boost::none) const {
     if(H1) *H1 = eye(dim());
     if(H2) *H2 = eye(p.dim());
 
@@ -100,15 +100,15 @@ struct LieVector : public Vector {
 
   /** between operation */
   LieVector between(const LieVector& l2,
-      boost::optional<gtsam::Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      OptionalJacobian<-1,-1> H1 = boost::none,
+      OptionalJacobian<-1,-1> H2 = boost::none) const {
     if(H1) *H1 = -eye(dim());
     if(H2) *H2 = eye(l2.dim());
     return LieVector(l2.vector() - vector());
   }
 
   /** invert the object and yield a new one */
-  LieVector inverse(boost::optional<gtsam::Matrix&> H=boost::none) const {
+  LieVector inverse(OptionalJacobian<-1,-1> H=boost::none) const {
     if(H) *H = -eye(dim());
 
     return LieVector(-1.0 * vector());
@@ -117,10 +117,16 @@ struct LieVector : public Vector {
   // Lie functions
 
   /** Expmap around identity */
-  static LieVector Expmap(const Vector& v) { return LieVector(v); }
+  static LieVector Expmap(const Vector& v, OptionalJacobian<-1,-1> H = boost::none) {
+    if (H) { CONCEPT_NOT_IMPLEMENTED; }
+    return LieVector(v);
+  }
 
   /** Logmap around identity - just returns with default cast back */
-  static Vector Logmap(const LieVector& p) { return p; }
+  static Vector Logmap(const LieVector& p, OptionalJacobian<-1,-1> H = boost::none) {
+    if (H) { CONCEPT_NOT_IMPLEMENTED; }
+    return p;
+  }
 
 private:
 

gtsam/base/concepts.h

@@ -229,6 +229,7 @@ struct ScalarTraits {
   // Typedefs required by all manifold types.
   typedef vector_space_tag structure_category;
   typedef additive_group_tag group_flavor;
+  typedef Scalar ManifoldType;
   enum { dimension = 1 };
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;