diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 2485418cfa..05f7f4bcc4 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -643,20 +643,25 @@ bool ShonanAveraging::checkOptimality(const Values &values) const { } /* ************************************************************************* */ -/// Create a tangent direction xi with eigenvector segment v_i template -Vector ShonanAveraging::MakeATangentVector(size_t p, const Vector &v, - size_t i) { +VectorValues ShonanAveraging::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 * 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 * 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 Values ShonanAveraging::LiftwithDescent(size_t p, const Values &values, const Vector &minEigenVector) { Values lifted = LiftTo(p, values); - for (auto it : lifted.filter()) { - // 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); } /* ************************************************************************* */ diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index ed94329a27..3fee780dd1 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -20,12 +20,13 @@ #include #include +#include #include #include +#include #include #include #include -#include #include #include @@ -200,8 +201,8 @@ template class GTSAM_EXPORT ShonanAveraging { /// 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; diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index cc4319e150..7cd2af860d 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/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)