12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Replace MakeATangentVector with MakeATangentVectorValues

release/4.3a0
jingwuOUO 2020-08-22 18:18:16 -04:00
parent a350295760
commit 3beb4df3a2
3 changed files with 25 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/sfm/ShonanAveraging.cpp
View File

@ -643,12 +643,16 @@ bool ShonanAveraging<d>::checkOptimality(const Values &values) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
/// Create a tangent direction xi with eigenvector segment v_i /// Create a VectorValues with eigenvector v
template <size_t d> template <size_t d>
Vector ShonanAveraging<d>::MakeATangentVector(size_t p, const Vector &v, VectorValues ShonanAveraging<d>::MakeATangentVectorValues(size_t p,
size_t i) { const Vector &v) {
VectorValues delta;
// Create a tangent direction xi with eigenvector segment v_i // Create a tangent direction xi with eigenvector segment v_i
const size_t dimension = SOn::Dimension(p); const size_t dimension = SOn::Dimension(p);
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>(d * i); const auto v_i = v.segment<d>(d * i);
Vector xi = Vector::Zero(dimension); Vector xi = Vector::Zero(dimension);
double sign = pow(-1.0, round((p + 1) / 2) + 1); double sign = pow(-1.0, round((p + 1) / 2) + 1);
@ -656,7 +660,9 @@ Vector ShonanAveraging<d>::MakeATangentVector(size_t p, const Vector &v,
xi(j + p - d - 1) = sign * v_i(d - j - 1); xi(j + p - d - 1) = sign * v_i(d - j - 1);
sign = -sign; sign = -sign;
} }
return xi; delta.insert(i, xi);
}
return delta;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -690,14 +696,8 @@ template <size_t d>
Values ShonanAveraging<d>::LiftwithDescent(size_t p, const Values &values, Values ShonanAveraging<d>::LiftwithDescent(size_t p, const Values &values,
const Vector &minEigenVector) { const Vector &minEigenVector) {
Values lifted = LiftTo<SOn>(p, values); Values lifted = LiftTo<SOn>(p, values);
for (auto it : lifted.filter<SOn>()) { VectorValues delta = MakeATangentVectorValues(p, minEigenVector);
// Create a tangent direction xi with eigenvector segment v_i return lifted.retract(delta);
// 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;
} }
/* ************************************************************************* */ /* ************************************************************************* */

7
gtsam/sfm/ShonanAveraging.h
View File

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

6
gtsam/sfm/tests/testShonanAveraging.cpp
View File

@ -121,7 +121,7 @@ TEST(ShonanAveraging3, tryOptimizingAt4) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ShonanAveraging3, MakeATangentVector) { TEST(ShonanAveraging3, MakeATangentVectorValues) {
Vector9 v; Vector9 v;
v << 1, 2, 3, 4, 5, 6, 7, 8, 9; v << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Matrix expected(5, 5); Matrix expected(5, 5);
@ -130,8 +130,8 @@ TEST(ShonanAveraging3, MakeATangentVector) {
0, 0, 0, 0, -6, // 0, 0, 0, 0, -6, //
0, 0, 0, 0, 0, // 0, 0, 0, 0, 0, //
4, 5, 6, 0, 0; 4, 5, 6, 0, 0;
const Vector xi_1 = ShonanAveraging3::MakeATangentVector(5, v, 1); const VectorValues delta = ShonanAveraging3::MakeATangentVectorValues(5, v);
const auto actual = SOn::Hat(xi_1); const auto actual = SOn::Hat(delta[1]);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }