Merge pull request #495 from borglab/jingwu/shonan

Refactor MakeATangentVector by using VectorValues and fix testcase failure

gtsam/sfm/ShonanAveraging.cpp

@@ -643,20 +643,25 @@ bool ShonanAveraging<d>::checkOptimality(const Values &values) const {
 }
 
 /* ************************************************************************* */
-/// Create a tangent direction xi with eigenvector segment v_i
 template <size_t d>
-Vector ShonanAveraging<d>::MakeATangentVector(size_t p, const Vector &v,
-                                              size_t i) {
+VectorValues ShonanAveraging<d>::TangentVectorValues(size_t p,
+                                                     const Vector &v) {
+  VectorValues delta;
   // Create a tangent direction xi with eigenvector segment v_i
   const size_t dimension = SOn::Dimension(p);
-  const auto v_i = v.segment<d>(d * i);
-  Vector xi = Vector::Zero(dimension);
-  double sign = pow(-1.0, round((p + 1) / 2) + 1);
-  for (size_t j = 0; j < d; j++) {
-    xi(j + p - d - 1) = sign * v_i(d - j - 1);
-    sign = -sign;
+  double sign0 = pow(-1.0, round((p + 1) / 2) + 1);
+  for (size_t i = 0; i < v.size() / d; i++) {
+    // Assumes key is 0-based integer
+    const auto v_i = v.segment<d>(d * i);
+    Vector xi = Vector::Zero(dimension);
+    double sign = sign0;
+    for (size_t j = 0; j < d; j++) {
+      xi(j + p - d - 1) = sign * v_i(d - j - 1);
+      sign = -sign;
+    }
+    delta.insert(i, xi);
   }
-  return xi;
+  return delta;
 }
 
 /* ************************************************************************* */
@@ -690,14 +695,8 @@ template <size_t d>
 Values ShonanAveraging<d>::LiftwithDescent(size_t p, const Values &values,
                                            const Vector &minEigenVector) {
   Values lifted = LiftTo<SOn>(p, values);
-  for (auto it : lifted.filter<SOn>()) {
-    // Create a tangent direction xi with eigenvector segment v_i
-    // Assumes key is 0-based integer
-    const Vector xi = MakeATangentVector(p, minEigenVector, it.key);
-    // Move the old value in the descent direction
-    it.value = it.value.retract(xi);
-  }
-  return lifted;
+  VectorValues delta = TangentVectorValues(p, minEigenVector);
+  return lifted.retract(delta);
 }
 
 /* ************************************************************************* */

gtsam/sfm/ShonanAveraging.h

@@ -20,12 +20,13 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
+#include <gtsam/dllexport.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/Rot3.h>
+#include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/LevenbergMarquardtParams.h>
 #include <gtsam/sfm/BinaryMeasurement.h>
 #include <gtsam/slam/dataset.h>
-#include <gtsam/dllexport.h>
 
 #include <Eigen/Sparse>
 #include <map>
@@ -200,8 +201,8 @@ public:
   /// Project pxdN Stiefel manifold matrix S to Rot3^N
   Values roundSolutionS(const Matrix &S) const;
 
-  /// Create a tangent direction xi with eigenvector segment v_i
-  static Vector MakeATangentVector(size_t p, const Vector &v, size_t i);
+  /// Create a VectorValues with eigenvector v_i
+  static VectorValues TangentVectorValues(size_t p, const Vector &v);
 
   /// Calculate the riemannian gradient of F(values) at values
   Matrix riemannianGradient(size_t p, const Values &values) const;

gtsam/sfm/tests/testShonanAveraging.cpp

@@ -121,18 +121,17 @@ TEST(ShonanAveraging3, tryOptimizingAt4) {
 }
 
 /* ************************************************************************* */
-TEST(ShonanAveraging3, MakeATangentVector) {
+TEST(ShonanAveraging3, TangentVectorValues) {
   Vector9 v;
   v << 1, 2, 3, 4, 5, 6, 7, 8, 9;
-  Matrix expected(5, 5);
-  expected << 0, 0, 0, 0, -4, //
-      0, 0, 0, 0, -5,         //
-      0, 0, 0, 0, -6,         //
-      0, 0, 0, 0, 0,          //
-      4, 5, 6, 0, 0;
-  const Vector xi_1 = ShonanAveraging3::MakeATangentVector(5, v, 1);
-  const auto actual = SOn::Hat(xi_1);
-  CHECK(assert_equal(expected, actual));
+  Vector expected0(10), expected1(10), expected2(10);
+  expected0 << 0, 3, -2, 1, 0, 0, 0, 0, 0, 0;
+  expected1 << 0, 6, -5, 4, 0, 0, 0, 0, 0, 0;
+  expected2 << 0, 9, -8, 7, 0, 0, 0, 0, 0, 0;
+  const VectorValues xi = ShonanAveraging3::TangentVectorValues(5, v);
+  EXPECT(assert_equal(expected0, xi[0]));
+  EXPECT(assert_equal(expected1, xi[1]));
+  EXPECT(assert_equal(expected2, xi[2]));
 }
 
 /* ************************************************************************* */
@@ -168,7 +167,8 @@ TEST(ShonanAveraging3, CheckWithEigen) {
       minEigenValue = min(lambdas(i), minEigenValue);
 
   // Actual check
-  EXPECT_DOUBLES_EQUAL(minEigenValue, lambda, 1e-12);
+  EXPECT_DOUBLES_EQUAL(0, lambda, 1e-11);
+  EXPECT_DOUBLES_EQUAL(0, minEigenValue, 1e-11);
 
   // Construct test descent direction (as minEigenVector is not predictable
   // across platforms, being one from a basically flat 3d- subspace)