diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 2485418cf..739fd9ebe 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -643,20 +643,26 @@ bool ShonanAveraging::checkOptimality(const Values &values) const { } /* ************************************************************************* */ -/// Create a tangent direction xi with eigenvector segment v_i +/// Create a VectorValues with eigenvector v template -Vector ShonanAveraging::MakeATangentVector(size_t p, const Vector &v, - size_t i) { +VectorValues ShonanAveraging::MakeATangentVectorValues(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; + for (size_t i = 0; i < v.size() / d; i++) { + // Create a tangent direction xi with eigenvector segment v_i + // Assumes key is 0-based integer + 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; + } + delta.insert(i, xi); } - return xi; + return delta; } /* ************************************************************************* */ @@ -690,14 +696,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 = MakeATangentVectorValues(p, minEigenVector); + return lifted.retract(delta); } /* ************************************************************************* */ diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index ed94329a2..cada7cfd6 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 @@ 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 MakeATangentVectorValues(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 cc4319e15..1cb5c06d1 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -121,7 +121,7 @@ TEST(ShonanAveraging3, tryOptimizingAt4) { } /* ************************************************************************* */ -TEST(ShonanAveraging3, MakeATangentVector) { +TEST(ShonanAveraging3, MakeATangentVectorValues) { Vector9 v; v << 1, 2, 3, 4, 5, 6, 7, 8, 9; Matrix expected(5, 5); @@ -130,8 +130,8 @@ TEST(ShonanAveraging3, MakeATangentVector) { 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); + const VectorValues delta = ShonanAveraging3::MakeATangentVectorValues(5, v); + const auto actual = SOn::Hat(delta[1]); CHECK(assert_equal(expected, actual)); }