Merge remote-tracking branch 'origin/develop' into feature/SmartFactors3

Conflicts:
	gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

.cproject

@@ -532,6 +532,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSimilarity3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testSimilarity3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testLieConfig.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

.settings/.gitignore

@@ -0,0 +1 @@
+/org.eclipse.cdt.codan.core.prefs

gtsam.h

@@ -156,7 +156,7 @@ virtual class Value {
   size_t dim() const;
 };
 
-#include <gtsam/base/LieScalar.h>
+#include <gtsam/base/LieScalar_Deprecated.h>
 class LieScalar {
   // Standard constructors
   LieScalar();
@@ -185,7 +185,7 @@ class LieScalar {
   static Vector Logmap(const gtsam::LieScalar& p);
 };
 
-#include <gtsam/base/LieVector.h>
+#include <gtsam/base/LieVector_Deprecated.h>
 class LieVector {
   // Standard constructors
   LieVector();
@@ -217,7 +217,7 @@ class LieVector {
   void serialize() const;
 };
 
-#include <gtsam/base/LieMatrix.h>
+#include <gtsam/base/LieMatrix_Deprecated.h>
 class LieMatrix {
   // Standard constructors
   LieMatrix();

gtsam/base/FastSet.h

@@ -19,15 +19,14 @@
 #pragma once
 
 #include <gtsam/base/FastDefaultAllocator.h>
-
-#include <set>
-#include <iostream>
-#include <string>
-#include <cmath>
 #include <boost/mpl/has_xxx.hpp>
 #include <boost/utility/enable_if.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/set.hpp>
+#include <set>
+#include <iostream>
+#include <string>
+#include <cmath>
 
 BOOST_MPL_HAS_XXX_TRAIT_DEF(print)
 

gtsam/base/GenericValue.h

@@ -26,7 +26,7 @@
 #include <boost/pool/pool_alloc.hpp>
 
 #include <cmath>
-#include <iostream>
+#include <iosfwd>
 #include <typeinfo> // operator typeid
 
 namespace gtsam {

gtsam/base/LieMatrix.cpp

@@ -15,7 +15,7 @@
  * @author Richard Roberts and Alex Cunningham
  */
 
-#include <gtsam/base/LieMatrix.h>
+#include <gtsam/base/LieMatrix_Deprecated.h>
 
 namespace gtsam {
 

gtsam/base/LieMatrix.h

@@ -11,147 +11,16 @@
 
 /**
  * @file LieMatrix.h
- * @brief A wrapper around Matrix providing Lie compatibility
+ * @brief External deprecation warning, see LieMatrix_Deprecated.h for details
- * @author Richard Roberts and Alex Cunningham
+ * @author Paul Drews
  */
 
 #pragma once
 
-#include <cstdarg>
-
 #ifdef _MSC_VER
 #pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.")
 #else
 #warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead."
 #endif
 
-#include <gtsam/base/VectorSpace.h>
+#include "gtsam/base/LieMatrix_Deprecated.h"
-#include <boost/serialization/nvp.hpp>
-
-namespace gtsam {
-
-/**
- * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
- * we can directly add double, Vector, and Matrix into values now, because of
- * gtsam::traits.
- */
-struct LieMatrix : public Matrix {
-
-  /// @name Constructors
-  /// @{
-  enum { dimension = Eigen::Dynamic };
-
-  /** default constructor - only for serialize */
-  LieMatrix() {}
-
-  /** 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 */
-  template<int M, int N>
-  LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {}
-#endif
-
-  /** constructor with size and initial data, row order ! */
-  LieMatrix(size_t m, size_t n, const double* const data) :
-      Matrix(Eigen::Map<const Matrix>(data, m, n)) {}
-
-  /// @}
-  /// @name Testable interface
-  /// @{
-
-  /** print @param s optional string naming the object */
-  GTSAM_EXPORT void print(const std::string& name="") const;
-
-  /** equality up to tolerance */
-  inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
-    return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);
-  }
-
-  /// @}
-  /// @name Standard Interface
-  /// @{
-
-  /** get the underlying matrix */
-  inline Matrix matrix() const {
-    return static_cast<Matrix>(*this);
-  }
-
-  /// @}
-
-  /// @name Group
-  /// @{
-  LieMatrix compose(const LieMatrix& q) { return (*this)+q;}
-  LieMatrix between(const LieMatrix& q) { return q-(*this);}
-  LieMatrix inverse() { return -(*this);}
-  /// @}
-
-  /// @name Manifold
-  /// @{
-  Vector localCoordinates(const LieMatrix& q) { return between(q).vector();}
-  LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));}
-  /// @}
-
-  /// @name Lie Group
-  /// @{
-  static Vector Logmap(const LieMatrix& p) {return p.vector();}
-  static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);}
-  /// @}
-
-  /// @name VectorSpace requirements
-  /// @{
-
-  /** Returns dimensionality of the tangent space */
-  inline size_t dim() const { return size(); }
-
-  /** Convert to vector, is done row-wise - TODO why? */
-  inline Vector vector() const {
-    Vector result(size());
-    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-        Eigen::RowMajor> RowMajor;
-    Eigen::Map<RowMajor>(&result(0), rows(), cols()) = *this;
-    return result;
-  }
-
-  /** identity - NOTE: no known size at compile time - so zero length */
-  inline static LieMatrix identity() {
-    throw std::runtime_error("LieMatrix::identity(): Don't use this function");
-    return LieMatrix();
-  }
-  /// @}
-
-private:
-
-  // Serialization function
-  friend class boost::serialization::access;
-  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
-    ar & boost::serialization::make_nvp("Matrix",
-       boost::serialization::base_object<Matrix>(*this));
-
-  }
-
-};
-
-
-template<>
-struct traits<LieMatrix> : public internal::VectorSpace<LieMatrix> {
-
-  // Override Retract, as the default version does not know how to initialize
-  static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v,
-      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
-    if (H1) *H1 = Eye(origin);
-    if (H2) *H2 = Eye(origin);
-    typedef const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
-        Eigen::RowMajor> RowMajor;
-    return origin + Eigen::Map<RowMajor>(&v(0), origin.rows(), origin.cols());
-  }
-
-};
-
-} // \namespace gtsam

gtsam/base/LieMatrix_Deprecated.h

@@ -0,0 +1,151 @@
+/* ----------------------------------------------------------------------------
+
+ * 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)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file LieMatrix.h
+ * @brief A wrapper around Matrix providing Lie compatibility
+ * @author Richard Roberts and Alex Cunningham
+ */
+
+#pragma once
+
+#include <cstdarg>
+
+#include <gtsam/base/VectorSpace.h>
+#include <boost/serialization/nvp.hpp>
+
+namespace gtsam {
+
+/**
+ * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
+ * we can directly add double, Vector, and Matrix into values now, because of
+ * gtsam::traits.
+ */
+struct LieMatrix : public Matrix {
+
+  /// @name Constructors
+  /// @{
+  enum { dimension = Eigen::Dynamic };
+
+  /** default constructor - only for serialize */
+  LieMatrix() {}
+
+  /** 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 */
+  template<int M, int N>
+  LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {}
+#endif
+
+  /** constructor with size and initial data, row order ! */
+  LieMatrix(size_t m, size_t n, const double* const data) :
+      Matrix(Eigen::Map<const Matrix>(data, m, n)) {}
+
+  /// @}
+  /// @name Testable interface
+  /// @{
+
+  /** print @param s optional string naming the object */
+  GTSAM_EXPORT void print(const std::string& name="") const;
+
+  /** equality up to tolerance */
+  inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
+    return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);
+  }
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /** get the underlying matrix */
+  inline Matrix matrix() const {
+    return static_cast<Matrix>(*this);
+  }
+
+  /// @}
+
+  /// @name Group
+  /// @{
+  LieMatrix compose(const LieMatrix& q) { return (*this)+q;}
+  LieMatrix between(const LieMatrix& q) { return q-(*this);}
+  LieMatrix inverse() { return -(*this);}
+  /// @}
+
+  /// @name Manifold
+  /// @{
+  Vector localCoordinates(const LieMatrix& q) { return between(q).vector();}
+  LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));}
+  /// @}
+
+  /// @name Lie Group
+  /// @{
+  static Vector Logmap(const LieMatrix& p) {return p.vector();}
+  static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);}
+  /// @}
+
+  /// @name VectorSpace requirements
+  /// @{
+
+  /** Returns dimensionality of the tangent space */
+  inline size_t dim() const { return size(); }
+
+  /** Convert to vector, is done row-wise - TODO why? */
+  inline Vector vector() const {
+    Vector result(size());
+    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+        Eigen::RowMajor> RowMajor;
+    Eigen::Map<RowMajor>(&result(0), rows(), cols()) = *this;
+    return result;
+  }
+
+  /** identity - NOTE: no known size at compile time - so zero length */
+  inline static LieMatrix identity() {
+    throw std::runtime_error("LieMatrix::identity(): Don't use this function");
+    return LieMatrix();
+  }
+  /// @}
+
+private:
+
+  // Serialization function
+  friend class boost::serialization::access;
+  template<class Archive>
+  void serialize(Archive & ar, const unsigned int version) {
+    ar & boost::serialization::make_nvp("Matrix",
+       boost::serialization::base_object<Matrix>(*this));
+
+  }
+
+};
+
+
+template<>
+struct traits<LieMatrix> : public internal::VectorSpace<LieMatrix> {
+
+  // Override Retract, as the default version does not know how to initialize
+  static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v,
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+    if (H1) *H1 = Eye(origin);
+    if (H2) *H2 = Eye(origin);
+    typedef const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+        Eigen::RowMajor> RowMajor;
+    return origin + Eigen::Map<RowMajor>(&v(0), origin.rows(), origin.cols());
+  }
+
+};
+
+} // \namespace gtsam

gtsam/base/LieScalar.cpp

@@ -8,7 +8,7 @@
 
 
 
-#include <gtsam/base/LieScalar.h>
+#include <gtsam/base/LieScalar_Deprecated.h>
 
 namespace gtsam {
   void LieScalar::print(const std::string& name) const {

gtsam/base/LieScalar.h

@@ -11,7 +11,7 @@
 
 /**
  * @file LieScalar.h
- * @brief A wrapper around scalar providing Lie compatibility
+ * @brief External deprecation warning, see LieScalar_Deprecated.h for details
  * @author Kai Ni
  */
 
@@ -23,69 +23,4 @@
   #warning "LieScalar.h is deprecated. Please use double/float instead."
 #endif
 
-#include <gtsam/dllexport.h>
+#include <gtsam/base/LieScalar_Deprecated.h>
-#include <gtsam/base/VectorSpace.h>
-
-namespace gtsam {
-
-  /**
-   * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
-   * we can directly add double, Vector, and Matrix into values now, because of
-   * gtsam::traits.
-   */
-  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_; }
-
-    /** convert vector */
-    Vector1 vector() const { Vector1 v; v<<d_; return v; }
-
-    /// @name Testable
-    /// @{
-    void print(const std::string& name="") const;
-    bool equals(const LieScalar& expected, double tol=1e-5) const {
-      return fabs(expected.d_ - d_) <= tol;
-    }
-    /// @}
-
-    /// @name Group
-    /// @{
-    static LieScalar identity() { return LieScalar(0);}
-    LieScalar compose(const LieScalar& q) { return (*this)+q;}
-    LieScalar between(const LieScalar& q) { return q-(*this);}
-    LieScalar inverse() { return -(*this);}
-    /// @}
-
-    /// @name Manifold
-    /// @{
-    size_t dim() const { return 1; }
-    Vector1 localCoordinates(const LieScalar& q) { return between(q).vector();}
-    LieScalar retract(const Vector1& v) {return compose(LieScalar(v[0]));}
-    /// @}
-
-    /// @name Lie Group
-    /// @{
-    static Vector1 Logmap(const LieScalar& p) { return p.vector();}
-    static LieScalar Expmap(const Vector1& v) { return LieScalar(v[0]);}
-    /// @}
-
-  private:
-      double d_;
-  };
-
-  template<>
-  struct traits<LieScalar> : public internal::ScalarTraits<LieScalar> {};
-
-} // \namespace gtsam

gtsam/base/LieScalar_Deprecated.h

@@ -0,0 +1,85 @@
+/* ----------------------------------------------------------------------------
+
+ * 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)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file LieScalar.h
+ * @brief A wrapper around scalar providing Lie compatibility
+ * @author Kai Ni
+ */
+
+#pragma once
+
+#include <gtsam/dllexport.h>
+#include <gtsam/base/VectorSpace.h>
+
+namespace gtsam {
+
+  /**
+   * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
+   * we can directly add double, Vector, and Matrix into values now, because of
+   * gtsam::traits.
+   */
+  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_; }
+
+    /** convert vector */
+    Vector1 vector() const { Vector1 v; v<<d_; return v; }
+
+    /// @name Testable
+    /// @{
+    void print(const std::string& name="") const;
+    bool equals(const LieScalar& expected, double tol=1e-5) const {
+      return fabs(expected.d_ - d_) <= tol;
+    }
+    /// @}
+
+    /// @name Group
+    /// @{
+    static LieScalar identity() { return LieScalar(0);}
+    LieScalar compose(const LieScalar& q) { return (*this)+q;}
+    LieScalar between(const LieScalar& q) { return q-(*this);}
+    LieScalar inverse() { return -(*this);}
+    /// @}
+
+    /// @name Manifold
+    /// @{
+    size_t dim() const { return 1; }
+    Vector1 localCoordinates(const LieScalar& q) { return between(q).vector();}
+    LieScalar retract(const Vector1& v) {return compose(LieScalar(v[0]));}
+    /// @}
+
+    /// @name Lie Group
+    /// @{
+    static Vector1 Logmap(const LieScalar& p) { return p.vector();}
+    static LieScalar Expmap(const Vector1& v) { return LieScalar(v[0]);}
+    /// @}
+
+  private:
+      double d_;
+  };
+
+  template<>
+  struct traits<LieScalar> : public internal::ScalarTraits<LieScalar> {};
+
+} // \namespace gtsam

