diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp
new file mode 100644
index 000000000..c805d8aab
--- /dev/null
+++ b/gtsam_unstable/geometry/Similarity3.cpp
@@ -0,0 +1,154 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+ */
+
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam_unstable/geometry/Similarity3.h>
+
+namespace gtsam {
+
+Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) {
+  R_ = R;
+  t_ = t;
+  s_ = s;
+}
+
+/// Return the translation
+const Vector3 Similarity3::t() const {
+  return t_.vector();
+}
+
+/// Return the rotation matrix
+const Matrix3 Similarity3::R() const {
+  return R_.matrix();
+}
+
+Similarity3::Similarity3() :
+        R_(), t_(), s_(1){
+}
+
+/// Construct pure scaling
+Similarity3::Similarity3(double s) {
+  s_ = s;
+}
+
+/// Construct from GTSAM types
+Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) {
+  R_ = R;
+  t_ = t;
+  s_ = s;
+}
+
+Similarity3 Similarity3::identity() {
+  std::cout << "Identity!" << std::endl;
+  return Similarity3(); }
+
+Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
+  std::cout << "Logmap!" << std::endl;
+  return Vector7();
+}
+
+Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
+  std::cout << "Expmap!" << std::endl;
+  return Similarity3();
+}
+
+bool Similarity3::operator==(const Similarity3& other) const {
+  return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_);
+}
+
+/// Compare with tolerance
+bool Similarity3::equals(const Similarity3& sim, double tol) const {
+  return rotation().equals(sim.rotation(), tol) && translation().equals(sim.translation(), tol)
+      && scale() < (sim.scale()+tol) && scale() > (sim.scale()-tol);
+}
+
+Point3 Similarity3::transform_from(const Point3& 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;
+}
+
+/** syntactic sugar for transform_from */
+inline Point3 Similarity3::operator*(const Point3& p) const {
+  return transform_from(p);
+}
+
+Similarity3 Similarity3::inverse() const {
+  Rot3 Rt = R_.inverse();
+  Point3 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;
+}
+
+/// Return the rotation matrix
+Rot3 Similarity3::rotation() const {
+  return R_;
+}
+
+/// Return the translation
+Point3 Similarity3::translation() const {
+  return t_;
+}
+
+/// Return the scale
+double Similarity3::scale() const {
+  return s_;
+}
+
+/// Update Similarity transform via 7-dim vector in tangent space
+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.
+  Rot3 r;  //Create a zero rotation to do our retraction.
+  return Similarity3( //
+      r.retract(v.head<3>()), // retract rotation using v[0,1,2]
+      Point3(v.segment<3>(3)), // Retract the translation
+      1.0 + v[6]); //finally, update scale using v[6]
+}
+
+/// 7-dimensional vector v in tangent space that makes other = this->retract(v)
+Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other,  ChartJacobian H) {
+  Rot3 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;
+}
+}
+
+
diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h
new file mode 100644
index 000000000..8d7ccf82f
--- /dev/null
+++ b/gtsam_unstable/geometry/Similarity3.h
@@ -0,0 +1,118 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+ */
+
+#ifndef GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_
+#define GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_
+
+
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/base/Manifold.h>
+
+namespace gtsam {
+
+/**
+ * 3D similarity transform
+ */
+class Similarity3: public LieGroup<Similarity3, 7> {
+
+  /** Pose Concept requirements */
+  typedef Rot3 Rotation;
+  typedef Point3 Translation;
+
+private:
+  Rot3 R_;
+  Point3 t_;
+  double s_;
+
+public:
+
+  Similarity3();
+
+  /// Construct pure scaling
+  Similarity3(double s);
+
+  /// Construct from GTSAM types
+  Similarity3(const Rot3& R, const Point3& t, double s);
+
+  /// Construct from Eigen types
+  Similarity3(const Matrix3& R, const Vector3& t, double s);
+
+  /// Return the translation
+  const Vector3 t() const;
+
+  /// Return the rotation matrix
+  const Matrix3 R() const;
+
+  static Similarity3 identity();
+
+  static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none);
+
+  static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none);
+
+  bool operator==(const Similarity3& other) const;
+
+  /// Compare with tolerance
+  bool equals(const Similarity3& sim, double tol) const;
+
+  Point3 transform_from(const Point3& p) const;
+
+  Matrix7 AdjointMap() const;
+
+  /** syntactic sugar for transform_from */
+  inline Point3 operator*(const Point3& p) const;
+
+  Similarity3 inverse() const;
+
+  Similarity3 operator*(const Similarity3& T) const;
+
+  void print(const std::string& s) 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;
+  };
+
+  /// Return the rotation matrix
+  Rot3 rotation() const;
+
+  /// Return the translation
+  Point3 translation() const;
+
+  /// Return the scale
+  double scale() const;
+
+  /// 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);
+  };
+
+  using LieGroup<Similarity3, 7>::inverse; // version with derivative
+};
+
+template<>
+struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
+}
+
+#endif /* GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ */
diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
index 9cae0ba5d..51125eb2d 100644
--- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp
+++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
@@ -14,202 +14,7 @@
  * @brief  Unit tests for Similarity3 class
  */
 
