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

removed svd and dependent methods and tests

release/4.3a0
Chris Beall 2010-10-23 14:37:06 +00:00
parent 1a2e1e11b4
commit 6c85702ee6
11 changed files with 2 additions and 574 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
base/Matrix.cpp
View File

@ -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 #if 0
/* ************************************************************************* */ /* ************************************************************************* */
// TODO, would be faster with Cholesky // TODO, would be faster with Cholesky
@ -1023,22 +949,6 @@ Matrix inverse_square_root(const Matrix& A) {
return inv; 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) { Matrix expm(const Matrix& A, size_t K) {

38
base/Matrix.h
View File

@ -360,47 +360,9 @@ Matrix vector_scale(const Matrix& A, const Vector& v); // column
Matrix skewSymmetric(double wx, double wy, double wz); Matrix skewSymmetric(double wx, double wy, double wz);
inline Matrix skewSymmetric(const Vector& w) { return skewSymmetric(w(0),w(1),w(2));} 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 */ /** Use SVD to calculate inverse square root of a matrix */
Matrix inverse_square_root(const Matrix& A); 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 */ /** Calculate the LL^t decomposition of a S.P.D matrix */
Matrix LLt(const Matrix& A); Matrix LLt(const Matrix& A);

113
base/tests/testMatrix.cpp
View File

@ -729,106 +729,6 @@ TEST( matrix, row_major_access )
DOUBLES_EQUAL(3,a[2],1e-9); 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 // update A, b
// A' \define A_{S}-ar and b'\define b-ad // A' \define A_{S}-ar and b'\define b-ad
@ -939,19 +839,6 @@ TEST( matrix, LLt )
EQUALITY(expected, LLt(M)); 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 ) TEST( matrix, multiplyAdd )
{ {

2
geometry/Makefile.am
View File

@ -26,7 +26,7 @@ check_PROGRAMS += tests/testStereoCamera
headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h
headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Expression.h headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Expression.h
sources += projectiveGeometry.cpp tensorInterface.cpp 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 # Timing tests
noinst_PROGRAMS = tests/timeRot3 tests/timeCalibratedCamera noinst_PROGRAMS = tests/timeRot3 tests/timeCalibratedCamera

55
geometry/projectiveGeometry.cpp
View File

@ -77,61 +77,6 @@ namespace gtsam {
return data; 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);
}
/* ************************************************************************* */ /* ************************************************************************* */

20
geometry/projectiveGeometry.h
View File

@ -50,23 +50,9 @@ namespace gtsam {
/** 2D-2D Homography */ /** 2D-2D Homography */
typedef tensors::Tensor2<3, 3> Homography2; 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 */ /** Fundamental Matrix */
typedef tensors::Tensor2<3, 3> FundamentalMatrix; 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 */ /** Triplet of points */
struct Triplet { struct Triplet {
Point2h first, second, third; Point2h first, second, third;
@ -84,12 +70,6 @@ namespace gtsam {
/** Trifocal Tensor */ /** Trifocal Tensor */
typedef tensors::Tensor3<3, 3, 3> TrifocalTensor; 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 */ /** 3D Point */
typedef tensors::Tensor1<4> Point3h; typedef tensors::Tensor1<4> Point3h;
Point3h point3h(double X, double Y, double Z, double W); Point3h point3h(double X, double Y, double Z, double W);

50
geometry/tests/testFundamental.cpp
View File

@ -63,56 +63,6 @@ TEST( Tensors, FundamentalMatrix)
CHECK(l2(a)*p(a) == 0); // p is on line l2 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() { int main() {

21
geometry/tests/testHomography2.cpp
View File

@ -87,27 +87,6 @@ TEST( Homography2, TestCase)
CHECK(assert_equality(l1(a), H(a,b)*l2(b))) 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 * Computes the homography H(I,_T) from template to image

178
geometry/tests/testTrifocal.cpp
View File

@ -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);
}
/* ************************************************************************* */

7
linear/NoiseModel.h
View File

@ -125,13 +125,6 @@ namespace gtsam {
*/ */
static shared_ptr Covariance(const Matrix& covariance, bool smart=false); 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 void print(const std::string& name) const;
virtual bool equals(const Base& expected, double tol) const; virtual bool equals(const Base& expected, double tol) const;
virtual Vector whiten(const Vector& v) const; virtual Vector whiten(const Vector& v) const;

2
linear/tests/testNoiseModel.cpp
View File

@ -55,7 +55,7 @@ TEST(NoiseModel, constructors)
vector<Gaussian::shared_ptr> m; vector<Gaussian::shared_ptr> m;
m.push_back(Gaussian::SqrtInformation(R)); m.push_back(Gaussian::SqrtInformation(R));
m.push_back(Gaussian::Covariance(Sigma)); 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::Sigmas(Vector_(3, sigma, sigma, sigma)));
m.push_back(Diagonal::Variances(Vector_(3, var, var, var))); m.push_back(Diagonal::Variances(Vector_(3, var, var, var)));
m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc))); m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc)));