gtsam/base/LieVector.cpp

@@ -15,8 +15,8 @@
  * @author Alex Cunningham
  */
 
+#include <gtsam/base/LieVector_Deprecated.h>
 #include <cstdarg>
-#include <gtsam/base/LieVector.h>
 
 using namespace std;
 

gtsam/base/LieVector.h

@@ -11,8 +11,8 @@
 
 /**
  * @file LieVector.h
- * @brief A wrapper around vector providing Lie compatibility
+ * @brief Deprecation warning for LieVector_Deprecated.h, see LieVector_Deprecated.h for details.
- * @author Alex Cunningham
+ * @author Paul Drews
  */
 
 #pragma once
@@ -23,100 +23,4 @@
 #warning "LieVector.h is deprecated. Please use Eigen::Vector instead."
 #endif
 
-#include <gtsam/base/VectorSpace.h>
+#include <gtsam/base/LieVector_Deprecated.h>
-
-namespace gtsam {
-
-/**
- * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
- * we can directly add double, Vector, and Matrix into values now, because of
- * gtsam::traits.
- */
-struct LieVector : public Vector {
-
-  enum { dimension = Eigen::Dynamic };
-
-  /** default constructor - should be unnecessary */
-  LieVector() {}
-
-  /** initialize from a normal vector */
-  LieVector(const Vector& v) : Vector(v) {}
-  
-  template <class V>
-  LieVector(const V& v) : Vector(v) {}
-
-// Currently TMP constructor causes ICE on MSVS 2013
-#if (_MSC_VER < 1800)
-  /** initialize from a fixed size normal vector */
-  template<int N>
-  LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
-#endif
-
-  /** wrap a double */
-  LieVector(double d) : Vector((Vector(1) << d).finished()) {}
-
-  /** constructor with size and initial data, row order ! */
-  GTSAM_EXPORT LieVector(size_t m, const double* const data);
-
-  /// @name Testable
-  /// @{
-  GTSAM_EXPORT void print(const std::string& name="") const;
-  bool equals(const LieVector& expected, double tol=1e-5) const {
-    return gtsam::equal(vector(), expected.vector(), tol);
-  }
-  /// @}
-
-  /// @name Group
-  /// @{
-  LieVector compose(const LieVector& q) { return (*this)+q;}
-  LieVector between(const LieVector& q) { return q-(*this);}
-  LieVector inverse() { return -(*this);}
-  /// @}
-
-  /// @name Manifold
-  /// @{
-  Vector localCoordinates(const LieVector& q) { return between(q).vector();}
-  LieVector retract(const Vector& v) {return compose(LieVector(v));}
-  /// @}
-
-  /// @name Lie Group
-  /// @{
-  static Vector Logmap(const LieVector& p) {return p.vector();}
-  static LieVector Expmap(const Vector& v) { return LieVector(v);}
-  /// @}
-
-  /// @name VectorSpace requirements
-  /// @{
-
-  /** get the underlying vector */
-  Vector vector() const {
-    return static_cast<Vector>(*this);
-  }
-
-  /** Returns dimensionality of the tangent space */
-  size_t dim() const { return this->size(); }
-
-  /** identity - NOTE: no known size at compile time - so zero length */
-  static LieVector identity() {
-    throw std::runtime_error("LieVector::identity(): Don't use this function");
-    return LieVector();
-  }
-
-  /// @}
-
-private:
-
-  // Serialization function
-  friend class boost::serialization::access;
-  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
-    ar & boost::serialization::make_nvp("Vector",
-       boost::serialization::base_object<Vector>(*this));
-  }
-};
-
-
-template<>
-struct traits<LieVector> : public internal::VectorSpace<LieVector> {};
-
-} // \namespace gtsam

gtsam/base/LieVector_Deprecated.h

@@ -0,0 +1,116 @@
+/* ----------------------------------------------------------------------------
+
+ * 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)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file LieVector.h
+ * @brief A wrapper around vector providing Lie compatibility
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <gtsam/base/VectorSpace.h>
+
+namespace gtsam {
+
+/**
+ * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
+ * we can directly add double, Vector, and Matrix into values now, because of
+ * gtsam::traits.
+ */
+struct LieVector : public Vector {
+
+  enum { dimension = Eigen::Dynamic };
+
+  /** default constructor - should be unnecessary */
+  LieVector() {}
+
+  /** initialize from a normal vector */
+  LieVector(const Vector& v) : Vector(v) {}
+  
+  template <class V>
+  LieVector(const V& v) : Vector(v) {}
+
+// Currently TMP constructor causes ICE on MSVS 2013
+#if (_MSC_VER < 1800)
+  /** initialize from a fixed size normal vector */
+  template<int N>
+  LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
+#endif
+
+  /** wrap a double */
+  LieVector(double d) : Vector((Vector(1) << d).finished()) {}
+
+  /** constructor with size and initial data, row order ! */
+  GTSAM_EXPORT LieVector(size_t m, const double* const data);
+
+  /// @name Testable
+  /// @{
+  GTSAM_EXPORT void print(const std::string& name="") const;
+  bool equals(const LieVector& expected, double tol=1e-5) const {
+    return gtsam::equal(vector(), expected.vector(), tol);
+  }
+  /// @}
+
+  /// @name Group
+  /// @{
+  LieVector compose(const LieVector& q) { return (*this)+q;}
+  LieVector between(const LieVector& q) { return q-(*this);}
+  LieVector inverse() { return -(*this);}
+  /// @}
+
+  /// @name Manifold
+  /// @{
+  Vector localCoordinates(const LieVector& q) { return between(q).vector();}
+  LieVector retract(const Vector& v) {return compose(LieVector(v));}
+  /// @}
+
+  /// @name Lie Group
+  /// @{
+  static Vector Logmap(const LieVector& p) {return p.vector();}
+  static LieVector Expmap(const Vector& v) { return LieVector(v);}
+  /// @}
+
+  /// @name VectorSpace requirements
+  /// @{
+
+  /** get the underlying vector */
+  Vector vector() const {
+    return static_cast<Vector>(*this);
+  }
+
+  /** Returns dimensionality of the tangent space */
+  size_t dim() const { return this->size(); }
+
+  /** identity - NOTE: no known size at compile time - so zero length */
+  static LieVector identity() {
+    throw std::runtime_error("LieVector::identity(): Don't use this function");
+    return LieVector();
+  }
+
+  /// @}
+
+private:
+
+  // Serialization function
+  friend class boost::serialization::access;
+  template<class Archive>
+  void serialize(Archive & ar, const unsigned int version) {
+    ar & boost::serialization::make_nvp("Vector",
+       boost::serialization::base_object<Vector>(*this));
+  }
+};
+
+
+template<>
+struct traits<LieVector> : public internal::VectorSpace<LieVector> {};
+
+} // \namespace gtsam