-#include <gtsam/geometry/Rot3.h>
-#include <gtsam/geometry/Point3.h>
-#include <gtsam/base/Manifold.h>
-
-namespace gtsam {
-
-/**
- * 3D similarity transform
- */
-class Similarity3: public LieGroup<Similarity3, 7> {
-
-  /** Pose Concept requirements */
-  typedef Rot3 Rotation;
-  typedef Point3 Translation;
-
-private:
-  Rot3 R_;
-  Point3 t_;
-  double s_;
-
-public:
-  /// Construct from Eigen types
-  Similarity3(const Matrix3& R, const Vector3& t, double s) {
-    R_ = R;
-    t_ = t;
-    s_ = s;
-  }
-
-  /// Return the translation
-  const Vector3 t() const {
-    return t_.vector();
-  }
-
-  /// Return the rotation matrix
-  const Matrix3 R() const {
-    return R_.matrix();
-  }
-
-public:
-
-  Similarity3() :
-    R_(), t_(), s_(1){
-  }
-
-  /// Construct pure scaling
-  Similarity3(double s) {
-    s_ = s;
-  }
-
-  /// Construct from GTSAM types
-  Similarity3(const Rot3& R, const Point3& t, double s) {
-    R_ = R;
-    t_ = t;
-    s_ = s;
-  }
-
-  static Similarity3 identity() {
-    std::cout << "Identity!" << std::endl;
-    return Similarity3(); }
-
-  static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none) {
-    std::cout << "Logmap!" << std::endl;
-    return Vector7();
-  }
-
-  static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none) {
-    std::cout << "Expmap!" << std::endl;
-    return Similarity3();
-  }
-
-  bool operator==(const Similarity3& other) const {
-    return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_);
-  }
-
-  /// Compare with tolerance
-  bool equals(const Similarity3& sim, double tol) const {
-    return rotation().equals(sim.rotation(), tol) && translation().equals(sim.translation(), tol)
-        && scale() < (sim.scale()+tol) && scale() > (sim.scale()-tol);
-  }
-
-  Point3 transform_from(const Point3& p) const {
-    /*if (Dpose) {
-      const Matrix3 R = R_.matrix();
-      Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
-      (*Dpose) << DR, R;
-    }
-    if (Dpoint)
-      *Dpoint = R_.matrix();*/
-    return R_ * (s_ * p) + t_;
-  }
-
-  Matrix7 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;
-  }
-
-  /** syntactic sugar for transform_from */
-  inline Point3 operator*(const Point3& p) const {
-    return transform_from(p);
-  }
-
-  Similarity3 inverse() const {
-    Rot3 Rt = R_.inverse();
-    Point3 sRt = R_.inverse() * (-s_ * t_);
-    return Similarity3(Rt, sRt, 1.0/s_);
-  }
-
-  Similarity3 operator*(const Similarity3& T) const {
-      return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_);
-    }
-
-  void 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;
-  }
-
-  /// @name Manifold
-  /// @{
-
-  /// 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;
-  }
-
-  /// Return the rotation matrix
-  Rot3 rotation() const {
-    return R_;
-  }
-
-  /// Return the translation
-  Point3 translation() const {
-    return t_;
-  }
-
-  /// Return the scale
-  double scale() const {
-    return s_;
-  }
-
-  /// Update Similarity transform via 7-dim vector in tangent space
-  struct ChartAtOrigin {
-    static Similarity3 Retract(const Vector7& v,  ChartJacobian H = boost::none) {
-
-    // 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.
-    Rot3 r;  //Create a zero rotation to do our retraction.
-    return Similarity3( //
-        r.retract(v.head<3>()), // retract rotation using v[0,1,2]
-        Point3(v.segment<3>(3)), // Retract the translation
-        1.0 + v[6]); //finally, update scale using v[6]
-  }
-
-  /// 7-dimensional vector v in tangent space that makes other = this->retract(v)
-  static Vector7 Local(const Similarity3& other,  ChartJacobian H = boost::none) {
-    Rot3 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;
-  }
-  };
-
-  using LieGroup<Similarity3, 7>::inverse; // version with derivative
-
-  /// @}
-
-  /// @name Lie Group
-  /// @{
-
-  // compose T1*T2
-  // between T2*inverse(T1)
-  // identity I4
-  // inverse inverse(T)
-
-  /// @}
-
-};
-
-template<>
-struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
-}
-
+#include <gtsam_unstable/geometry/Similarity3.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
@@ -359,24 +164,20 @@ TEST(Similarity3, manifold_first_order)
 
 TEST(Similarity3, Optimization) {
   Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
-  //prior.print("Goal Transform");
   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);
-  //graph.print("Full Graph");
 
   Values initial;
   initial.insert<Similarity3>(key, Similarity3());
-  //initial.print("Initial Estimate");
 
   Values result;
   LevenbergMarquardtParams params;
   params.setVerbosityLM("TRYCONFIG");
   result = LevenbergMarquardtOptimizer(graph, initial).optimize();
-  //result.print("Optimized Estimate");
   EXPECT(assert_equal(prior, result.at<Similarity3>(key), 1e-4));
 }
 
@@ -390,12 +191,14 @@ TEST(Similarity3, Optimization2) {
   //prior.print("Goal Transform");
   noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
   SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(
-        (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.010).finished());
+        (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1).finished());
+  SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas(
+          (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.01).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(1), m4, betweenNoise);
+  BetweenFactor<Similarity3> b4(X(4), X(1), m4, betweenNoise2);
 
 
   NonlinearFactorGraph graph;
@@ -409,9 +212,9 @@ TEST(Similarity3, Optimization2) {
 
   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.0));
-  initial.insert<Similarity3>(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(1, 1.5, 0), 1.0));
-  initial.insert<Similarity3>(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.0));
+  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.print("Initial Estimate");