Initial broken commit of Similarity Transform

gtsam_unstable/geometry/tests/testSimilarity3.cpp

@@ -0,0 +1,125 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testSimilarity3.cpp
+ * @brief  Unit tests for Similarity3 class
+ */
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Point3.h>
+
+namespace gtsam {
+
+/**
+ * 3D similarity transform
+ */
+class Similarity3: private Matrix4 {
+  Similarity3() :
+      Matrix4::Identity() {
+  }
+
+  /// Construct pure scaling
+  Similarity3(double s) :
+      Matrix4::Identity() {
+    this->topLeftCorner<3, 3>() = s * Matrix3::Identity();
+  }
+
+  /// Construct from Eigen types
+  Similarity3(const Matrix3& R, const Vector3& t, double s) {
+    *this << s * R, t, 0, 0, 0, 1;
+  }
+
+  /// Construct from GTSAM types
+  Similarity3(const Rot3& R, const Point3& t, double s) {
+    *this << s * R.matrix(), t.vector, 0, 0, 0, 1;
+  }
+
+  /// @name Manifold
+  /// @{
+
+  /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
+  inline static size_t Dim() {
+    return 7;
+  }
+
+  /// Dimensionality of tangent space = 3 DOF
+  inline size_t dim() const {
+    return 7;
+  }
+
+  /// Update Similarity transform via 7-dim vector in tangent space
+  Similarity3 retract(const Vector& v) const {
+
+    // Get rotation - translation - scale from 4*4
+    Rot3 R(this->topLeftCorner<3, 3>);
+    Vector3 t(this->topRightCorner<3, 1>);
+    double s(this->at(3, 3));
+
+    return Similarity3( //
+        R.retract(v.head<3>()), // retract rotation using v[0,1,2]
+        t + v.vector.segment < 3 > (3), // retract translation via v[3,4,5]
+        s + v[6]); // finally, update scale using v[6]
+  }
+
+  /// 7-dimensional vector v in tangent space that makes other = this->retract(v)
+  Vector localCoordinates(const Similarity3& other) const {
+    return Vector7::Zero();
+  }
+
+  /// @}
+
+  /// @name Lie Group
+  /// @{
+
+  // compose T1*T2
+  // between T2*inverse(T1)
+  // identity I4
+  // inverse inverse(T)
+
+  /// @}
+
+};
+}
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace gtsam;
+using namespace std;
+
+//******************************************************************************
+TEST(Similarity3, constructors) {
+  Similarity3 test;
+}
+
+//******************************************************************************
+TEST(Similarity3, manifold) {
+  EXPECT_LONGS_EQUAL(7, Similarity3::Dim());
+  Vector7 z = Vector7::Zero();
+  Similarity3 sim;
+  EXPECT(sim.retract(z)==sim);
+
+  Vector7 v = Vector7::Zero();
+  v(6) = 2;
+  Similarity3 sim;
+  EXPECT(sim.retract(z)==sim);
+
+  // TODO add unit tests for retract and localCoordinates
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
+