gtsam/base/Matrix.cpp

@@ -20,6 +20,8 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/FastList.h>
+#include <gtsam/3rdparty/Eigen/Eigen/SVD>  
+#include <gtsam/3rdparty/Eigen/Eigen/LU>
 
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
@@ -180,6 +182,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec
 }
 
 /* ************************************************************************* */
+//3 argument call
 void print(const Matrix& A, const string &s, ostream& stream) {
   size_t m = A.rows(), n = A.cols();
 
@@ -198,6 +201,12 @@ void print(const Matrix& A, const string &s, ostream& stream) {
   stream << "];" << endl;
 }
 
+/* ************************************************************************* */
+//1 or 2 argument call
+void print(const Matrix& A, const string &s){
+  print(A, s, cout);
+}
+
 /* ************************************************************************* */
 void save(const Matrix& A, const string &s, const string& filename) {
   fstream stream(filename.c_str(), fstream::out | fstream::app);
@@ -697,6 +706,19 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix,
   return ss.str();
 }
 
+void inplace_QR(Matrix& A){
+  size_t rows = A.rows();
+  size_t cols = A.cols();
+  size_t size = std::min(rows,cols);
 
+  typedef Eigen::internal::plain_diag_type<Matrix>::type HCoeffsType;
+  typedef Eigen::internal::plain_row_type<Matrix>::type RowVectorType;
+  HCoeffsType hCoeffs(size);
+  RowVectorType temp(cols);
+
+  Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>::run(A, hCoeffs, 48, temp.data());
+
+  zeroBelowDiagonal(A);
+}
 
 } // namespace gtsam

gtsam/base/Matrix.h

@@ -21,11 +21,14 @@
 // \callgraph
 
 #pragma once
-
 #include <gtsam/base/Vector.h>
+#include <boost/math/special_functions/fpclassify.hpp>
+#include <gtsam/3rdparty/Eigen/Eigen/Core>
+#include <gtsam/3rdparty/Eigen/Eigen/Cholesky>
+#include <gtsam/3rdparty/Eigen/Eigen/LU>
 #include <boost/format.hpp>
 #include <boost/tuple/tuple.hpp>
-#include <boost/math/special_functions/fpclassify.hpp>
+
 
 /**
  * Matrix is a typedef in the gtsam namespace
@@ -198,9 +201,14 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
 }
 
 /**
- * print a matrix
+ * print without optional string, must specify cout yourself
  */
-GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout);
+GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream);
+
+/**
+ * print with optional string to cout
+ */
+GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "");
 
 /**
  * save a matrix to file, which can be loaded by matlab
@@ -367,21 +375,7 @@ GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);
  * @param A is the input matrix, and is the output
  * @param clear_below_diagonal enables zeroing out below diagonal
  */
-template <class MATRIX>
+void inplace_QR(Matrix& A);
-void inplace_QR(MATRIX& A) {
-  size_t rows = A.rows();
-  size_t cols = A.cols();
-  size_t size = std::min(rows,cols);
-
-  typedef Eigen::internal::plain_diag_type<Matrix>::type HCoeffsType;
-  typedef Eigen::internal::plain_row_type<Matrix>::type RowVectorType;
-  HCoeffsType hCoeffs(size);
-  RowVectorType temp(cols);
-
-  Eigen::internal::householder_qr_inplace_blocked<MATRIX, HCoeffsType>::run(A, hCoeffs, 48, temp.data());
-
-  zeroBelowDiagonal(A);
-}
 
 /**
  * Imperative algorithm for in-place full elimination with

gtsam/base/Value.h

@@ -18,9 +18,10 @@
 
 #pragma once
 
-#include <memory>
-#include <boost/serialization/assume_abstract.hpp>
 #include <gtsam/base/Vector.h>
+#include <boost/serialization/assume_abstract.hpp>
+#include <memory>
+
 
 namespace gtsam {
 

gtsam/base/Vector.cpp

@@ -16,20 +16,18 @@
  * @author  Frank Dellaert
  */
 
+#include <gtsam/base/Vector.h>
+#include <boost/foreach.hpp>
+#include <boost/optional.hpp>
+#include <stdexcept>
 #include <cstdarg>
 #include <limits>
-#include <iostream>
 #include <fstream>
 #include <sstream>
 #include <iomanip>
 #include <cmath>
-#include <stdexcept>
-#include <boost/foreach.hpp>
-#include <boost/optional.hpp>
 #include <cstdio>
-
+#include <vector>
-#include <gtsam/base/Vector.h>
-
 
 using namespace std;
 
@@ -55,6 +53,7 @@ Vector delta(size_t n, size_t i, double value) {
 }
 
 /* ************************************************************************* */
