wedge

cpp/Lie.h

@@ -8,7 +8,7 @@
 #pragma once
 
 #include <string>
-#include "Vector.h"
+#include "Matrix.h"
 
 namespace gtsam {
 
@@ -119,4 +119,22 @@ namespace gtsam {
   			bracket(X, X_Y));
   }
 
+  /**
+   * Declaration of wedge (see Murray94book) used to convert
+   * from n exponential coordinates to n*n element of the Lie algebra
+   */
+  template <class T> Matrix wedge(const Vector& x);
+
+  /**
+   * Exponential map given exponential coordinates
+   * class T needs a wedge<> function and a constructor from Matrix
+   * @param x exponential coordinates, vector of size n
+   * @ return a T
+   */
+  template <class T>
+  T expm(const Vector& x, int K=7) {
+  	Matrix xhat = wedge<T>(x);
+    return expm(xhat,K);
+  }
+
 } // namespace gtsam

cpp/Pose3.cpp

@@ -22,7 +22,7 @@ namespace gtsam {
   // Calculate Adjoint map
   // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
   // Experimental - unit tests of derivatives based on it do not check out yet
-  static Matrix AdjointMap(const Pose3& p) {
+  Matrix AdjointMap(const Pose3& p) {
 		const Matrix R = p.rotation().matrix();
 		const Vector t = p.translation().vector();
 		Matrix A = skewSymmetric(t)*R;

cpp/Pose3.h

@@ -145,4 +145,31 @@ namespace gtsam {
   Pose3 between(const Pose3& p1, const Pose3& p2,
   		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
 
+  /**
+   * wedge for Pose3:
+   * @param xi 6-dim twist (omega,v) where
+   *  omega = (wx,wy,wz) 3D angular velocity
+   *  v (vx,vy,vz) = 3D velocity
+   * @return xihat, 4*4 element of Lie algebra that can be exponentiated
+   */
+  inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
+  	return Matrix_(4,4,
+  			 0.,-wz,  wy,  vx,
+  			 wz,  0.,-wx,  vy,
+  			-wy, wx,   0., vz,
+  			 0.,  0.,  0.,  0.);
+  }
+
+  /**
+   * wedge for Pose3:
+   * @param xi 6-dim twist (omega,v) where
+   *  omega = 3D angular velocity
+   *  v = 3D velocity
+   * @return xihat, 4*4 element of Lie algebra that can be exponentiated
+   */
+  template <>
+  inline Matrix wedge<Pose3>(const Vector& xi) {
+  	return wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
+  }
+
 } // namespace gtsam

cpp/testPose3.cpp

@@ -3,7 +3,6 @@
  * @brief  Unit tests for Pose3 class
  */
 
-
 #include <CppUnitLite/TestHarness.h>
 #include "numericalDerivative.h"
 #include "Pose3.h"
@@ -51,6 +50,31 @@ TEST(Pose3, expmap_b)
   CHECK(assert_equal(expected, p2));
 }
 
+/* ************************************************************************* *
+// test case for screw motion in the plane
+namespace screw {
+  double a=0.3, c=cos(a), s=sin(a), w=0.3;
+	Vector xi = Vector_(6, 0.0, 0.0, w, w, 0.0, 0.0);
+	Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
+	Point3 expectedT(0.29552, 0.0446635, 0);
+	Pose3 expected(expectedR, expectedT);
+}
+
+TEST(Pose3, expmap_c)
+{
+  CHECK(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
+  CHECK(assert_equal(screw::expected, expmap<Pose3>(screw::xi),1e-6));
+}
+
+TEST(Pose3, Adjoint)
+{
+	// assert that T*exp(xi)*T^-1
+	Pose3 expected = T * expmap<Pose3>(screw::xi) * inverse(T);
+  // is equal to exp(Ad_T(xi))
+	Vector xiprime = Adjoint(T, screw::xi);
+	CHECK(assert_equal(expected, expmap<Pose3>(xiprime), 1e-6));
+}
+
 /* ************************************************************************* */
 TEST( Pose3, compose )
 {