Putting sub matrices along the diagonal of a big matrix. dexpL for Rot3 and Point3

gtsam/base/Matrix.cpp

@@ -72,6 +72,11 @@ Matrix zeros( size_t m, size_t n ) {
   return Matrix::Zero(m,n);
 }
 
+/* ************************************************************************* */
+Matrix ones( size_t m, size_t n ) {
+  return Matrix::Ones(m,n);
+}
+
 /* ************************************************************************* */
 Matrix eye( size_t m, size_t n) {
   return Matrix::Identity(m, n);
@@ -280,6 +285,23 @@ void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j)
   fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
 }
 
+/* ************************************************************************* */
+Matrix diag(const std::vector<Matrix>& Hs) {
+  size_t rows = 0, cols = 0;
+  for (size_t i = 0; i<Hs.size(); ++i) {
+    rows+= Hs[i].rows();
+    cols+= Hs[i].cols();
+  }
+  Matrix results(rows,cols);
+  size_t r = 0, c = 0;
+  for (size_t i = 0; i<Hs.size(); ++i) {
+    insertSub(results, Hs[i], r, c);
+    r+=Hs[i].rows();
+    c+=Hs[i].cols();
+  }
+  return results;
+}
+
 /* ************************************************************************* */
 void insertColumn(Matrix& A, const Vector& col, size_t j) {
   A.col(j) = col;

gtsam/base/Matrix.h

@@ -76,6 +76,11 @@ GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, ...);
  */
 GTSAM_EXPORT Matrix zeros(size_t m, size_t n);
 
+/**
+ * Creates an ones matrix, with matlab-like syntax
+ */
+GTSAM_EXPORT Matrix ones(size_t m, size_t n);
+
 /**
  * Creates an identity matrix, with matlab-like syntax
  *
@@ -238,6 +243,11 @@ Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1,
  */
 GTSAM_EXPORT void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j);
 
+/**
+ * Create a matrix with submatrices along its diagonal
+ */
+GTSAM_EXPORT Matrix diag(const std::vector<Matrix>& Hs);
+
 /**
  * Extracts a column view from a matrix that avoids a copy
  * @param A matrix to extract column from

gtsam/base/tests/testMatrix.cpp

@@ -262,6 +262,30 @@ TEST( matrix, insert_sub )
   EXPECT(assert_equal(expected, big));
 }
 
+/* ************************************************************************* */
+TEST( matrix, diagMatrices )
+{
+  std::vector<Matrix> Hs;
+  Hs.push_back(ones(3,3));
+  Hs.push_back(ones(4,4)*2);
+  Hs.push_back(ones(2,2)*3);
+
+  Matrix actual = diag(Hs);
+
+  Matrix expected = Matrix_(9, 9,
+      1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+      1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+      1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0,
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0);
+
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 TEST( matrix, stream_read ) {
   Matrix expected = Matrix_(3,4,

gtsam/geometry/Point3.h

@@ -143,6 +143,11 @@ namespace gtsam {
     /** Log map at identity - return the x,y,z of this point */
     static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); }
 
+    /// Left-trivialized derivative of the exponential map
+    static Matrix dexpL(const Vector& v) {
+      return eye(3);
+    }
+
     /// @}
     /// @name Vector Space
     /// @{

gtsam/geometry/Rot3.h

@@ -268,6 +268,9 @@ namespace gtsam {
      */
     static Vector3 Logmap(const Rot3& R);
 
+    /// Left-trivialized derivative of the exponential map
+    static Matrix3 dexpL(const Vector3& v);
+
     /// @}
     /// @name Group Action on Point3
     /// @{

gtsam/geometry/Rot3M.cpp

@@ -303,6 +303,19 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const
   }
 }
 
+/* ************************************************************************* */
+/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
+Matrix3 Rot3::dexpL(const Vector3& v) {
+  if(zero(v)) return eye(3);
+  Matrix x = skewSymmetric(v);
+  Matrix x2 = x*x;
+  double theta = v.norm(), vi = theta/2.0;
+  double s1 = sin(vi)/vi;
+  double s2 = (theta - sin(theta))/(theta*theta*theta);
+  Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
+  return res;
+}
+
 /* ************************************************************************* */
 Matrix3 Rot3::matrix() const {
   return rot_;

gtsam/geometry/tests/testRot3M.cpp

@@ -402,6 +402,25 @@ TEST( Rot3, between )
   CHECK(assert_equal(numericalH2,actualH2));
 }
 
+/* ************************************************************************* */
+Vector w = Vector_(3, 0.1, 0.27, -0.2);
+
+// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes?
+// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
+// => y = log (exp(-w) * exp(w+dw))
+Vector testDexpL(const LieVector& dw) {
+  Vector y = Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
+  return y;
+}
+
+TEST( Rot3, dexpL) {
+  Matrix actualDexpL = Rot3::dexpL(w);
+  Matrix expectedDexpL = numericalDerivative11(
+      boost::function<Vector(const LieVector&)>(
+          boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2);
+  EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
+}
+
 /* ************************************************************************* */
 TEST( Rot3, xyz )
 {