+//3 argument call
 void print(const Vector& v, const string& s, ostream& stream) {
   size_t n = v.size();
 
@@ -65,6 +64,12 @@ void print(const Vector& v, const string& s, ostream& stream) {
   stream << "];" << endl;
 }
 
+/* ************************************************************************* */
+//1 or 2 argument call
+void print(const Vector& v, const string& s) {
+    print(v, s, cout);
+}
+
 /* ************************************************************************* */
 void save(const Vector& v, const string &s, const string& filename) {
   fstream stream(filename.c_str(), fstream::out | fstream::app);

gtsam/base/Vector.h

@@ -18,13 +18,12 @@
 
 // \callgraph
 
-#pragma once
 
-#include <list>
+#pragma once
-#include <vector>
-#include <iostream>
 #include <gtsam/global_includes.h>
-#include <gtsam/3rdparty/gtsam_eigen_includes.h>
+#include <gtsam/3rdparty/Eigen/Eigen/Core>
+#include <iosfwd>
+#include <list>
 
 namespace gtsam {
 
@@ -97,9 +96,14 @@ GTSAM_EXPORT bool zero(const Vector& v);
 inline size_t dim(const Vector& v) { return v.size(); }
 
 /**
- * print with optional string
+ * print without optional string, must specify cout yourself
  */
-GTSAM_EXPORT void print(const Vector& v, const std::string& s = "", std::ostream& stream = std::cout);
+GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream);
+
+/**
+ * print with optional string to cout
+ */
+GTSAM_EXPORT void print(const Vector& v, const std::string& s = "");
 
 /**
  * save a vector to file, which can be loaded by matlab

gtsam/base/tests/testLieMatrix.cpp

@@ -15,10 +15,10 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/LieMatrix_Deprecated.h>
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>
-#include <gtsam/base/LieMatrix.h>
 
 using namespace gtsam;
 

gtsam/base/tests/testLieScalar.cpp

@@ -15,10 +15,10 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/LieScalar_Deprecated.h>
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>
-#include <gtsam/base/LieScalar.h>
 
 using namespace gtsam;
 

gtsam/base/tests/testLieVector.cpp

@@ -15,10 +15,10 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/LieVector_Deprecated.h>
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>
-#include <gtsam/base/LieVector.h>
 
 using namespace gtsam;
 

gtsam/base/tests/testTestableAssertions.cpp

@@ -15,8 +15,8 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/LieScalar_Deprecated.h>
 
-#include <gtsam/base/LieScalar.h>
 #include <gtsam/base/TestableAssertions.h>
 
 using namespace gtsam;

gtsam/base/types.cpp

@@ -17,10 +17,9 @@
  * @addtogroup base
  */
 
-#include <boost/lexical_cast.hpp>
-
 #include <gtsam/base/types.h>
 #include <gtsam/inference/Symbol.h>
+#include <boost/lexical_cast.hpp>
 
 namespace gtsam {
 

gtsam/base/types.h

@@ -21,18 +21,18 @@
 
 #include <gtsam/dllexport.h>
 #include <gtsam/config.h>
-
-#include <cstddef>
-#include <string>
-#include <iostream>
 #include <boost/function/function1.hpp>
 #include <boost/range/concepts.hpp>
 #include <boost/optional.hpp>
+#include <cstddef>
+#include <string>
+#include <ostream>
 
 #ifdef GTSAM_USE_TBB
 #include <tbb/task_scheduler_init.h>
 #include <tbb/tbb_exception.h>
 #include <tbb/scalable_allocator.h>
+#include <iostream>
 #endif
 
 #ifdef GTSAM_USE_EIGEN_MKL_OPENMP

gtsam/geometry/EssentialMatrix.h

@@ -10,7 +10,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Point2.h>
-#include <iostream>
+#include <iosfwd>
 
 namespace gtsam {
 

gtsam/nonlinear/Values.h

@@ -26,14 +26,9 @@
 
 #include <gtsam/base/GenericValue.h>
 #include <gtsam/base/VectorSpace.h>
-#include <gtsam/base/FastMap.h>
 #include <gtsam/inference/Key.h>
-
-#include <boost/optional.hpp>
-#include <boost/ptr_container/ptr_map.hpp>
 #include <boost/iterator/transform_iterator.hpp>
 #include <boost/iterator/filter_iterator.hpp>
-#include <boost/function.hpp>
 #ifdef __GNUC__
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-variable"
@@ -43,7 +38,6 @@
 #pragma GCC diagnostic pop
 #endif
 #include <boost/ptr_container/serialize_ptr_map.hpp>
-#include <boost/iterator_adaptors.hpp>
 
 #include <string>
 #include <utility>

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -232,8 +232,12 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
   values.insert(x2, cam2);
   // initialize third pose with some noise, we expect it to move back to original pose_above
   values.insert(x3, Camera(pose_above * noise_pose, sharedK2));
-  if (isDebugTest)
+  EXPECT(
-    values.at<Camera>(x3).print("Camera before optimization: ");
+      assert_equal(
+          Pose3(
+              Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
+                  -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
+              Point3(0.1, -0.1, 1.9)), values.at<Camera>(x3).pose()));
 
   LevenbergMarquardtParams params;
   if (isDebugTest)
@@ -252,8 +256,6 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
 //  VectorValues delta = GFG->optimize();
 
   // result.print("results of 3 camera, 3 landmark optimization \n");
-  if (isDebugTest)
-    result.at<Camera>(x3).print("Camera after optimization: ");
   EXPECT(assert_equal(cam3, result.at<Camera>(x3), 1e-8));
   if (isDebugTest)
     tictoc_print_();
@@ -442,8 +444,13 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
   values.insert(x2, cam2);
   // initialize third pose with some noise, we expect it to move back to original pose_above
   values.insert(x3, Camera(pose_above * noise_pose, sharedK));
-  if (isDebugTest)
+  EXPECT(
-    values.at<Camera>(x3).print("Camera before optimization: ");
+      assert_equal(
+          Pose3(
+              Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656,
+                  -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364,
+                  -0.0313952598), Point3(0.1, -0.1, 1.9)),
+          values.at<Camera>(x3).pose()));
 
   LevenbergMarquardtParams params;
   if (isDebugTest)
@@ -459,8 +466,6 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
   tictoc_finishedIteration_();
 
   // result.print("results of 3 camera, 3 landmark optimization \n");
-  if (isDebugTest)
-    result.at<Camera>(x3).print("Camera after optimization: ");
   EXPECT(assert_equal(cam3, result.at<Camera>(x3), 1e-7));
   if (isDebugTest)
     tictoc_print_();
@@ -819,8 +824,14 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
   values.insert(x2, cam2);
   // initialize third pose with some noise, we expect it to move back to original pose_above
   values.insert(x3, Camera(pose_above * noise_pose, sharedK));
-  if (isDebugTest)
+  EXPECT(
-    values.at<Camera>(x3).print("Camera before optimization: ");
+      assert_equal(
+          Pose3(
+              Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
+                  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
+                  -0.130455917),
+              Point3(0.0897734171, -0.110201006, 0.901022872)),
+          values.at<Camera>(x3).pose()));
 
   boost::shared_ptr<GaussianFactor> factor1 = smartFactor1->linearize(values);
   boost::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values);
@@ -900,8 +911,14 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
   values.insert(x2, Camera(pose2 * noise_pose, sharedK2));
   // initialize third pose with some noise, we expect it to move back to original pose_above
   values.insert(x3, Camera(pose3 * noise_pose * noise_pose, sharedK2));
-  if (isDebugTest)
+  EXPECT(
-    values.at<Camera>(x3).print("Camera before optimization: ");
+      assert_equal(
+          Pose3(
+              Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814,
+                  -0.572308662, -0.324093872, 0.639358349, -0.521617766,
+                  -0.564921063),
+              Point3(0.145118171, -0.252907438, 0.819956033)),
+          values.at<Camera>(x3).pose()));
 
   LevenbergMarquardtParams params;
   if (isDebugTest)
@@ -917,12 +934,17 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
   tictoc_finishedIteration_();
 
   // result.print("results of 3 camera, 3 landmark optimization \n");
-  if (isDebugTest)
-    result.at<Camera>(x3).print("Camera after optimization: ");
   cout
       << "TEST COMMENTED: rotation only version of smart factors has been deprecated "
       << endl;
-  // EXPECT(assert_equal(pose_above,result.at<Camera>(x3)));
+  EXPECT(
+      assert_equal(
+          Pose3(
+              Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814,
+                  -0.572308662, -0.324093872, 0.639358349, -0.521617766,
+                  -0.564921063),
+              Point3(0.145118171, -0.252907438, 0.819956033)),
+          result.at<Camera>(x3).pose()));
   if (isDebugTest)
     tictoc_print_();
 }
