Added prototype derivatives for transform_from

gtsam_unstable/geometry/Similarity3.cpp

@@ -63,7 +63,8 @@ bool Similarity3::equals(const Similarity3& sim, double tol) const {
       && s_ > (sim.s_ - tol);
 }
 
-Point3 Similarity3::transform_from(const Point3& p) const {
+Point3 Similarity3::transform_from(const Point3& p, //
+    OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const {
   return R_ * (s_ * p) + t_;
 }
 
@@ -72,8 +73,9 @@ Matrix7 Similarity3::AdjointMap() const {
   const Vector3 t = t_.vector();
   Matrix3 A = s_ * skewSymmetric(t) * R;
   Matrix7 adj;
-  adj << s_ * R, A, -s_ * t, Z_3x3, R, Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<
-      double, 1, 6>::Zero(), 1;
+  adj << s_ * R, A, -s_ * t, Z_3x3, R, //
+  Matrix31::Zero(), //
+  Matrix16::Zero(), 1;
   return adj;
 }
 

gtsam_unstable/geometry/Similarity3.h

@@ -82,7 +82,9 @@ public:
   /// Return the inverse
   Similarity3 inverse() const;
 
-  Point3 transform_from(const Point3& p) const;
+  Point3 transform_from(const Point3& p, //
+      OptionalJacobian<3, 7> H1 = boost::none, //
+      OptionalJacobian<3, 3> H2 = boost::none) const;
 
   /** syntactic sugar for transform_from */
   Point3 operator*(const Point3& p) const;
@@ -96,17 +98,17 @@ public:
   /// Return a GTSAM rotation
   const Rot3& rotation() const {
     return R_;
-  };
+  }
 
   /// Return a GTSAM translation
   const Point3& translation() const {
     return t_;
-  };
+  }
 
   /// Return the scale
   double scale() const {
     return s_;
-  };
+  }
 
   /// Convert to a rigid body pose
   operator Pose3() const;
@@ -114,12 +116,12 @@ public:
   /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
   inline static size_t Dim() {
     return 7;
-  };
+  }
 
   /// Dimensionality of tangent space = 7 DOF
   inline size_t dim() const {
     return 7;
-  };
+  }
 
   /// @}
   /// @name Chart
@@ -130,7 +132,8 @@ public:
     static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none);
 
     /// 7-dimensional vector v in tangent space that makes other = this->retract(v)
-  static Vector7 Local(const Similarity3& other,  ChartJacobian H = boost::none);
+    static Vector7 Local(const Similarity3& other,
+        ChartJacobian H = boost::none);
   };
 
   /// Project from one tangent space to another
@@ -141,12 +144,16 @@ public:
   /// @{
 
   /// Not currently implemented, required because this is a lie group
-  static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none);
-  static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none);
+  static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm =
+      boost::none);
+  static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm =
+      boost::none);
 
-  using LieGroup<Similarity3, 7>::inverse; // version with derivative
+  using LieGroup<Similarity3, 7>::inverse;
+  // version with derivative
 };
 
 template<>
-struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
+struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {
+};
 }

gtsam_unstable/geometry/tests/testSimilarity3.cpp

@@ -23,10 +23,14 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Testable.h>
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
+
 using namespace gtsam;
 using namespace std;
 using symbol_shorthand::X;
@@ -40,6 +44,9 @@ static Similarity3 T2(Rot3::rodriguez(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2),
     1);
 static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1);
 
+// Simpler transform
+Similarity3 T4(Rot3(), Point3(1, 1, 0), 2);
+
 //******************************************************************************
 TEST(Similarity3, Constructors) {
   Similarity3 test;
@@ -154,6 +161,24 @@ TEST(Similarity3, manifold_first_order) {
   EXPECT(assert_equal(t1, t2.retract(d21)));
 }
 
+//******************************************************************************
+// 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 derivative
+  boost::function<Point3(Similarity3,Point3)> f = boost::bind(
+      &Similarity3::transform_from, _1, _2, boost::none, boost::none);
+  Point3 q(1, 0, 0);
+  Matrix expectedH1 = numericalDerivative21<Point3,Similarity3,Point3>(f,T4,q);
+  Matrix expectedH2 = numericalDerivative22<Point3,Similarity3,Point3>(f,T4,q);
+  Matrix actualH1,actualH2;
+  Point3 p = T4.transform_from(q,actualH1,actualH2);
+  EXPECT(assert_equal(expectedH1, actualH1));
+  EXPECT(assert_equal(expectedH2, actualH2));
+}
+
 //******************************************************************************
 // Test very simple prior optimization example
 TEST(Similarity3, Optimization) {