testmat.cc

一个用MATLAB语言编写的摄像机标定工具箱,内容丰富

//
// testmat.cc
//
// $Id: testmat.cc,v 1.1.1.1 2001/02/28 00:28:38 cstolte Exp $
//

#include <iostream>
using namespace std;
#include <unistd.h>
#include <sgl.h>
#include "testsgl.h"

#ifndef __GNUG__
int main() {
    return 0;
}
#else 

static void check_mats(const Mat<5, double> &m1, const Mat<5, double> &m2) {
    for (int i = 0; i < 5; ++i)
	for (int j = 0; j < 5; ++j)
	    assert(equal(m1[i][j], m2[i][j]));
}

static void test_Mat() {
    const int SIZE = 5, SZSQ = SIZE * SIZE;
    double v[SZSQ], v2[SZSQ];
    for (int a = 0; a < SIZE*SIZE; ++a) {
	v[a] = doub_random();
	v2[a] = doub_random();
    }
		     
    Mat<5, double> m1(v);
    double d = m1.determinant();
    if (!equal(d, 0)) {
	Mat<5, double> inv(m1);
	inv.invert();
	Mat<5, double> id1, id2;
	id1 = m1;
	id1 *= inv;
	id2 = m1 * inv;
	Mat<5, double> id3;
	id3.identity();
	for (int i = 0; i < 5; ++i) 
	    for (int j = 0; j < 5; ++j) {
		if (i == j) {
		    assert(equal(id1[i][j], 1));
		    assert(equal(id2[i][j], 1));
		    assert(equal(id3[i][j], 1));
		}
		else {
		    assert(equal(id1[i][j], 0));
		    assert(equal(id2[i][j], 0));
		    assert(equal(id3[i][j], 0));
		}
	    }
    }
    Mat<5, double> m2(v2);
    Mat<5, double> m3, m4;
    m3 = m2 + m1;
    m4 = m2 - (-m1);
    check_mats(m3, m4);
    m4 = m1;
    m4 += m2;
    check_mats(m3, m4);
    m3 = m2 - m1;
    m4 = m2;
    m4 -= m1;
    check_mats(m3, m4);
    m4 = m2;
    m4 += -m1;
    check_mats(m3, m4);
    d = doub_random();
    m3 = m2 * d;
    m4 = d * m2;
    check_mats(m3, m4);
    d = doub_random();
    if (d == 0 || d == 1) d += 1.5;
    m3 = m2 / d;
    m4 = m2 * (1. / d);
    check_mats(m3, m4);
    m4 = m2;
    m4 /= d;
    assert(m4 != m2);
    assert(m4 == m4);
    check_mats(m3, m4);
    m4 = m2;
    m4 *= m1;
    m3 = m2 * m1;
    check_mats(m3, m4);
    m3 = m1;
    m3.transpose();
    m3.transpose();
    assert(m3 == m1);
    for (int i = 0; i < 5; ++i)
	for (int j = 0; j < 5; ++j) {
	    assert(m3[i][j] == m3.row(i)[j]);
	    assert(m3[i][j] == m3.column(j)[i]);
	}
}

int main(int argc, char *argv[]) {
    ArgParser ap;
    int iterations = 50000;
    int seed = getpid();
    ap('n', "num-iterations", &iterations);
    ap('s', "seed", &seed);
    ap.process("testmat", argc, argv);

    cerr << "testmat: doing " << iterations << " iterations. Seed = " << seed << endl;
    srandom(seed);
    for (int i = 0; i < iterations; ++i) {
	if (!(i % 1000))
	    cerr << '.';
	test_Mat();

	Projection3 p1, p2, p3, p4;
	Mat<4, double> m1, m2, m3;
	for (int i = 0; i < 4; ++i)
	    for (int j = 0; j < 4; ++j) {
		p1.T[i][j] = m1[i][j] = doub_random();
	    }
	double d1 = m1.determinant();
	double d2 = p1.determinant();
	assert(equal(d1, d2));
	if (d1 != 0) {
	    p2 = p1;
	    p2.invert();
	    m2 = m1;
	    m2.invert();
	    p3 = p1 * p2;
	    p4 = p1;
	    p4 *= p2;
	    for (int i = 0; i < 4; ++i)
		for (int j = 0; j < 4; ++j) {
		    assert(equal(p2.T[i][j], m2[i][j]));
		    assert(equal(p1.T[i][j], m1[i][j]));
		    assert(equal(p3.T[i][j], p4.T[i][j]));
		    if (i == j) {
			assert(equal(p3.T[i][j], 1));
			assert(equal(p4.T[i][j], 1));
		    }
		    else {
			assert(equal(p3.T[i][j], 0));
			assert(equal(p4.T[i][j], 0));
		    }
		}
	}
    }
    cerr << endl << "matrix routines passed all tests." << endl;
    return 0;
}
#endif // __GNUG__