testtrans.cc

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

//
// testtrans.cc
//
// $Id: testtrans.cc,v 1.1.1.1 2001/02/28 00:28:39 cstolte Exp $
//

#include <sgl.h>
#include <assert.h>
#include <iostream>
#include <unistd.h>
#include "testsgl.h"

static void test_Matrix2();
static void test_Transform2();
static void test_Matrix3();
static void test_Transform3();
static void test_Projection3();

int iteration = 0;

int main(int argc, char **argv) {
    int seed = getpid();
    int times = 100000;
    bool forever = false;
    ArgParser ap;
    ap('n', "num-times", &times, Optional);
    ap('s', "seed", &seed, Optional);
    ap('f', "forever", &forever, Optional);
    ap.process("testtrans", argc, argv);

    if (forever == true)
	times = -1;
    cerr << "Starting test of transformations. Seed = " << seed 
	<< ", iterations = " <<	times << endl;
    srand48(seed);

    for (iteration = 0; iteration != times; ++iteration) {
	if (!(iteration % 10000)) cerr << '.';

	test_Matrix2();
	test_Transform2();
	test_Matrix3();
	test_Transform3();
	test_Projection3();
    }
    cerr << endl << " passed all tests!" << endl;
}

static void test_Matrix2() {
    Matrix2 m1, m2;
    m1.identity(); m2.identity();
    Point2 p(doub_random(), doub_random());
    Vector2 v(doub_random(), doub_random());
    Normal2 n(doub_random(), doub_random());

    checkUV(m1.apply(p), p);
    checkUV(m1.apply(v), v);
    checkUV(m1.apply(n), n);

    double d1 = doub_random(), d2 = doub_random();
    m1.scale(d1, d2);
    checkUV(m1.apply(p), Point2(p.u * d1, p.v * d2));
    checkUV(m1.apply(v), Vector2(v.u * d1, v.v * d2));
    Matrix2 m11 = m1;
    m11.invert().transpose();
    checkUV(m1.apply(n), m11.apply(Vector2(n.u, n.v)));
    m1 = m2;

    double d = doub_random();
    m1.rotate(d);
    m1.rotate(-d);
    checkUV(m1.apply(p), p);
    checkUV(m1.apply(v), v);
    checkUV(m1.apply(n), n);
	
    m1.rotate(d).scale(d2, d1);
    m2 = m1;
    m2.invert();
    checkUV(m2.apply(m1.apply(p)), p);
    checkUV(m2.apply(m1.apply(v)), v);
    checkUV(m2.apply(m1.apply(n)), n);
	
    Matrix2 m3 = m2 * m1;
    checkUV(m3.apply(p), m1.apply(m2.apply(p)));
    checkUV(m3.apply(v), m1.apply(m2.apply(v)));
    checkUV(m3.apply(n), m1.apply(m2.apply(n)));
    m2 *= m1;
    assert(m3 == m2);

    m1 = m2;
    if (d1 != 1 || d2 != 1)
	assert(m1.scale(d2, d1) != m2);

    checkUV(Matrix2::unit.apply(p), p);
    checkUV(Matrix2::unit.apply(v), v);
    checkUV(Matrix2::unit.apply(n), n);
}

