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

There was no unit test for DLT. Guess what, it never survived the switch to Eigen. Fixed now.

release/4.3a0
Frank Dellaert 2012-06-16 03:23:38 +00:00
parent 1e190f7c90
commit f15d242a78
2 changed files with 52 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/base/Matrix.cpp
View File

@ -671,31 +671,21 @@ void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V) {
boost::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {
// Check size of A
int m = A.rows(), n = A.cols();
if (m < n) throw invalid_argument(
"DLT: m<n, pad A with zero rows if needed.");
int n = A.rows(), p = A.cols(), m = min(n,p);
// Do SVD on A
Matrix U, V;
Vector S;
// static const bool sort = false;
svd(A, U, S, V); // TODO: is it a problem for this to be sorted?
Eigen::JacobiSVD<Matrix> svd(A, Eigen::ComputeFullV);
Vector s = svd.singularValues();
Matrix V = svd.matrixV();
// Find rank
int rank = 0;
for (int j = 0; j < n; j++)
if (S(j) > rank_tol) rank++;
// Find minimum singular value and corresponding column index
int min_j = n - 1;
double min_S = S(min_j);
for (int j = 0; j < n - 1; j++)
if (S(j) < min_S) {
min_j = j;
min_S = S(j);
}
for (int j = 0; j < m; j++)
if (s(j) > rank_tol) rank++;
// Return rank, minimum singular value, and corresponding column of V
return boost::tuple<int, double, Vector>(rank, min_S, Vector(column(V, min_j)));
// Return rank, error, and corresponding column of V
double error = m<p ? 0 : s(m-1);
return boost::tuple<int, double, Vector>(rank, error, Vector(column(V, p-1)));
}
/* ************************************************************************* */

63
gtsam/base/tests/testMatrix.cpp
View File

@ -1052,32 +1052,55 @@ TEST( matrix, svd3 )
/* ************************************************************************* */
TEST( matrix, svd4 )
{
Matrix U, V;
Vector s;
Matrix U, V;
Vector s;
Matrix A = Matrix_(3,2,
0.8147, 0.9134,
0.9058, 0.6324,
0.1270, 0.0975);
Matrix A = Matrix_(3,2,
0.8147, 0.9134,
0.9058, 0.6324,
0.1270, 0.0975);
Matrix expectedU = Matrix_(3,2,
0.7397, 0.6724,
0.6659, -0.7370,
0.0970, -0.0689);
Matrix expectedU = Matrix_(3,2,
0.7397, 0.6724,
0.6659, -0.7370,
0.0970, -0.0689);
Vector expected_s = Vector_(2, 1.6455, 0.1910);
Vector expected_s = Vector_(2, 1.6455, 0.1910);
Matrix expectedV = Matrix_(2,2,
0.7403, -0.6723,
0.6723, 0.7403);
Matrix expectedV = Matrix_(2,2,
0.7403, -0.6723,
0.6723, 0.7403);
svd(A, U, s, V);
Matrix reconstructed = U * diag(s) * trans(V);
svd(A, U, s, V);
Matrix reconstructed = U * diag(s) * trans(V);
EXPECT(assert_equal(A, reconstructed, 1e-4));
EXPECT(assert_equal(expectedU,U, 1e-3));
EXPECT(assert_equal(expected_s,s, 1e-4));
EXPECT(assert_equal(expectedV,V, 1e-4));
EXPECT(assert_equal(A, reconstructed, 1e-4));
EXPECT(assert_equal(expectedU,U, 1e-3));
EXPECT(assert_equal(expected_s,s, 1e-4));
EXPECT(assert_equal(expectedV,V, 1e-4));
}
/* ************************************************************************* */
TEST( matrix, DLT )
{
Matrix A = Matrix_(8,9,
0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11,
0.44, -0.66, -15.84, 0.34, -0.51, -12.24, -1.64, 2.46, 59.04,
0.69, -8.28, -12.19, -0.48, 5.76, 8.48, -1.89, 22.68, 33.39,
0.96, -8.4, -17.76, -0.6, 5.25, 11.1, -3.36, 29.4, 62.16,
1.25, 0.3, 2.75, -3.5, -0.84, -7.7, 16.25, 3.9, 35.75,
1.56, 0.42, 4.56, -3.38, -0.91, -9.88, 22.36, 6.02, 65.36,
1.89, 2.24, 3.99, 3.24, 3.84, 6.84, 18.09, 21.44, 38.19,
2.24, 2.48, 6.24, 3.08, 3.41, 8.58, 24.64, 27.28, 68.64
);
int rank;
double error;
Vector actual;
boost::tie(rank,error,actual) = DLT(A);
Vector expected = Vector_(9, -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0);
EXPECT_LONGS_EQUAL(8,rank);
EXPECT_DOUBLES_EQUAL(0,error,1e-8);
EXPECT(assert_equal(expected, actual, 1e-4));
}
/* ************************************************************************* */