diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 94c130f9f..83916bd1b 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -15,14 +15,23 @@
  * @author Frank Dellaert
  **/
 
-#include <gtsam/geometry/SO3.h>
-#include <gtsam/base/testLie.h>
-#include <gtsam/base/Testable.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/geometry/SOt.h>
 
 using namespace std;
 using namespace gtsam;
 
+//******************************************************************************
+TEST(SO3, Identity) {
+  const SO3 R;
+  EXPECT_LONGS_EQUAL(3, R.rows());
+  EXPECT_LONGS_EQUAL(3, SO3::dimension);
+  EXPECT_LONGS_EQUAL(3, SO3::Dim());
+  EXPECT_LONGS_EQUAL(3, R.dim());
+}
+
 //******************************************************************************
 TEST(SO3, Concept) {
   BOOST_CONCEPT_ASSERT((IsGroup<SO3>));
@@ -50,7 +59,21 @@ TEST(SO3, Local) {
 TEST(SO3, Retract) {
   Vector3 v(0, 0, 0.1);
   SO3 actual = traits<SO3>::Retract(R1, v);
-  EXPECT(actual.isApprox(R2));
+  EXPECT(assert_equal(R2, actual));
+}
+
+//******************************************************************************
+TEST(SO3, Logmap) {
+  Vector3 expected(0, 0, 0.1);
+  Vector3 actual = SO3::Logmap(R1.between(R2));
+  EXPECT(assert_equal((Vector)expected, actual));
+}
+
+//******************************************************************************
+TEST(SO3, Expmap) {
+  Vector3 v(0, 0, 0.1);
+  SO3 actual = R1 * SO3::Expmap(v);
+  EXPECT(assert_equal(R2, actual));
 }
 
 //******************************************************************************
@@ -231,17 +254,17 @@ TEST(SO3, JacobianLogmap) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ApplyDexp) {
   Matrix aH1, aH2;
   for (bool nearZeroApprox : {true, false}) {
     boost::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
+          return sot::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
         };
     for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
                           Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-      so3::DexpFunctor local(omega, nearZeroApprox);
+      sot::DexpFunctor local(omega, nearZeroApprox);
       for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
                         Vector3(0.4, 0.3, 0.2)}) {
         EXPECT(assert_equal(Vector3(local.dexp() * v),
@@ -254,17 +277,17 @@ TEST(SO3, ApplyDexp) {
   }
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ApplyInvDexp) {
   Matrix aH1, aH2;
   for (bool nearZeroApprox : {true, false}) {
     boost::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
+          return sot::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
         };
     for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
                           Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-      so3::DexpFunctor local(omega, nearZeroApprox);
+      sot::DexpFunctor local(omega, nearZeroApprox);
       Matrix invDexp = local.dexp().inverse();
       for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
                         Vector3(0.4, 0.3, 0.2)}) {
@@ -278,15 +301,13 @@ TEST(SO3, ApplyInvDexp) {
   }
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, vec) {
-  const Vector9 expected = Eigen::Map<Vector9>(R2.data());
+  const Vector9 expected = Eigen::Map<const Vector9>(R2.matrix().data());
   Matrix actualH;
   const Vector9 actual = R2.vec(actualH);
   CHECK(assert_equal(expected, actual));
-  boost::function<Vector9(const SO3&)> f = [](const SO3& Q) {
-    return Q.vec();
-  };
+  boost::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
   const Matrix numericalH = numericalDerivative11(f, R2, 1e-5);
   CHECK(assert_equal(numericalH, actualH));
 }
diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp
index 037280136..29ae2d50f 100644
--- a/gtsam/geometry/tests/testSO4.cpp
+++ b/gtsam/geometry/tests/testSO4.cpp
@@ -19,10 +19,9 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/lieProxies.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/geometry/SO3.h>
+#include <gtsam/geometry/SOt.h>
 #include <gtsam/geometry/SO4.h>
 
-
 #include <CppUnitLite/TestHarness.h>
 
 #include <iostream>
@@ -30,6 +29,16 @@
 using namespace std;
 using namespace gtsam;
 
+//******************************************************************************
+
+TEST(SO4, Identity) {
+  const SO4 R;
+  EXPECT_LONGS_EQUAL(4, R.rows());
+  EXPECT_LONGS_EQUAL(6, SO4::dimension);
+  EXPECT_LONGS_EQUAL(6, SO4::Dim());
+  EXPECT_LONGS_EQUAL(6, R.dim());
+}
+
 //******************************************************************************
 TEST(SO4, Concept) {
   BOOST_CONCEPT_ASSERT((IsGroup<SO4>));
@@ -39,9 +48,9 @@ TEST(SO4, Concept) {
 
 //******************************************************************************
 SO4 id;
-Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
+Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
 SO4 Q1 = SO4::Expmap(v1);
-Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.00, 0.00, 0.00).finished();
+Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished();
 SO4 Q2 = SO4::Expmap(v2);
 Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
 SO4 Q3 = SO4::Expmap(v3);
@@ -55,12 +64,12 @@ TEST(SO4, Random) {
 //******************************************************************************
 TEST(SO4, Expmap) {
   // If we do exponential map in SO(3) subgroup, topleft should be equal to R1.
-  auto R1 = SO3::Expmap(v1.head<3>());
-  EXPECT((Q1.topLeft().isApprox(R1)));
+  auto R1 = SO3::Expmap(v1.tail<3>()).matrix();
+  EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1)));
 
   // Same here
-  auto R2 = SO3::Expmap(v2.head<3>());
-  EXPECT((Q2.topLeft().isApprox(R2)));
+  auto R2 = SO3::Expmap(v2.tail<3>()).matrix();
+  EXPECT((Q2.matrix().topLeftCorner<3, 3>().isApprox(R2)));
 
   // Check commutative subgroups
   for (size_t i = 0; i < 6; i++) {
@@ -69,7 +78,7 @@ TEST(SO4, Expmap) {
     SO4 Q1 = SO4::Expmap(xi);
     xi[i] = 3;
     SO4 Q2 = SO4::Expmap(xi);
-    EXPECT(((Q1 * Q2).isApprox(Q2 * Q1)));
+    EXPECT(assert_equal(Q1 * Q2, Q2 * Q1));
   }
 }
 
@@ -83,7 +92,7 @@ TEST(SO4, Cayley) {
 TEST(SO4, Retract) {
   auto v = Vector6::Zero();
   SO4 actual = traits<SO4>::Retract(id, v);
-  EXPECT(actual.isApprox(id));
+  EXPECT(assert_equal(id, actual));
 }
 
 //******************************************************************************
@@ -108,7 +117,7 @@ TEST(SO4, Invariants) {
   EXPECT(check_manifold_invariants(Q1, Q2));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO4, compose) {
   SO4 expected = Q1 * Q2;
   Matrix actualH1, actualH2;
@@ -124,10 +133,10 @@ TEST(SO4, compose) {
   CHECK(assert_equal(numericalH2, actualH2));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO4, vec) {
-  using Vector16 = SO4::Vector16;
-  const Vector16 expected = Eigen::Map<Vector16>(Q2.data());
+  using Vector16 = SO4::VectorN2;
+  const Vector16 expected = Eigen::Map<const Vector16>(Q2.matrix().data());
   Matrix actualH;
   const Vector16 actual = Q2.vec(actualH);
   CHECK(assert_equal(expected, actual));
@@ -138,31 +147,31 @@ TEST(SO4, vec) {
   CHECK(assert_equal(numericalH, actualH));
 }
 
-/* ************************************************************************* */
-TEST(SO4, topLeft) {
-  const Matrix3 expected = Q3.topLeftCorner<3, 3>();
-  Matrix actualH;
-  const Matrix3 actual = Q3.topLeft(actualH);
-  CHECK(assert_equal(expected, actual));
-  boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
-    return Q3.topLeft();
-  };
-  const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
-  CHECK(assert_equal(numericalH, actualH));
-}
+// /* *************************************************************************
+// */ TEST(SO4, topLeft) {
+//   const Matrix3 expected = Q3.topLeftCorner<3, 3>();
+//   Matrix actualH;
+//   const Matrix3 actual = Q3.topLeft(actualH);
+//   CHECK(assert_equal(expected, actual));
+//   boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
+//     return Q3.topLeft();
+//   };
+//   const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
+//   CHECK(assert_equal(numericalH, actualH));
+// }
 
-/* ************************************************************************* */
-TEST(SO4, stiefel) {
-  const Matrix43 expected = Q3.leftCols<3>();
-  Matrix actualH;
-  const Matrix43 actual = Q3.stiefel(actualH);
-  CHECK(assert_equal(expected, actual));
-  boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
-    return Q3.stiefel();
-  };
-  const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
-  CHECK(assert_equal(numericalH, actualH));
-}
+// /* *************************************************************************
+// */ TEST(SO4, stiefel) {
+//   const Matrix43 expected = Q3.leftCols<3>();
+//   Matrix actualH;
+//   const Matrix43 actual = Q3.stiefel(actualH);
+//   CHECK(assert_equal(expected, actual));
+//   boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
+//     return Q3.stiefel();
+//   };
+//   const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
+//   CHECK(assert_equal(numericalH, actualH));
+// }
 
 //******************************************************************************
 int main() {