static void test_Transform2() {
    Transform2 m1;
    Matrix2 m11;
    m1.identity();
    m11.identity();
    Point2 p(doub_random(), doub_random());
    Vector2 v(doub_random(), doub_random());
    Normal2 n(doub_random(), doub_random());

    checkUV(m1.apply(p), p);
    checkUV(m1.apply(v), v);
    checkUV(m1.apply(n), n);

    double d1 = doub_random(), d2 = doub_random(), d3a = doub_random();
    m1.scale(d1, d2);  m11.scale(d1, d2);
    checkUV(m1.apply(p), Point2(p.u * d1, p.v * d2));
    checkUV(m1.apply(v), Vector2(v.u * d1, v.v * d2));
    m1.rotate(d3a);
    m11.rotate(d3a);
    checkUV(m11.apply(p), m1.apply(p));
    checkUV(m11.apply(v), m1.apply(v));
    m1 = Transform2(m11);
    checkUV(m11.apply(p), m1.apply(p));
    checkUV(m11.apply(v), m1.apply(v));
//CO	Transform2 m11 = m1;
//CO	m11.invert().transpose();
//CO	checkUV(m1.apply(n), m11.apply(Vector2(n.u, n.v)));
    m1.identity();
    
    double d3 = doub_random(), d4 = doub_random();
    m1.translate(d3, d4);
    checkUV(m1.apply(p), Point2(p.u + d3, p.v + d4));
    checkUV(m1.apply(v), Point2(v.u , v.v));
    m1.scale(d1, d2);
    checkUV(m1.apply(p), Point2((p.u + d3) * d1, (p.v + d4) * d2));
    checkUV(m1.apply(v), Point2(v.u * d1, v.v * d2));
//CO	m11 = m1;
//CO	m11.invert.transpose();
//CO	checkUV(m1.apply(n), m11.apply(Vector2(n.u, n.v)));
    m1.identity();
    
    double d = doub_random();
    m1.rotate(d);
    m1.rotate(-d);
    checkUV(m1.apply(p), p);
    checkUV(m1.apply(v), v);
    checkUV(m1.apply(n), n);
    
    m1.rotate(d).translate(doub_random(), doub_random());
    m1.scale(d2, d1).translate(doub_random(), doub_random());
    Transform2 m2 = m1;
    m2.invert();
    checkUV(m2.apply(m1.apply(p)), p);
    checkUV(m2.apply(m1.apply(v)), v);
    checkUV(m2.apply(m1.apply(n)), n);
    
    Transform2 m3 = m2 * m1;
    checkUV(m3.apply(p), m1.apply(m2.apply(p)));
    checkUV(m3.apply(v), m1.apply(m2.apply(v)));
    checkUV(m3.apply(n), m1.apply(m2.apply(n)));
    m2 *= m1;
    assert(m3 == m2);

    m1 = m2;
    if (d1 != 1 && d2 != 1)
	assert(m1.scale(d2, d1) != m2);
    
    checkUV(Transform2::unit.apply(p), p);
    checkUV(Transform2::unit.apply(v), v);
    checkUV(Transform2::unit.apply(n), n);
}

static void test_Matrix3() {
    Matrix3 m1, m2;
    m1.identity(); m2.identity();
    Point3 p(doub_random(), doub_random(), doub_random());
    Vector3 v(doub_random(), doub_random(), doub_random());
    Normal3 n(doub_random(), doub_random(), doub_random());

    checkXYZ(m1.apply(p), p);
    checkXYZ(m1.apply(v), v);
    checkXYZ(m1.apply(n), n);

    double d1 = doub_random(), d2 = doub_random(), d3 = doub_random();
    m1.scale(d1, d2, d3);
    checkXYZ(m1.apply(p), Point3(p.x * d1, p.y * d2, p.z * d3));
    checkXYZ(m1.apply(v), Vector3(v.x * d1, v.y * d2,v.z * d3));
    Matrix3 m11 = m1;
    m11.invert().transpose();
    checkXYZ(m1.apply(n), m11.apply(Vector3(n.x, n.y, n.z)));
    m1 = m2;

    double d = doub_random();
    m1.rotatex(d);
    m1.rotatex(-d);
    checkXYZ(m1.apply(p), p);
    checkXYZ(m1.apply(v), v);
    checkXYZ(m1.apply(n), n);
    d = doub_random();
    m1.rotatey(d);
    m1.rotatey(-d);
    checkXYZ(m1.apply(p), p);
    checkXYZ(m1.apply(v), v);
    checkXYZ(m1.apply(n), n);
    d = doub_random();
    m1.rotatez(d);
    m1.rotatez(-d);
    checkXYZ(m1.apply(p), p);
    checkXYZ(m1.apply(v), v);
    checkXYZ(m1.apply(n), n);
    double dd = doub_random();
    m1.rotatex(d).rotatez(dd);
    m1.rotatez(-dd).rotatex(-d);
    checkXYZ(m1.apply(p), p);
    checkXYZ(m1.apply(v), v);
    checkXYZ(m1.apply(n), n);

    if (m1.determinant() != 0)
	assert(equal(m1.determinant(), m1.determinant(m1.adjoint())));

    m1.rotate(dd, Vector3(doub_random(), doub_random(), doub_random()));
    m1.scale(d2, d1, d3);
    m2 = m1;
    m2.invert();
    checkXYZ(m2.apply(m1.apply(p)), p);
    checkXYZ(m2.apply(m1.apply(v)), v);
    checkXYZ(m2.apply(m1.apply(n)), n);
	
    Matrix3 m3 = m2 * m1;
    checkXYZ(m3.apply(p), m1.apply(m2.apply(p)));
    checkXYZ(m3.apply(v), m1.apply(m2.apply(v)));
    checkXYZ(m3.apply(n), m1.apply(m2.apply(n)));
    m2 *= m1;
    assert(m3 == m2);
    m1 = m2;
    if (d1 != 1 && d2 != 1 && d3 != 1)
	assert(m1.scale(d2, d1, d3) != m2);

    checkXYZ(Matrix3::unit.apply(p), p);
    checkXYZ(Matrix3::unit.apply(v), v);
    checkXYZ(Matrix3::unit.apply(n), n);

    m1.identity(); m2.identity();
    d1 = doub_random();
    m1.rotatex(d1);
    m2.rotate(d1, Vector3(1, 0, 0));
    checkXYZ(m1.apply(p), m2.apply(p));
    checkXYZ(m1.apply(v), m2.apply(v));
    checkXYZ(m1.apply(n), m2.apply(n));

    m1.identity(); m2.identity();
    d1 = doub_random();
    m1.rotatey(d1);
    m2.rotate(d1, Vector3(0, 1, 0));
    checkXYZ(m1.apply(p), m2.apply(p));
    checkXYZ(m1.apply(v), m2.apply(v));
    checkXYZ(m1.apply(n), m2.apply(n));

    m1.identity(); m2.identity();
    d1 = doub_random();
    m1.rotatez(d1);
    m2.rotate(d1, Vector3(0, 0, 1));
    checkXYZ(m1.apply(p), m2.apply(p));
    checkXYZ(m1.apply(v), m2.apply(v));
    checkXYZ(m1.apply(n), m2.apply(n));

    p = Point3(1,0,0);
    m1.identity();
    m1.rotatey(M_PI / 2);
    checkXYZ(m1.apply(p), Point3(0,0,-1));
    m1.identity();
    m1.rotatez(M_PI / 2);
    checkXYZ(m1.apply(p), Point3(0,1,0));
    p = Point3(0,1,0);
    m1.identity().rotatex(M_PI / 2);
    checkXYZ(m1.apply(p), Point3(0,0,1));
}

