diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp
new file mode 100644
index 000000000..ebbc91c01
--- /dev/null
+++ b/gtsam/geometry/SO4.cpp
@@ -0,0 +1,226 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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    SO4.cpp
+ * @brief   4*4 matrix representation of SO(4)
+ * @author  Frank Dellaert
+ * @author  Luca Carlone
+ */
+
+#include <gtsam/base/concepts.h>
+#include <gtsam/base/timing.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/geometry/Unit3.h>
+
+#include <Eigen/Eigenvalues>
+#include <boost/random.hpp>
+
+#include <cmath>
+#include <iostream>
+
+using namespace std;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+static Vector3 randomOmega(boost::mt19937 &rng) {
+  static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
+  return Unit3::Random(rng).unitVector() * randomAngle(rng);
+}
+
+/* ************************************************************************* */
+// Create random SO(4) element using direct product of lie algebras.
+SO4 SO4::Random(boost::mt19937 &rng) {
+  Vector6 delta;
+  delta << randomOmega(rng), randomOmega(rng);
+  return SO4::Expmap(delta);
+}
+
+/* ************************************************************************* */
+void SO4::print(const string &s) const { cout << s << *this << endl; }
+
+/* ************************************************************************* */
+bool SO4::equals(const SO4 &R, double tol) const {
+  return equal_with_abs_tol(*this, R, tol);
+}
+
+//******************************************************************************
+Matrix4 SO4::Hat(const Vector6 &xi) {
+  // skew symmetric matrix X = xi^
+  // Unlike Luca, makes upper-left the SO(3) subgroup.
+  // See also
+  // http://scipp.ucsc.edu/~haber/archives/physics251_13/groups13_sol4.pdf
+  Matrix4 Y = Z_4x4;
+  Y(0, 1) = -xi(2);
+  Y(0, 2) = +xi(1);
+  Y(1, 2) = -xi(0);
+  Y(0, 3) = -xi(3);
+  Y(1, 3) = -xi(4);
+  Y(2, 3) = -xi(5);
+  return Y - Y.transpose();
+}
+/* ************************************************************************* */
+Vector6 SO4::Vee(const Matrix4 &X) {
+  Vector6 xi;
+  xi(2) = -X(0, 1);
+  xi(1) = X(0, 2);
+  xi(0) = -X(1, 2);
+  xi(3) = -X(0, 3);
+  xi(4) = -X(1, 3);
+  xi(5) = -X(2, 3);
+  return xi;
+}
+
+//******************************************************************************
+/* Exponential map, porting MATLAB implementation by Luca, which follows
+ * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
+ * Ramona-Andreaa Rohan */
+SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H) {
+  if (H) throw std::runtime_error("SO4::Expmap Jacobian");
+  gttic(SO4_Expmap);
+
+  // skew symmetric matrix X = xi^
+  const Matrix4 X = Hat(xi);
+
+  // do eigen-decomposition
+  auto eig = Eigen::EigenSolver<Matrix4>(X);
+  Eigen::Vector4cd e = eig.eigenvalues();
+  using std::abs;
+  sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
+    return abs(a.imag()) > abs(b.imag());
+  });
+
+  // Get a and b from eigenvalues +/i ai and +/- bi
+  double a = e[0].imag(), b = e[2].imag();
+  if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
+    throw runtime_error("SO4::Expmap: wrong eigenvalues.");
+  }
+
+  // Build expX = exp(xi^)
+  Matrix4 expX;
+  using std::cos;
+  using std::sin;
+  const auto X2 = X * X;
+  const auto X3 = X2 * X;
+  double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
+  if (a != 0 && b == 0) {
+    double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
+    return I_4x4 + X + c2 * X2 + c3 * X3;
+  } else if (a == b && b != 0) {
+    double sin_a = sin(a), cos_a = cos(a);
+    double c0 = (a * sin_a + 2 * cos_a) / 2,
+           c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
+           c3 = (sin_a - a * cos_a) / (2 * a3);
+    return c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3;
+  } else if (a != b) {
+    double sin_a = sin(a), cos_a = cos(a);
+    double sin_b = sin(b), cos_b = cos(b);
+    double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
+           c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
+           c2 = (cos_a - cos_b) / (b2 - a2),
+           c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
+    return c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3;
+  } else {
+    return I_4x4;
+  }
+}
+
+//******************************************************************************
+Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) {
+  if (H) throw std::runtime_error("SO4::Logmap Jacobian");
+  throw std::runtime_error("SO4::Logmap");
+}
+
+/* ************************************************************************* */
+SO4 SO4::ChartAtOrigin::Retract(const Vector6 &xi, ChartJacobian H) {
+  if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
+  gttic(SO4_Retract);
+  const Matrix4 X = Hat(xi / 2);
+  return (I_4x4 + X) * (I_4x4 - X).inverse();
+}
+
+/* ************************************************************************* */
+Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H) {
+  if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
+  const Matrix4 X = (I_4x4 - Q) * (I_4x4 + Q).inverse();
+  return -2 * Vee(X);
+}
+
+/* ************************************************************************* */
+static SO4::Vector16 vec(const SO4 &Q) {
+  return Eigen::Map<const SO4::Vector16>(Q.data());
+}
+
+static const std::vector<const Matrix4> G(
+    {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
+     SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
+     SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});
+
+static const Eigen::Matrix<double, 16, 6> P =
+    (Eigen::Matrix<double, 16, 6>() << vec(G[0]), vec(G[1]), vec(G[2]),
+     vec(G[3]), vec(G[4]), vec(G[5]))
+        .finished();
+
+/* ************************************************************************* */
+Matrix6 SO4::AdjointMap() const {
+  gttic(SO4_AdjointMap);
+  // Elaborate way of calculating the AdjointMap
+  // TODO(frank): find a closed form solution. In SO(3) is just R :-/
+  const SO4 &Q = *this;
+  const SO4 Qt = this->inverse();
+  Matrix6 A;
+  for (size_t i = 0; i < 6; i++) {
+    // Calculate column i of linear map for coeffcient of Gi
+    A.col(i) = SO4::Vee(Q * G[i] * Qt);
+  }
+  return A;
+}
+
+/* ************************************************************************* */
+SO4::Vector16 SO4::vec(OptionalJacobian<16, 6> H) const {
+  const SO4 &Q = *this;
+  if (H) {
+    // As Luca calculated, this is (I4 \oplus Q) * P
+    *H << Q * P.block<4, 6>(0, 0), Q * P.block<4, 6>(4, 0),
+        Q * P.block<4, 6>(8, 0), Q * P.block<4, 6>(12, 0);
+  }
+  return gtsam::vec(Q);
+};
+
+/* ************************************************************************* */
+Matrix3 SO4::topLeft(OptionalJacobian<9, 6> H) const {
+  const Matrix3 M = this->topLeftCorner<3, 3>();
+  const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
+                q = this->topRightCorner<3, 1>();
+  if (H) {
+    *H << Z_3x1, -m3, m2, q, Z_3x1, Z_3x1,  //
+        m3, Z_3x1, -m1, Z_3x1, q, Z_3x1,    //
+        -m2, m1, Z_3x1, Z_3x1, Z_3x1, q;
+  }
+  return M;
+}
+
+/* ************************************************************************* */
+Matrix43 SO4::stiefel(OptionalJacobian<12, 6> H) const {
+  const Matrix43 M = this->leftCols<3>();
+  const auto &m1 = col(0), m2 = col(1), m3 = col(2), q = col(3);
+  if (H) {
+    *H << Z_4x1, -m3, m2, q, Z_4x1, Z_4x1,  //
+        m3, Z_4x1, -m1, Z_4x1, q, Z_4x1,    //
+        -m2, m1, Z_4x1, Z_4x1, Z_4x1, q;
+  }
+  return M;
+}
+
+/* ************************************************************************* */
+
+}  // end namespace gtsam
diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h
new file mode 100644
index 000000000..e270c9555
--- /dev/null
+++ b/gtsam/geometry/SO4.h
@@ -0,0 +1,146 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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    SO4.h
+ * @brief   4*4 matrix representation of SO(4)
+ * @author  Frank Dellaert
+ * @author  Luca Carlone
+ * @date    March 2019
+ */
+
+#pragma once
+
+#include <gtsam/base/Group.h>
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Matrix.h>
+
+#include <boost/random/mersenne_twister.hpp>
+
+#include <iosfwd>
+#include <string>
+
+namespace gtsam {
+
+/**
+ *  True SO(4), i.e., 4*4 matrix subgroup
+ */
+class SO4 : public Matrix4, public LieGroup<SO4, 6> {
+ public:
+  enum { N = 4 };
+  enum { dimension = 6 };
+
+  typedef Eigen::Matrix<double, 16, 1> Vector16;
+
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor creates identity
+  SO4() : Matrix4(I_4x4) {}
+
+  /// Constructor from Eigen Matrix
+  template <typename Derived>
+  SO4(const MatrixBase<Derived> &R) : Matrix4(R.eval()) {}
+
+  /// Random SO(4) element (no big claims about uniformity)
+  static SO4 Random(boost::mt19937 &rng);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  void print(const std::string &s) const;
+  bool equals(const SO4 &R, double tol) const;
+
+  /// @}
+  /// @name Group
+  /// @{
+
+  /// identity rotation for group operation
+  static SO4 identity() { return I_4x4; }
+
+  /// inverse of a rotation = transpose
+  SO4 inverse() const { return this->transpose(); }
+
+  /// @}
+  /// @name Lie Group
+  /// @{
+
+  static Matrix4 Hat(const Vector6 &xi);  ///< make skew symmetric matrix
+  static Vector6 Vee(const Matrix4 &X);   ///< inverse of Hat
+  static SO4 Expmap(const Vector6 &xi,
+                    ChartJacobian H = boost::none);  ///< exponential map
+  static Vector6 Logmap(const SO4 &Q,
+                        ChartJacobian H = boost::none);  ///< and its inverse
+  Matrix6 AdjointMap() const;
+
+  // Chart at origin
+  struct ChartAtOrigin {
+    static SO4 Retract(const Vector6 &omega, ChartJacobian H = boost::none);
+    static Vector6 Local(const SO4 &R, ChartJacobian H = boost::none);
+  };
+
+  using LieGroup<SO4, 6>::inverse;
+
+  /// @}
+  /// @name Other methods
+  /// @{
+
+  /// Vectorize
+  Vector16 vec(OptionalJacobian<16, 6> H = boost::none) const;
+
+  /// Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3).
+  Matrix3 topLeft(OptionalJacobian<9, 6> H = boost::none) const;
+
+  /// Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> S \in St(3,4).
+  Matrix43 stiefel(OptionalJacobian<12, 6> H = boost::none) const;
+
+  /// Return matrix (for wrapper)
+  const Matrix4 &matrix() const { return *this; }
+
+  /// @
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &boost::serialization::make_nvp("Q11", (*this)(0, 0));
+    ar &boost::serialization::make_nvp("Q12", (*this)(0, 1));
+    ar &boost::serialization::make_nvp("Q13", (*this)(0, 2));
+    ar &boost::serialization::make_nvp("Q14", (*this)(0, 3));
+
+    ar &boost::serialization::make_nvp("Q21", (*this)(1, 0));
+    ar &boost::serialization::make_nvp("Q22", (*this)(1, 1));
+    ar &boost::serialization::make_nvp("Q23", (*this)(1, 2));
+    ar &boost::serialization::make_nvp("Q24", (*this)(1, 3));
+
+    ar &boost::serialization::make_nvp("Q31", (*this)(2, 0));
+    ar &boost::serialization::make_nvp("Q32", (*this)(2, 1));
+    ar &boost::serialization::make_nvp("Q33", (*this)(2, 2));
+    ar &boost::serialization::make_nvp("Q34", (*this)(2, 3));
+
+    ar &boost::serialization::make_nvp("Q41", (*this)(3, 0));
+    ar &boost::serialization::make_nvp("Q42", (*this)(3, 1));
+    ar &boost::serialization::make_nvp("Q43", (*this)(3, 2));
+    ar &boost::serialization::make_nvp("Q44", (*this)(3, 3));
+  }
+
+};  // SO4
+
+template <>
+struct traits<SO4> : Testable<SO4>, internal::LieGroupTraits<SO4> {};
+
+template <>
+struct traits<const SO4> : Testable<SO4>, internal::LieGroupTraits<SO4> {};
+
+}  // end namespace gtsam
diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp
new file mode 100644
index 000000000..037280136
--- /dev/null
+++ b/gtsam/geometry/tests/testSO4.cpp
@@ -0,0 +1,172 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testSO4.cpp
+ * @brief  Unit tests for SO4, as a GTSAM-adapted Lie Group
+ * @author Frank Dellaert
+ **/
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/lieProxies.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/SO3.h>
+#include <gtsam/geometry/SO4.h>
+
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+//******************************************************************************
+TEST(SO4, Concept) {
+  BOOST_CONCEPT_ASSERT((IsGroup<SO4>));
+  BOOST_CONCEPT_ASSERT((IsManifold<SO4>));
+  BOOST_CONCEPT_ASSERT((IsLieGroup<SO4>));
+}
+
+//******************************************************************************
+SO4 id;
+Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 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();
+SO4 Q2 = SO4::Expmap(v2);
+Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
+SO4 Q3 = SO4::Expmap(v3);
+
+//******************************************************************************
+TEST(SO4, Random) {
+  boost::mt19937 rng(42);
+  auto Q = SO4::Random(rng);
+  EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
+}
+//******************************************************************************
+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)));
+
+  // Same here
+  auto R2 = SO3::Expmap(v2.head<3>());
+  EXPECT((Q2.topLeft().isApprox(R2)));
+
+  // Check commutative subgroups
+  for (size_t i = 0; i < 6; i++) {
+    Vector6 xi = Vector6::Zero();
+    xi[i] = 2;
+    SO4 Q1 = SO4::Expmap(xi);
+    xi[i] = 3;
+    SO4 Q2 = SO4::Expmap(xi);
+    EXPECT(((Q1 * Q2).isApprox(Q2 * Q1)));
+  }
+}
+
+//******************************************************************************
+TEST(SO4, Cayley) {
+  CHECK(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100)));
+  CHECK(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100)));
+}
+
+//******************************************************************************
+TEST(SO4, Retract) {
+  auto v = Vector6::Zero();
+  SO4 actual = traits<SO4>::Retract(id, v);
+  EXPECT(actual.isApprox(id));
+}
+
+//******************************************************************************
+TEST(SO4, Local) {
+  auto v0 = Vector6::Zero();
+  Vector6 actual = traits<SO4>::Local(id, id);
+  EXPECT(assert_equal((Vector)v0, actual));
+}
+
+//******************************************************************************
+TEST(SO4, Invariants) {
+  EXPECT(check_group_invariants(id, id));
+  EXPECT(check_group_invariants(id, Q1));
+  EXPECT(check_group_invariants(Q2, id));
+  EXPECT(check_group_invariants(Q2, Q1));
+  EXPECT(check_group_invariants(Q1, Q2));
+
+  EXPECT(check_manifold_invariants(id, id));
+  EXPECT(check_manifold_invariants(id, Q1));
+  EXPECT(check_manifold_invariants(Q2, id));
+  EXPECT(check_manifold_invariants(Q2, Q1));
+  EXPECT(check_manifold_invariants(Q1, Q2));
+}
+
+/* ************************************************************************* */
+TEST(SO4, compose) {
+  SO4 expected = Q1 * Q2;
+  Matrix actualH1, actualH2;
+  SO4 actual = Q1.compose(Q2, actualH1, actualH2);
+  CHECK(assert_equal(expected, actual));
+
+  Matrix numericalH1 =
+      numericalDerivative21(testing::compose<SO4>, Q1, Q2, 1e-2);
+  CHECK(assert_equal(numericalH1, actualH1));
+
+  Matrix numericalH2 =
+      numericalDerivative22(testing::compose<SO4>, Q1, Q2, 1e-2);
+  CHECK(assert_equal(numericalH2, actualH2));
+}
+
+/* ************************************************************************* */
+TEST(SO4, vec) {
+  using Vector16 = SO4::Vector16;
+  const Vector16 expected = Eigen::Map<Vector16>(Q2.data());
+  Matrix actualH;
+  const Vector16 actual = Q2.vec(actualH);
+  CHECK(assert_equal(expected, actual));
+  boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
+    return Q.vec();
+  };
+  const Matrix numericalH = numericalDerivative11(f, Q2, 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));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************