@@ -990,8 +1012,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
   values.insert(x2, cam2);
   // initialize third pose with some noise, we expect it to move back to original pose_above
   values.insert(x3, Camera(pose_above * noise_pose, sharedK));
-  if (isDebugTest)
+  EXPECT(
-    values.at<Camera>(x3).print("Camera before optimization: ");
+      assert_equal(
+          Pose3(
+              Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
+                  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
+                  -0.130455917),
+              Point3(0.0897734171, -0.110201006, 0.901022872)),
+          values.at<Camera>(x3).pose()));
 
   LevenbergMarquardtParams params;
   if (isDebugTest)
@@ -1007,12 +1035,17 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
   tictoc_finishedIteration_();
 
   // result.print("results of 3 camera, 3 landmark optimization \n");
-  if (isDebugTest)
-    result.at<Camera>(x3).print("Camera after optimization: ");
   cout
       << "TEST COMMENTED: rotation only version of smart factors has been deprecated "
       << endl;
-  // EXPECT(assert_equal(pose_above,result.at<Camera>(x3)));
+  EXPECT(
+      assert_equal(
+          Pose3(
+              Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
+                  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
+                  -0.130455917),
+              Point3(0.0897734171, -0.110201006, 0.901022872)),
+          result.at<Camera>(x3).pose()));
   if (isDebugTest)
     tictoc_print_();
 }
@@ -1236,9 +1269,12 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
   values.insert(x2, cam2);
   // initialize third pose with some noise, we expect it to move back to original pose_above
   values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK));
-  if (isDebugTest)
+  EXPECT(
-    values.at<Camera>(x3).print("Camera before optimization: ");
+      assert_equal(
-
+          Pose3(
+              Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
+                  -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
+              Point3(0.1, -0.1, 1.9)), values.at<Camera>(x3).pose()));
   LevenbergMarquardtParams params;
   if (isDebugTest)
     params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
@@ -1252,9 +1288,6 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
   gttoc_(SmartProjectionPoseFactor);
   tictoc_finishedIteration_();
 
-  // result.print("results of 3 camera, 3 landmark optimization \n");
-  if (isDebugTest)
-    result.at<Camera>(x3).print("Camera after optimization: ");
   EXPECT(assert_equal(cam3, result.at<Camera>(x3), 1e-6));
   if (isDebugTest)
     tictoc_print_();
@@ -1342,8 +1375,14 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
   values.insert(x2, cam2);
   // initialize third pose with some noise, we expect it to move back to original pose_above
   values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK));
-  if (isDebugTest)
+  EXPECT(
-    values.at<Camera>(x3).print("Camera before optimization: ");
+      assert_equal(
+          Pose3(
+              Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
+                  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
+                  -0.130455917),
+              Point3(0.0897734171, -0.110201006, 0.901022872)),
+          values.at<Camera>(x3).pose()));
 
   LevenbergMarquardtParams params;
   if (isDebugTest)
@@ -1358,13 +1397,17 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
   gttoc_(SmartProjectionPoseFactor);
   tictoc_finishedIteration_();
 
-  // result.print("results of 3 camera, 3 landmark optimization \n");
-  if (isDebugTest)
-    result.at<Camera>(x3).print("Camera after optimization: ");
   cout
       << "TEST COMMENTED: rotation only version of smart factors has been deprecated "
       << endl;
-  // EXPECT(assert_equal(pose_above,result.at<Camera>(x3)));
+  EXPECT(
+      assert_equal(
+          Pose3(
+              Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
+                  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
+                  -0.130455917),
+              Point3(0.0897734171, -0.110201006, 0.901022872)),
+          values.at<Camera>(x3).pose()));
   if (isDebugTest)
     tictoc_print_();
 }

gtsam_unstable/geometry/Similarity3.cpp

@@ -0,0 +1,124 @@
+/* ----------------------------------------------------------------------------
+
+ * 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)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Similarity3.cpp
+ * @brief  Implementation of Similarity3 transform
+ * @author Paul Drews
+ */
+
+#include <gtsam_unstable/geometry/Similarity3.h>
+
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+
+#include <gtsam/base/Manifold.h>
+
+namespace gtsam {
+
+Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
+      R_(R), t_(t), s_(s) {
+}
+
+Similarity3::Similarity3() :
+            R_(), t_(), s_(1){
+}
+
+Similarity3::Similarity3(double s) :
+  s_ (s) {
+}
+
+Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) :
+      R_ (R), t_ (t), s_ (s) {
+}
+
+Similarity3::operator Pose3() const {
+  return Pose3(R_, s_*t_);
+}
+
+Similarity3 Similarity3::identity() {
+  return Similarity3(); }
+
+//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
+//  return Vector7();
+//}
+//
+//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
+//  return Similarity3();
+//}
+
+bool Similarity3::operator==(const Similarity3& other) const {
+  return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_);
+}
+
+bool Similarity3::equals(const Similarity3& sim, double tol) const {
+  return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol)
+      && s_ < (sim.s_+tol) && s_ > (sim.s_-tol);
+}
+
+Similarity3::Translation Similarity3::transform_from(const Translation& p) const {
+  return R_ * (s_ * p) + t_;
+}
+
+Matrix7 Similarity3::AdjointMap() const{
+  const Matrix3 R = R_.matrix();
+  const Vector3 t = t_.vector();
+  Matrix3 A = s_ * skewSymmetric(t) * R;
+  Matrix7 adj;
+  adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 1, 6>::Zero(), 1;
+  return adj;
+}
+
+inline Similarity3::Translation Similarity3::operator*(const Translation& p) const {
+  return transform_from(p);
+}
+
+Similarity3 Similarity3::inverse() const {
+  Rotation Rt = R_.inverse();
+  Translation sRt = R_.inverse() * (-s_ * t_);
+  return Similarity3(Rt, sRt, 1.0/s_);
+}
+
+Similarity3 Similarity3::operator*(const Similarity3& T) const {
+  return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_);
+}
+
+void Similarity3::print(const std::string& s) const {
+  std::cout << std::endl;
+  std::cout << s;
+  rotation().print("R:\n");
+  translation().print("t: ");
+  std::cout << "s: " << scale() << std::endl;
+}
+
+Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v,  ChartJacobian H) {
+  // Will retracting or localCoordinating R work if R is not a unit rotation?
+  // Also, how do we actually get s out?  Seems like we need to store it somewhere.
+  Rotation r;  //Create a zero rotation to do our retraction.
+  return Similarity3( //
+      r.retract(v.head<3>()), // retract rotation using v[0,1,2]
+      Translation(v.segment<3>(3)), // Retract the translation
+      1.0 + v[6]); //finally, update scale using v[6]
+}
+
+Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other,  ChartJacobian H) {
+  Rotation r;  //Create a zero rotation to do the retraction
+  Vector7 v;
+  v.head<3>() = r.localCoordinates(other.R_);
+  v.segment<3>(3) = other.t_.vector();
+  //v.segment<3>(3) = translation().localCoordinates(other.translation());
+  v[6] = other.s_ - 1.0;
+  return v;
+}
+}
+
+

