From e0e559085673a7f02029cc74b6db7e60e043c371 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Mon, 2 Mar 2015 21:08:23 -0800
Subject: [PATCH] matrix() returns 4*4 matrix \in GL(4)

---
 gtsam_unstable/geometry/Similarity3.cpp         |  7 +++++++
 gtsam_unstable/geometry/Similarity3.h           |  3 +++
 .../geometry/tests/testSimilarity3.cpp          | 17 +++++++++++++++++
 3 files changed, 27 insertions(+)

diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp
index 683f73172..aaa1acf4a 100644
--- a/gtsam_unstable/geometry/Similarity3.cpp
+++ b/gtsam_unstable/geometry/Similarity3.cpp
@@ -68,6 +68,13 @@ Point3 Similarity3::transform_from(const Point3& p, //
   return R_ * (s_ * p) + t_;
 }
 
+const Matrix4 Similarity3::matrix() const {
+  Matrix4 T;
+  T.topRows<3>() << s_ * R_.matrix(), t_.vector();
+  T.bottomRows<1>() << 0, 0, 0, 1;
+  return T;
+}
+
 Matrix7 Similarity3::AdjointMap() const {
   const Matrix3 R = R_.matrix();
   const Vector3 t = t_.vector();
diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h
index 0075f05ec..9e9ece8f5 100644
--- a/gtsam_unstable/geometry/Similarity3.h
+++ b/gtsam_unstable/geometry/Similarity3.h
@@ -95,6 +95,9 @@ public:
   /// @name Standard interface
   /// @{
 
+  /// Calculate 4*4 matrix group equivalent
+  const Matrix4 matrix() const;
+
   /// Return a GTSAM rotation
   const Rot3& rotation() const {
     return R_;
diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
index c68b03d8d..a569e3e8f 100644
--- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp
+++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
@@ -161,12 +161,29 @@ TEST(Similarity3, manifold_first_order) {
   EXPECT(assert_equal(t1, t2.retract(d21)));
 }
 
+//******************************************************************************
+// Return as a 4*4 Matrix
+TEST(Similarity3, Matrix) {
+  Matrix4 expected;
+  expected <<
+      2, 0, 0, 1,
+      0, 2, 0, 1,
+      0, 0, 2, 0,
+      0, 0, 0, 1;
+  Matrix4 actual = T4.matrix();
+  EXPECT(assert_equal(expected, actual));
+}
 //******************************************************************************
 // Group action on Point3 (with simpler transform)
 TEST(Similarity3, GroupAction) {
   EXPECT(assert_equal(Point3(1, 1, 0), T4 * Point3(0, 0, 0)));
   EXPECT(assert_equal(Point3(3, 1, 0), T4 * Point3(1, 0, 0)));
 
+  // Test actual group action on R^4
+  Vector4 qh; qh << 1,0,0,1;
+  Vector4 ph; ph << 3,1,0,1;
+  EXPECT(assert_equal((Vector)ph, T4.matrix()*qh));
+
   // Test derivative
   boost::function<Point3(Similarity3,Point3)> f = boost::bind(
       &Similarity3::transform_from, _1, _2, boost::none, boost::none);