matrix() returns 4*4 matrix \in GL(4)

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();

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_;

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);