gtsam_unstable/geometry/Similarity3.h

@@ -0,0 +1,150 @@
+/* ----------------------------------------------------------------------------
+
+ * 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)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Similarity3.h
+ * @brief  Implementation of Similarity3 transform
+ * @author Paul Drews
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/Manifold.h>
+
+namespace gtsam {
+
+// Forward declarations
+class Pose3;
+
+/**
+ * 3D similarity transform
+ */
+class Similarity3: public LieGroup<Similarity3, 7> {
+
+  /** Pose Concept requirements */
+  typedef Rot3 Rotation;
+  typedef Point3 Translation;
+
+private:
+  Rotation R_;
+  Translation t_;
+  double s_;
+
+public:
+
+  /// @name Constructors
+  /// @{
+
+  Similarity3();
+
+  /// Construct pure scaling
+  Similarity3(double s);
+
+  /// Construct from GTSAM types
+  Similarity3(const Rotation& R, const Translation& t, double s);
+
+  /// Construct from Eigen types
+  Similarity3(const Matrix3& R, const Vector3& t, double s);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Compare with tolerance
+  bool equals(const Similarity3& sim, double tol) const;
+
+  /// Compare with standard tolerance
+  bool operator==(const Similarity3& other) const;
+
+  /// Print with optional string
+  void print(const std::string& s) const;
+
+  /// @}
+  /// @name Group
+  /// @{
+
+  /// Return an identity transform
+  static Similarity3 identity();
+
+  /// Return the inverse
+  Similarity3 inverse() const;
+
+  Translation transform_from(const Translation& p) const;
+
+  /** syntactic sugar for transform_from */
+  inline Translation operator*(const Translation& p) const;
+
+  Similarity3 operator*(const Similarity3& T) const;
+
+  /// @}
+  /// @name Standard interface
+  /// @{
+
+  /// Return a GTSAM rotation
+  const Rotation& rotation() const {
+    return R_;
+  };
+
+  /// Return a GTSAM translation
+  const Translation& translation() const {
+    return t_;
+  };
+
+  /// Return the scale
+  double scale() const {
+    return s_;
+  };
+
+  /// Convert to a rigid body pose
+  operator Pose3() const;
+
+  /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
+  inline static size_t Dim() {
+    return 7;
+  };
+
+  /// Dimensionality of tangent space = 7 DOF
+  inline size_t dim() const {
+    return 7;
+  };
+
+  /// @}
+  /// @name Chart
+  /// @{
+
+  /// Update Similarity transform via 7-dim vector in tangent space
+  struct ChartAtOrigin {
+    static Similarity3 Retract(const Vector7& v,  ChartJacobian H = boost::none);
+
+  /// 7-dimensional vector v in tangent space that makes other = this->retract(v)
+  static Vector7 Local(const Similarity3& other,  ChartJacobian H = boost::none);
+  };
+
+  /// Project from one tangent space to another
+  Matrix7 AdjointMap() const;
+
+  /// @}
+  /// @name Stubs
+  /// @{
+
+  /// Not currently implemented, required because this is a lie group
+  static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none);
+  static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none);
+
+  using LieGroup<Similarity3, 7>::inverse; // version with derivative
+};
+
+template<>
+struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
+}

gtsam_unstable/geometry/tests/testSimilarity3.cpp

