diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 739fd9ebe..05f7f4bcc 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -643,19 +643,18 @@ bool ShonanAveraging::checkOptimality(const Values &values) const { } /* ************************************************************************* */ -/// Create a VectorValues with eigenvector v template -VectorValues ShonanAveraging::MakeATangentVectorValues(size_t p, - const Vector &v) { +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); + double sign0 = pow(-1.0, round((p + 1) / 2) + 1); 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); + double sign = sign0; for (size_t j = 0; j < d; j++) { xi(j + p - d - 1) = sign * v_i(d - j - 1); sign = -sign; @@ -696,7 +695,7 @@ template Values ShonanAveraging::LiftwithDescent(size_t p, const Values &values, const Vector &minEigenVector) { Values lifted = LiftTo(p, values); - VectorValues delta = MakeATangentVectorValues(p, minEigenVector); + VectorValues delta = TangentVectorValues(p, minEigenVector); return lifted.retract(delta); } diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index cada7cfd6..3fee780dd 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -202,7 +202,7 @@ public: Values roundSolutionS(const Matrix &S) const; /// Create a VectorValues with eigenvector v_i - static VectorValues MakeATangentVectorValues(size_t p, const Vector &v); + 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;