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

Changed TangentVectorValues test

release/4.3a0
jingwuOUO 2020-08-25 14:48:25 -04:00
parent 491405a5f1
commit 1980dcf1f5
1 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -121,18 +121,17 @@ TEST(ShonanAveraging3, tryOptimizingAt4) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ShonanAveraging3, MakeATangentVectorValues) { TEST(ShonanAveraging3, TangentVectorValues) {
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); Vector expected0(10), expected1(10), expected2(10);
expected << 0, 0, 0, 0, -4, // expected0 << 0, 3, -2, 1, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, -5, // expected1 << 0, 6, -5, 4, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, -6, // expected2 << 0, 9, -8, 7, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, // const VectorValues xi = ShonanAveraging3::TangentVectorValues(5, v);
4, 5, 6, 0, 0; EXPECT(assert_equal(expected0, xi[0]));
const VectorValues delta = ShonanAveraging3::MakeATangentVectorValues(5, v); EXPECT(assert_equal(expected1, xi[1]));
const auto actual = SOn::Hat(delta[1]); EXPECT(assert_equal(expected2, xi[2]));
CHECK(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -158,17 +157,18 @@ TEST(ShonanAveraging3, CheckWithEigen) {
double lambda = kShonan.computeMinEigenValue(Qstar3); double lambda = kShonan.computeMinEigenValue(Qstar3);
// Check Eigenvalue with slow Eigen version, converts matrix A to dense matrix! // Check Eigenvalue with slow Eigen version, converts matrix A to dense matrix!
// const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3); const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3);
// auto A = kShonan.computeA(S); auto A = kShonan.computeA(S);
// bool computeEigenvectors = false; bool computeEigenvectors = false;
// Eigen::EigenSolver<Matrix> eigenSolver(Matrix(A), computeEigenvectors); Eigen::EigenSolver<Matrix> eigenSolver(Matrix(A), computeEigenvectors);
// auto lambdas = eigenSolver.eigenvalues().real(); auto lambdas = eigenSolver.eigenvalues().real();
// double minEigenValue = lambdas(0); double minEigenValue = lambdas(0);
// for (int i = 1; i < lambdas.size(); i++) for (int i = 1; i < lambdas.size(); i++)
// minEigenValue = min(lambdas(i), minEigenValue); minEigenValue = min(lambdas(i), minEigenValue);
// Actual check // Actual check
EXPECT_DOUBLES_EQUAL(0, lambda, 1e-11); EXPECT_DOUBLES_EQUAL(0, lambda, 1e-11);
EXPECT_DOUBLES_EQUAL(0, minEigenValue, 1e-11);
// Construct test descent direction (as minEigenVector is not predictable // Construct test descent direction (as minEigenVector is not predictable
// across platforms, being one from a basically flat 3d- subspace) // across platforms, being one from a basically flat 3d- subspace)