@@ -0,0 +1,256 @@
+/* ----------------------------------------------------------------------------
+
+ * 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)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   testSimilarity3.cpp
+ * @brief  Unit tests for Similarity3 class
+ * @author Paul Drews
+ */
+
+#include <gtsam_unstable/geometry/Similarity3.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+
+#include <gtsam/base/Testable.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace gtsam;
+using namespace std;
+using symbol_shorthand::X;
+
+GTSAM_CONCEPT_TESTABLE_INST(Similarity3)
+
+static Point3 P(0.2,0.7,-2);
+static Rot3 R = Rot3::rodriguez(0.3,0,0);
+static Similarity3 T(R,Point3(3.5,-8.2,4.2),1);
+static Similarity3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1);
+static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1);
+
+//******************************************************************************
+TEST(Similarity3, Constructors) {
+  Similarity3 test;
+}
+
+//******************************************************************************
+TEST(Similarity3, Getters) {
+  Similarity3 test;
+  EXPECT(assert_equal(Rot3(), test.rotation()));
+  EXPECT(assert_equal(Point3(), test.translation()));
+  EXPECT_DOUBLES_EQUAL(1.0, test.scale(), 1e-9);
+}
+
+//******************************************************************************
+TEST(Similarity3, Getters2) {
+  Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7);
+  EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation()));
+  EXPECT(assert_equal(Point3(4, 5, 6), test.translation()));
+  EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9);
+}
+
+TEST(Similarity3, AdjointMap) {
+  Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
+  Matrix7 result;
+  result <<    -1.5739,   -2.4512,   -6.3651,  -50.7671,  -11.2503,   16.8859,  -28.0000,
+      6.3167,   -2.9884,   -0.4111,    0.8502,    8.6373,  -49.7260,  -35.0000,
+     -2.5734,   -5.8362,    2.8839,   33.1363,    0.3024,   30.1811,  -42.0000,
+           0,         0,         0,   -0.2248,   -0.3502,   -0.9093,         0,
+           0,         0,         0,    0.9024,   -0.4269,   -0.0587,         0,
+           0,         0,         0,   -0.3676,   -0.8337,    0.4120,         0,
+           0,         0,         0,         0,         0,         0,    1.0000;
+  EXPECT(assert_equal(result, test.AdjointMap(), 1e-3));
+}
+
+TEST(Similarity3, inverse) {
+  Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
+  Matrix3 Re;
+  Re <<    -0.2248,    0.9024,   -0.3676,
+   -0.3502,   -0.4269,   -0.8337,
+   -0.9093,   -0.0587,    0.4120;
+  Vector3 te(-9.8472, 59.7640, 10.2125);
+  Similarity3 expected(Re, te, 1.0/7.0);
+  EXPECT(assert_equal(expected, test.inverse(), 1e-3));
+}
+
+TEST(Similarity3, multiplication) {
+  Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
+  Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11);
+  Matrix3 re;
+  re << 0.0688,    0.9863,   -0.1496,
+     -0.5665,   -0.0848,   -0.8197,
+     -0.8211,    0.1412,    0.5530;
+  Vector3 te(-13.6797, 3.2441, -5.7794);
+  Similarity3 expected(re, te, 77);
+  EXPECT(assert_equal(expected, test1*test2, 1e-2));
+}
+
+//******************************************************************************
+TEST(Similarity3, Manifold) {
+  EXPECT_LONGS_EQUAL(7, Similarity3::Dim());
+  Vector z = Vector7::Zero();
+  Similarity3 sim;
+  EXPECT(sim.retract(z) == sim);
+
+  Vector7 v = Vector7::Zero();
+  v(6) = 2;
+  Similarity3 sim2;
+  EXPECT(sim2.retract(z) == sim2);
+
+  EXPECT(assert_equal(z, sim2.localCoordinates(sim)));
+
+  Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1);
+  Vector v3(7);
+  v3 << 0, 0, 0, 1, 2, 3, 0;
+  EXPECT(assert_equal(v3, sim2.localCoordinates(sim3)));
+
+//  Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1);
+  Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1);
+
+  Vector vlocal = sim.localCoordinates(other);
+
+  EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2));
+
+  Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1);
+  Rot3 R = Rot3::rodriguez(0.3,0,0);
+
+  Vector vlocal2 = sim.localCoordinates(other2);
+
+  EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2));
+
+  // TODO add unit tests for retract and localCoordinates
+}
+
+/* ************************************************************************* */
+TEST( Similarity3, retract_first_order)
+{
+  Similarity3 id;
+  Vector v = zero(7);
+  v(0) = 0.3;
+  EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v),1e-2));
+  v(3)=0.2;v(4)=0.7;v(5)=-2;
+  EXPECT(assert_equal(Similarity3(R, P, 1),id.retract(v),1e-2));
+}
+
+/* ************************************************************************* */
+TEST(Similarity3, localCoordinates_first_order)
+{
+  Vector d12 = repeat(7,0.1);
+  d12(6) = 1.0;
+  Similarity3 t1 = T, t2 = t1.retract(d12);
+  EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
+}
+
+/* ************************************************************************* */
+TEST(Similarity3, manifold_first_order)
+{
+  Similarity3 t1 = T;
+  Similarity3 t2 = T3;
+  Similarity3 origin;
+  Vector d12 = t1.localCoordinates(t2);
+  EXPECT(assert_equal(t2, t1.retract(d12)));
+  Vector d21 = t2.localCoordinates(t1);
+  EXPECT(assert_equal(t1, t2.retract(d21)));
+}
+
+TEST(Similarity3, Optimization) {
+  Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
+  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
+  Symbol key('x',1);
+  PriorFactor<Similarity3> factor(key, prior, model);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(factor);
+
+  Values initial;
+  initial.insert<Similarity3>(key, Similarity3());
+
+  Values result;
+  LevenbergMarquardtParams params;
+  params.setVerbosityLM("TRYCONFIG");
+  result = LevenbergMarquardtOptimizer(graph, initial).optimize();
+  EXPECT(assert_equal(prior, result.at<Similarity3>(key), 1e-4));
+}
+
+TEST(Similarity3, Optimization2) {
+  Similarity3 prior = Similarity3();
+  Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0);
+  Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0);
+  Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0);
+  Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0);
+  Similarity3 loop = Similarity3(1.42);
+
+  //prior.print("Goal Transform");
+  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 0.01);
+  SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(
+        (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished());
+  SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas(
+          (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished());
+  PriorFactor<Similarity3> factor(X(1), prior, model);
+  BetweenFactor<Similarity3> b1(X(1), X(2), m1, betweenNoise);
+  BetweenFactor<Similarity3> b2(X(2), X(3), m2, betweenNoise);
+  BetweenFactor<Similarity3> b3(X(3), X(4), m3, betweenNoise);
+  BetweenFactor<Similarity3> b4(X(4), X(5), m4, betweenNoise);
+  BetweenFactor<Similarity3> lc(X(5), X(1), loop, betweenNoise2);
+
+
+
+  NonlinearFactorGraph graph;
+  graph.push_back(factor);
+  graph.push_back(b1);
+  graph.push_back(b2);
+  graph.push_back(b3);
+  graph.push_back(b4);
+  graph.push_back(lc);
+
+  //graph.print("Full Graph\n");
+
+  Values initial;
+  initial.insert<Similarity3>(X(1), Similarity3());
+  initial.insert<Similarity3>(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1));
+  initial.insert<Similarity3>(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
+  initial.insert<Similarity3>(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3));
+  initial.insert<Similarity3>(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0));
+
+  //initial.print("Initial Estimate\n");
+
+  Values result;
+  result = LevenbergMarquardtOptimizer(graph, initial).optimize();
+  //result.print("Optimized Estimate\n");
+  Pose3 p1, p2, p3, p4, p5;
+  p1 = Pose3(result.at<Similarity3>(X(1)));
+  p2 = Pose3(result.at<Similarity3>(X(2)));
+  p3 = Pose3(result.at<Similarity3>(X(3)));
+  p4 = Pose3(result.at<Similarity3>(X(4)));
+  p5 = Pose3(result.at<Similarity3>(X(5)));
+
+  //p1.print("Pose1");
+  //p2.print("Pose2");
+  //p3.print("Pose3");
+  //p4.print("Pose4");
+  //p5.print("Pose5");
+
+  Similarity3 expected(0.7);
+  EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
+

gtsam_unstable/gtsam_unstable.h

@@ -477,7 +477,7 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
   DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model);
 };
 
-#include <gtsam/base/LieScalar.h>
+#include <gtsam/base/LieScalar_Deprecated.h>
 
 #include <gtsam_unstable/dynamics/VelocityConstraint3.h>
 virtual class VelocityConstraint3 : gtsam::NonlinearFactor {

gtsam_unstable/slam/serialization.cpp

@@ -5,6 +5,8 @@
  * @author Alex Cunningham
  */
 
+#include <gtsam/base/LieMatrix_Deprecated.h>
+#include <gtsam/base/LieVector_Deprecated.h>
 #include <gtsam/slam/serialization.h>
 #include <gtsam/base/serialization.h>
 
@@ -22,9 +24,6 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/GaussianISAM.h>
 #include <gtsam/linear/GaussianMultifrontalSolver.h>
-#include <gtsam/base/LieVector.h>
-#include <gtsam/base/LieMatrix.h>
-//#include <gtsam/geometry/StereoPoint2.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3DS2.h>

gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp

@@ -20,9 +20,8 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/LieVector.h>
-
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/LieVector_Deprecated.h>
 
 using namespace std;
 using namespace gtsam;

tests/testGaussianISAM2.cpp

@@ -23,10 +23,9 @@
 #include <gtsam/base/debug.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/treeTraversal-inst.h>
-#include <gtsam/base/LieScalar.h>
-
 #include <boost/foreach.hpp>
 #include <boost/assign/list_of.hpp>
+#include <gtsam/base/LieScalar_Deprecated.h>
 using namespace boost::assign;
 #include <boost/range/adaptor/map.hpp>
 namespace br { using namespace boost::adaptors; using namespace boost::range; }