static void test_Transform3() {
    Transform3 m1, m2;
    m1.identity(); m2.identity();
    Point3 p(doub_random(), doub_random(), doub_random());
    Vector3 v(doub_random(), doub_random(), doub_random());
    Normal3 n(doub_random(), doub_random(), doub_random());

    checkXYZ(m1.apply(p), p);
    checkXYZ(m1.apply(v), v);
    checkXYZ(m1.apply(n), n);

    double d1 = doub_random(), d2 = doub_random(), d3 = doub_random();
    m1.scale(d1, d2, d3);
    checkXYZ(m1.apply(p), Point3(p.x * d1, p.y * d2, p.z * d3));
    checkXYZ(m1.apply(v), Vector3(v.x * d1, v.y * d2,v.z * d3));
//    Transform3 m11 = m1;
//    m11.invert().transpose();
//    checkXYZ(m1.apply(n), m11.apply(Vector3(n.x, n.y, n.z)));
    m1 = m2;

    double d = doub_random();
    m1.rotatex(d);
    m1.rotatex(-d);
    checkXYZ(m1.apply(p), p);
    checkXYZ(m1.apply(v), v);
    checkXYZ(m1.apply(n), n);
    d = doub_random();
    m1.rotatey(d);
    m1.rotatey(-d);
    checkXYZ(m1.apply(p), p);
    checkXYZ(m1.apply(v), v);
    checkXYZ(m1.apply(n), n);
    d = doub_random();
    m1.rotatez(d);
    m1.rotatez(-d);
    checkXYZ(m1.apply(p), p);
    checkXYZ(m1.apply(v), v);
    checkXYZ(m1.apply(n), n);
    double dd = doub_random();
    m1.rotatex(d).rotatez(dd);
    m1.rotatez(-dd).rotatex(-d);
    checkXYZ(m1.apply(p), p);
    checkXYZ(m1.apply(v), v);
    checkXYZ(m1.apply(n), n);

    m1.identity();
    Transform3 m3(m1);
    assert(m1 == m3);

    m1.rotate(dd, Vector3(doub_random(), doub_random(), doub_random()));
    m1.scale(d2, d1, d3);
    m1.translate(doub_random(), doub_random(), doub_random());
    m2 = m1;
    m2.invert();
    checkXYZ(m2.apply(m1.apply(p)), p);
    checkXYZ(m2.apply(m1.apply(v)), v);
    checkXYZ(m2.apply(m1.apply(n)), n);

    m1 = m3;
    d1 = doub_random(), d2 = doub_random(), d3 = doub_random();
    m2 = m1;
    m1.translate(d1, d2, d3);
    checkXYZ(m1.apply(p), m2.apply(p) + Vector3(d1, d2, d3));
    checkXYZ(m1.apply(v), m2.apply(v));
    checkXYZ(m1.apply(n), m2.apply(n));

    m3 = m2 * m1;
    checkXYZ(m3.apply(p), m1.apply(m2.apply(p)));
    checkXYZ(m3.apply(v), m1.apply(m2.apply(v)));
    checkXYZ(m3.apply(n), m1.apply(m2.apply(n)));
    m2 *= m1;
    assert(m3 == m2);
    m1 = m2;
    if (d1 != 1 && d2 != 1 && d3 != 1)
	assert(m1.scale(d2, d1, d3) != m2);

    checkXYZ(Transform3::unit.apply(p), p);
    checkXYZ(Transform3::unit.apply(v), v);
    checkXYZ(Transform3::unit.apply(n), n);

    m1.identity(); m2.identity();
    d1 = doub_random();
    m1.rotatex(d1);
    m2.rotate(d1, Vector3(1, 0, 0));
    checkXYZ(m1.apply(p), m2.apply(p));
    checkXYZ(m1.apply(v), m2.apply(v));
    checkXYZ(m1.apply(n), m2.apply(n));

    m1.identity(); m2.identity();
    d1 = doub_random();
    m1.rotatey(d1);
    m2.rotate(d1, Vector3(0, 1, 0));
    checkXYZ(m1.apply(p), m2.apply(p));
    checkXYZ(m1.apply(v), m2.apply(v));
    checkXYZ(m1.apply(n), m2.apply(n));

    m1.identity(); m2.identity();
    d1 = doub_random();
    m1.rotatez(d1);
    m2.rotate(d1, Vector3(0, 0, 1));
    checkXYZ(m1.apply(p), m2.apply(p));
    checkXYZ(m1.apply(v), m2.apply(v));
    checkXYZ(m1.apply(n), m2.apply(n));

    p = Point3(1,0,0);
    m1.identity();
    m1.rotatey(M_PI / 2);
    checkXYZ(m1.apply(p), Point3(0,0,-1));
    m1.identity();
    m1.rotatez(M_PI / 2);
    checkXYZ(m1.apply(p), Point3(0,1,0));
    p = Point3(0,1,0);
    m1.identity().rotatex(M_PI / 2);
    checkXYZ(m1.apply(p), Point3(0,0,1));
}

