removed svd and dependent methods and tests
Chris Beall
parent
1a2e1e11b4
commit
6c85702ee6
|
|
@ -915,80 +915,6 @@ Matrix cholesky_inverse(const Matrix &A)
|
|||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** SVD */
|
||||
/* ************************************************************************* */
|
||||
|
||||
// version with in place modification of A
|
||||
void svd(Matrix& A, Vector& s, Matrix& V, bool sort) {
|
||||
|
||||
const size_t m=A.size1(), n=A.size2();
|
||||
if( m < n )
|
||||
throw invalid_argument("in-place svd calls NRC which needs matrix A with m>n");
|
||||
|
||||
double * q = new double[n]; // singular values
|
||||
|
||||
// create NRC matrices, u is in place
|
||||
V = Matrix(n,n);
|
||||
double **u = createNRC(A), **v = createNRC(V);
|
||||
|
||||
// perform SVD
|
||||
// need to pass pointer - 1 in NRC routines so u[1][1] is first element !
|
||||
svdcmp(u-1,m,n,q-1,v-1, sort);
|
||||
|
||||
// copy singular values back
|
||||
s.resize(n);
|
||||
copy(q,q+n,s.begin());
|
||||
|
||||
delete[] v;
|
||||
delete[] q; //switched to array delete
|
||||
delete[] u;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void svd(const Matrix& A, Matrix& U, Vector& s, Matrix& V, bool sort) {
|
||||
const size_t m=A.size1(), n=A.size2();
|
||||
if( m < n ) {
|
||||
V = trans(A);
|
||||
svd(V,s,U,sort); // A'=V*diag(s)*U'
|
||||
}
|
||||
else{
|
||||
U = A; // copy
|
||||
svd(U,s,V,sort); // call in-place version
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {
|
||||
|
||||
// Check size of A
|
||||
int m = A.size1(), n = A.size2();
|
||||
if (m<n) throw invalid_argument("DLT: m<n, pad A with zero rows if needed.");
|
||||
|
||||
// Do SVD on A
|
||||
Matrix U, V;
|
||||
Vector S;
|
||||
static const bool sort = false;
|
||||
svd(A, U, S, V, sort);
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// Return rank, minimum singular value, and corresponding column of V
|
||||
return boost::tuple<int, double, Vector>(rank, min_S, column_(V, min_j));
|
||||
}
|
||||
|
||||
#if 0
|
||||
/* ************************************************************************* */
|
||||
// TODO, would be faster with Cholesky
|
||||
|
|
@ -1023,22 +949,6 @@ Matrix inverse_square_root(const Matrix& A) {
|
|||
return inv;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix square_root_positive(const Matrix& A) {
|
||||
size_t m = A.size2(), n = A.size1();
|
||||
if (m!=n)
|
||||
throw invalid_argument("inverse_square_root: A must be square");
|
||||
|
||||
// Perform SVD, TODO: symmetric SVD?
|
||||
Matrix U,V;
|
||||
Vector S;
|
||||
svd(A,U,S,V,false);
|
||||
|
||||
// invert and sqrt diagonal of S
|
||||
// We also arbitrarily choose sign to make result have positive signs
|
||||
for(size_t i = 0; i<m; i++) S(i) = - pow(S(i),0.5);
|
||||
return vector_scale(S, V); // V*S;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix expm(const Matrix& A, size_t K) {
|
||||
|
|
|
|||
|
|
@ -360,47 +360,9 @@ Matrix vector_scale(const Matrix& A, const Vector& v); // column
|
|||
Matrix skewSymmetric(double wx, double wy, double wz);
|
||||
inline Matrix skewSymmetric(const Vector& w) { return skewSymmetric(w(0),w(1),w(2));}
|
||||
|
||||
/**
|
||||
* SVD computes economy SVD A=U*S*V'
|
||||
* @param A an m*n matrix
|
||||
* @param U output argument: rotation matrix
|
||||
* @param S output argument: vector of singular values, sorted by default, pass false as last argument to avoid sorting!!!
|
||||
* @param V output argument: rotation matrix
|
||||
* @param sort boolean flag to sort singular values and V
|
||||
* if m > n then U*S*V' = (m*n)*(n*n)*(n*n) (the m-n columns of U are of no use)
|
||||
* if m < n then U*S*V' = (m*m)*(m*m)*(m*n) (the n-m columns of V are of no use)
|
||||
* Careful! The dimensions above reflect V', not V, which is n*m if m<n.
|
||||
* U is a basis in R^m, V is a basis in R^n
|
||||
* You can just pass empty matrices U,V, and vector S, they will be re-allocated.
|
||||
*/
|
||||
void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V, bool sort=true);
|
||||
|
||||
/*
|
||||
* In place SVD, will throw an exception when m < n
|
||||
* @param A an m*n matrix is modified to contain U
|
||||
* @param S output argument: vector of singular values, sorted by default, pass false as last argument to avoid sorting!!!
|
||||
* @param V output argument: rotation matrix
|
||||
* @param sort boolean flag to sort singular values and V
|
||||
* if m > n then U*S*V' = (m*n)*(n*n)*(n*n) (the m-n columns of U are of no use)
|
||||
* You can just pass empty matrix V and vector S, they will be re-allocated.
|
||||
*/
|
||||
void svd(Matrix& A, Vector& S, Matrix& V, bool sort=true);
|
||||
|
||||
/**
|
||||
* Direct linear transform algorithm that calls svd
|
||||
* to find a vector v that minimizes the algebraic error A*v
|
||||
* @param A of size m*n, where m>=n (pad with zero rows if not!)
|
||||
* Returns rank of A, minimum error (singular value),
|
||||
* and corresponding eigenvector (column of V, with A=U*S*V')
|
||||
*/
|
||||
boost::tuple<int,double,Vector> DLT(const Matrix& A, double rank_tol=1e-9);
|
||||
|
||||
/** Use SVD to calculate inverse square root of a matrix */
|
||||
Matrix inverse_square_root(const Matrix& A);
|
||||
|
||||
/** Use SVD to calculate the positive square root of a matrix */
|
||||
Matrix square_root_positive(const Matrix& A);
|
||||
|
||||
/** Calculate the LL^t decomposition of a S.P.D matrix */
|
||||
Matrix LLt(const Matrix& A);
|
||||
|
||||
|
|
|
|||
|
|
@ -729,106 +729,6 @@ TEST( matrix, row_major_access )
|
|||
DOUBLES_EQUAL(3,a[2],1e-9);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, svd1 )
|
||||
{
|
||||
double data[] = { 2, 1, 0 };
|
||||
Vector v(3);
|
||||
copy(data, data + 3, v.begin());
|
||||
Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1)
|
||||
* Matrix(trans(V1));
|
||||
Matrix U, V;
|
||||
Vector s;
|
||||
svd(A, U, s, V);
|
||||
Matrix S = diag(s);
|
||||
EQUALITY(U*S*Matrix(trans(V)),A);
|
||||
EQUALITY(S,S1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Sample A matrix for SVD
|
||||
static double sampleData[] ={0,-2, 0,0, 3,0};
|
||||
static Matrix sampleA = Matrix_(3, 2, sampleData);
|
||||
static Matrix sampleAt = trans(sampleA);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, svd2 )
|
||||
{
|
||||
Matrix U, V;
|
||||
Vector s;
|
||||
|
||||
Matrix expectedU = Matrix_(3, 2, 0.,1.,0.,0.,-1.,0.);
|
||||
Vector expected_s = Vector_(2, 3.,2.);
|
||||
Matrix expectedV = Matrix_(2, 2, -1.,0.,0.,-1.);
|
||||
|
||||
svd(sampleA, U, s, V);
|
||||
|
||||
EQUALITY(expectedU,U);
|
||||
CHECK(equal_with_abs_tol(expected_s,s,1e-9));
|
||||
EQUALITY(expectedV,V);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, svd3 )
|
||||
{
|
||||
Matrix U, V;
|
||||
Vector s;
|
||||
|
||||
Matrix expectedU = Matrix_(2, 2, -1.,0.,0.,-1.);
|
||||
Vector expected_s = Vector_(2, 3.0,2.0);
|
||||
Matrix expectedV = Matrix_(3, 2, 0.,1.,0.,0.,-1.,0.);
|
||||
|
||||
svd(sampleAt, U, s, V);
|
||||
Matrix S = diag(s);
|
||||
Matrix t = prod(U,S);
|
||||
Matrix Vt = trans(V);
|
||||
|
||||
EQUALITY(sampleAt, prod(t,Vt));
|
||||
EQUALITY(expectedU,U);
|
||||
CHECK(equal_with_abs_tol(expected_s,s,1e-9));
|
||||
EQUALITY(expectedV,V);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Homography matrix for points
|
||||
//Point2h(0, 0, 1), Point2h(4, 5, 1);
|
||||
//Point2h(1, 0, 1), Point2h(5, 5, 1);
|
||||
//Point2h(1, 1, 1), Point2h(5, 6, 1);
|
||||
//Point2h(0, 1, 1), Point2h(4, 6, 1);
|
||||
static double homography_data[] = {0,0,0,-4,-5,-1,0,0,0,
|
||||
4,5,1,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,-5,-5,-1,0,0,0,
|
||||
5,5,1,0,0,0,-5,-5,-1,
|
||||
0,0,0,5,5,1,0,0,0,
|
||||
0,0,0,-5,-6,-1,5,6,1,
|
||||
5,6,1,0,0,0,-5,-6,-1,
|
||||
-5,-6,-1,5,6,1,0,0,0,
|
||||
0,0,0,-4,-6,-1,4,6,1,
|
||||
4,6,1,0,0,0,0,0,0,
|
||||
-4,-6,-1,0,0,0,0,0,0};
|
||||
static Matrix homographyA = Matrix_(12, 9, homography_data);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, svd_sort )
|
||||
{
|
||||
Matrix U1, U2, V1, V2;
|
||||
Vector s1, s2;
|
||||
svd(homographyA, U1, s1, V1);
|
||||
for(int i = 0 ; i < 8 ; i++)
|
||||
CHECK(s1[i]>=s1[i+1]); // Check if singular values are sorted
|
||||
|
||||
svd(homographyA, U2, s2, V2, false);
|
||||
CHECK(s1[8]==s2[7]); // Check if swapping is done
|
||||
CHECK(s1[7]==s2[8]);
|
||||
Vector v17 = column_(V1, 7);
|
||||
Vector v18 = column_(V1, 8);
|
||||
Vector v27 = column_(V2, 7);
|
||||
Vector v28 = column_(V2, 8);
|
||||
CHECK(v17==v28); // Check if vectors are also swapped correctly
|
||||
CHECK(v18==v27); // Check if vectors are also swapped correctly
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// update A, b
|
||||
// A' \define A_{S}-ar and b'\define b-ad
|
||||
|
|
@ -939,19 +839,6 @@ TEST( matrix, LLt )
|
|||
EQUALITY(expected, LLt(M));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, square_root_positive )
|
||||
{
|
||||
Matrix cov = Matrix_(3, 3, 4.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 100.0);
|
||||
|
||||
Matrix expected = Matrix_(3, 3, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0,
|
||||
10.0);
|
||||
|
||||
Matrix actual = square_root_positive(cov);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
CHECK(assert_equal(cov, prod(trans(actual),actual)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, multiplyAdd )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ check_PROGRAMS += tests/testStereoCamera
|
|||
headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h
|
||||
headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Expression.h
|
||||
sources += projectiveGeometry.cpp tensorInterface.cpp
|
||||
check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testTrifocal tests/testFundamental
|
||||
check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental
|
||||
|
||||
# Timing tests
|
||||
noinst_PROGRAMS = tests/timeRot3 tests/timeCalibratedCamera
|
||||
|
|
|
|||
|
|
@ -77,61 +77,6 @@ namespace gtsam {
|
|||
return data;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Homography2 estimateHomography2(const list<Correspondence>& correspondences) {
|
||||
// Generate entries of A matrix for linear estimation
|
||||
size_t m = correspondences.size();
|
||||
if (m<4) throw invalid_argument("estimateHomography2: need at least 4 correspondences");
|
||||
Matrix A;
|
||||
BOOST_FOREACH(const Correspondence& p, correspondences) {
|
||||
Matrix Ap = reshape(p.first(a) * (eta(_b, _c, _d) * p.second(b)),3,9);
|
||||
A = stack(2,&A,&Ap);
|
||||
}
|
||||
|
||||
// Call DLT and reshape to Homography
|
||||
int rank; double error; Vector v;
|
||||
boost::tie(rank, error, v) = DLT(A);
|
||||
if (rank < 8) throw invalid_argument("estimateHomography2: not enough data");
|
||||
return reshape2<3, 3> (v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FundamentalMatrix estimateFundamentalMatrix(const list<Correspondence>& correspondences) {
|
||||
// Generate entries of A matrix for linear estimation
|
||||
size_t m = correspondences.size();
|
||||
if (m<8) throw invalid_argument("estimateFundamentalMatrix: need at least 8 correspondences");
|
||||
if (m<9) m = 9; // make sure to pad with zeros in minimal case
|
||||
Matrix A = zeros(m,9);
|
||||
size_t i = 0;
|
||||
BOOST_FOREACH(const Correspondence& p, correspondences)
|
||||
Row(A,i++) = toVector(p.first(a) * p.second(b));
|
||||
|
||||
// Call DLT and reshape to Homography
|
||||
int rank; double error; Vector v;
|
||||
boost::tie(rank, error, v) = DLT(A);
|
||||
if (rank < 8) throw invalid_argument("estimateFundamentalMatrix: not enough data");
|
||||
return reshape2<3, 3> (v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TrifocalTensor estimateTrifocalTensor(const list<Triplet>& triplets) {
|
||||
// Generate entries of A matrix for linear estimation
|
||||
Matrix A;
|
||||
BOOST_FOREACH(const Triplet& p, triplets) {
|
||||
Tensor3<3,3,3> T3 = p.first(a)* (eta(_d,_b,_e) * p.second(d));
|
||||
Tensor2<3,3> T2 = eta(_f,_c,_g) * p.third(f);
|
||||
// Take outer product of rank 3 and rank 2, then swap indices 3,4 for reshape
|
||||
// We get a rank 5 tensor T5(a,_b,_c,_e,_g), where _e and _g index the rows...
|
||||
Matrix Ap = reshape((T3(a,_b,_e) * T2(_c,_g)).swap34(), 9, 27);
|
||||
A = stack(2,&A,&Ap);
|
||||
}
|
||||
|
||||
// Call DLT and reshape to Homography
|
||||
int rank; double error; Vector v;
|
||||
boost::tie(rank, error, v) = DLT(A);
|
||||
if (rank < 26) throw invalid_argument("estimateTrifocalTensor: not enough data");
|
||||
return reshape3<3,3,3>(v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -50,23 +50,9 @@ namespace gtsam {
|
|||
/** 2D-2D Homography */
|
||||
typedef tensors::Tensor2<3, 3> Homography2;
|
||||
|
||||
/**
|
||||
* Estimate homography from point correspondences
|
||||
* Result is H(_a,b) s.t. p.second(b) = H(_a,b) * p.first(a) for all p
|
||||
*/
|
||||
Homography2 estimateHomography2(
|
||||
const std::list<Correspondence>& correspondences);
|
||||
|
||||
/** Fundamental Matrix */
|
||||
typedef tensors::Tensor2<3, 3> FundamentalMatrix;
|
||||
|
||||
/**
|
||||
* Estimate fundamental matrix from point correspondences
|
||||
* Result is F(_a,_b) s.t. H(_a,_b) * p.first(a) * p.second(b) == 0 for all p
|
||||
*/
|
||||
FundamentalMatrix estimateFundamentalMatrix(
|
||||
const std::list<Correspondence>& correspondences);
|
||||
|
||||
/** Triplet of points */
|
||||
struct Triplet {
|
||||
Point2h first, second, third;
|
||||
|
|
@ -84,12 +70,6 @@ namespace gtsam {
|
|||
/** Trifocal Tensor */
|
||||
typedef tensors::Tensor3<3, 3, 3> TrifocalTensor;
|
||||
|
||||
/**
|
||||
* Estimate trifocal Tensor from point triplets
|
||||
* Result is T(_a,b,c)
|
||||
*/
|
||||
TrifocalTensor estimateTrifocalTensor(const std::list<Triplet>& triplets);
|
||||
|
||||
/** 3D Point */
|
||||
typedef tensors::Tensor1<4> Point3h;
|
||||
Point3h point3h(double X, double Y, double Z, double W);
|
||||
|
|
|
|||
|
|
@ -63,56 +63,6 @@ TEST( Tensors, FundamentalMatrix)
|
|||
CHECK(l2(a)*p(a) == 0); // p is on line l2
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Stereo setup, -1,1
|
||||
/* ************************************************************************* */
|
||||
// t points towards origin
|
||||
double left__[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { +1, 0, 0 } };
|
||||
double right_[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { -1, 0, 0 } };
|
||||
//double right_[4][3] = { { cos(0.1), -sin(0.1), 0 }, { sin(0.1), cos(0.1), 0 }, { 0, 0, 1 }, { -1, 0, 0 } };
|
||||
ProjectiveCamera ML(left__), MR(right_);
|
||||
|
||||
// Cube
|
||||
Point3h P1 = point3h(-1, -1, 3 - 1, 1);
|
||||
Point3h P2 = point3h(-1, -1, 3 + 1, 1);
|
||||
Point3h P3 = point3h(-1, +1, 3 - 1, 1);
|
||||
Point3h P4 = point3h(-1, +1, 3 + 1, 1);
|
||||
Point3h P5 = point3h(+1, -1, 3 - 1, 1);
|
||||
Point3h P6 = point3h(+1, -1, 3 + 1, 1);
|
||||
Point3h P7 = point3h(+1, +1, 3 - 1, 1);
|
||||
Point3h P8 = point3h(+1, +1, 3 + 1, 1);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Tensors, FundamentalMatrix2)
|
||||
{
|
||||
// The correct fundamental matrix is given by formula 9.1 in HZ 2nd ed., p. 244
|
||||
// F = \hat(e')P'P+
|
||||
// and is very simple
|
||||
double f[3][3] = {{0,0,0}
|
||||
,{0,0,-1}
|
||||
,{0,1,0}
|
||||
};
|
||||
FundamentalMatrix F(f);
|
||||
|
||||
// Create a list of correspondences
|
||||
list<Point3h> points;
|
||||
Point3h P9 = point3h(-2,3,4,1);
|
||||
Point3h P10 = point3h(1,1,5,1);
|
||||
points += P1, P2, P3, P4, P5, P6, P7, P8, P9, P10;
|
||||
list<Correspondence> correspondences;
|
||||
BOOST_FOREACH(const Point3h& P, points) {
|
||||
Correspondence p(ML(a,A)*P(A), MR(b,A)*P(A));
|
||||
DOUBLES_EQUAL(0,F(a,b) * p.first(a) * p.second(b),1e-9);
|
||||
correspondences += p;
|
||||
}
|
||||
|
||||
// let's check it for another arbitrary point
|
||||
Point2h left(ML(a,A)*P9(A)), right(MR(b,A)*P9(A));
|
||||
DOUBLES_EQUAL(0,F(a,b) * left(a) * right(b),1e-9);
|
||||
|
||||
FundamentalMatrix actual = estimateFundamentalMatrix(correspondences);
|
||||
CHECK(assert_equivalent(F(a,b),actual(a,b),0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
|
|
|
|||
|
|
@ -87,27 +87,6 @@ TEST( Homography2, TestCase)
|
|||
CHECK(assert_equality(l1(a), H(a,b)*l2(b)))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Homography2, Estimate)
|
||||
{
|
||||
list<Correspondence> correspondences;
|
||||
correspondences += p1, p2, p3, p4;
|
||||
Homography2 estimatedH = estimateHomography2(correspondences);
|
||||
CHECK(assert_equivalent(H(_a,b),estimatedH(_a,b)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Homography2, EstimateReverse)
|
||||
{
|
||||
double h[3][3] = { { 1, 0, -4 }, { 0, 1, -5 }, { 0, 0, 1 } };
|
||||
Homography2 reverse(h);
|
||||
|
||||
list<Correspondence> correspondences;
|
||||
correspondences += p1.swap(), p2.swap(), p3.swap(), p4.swap();
|
||||
Homography2 estimatedH = estimateHomography2(correspondences);
|
||||
CHECK(assert_equality(reverse(_a,b),estimatedH(_a,b)*(1.0/estimatedH(2,2))));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Computes the homography H(I,_T) from template to image
|
||||
|
|
|
|||
|
|
@ -1,178 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* testTrifocal.cpp
|
||||
* @brief trifocal tensor estimation
|
||||
* Created on: Feb 9, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <gtsam/CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/geometry/tensors.h>
|
||||
#include <gtsam/geometry/tensorInterface.h>
|
||||
#include <gtsam/geometry/projectiveGeometry.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace tensors;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Indices
|
||||
|
||||
Index<3, 'a'> a, _a;
|
||||
Index<3, 'b'> b, _b;
|
||||
Index<3, 'c'> c, _c;
|
||||
Index<3, 'd'> d, _d;
|
||||
Index<3, 'e'> e, _e;
|
||||
Index<3, 'f'> f, _f;
|
||||
Index<3, 'g'> g, _g;
|
||||
|
||||
Index<4, 'A'> A;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 3 Camera setup in trifocal stereo setup, -1,0,1
|
||||
/* ************************************************************************* */
|
||||
double left__[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { -1, 0, 0 } };
|
||||
double middle[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { +0, 0, 0 } };
|
||||
double right_[4][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }, { +1, 0, 0 } };
|
||||
ProjectiveCamera ML(left__), MM(middle), MR(right_);
|
||||
|
||||
// Cube
|
||||
Point3h P1 = point3h(-1, -1, 3 - 1, 1);
|
||||
Point3h P2 = point3h(-1, -1, 3 + 1, 1);
|
||||
Point3h P3 = point3h(-1, +1, 3 - 1, 1);
|
||||
Point3h P4 = point3h(-1, +1, 3 + 1, 1);
|
||||
Point3h P5 = point3h(+1, -1, 3 - 1, 1);
|
||||
Point3h P6 = point3h(+1, -1, 3 + 1, 1);
|
||||
Point3h P7 = point3h(+1, +1, 3 - 1, 1);
|
||||
Point3h P8 = point3h(+1, +1, 3 + 1, 1);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Manohar's homework
|
||||
TEST(Tensors, TrifocalTensor)
|
||||
{
|
||||
// Checked with MATLAB !
|
||||
double t[3][3][3] = {
|
||||
{ { -0.301511, 0, 0 }, { 0, -0.603023, 0 }, { 0, 0,-0.603023 } },
|
||||
{ { 0, 0.301511, 0 }, { 0, 0, 0 }, { 0, 0, 0 } },
|
||||
{ { 0, 0, 0.301511 }, { 0, 0, 0 }, { 0, 0, 0 } }
|
||||
};
|
||||
TrifocalTensor T(t);
|
||||
//print(T(a,b,c));
|
||||
|
||||
list<Point3h> points;
|
||||
points += P1, P2, P3, P4, P5, P6, P7, P8;
|
||||
|
||||
Eta3 eta;
|
||||
|
||||
list<Triplet> triplets;
|
||||
double data[3][3] = { { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 } };
|
||||
Tensor2<3,3> zero(data);
|
||||
BOOST_FOREACH(const Point3h& P, points) {
|
||||
// form triplet
|
||||
Triplet p(ML(a,A)*P(A), MM(b,A)*P(A), MR(c,A)*P(A));
|
||||
// check trifocal constraint
|
||||
Tensor2<3,3> T1 = T(_a,b,c) * p.first(a);
|
||||
Tensor2<3,3> T2 = eta(_d,_b,_e) * p.second(d);
|
||||
Tensor2<3,3> T3 = eta(_f,_c,_g) * p.third(f);
|
||||
CHECK(assert_equality(zero(_e,_g), (T1(b,c) * T2(_b,_e)) * T3(_c,_g),1e-4));
|
||||
triplets += p;
|
||||
}
|
||||
|
||||
// We will form the rank 5 tensor by multiplying a rank 3 and rank 2
|
||||
// Let's check the answer for the first point:
|
||||
Triplet p = triplets.front();
|
||||
|
||||
// This checks the rank 3 (with answer checked in MATLAB);
|
||||
double matlab3[3][3][3] = {
|
||||
{{ -0, -0, 0}, { 4, 2, -4}, { 2, 1, -2}},
|
||||
{{ -4, -2, 4}, {-0, -0, 0}, {-2, -1, 2}},
|
||||
{{ -2, -1, 2}, { 2, 1, -2}, {-0, -0, 0}}
|
||||
};
|
||||
Tensor3<3,3,3> expected3(matlab3);
|
||||
CHECK(assert_equality(expected3(a,_b,_e), p.first(a)* (eta(_d,_b,_e) * p.second(d))));
|
||||
|
||||
// This checks the rank 2 (with answer checked in MATLAB);
|
||||
double matlab2[3][3] = { {0, -2, -1}, {2, 0, 0}, {1, 0, 0}};
|
||||
Tensor2<3,3> expected2(matlab2);
|
||||
CHECK(assert_equality(expected2(_c,_g), eta(_f,_c,_g) * p.third(f)));
|
||||
|
||||
TrifocalTensor actual = estimateTrifocalTensor(triplets);
|
||||
|
||||
//print(actual(a,b,c));
|
||||
CHECK(assert_equality(T(_a,b,c),actual(_a,b,c),1e-6));
|
||||
}
|
||||
|
||||
TEST(Tensors, TrifocalTensor1){
|
||||
// Manually clicked points
|
||||
// Points in frame1
|
||||
// 339 336 281 51 367 265 135
|
||||
// 152 344 246 210 76 248 246
|
||||
// Points in frame2
|
||||
// 380 381 311 108 395 294 161
|
||||
// 148 340 242 208 73 243 242
|
||||
// Points in frame3
|
||||
// 440 441 360 181 444 344 207
|
||||
// 151 343 246 212 74 247 245
|
||||
|
||||
Triplet p1(point2h(339,152,1), point2h(380,148,1), point2h(440,151,1));
|
||||
Triplet p2(point2h(336,344,1), point2h(381,340,1), point2h(441,343,1));
|
||||
Triplet p3(point2h(281,246,1), point2h(311,242,1), point2h(360,246,1));
|
||||
Triplet p4(point2h(51,210,1 ), point2h(108,208,1), point2h(181,212,1));
|
||||
Triplet p5(point2h(367,76,1 ), point2h(395,73,1 ), point2h(444,74,1) );
|
||||
Triplet p6(point2h(265,248,1), point2h(294,243,1), point2h(344,247,1));
|
||||
Triplet p7(point2h(135,246,1), point2h(161,242,1), point2h(207,245,1));
|
||||
list<Triplet> triplets;
|
||||
triplets += p1, p2, p3, p4, p5, p6, p7;
|
||||
|
||||
// Checked with MATLAB !
|
||||
/*double t[3][3][3] = {
|
||||
{ { -0.0145,0.0081,0.0000}, {-0.0004,-0.0180,0.0000}, {0.2334,-0.6283,-0.0230}},
|
||||
{ { -0.0162,-0.0001,0.0000}, {0.0049,-0.0075,0.0000}, {0.7406,0.0209,-0.0140}},
|
||||
{ { -0.0001,-0.0000,-0.0000}, {-0.0000,-0.0001,0.0000}, {0.0096,0.0063,-0.0000}}
|
||||
};*/
|
||||
double t[3][3][3] = {
|
||||
{ { 0.0145,0.0004,-0.2334}, {-0.0081,0.0180,0.6283}, {0.0000,0.0000,0.0230}},
|
||||
{ { 0.0162,-0.0049,-0.7406}, {0.0001,-0.0075,-0.0209}, {0.0000,0.0000,0.0140}},
|
||||
{ { 0.0001,-0.0000,-0.0096}, {0.0000,0.0001,-0.0063}, {0.0000,-0.0000,-0.0000}}
|
||||
};
|
||||
TrifocalTensor T(t);
|
||||
|
||||
TrifocalTensor actual = estimateTrifocalTensor(triplets);
|
||||
|
||||
Eta3 eta;
|
||||
double data[3][3] = { { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 } };
|
||||
Tensor2<3,3> zero(data);
|
||||
|
||||
BOOST_FOREACH(const Triplet& t, triplets) {
|
||||
Tensor2<3,3> T1 = actual(_a,b,c) * t.first(a);
|
||||
Tensor2<3,3> T2 = eta(_d,_b,_e) * t.second(d);
|
||||
Tensor2<3,3> T3 = eta(_f,_c,_g) * t.third(f);
|
||||
CHECK(assert_equality(zero(_e,_g), (T1(b,c) * T2(_b,_e)) * T3(_c,_g),1e-2));
|
||||
}
|
||||
|
||||
CHECK(assert_equality(T(_a,b,c), actual(_a,b,c),1e-1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -125,13 +125,6 @@ namespace gtsam {
|
|||
*/
|
||||
static shared_ptr Covariance(const Matrix& covariance, bool smart=false);
|
||||
|
||||
/**
|
||||
* A Gaussian noise model created by specifying an information matrix.
|
||||
*/
|
||||
static shared_ptr Information(const Matrix& Q) {
|
||||
return shared_ptr(new Gaussian(Q.size1(),square_root_positive(Q)));
|
||||
}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual bool equals(const Base& expected, double tol) const;
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@ TEST(NoiseModel, constructors)
|
|||
vector<Gaussian::shared_ptr> m;
|
||||
m.push_back(Gaussian::SqrtInformation(R));
|
||||
m.push_back(Gaussian::Covariance(Sigma));
|
||||
m.push_back(Gaussian::Information(Q));
|
||||
//m.push_back(Gaussian::Information(Q));
|
||||
m.push_back(Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)));
|
||||
m.push_back(Diagonal::Variances(Vector_(3, var, var, var)));
|
||||
m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc)));
|
||||
|
|
|
|||
Loading…
Reference in New Issue