static void test_Projection3() {
    Projection3 m1, m2;
    m1.identity(); m2.identity();
    Point3 p(doub_random(), doub_random(), doub_random());
    Vector3 v(doub_random(), doub_random(), doub_random());
    Normal3 n(doub_random(), doub_random(), doub_random());

    checkXYZ(m1.project(p), p);
    checkXYZ(m1.project(v), v);
    checkXYZ(m1.project(n), n);

    double d1 = doub_random(), d2 = doub_random(), d3 = doub_random();
    m1.scale(d1, d2, d3);
    checkXYZ(m1.project(p), Point3(p.x * d1, p.y * d2, p.z * d3));
    checkXYZ(m1.project(v), Vector3(v.x * d1, v.y * d2,v.z * d3));
//    Projection3 m11 = m1;
//    m11.invert().transpose();
//    checkXYZ(m1.project(n), m11.project(Vector3(n.x, n.y, n.z)));
    m1 = m2;

    double d = doub_random();
    m1.rotatex(d);
    m1.rotatex(-d);
    checkXYZ(m1.project(p), p);
    checkXYZ(m1.project(v), v);
    checkXYZ(m1.project(n), n);
    d = doub_random();
    m1.rotatey(d);
    m1.rotatey(-d);
    checkXYZ(m1.project(p), p);
    checkXYZ(m1.project(v), v);
    checkXYZ(m1.project(n), n);
    d = doub_random();
    m1.rotatez(d);
    m1.rotatez(-d);
    checkXYZ(m1.project(p), p);
    checkXYZ(m1.project(v), v);
    checkXYZ(m1.project(n), n);
    double dd = doub_random();
    m1.rotatex(d).rotatez(dd);
    m1.rotatez(-dd).rotatex(-d);
    checkXYZ(m1.project(p), p);
    checkXYZ(m1.project(v), v);
    checkXYZ(m1.project(n), n);

    m1.identity();
    Projection3 m3(m1);
    assert(m1 == m3);

    m1.rotate(dd, Vector3(doub_random(), doub_random(), doub_random()));
    m1.scale(d2, d1, d3);
    m1.translate(doub_random(), doub_random(), doub_random());
    m2 = m1;
    m2.invert();
    checkXYZ(m2.project(m1.project(p)), p);
    checkXYZ(m2.project(m1.project(v)), v);
    checkXYZ(m2.project(m1.project(n)), n);

    m1 = m3;
    d1 = doub_random(), d2 = doub_random(), d3 = doub_random();
    m2 = m1;
    m1.translate(d1, d2, d3);
    checkXYZ(m1.project(p), m2.project(p) + Vector3(d1, d2, d3));
    checkXYZ(m1.project(v), m2.project(v));
    checkXYZ(m1.project(n), m2.project(n));

    m3 = m2 * m1;
    checkXYZ(m3.project(p), m1.project(m2.project(p)));
    checkXYZ(m3.project(v), m1.project(m2.project(v)));
    checkXYZ(m3.project(n), m1.project(m2.project(n)));
    m2 *= m1;
    assert(m3 == m2);
    m1 = m2;
    if (d1 != 1 && d2 != 1 && d3 != 1)
	assert(m1.scale(d2, d1, d3) != m2);

    m1.identity();
    checkXYZ(m1.project(p), p);
    checkXYZ(m1.project(v), v);
    checkXYZ(m1.project(n), n);

    m1.identity(); m2.identity();
    d1 = doub_random();
    m1.rotatex(d1);
    m2.rotate(d1, Vector3(1, 0, 0));
    checkXYZ(m1.project(p), m2.project(p));
    checkXYZ(m1.project(v), m2.project(v));
    checkXYZ(m1.project(n), m2.project(n));

    m1.identity(); m2.identity();
    d1 = doub_random();
    m1.rotatey(d1);
    m2.rotate(d1, Vector3(0, 1, 0));
    checkXYZ(m1.project(p), m2.project(p));
    checkXYZ(m1.project(v), m2.project(v));
    checkXYZ(m1.project(n), m2.project(n));

    m1.identity(); m2.identity();
    d1 = doub_random();
    m1.rotatez(d1);
    m2.rotate(d1, Vector3(0, 0, 1));
    checkXYZ(m1.project(p), m2.project(p));
    checkXYZ(m1.project(v), m2.project(v));
    checkXYZ(m1.project(n), m2.project(n));

    p = Point3(1,0,0);
    m1.identity();
    m1.rotatey(M_PI / 2);
    checkXYZ(m1.project(p), Point3(0,0,-1));
    m1.identity();
    m1.rotatez(M_PI / 2);
    checkXYZ(m1.project(p), Point3(0,1,0));
    p = Point3(0,1,0);
    m1.identity().rotatex(M_PI / 2);
    checkXYZ(m1.project(p), Point3(0,0,1));
}