testsgl.cc

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

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

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

static double doub_random();
static void test_Point2();
static void test_Vector2();
static void test_Normal2();
static void test_Point3();
static void test_Vector3();
static void test_Normal3();
static void test_Interval();
static void test_RGB();
static void test_RGBA();
static void test_homogeneous2();
static void test_homogeneous3();

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);
//CO    ap('f', "forever", &forever, Optional);
    ap.process("test", argc, argv);

    if (forever == true)
	times = -1;
    cerr << "Starting test of . Seed = " << seed << ", iterations = " <<
	times << endl;
    srandom(seed);
    //assert(m_pi > 3 && m_pi < 4); // just to be sure the hack works

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

	test_Point2();
	test_Vector2();
	test_Normal2();
	test_Point3();
	test_Vector3();
	test_Normal3();
	test_Interval();
	test_RGB();
	test_RGBA();
	test_homogeneous2();
	test_homogeneous3();
    }
    cerr << endl << " passed all tests!" << endl;
}
	
static void test_Point2() {
    Point2 p2(doub_random(), doub_random());
    Vector2 v2(doub_random(), doub_random());

    checkUV(p2-p2, Point2::origin);
    checkUV(p2+-v2, p2 - v2);
    Point2 p2a = p2;
    p2a += v2;
    checkUV(p2a, p2 + v2);
    p2a -= v2;
    p2a -= v2;
    checkUV(p2a, p2 - v2);
    assert(p2a == p2a);
    assert(p2a != p2);
    assert(distance(p2, p2a) == sqrt(distance_squared(p2a, p2)));
    assert(distance(p2, p2a) == distance(p2a, p2));
    for (double d = 0; d < 1; d += .1)
	assert(lerp(p2, p2a, d).between(p2, p2a));
    assert(!(lerp(p2, p2a, -.1).between(p2, p2a)));
    assert(!(lerp(p2, p2a, 1.1).between(p2, p2a)));
}

static void test_Vector2() {
    Vector2 v1(doub_random(), doub_random());
    Vector2 v2(doub_random(), doub_random());
    Vector2 v3(doub_random(), doub_random());
    Vector2 v0(0, 0);

    checkUV(-v2, v0 - v2);
    checkUV(-(-v3), v3);
    checkUV(v1-v2, -v2+v1);
    checkUV(v1-v2, v1+-v2);
    checkUV(v1+(v2-v3), (v1+v2)-v3);
    v3 = v1;
    checkUV(v1, v3);
    v3 += v2;
    checkUV(v3, v1+v2);
    v3 -= v2;
    v3 -= v2;
    checkUV(v3+v2, v1);
    checkUV(v3, v1-v2);

    double d = doub_random();
    if (d == 0) d += .0001;

    checkUV(v2*d, d * v2);
    v1 = v2;
    v1 /= d;
    checkUV(v1, v2 / d);
    v1 *= d;
    checkUV(v1, v2);
    v1 = v2;
    assert(v1 == v2);
    d = 0;
    while (d < 1)
	d = fabs(doub_random());
    v1 /= d;
    assert(v1 != v2);
    assert(v1.length() < v2.length());
    assert(equal(sqrt(v1.length_squared()), v1.length()));
    assert(equal(v1.length(), sqrt(dot(v1, v1))));
    assert(v1 == v1.transpose().transpose());
    v1 = v1.normalize();
    assert(equal(v1.length(), 1));
}

static void test_Normal2() {
    Normal2 n1(doub_random(), doub_random());
    Normal2 n2(doub_random(), doub_random());
    Normal2 n0(0, 0);

    checkUV(n0 - n1, -n1);
    checkUV(-(-n1), n1);
    checkUV(n1+n2, n2+n1);
    checkUV(n1+n0, n1);
    Normal2 n3(n1);
    checkUV(n1, n3);
    n1 += n2;
    checkUV(n1, n2 + n3);
    checkUV(n1 - n2, n3);
    n1 -= n2;
    n1 -= n2;
    checkUV(n1 + n2, n3);
    checkUV(n3 - n2, n1);
    n1 = n3;
    double d = doub_random();
    n1 *= d;
    checkUV(n1, n3 * d);
    checkUV(d * n3, n3 * d);
    d = fabs(doub_random());
    if (d < 1)
	d += 1;
    n1 = n3;
    n1 *= d;
    checkUV(n1 / d, n3);
    n1 /= d;
    checkUV(n1, n3);
    n1 = n2;
    assert(n1 == n2);
    assert(n1.transpose().transpose() == n1);
    n1 *= d;
    assert(n1 != n2);

    assert(dot(n1,n2) == dot(n2, n1));
    Vector2 v(doub_random(), doub_random());
    assert(dot(n1,v) == dot(v, n1));
    assert(cos(n3,n2) == cos(n2, n3));
    assert(equal(n1.length(), sqrt(dot(n1, n1))));
    assert(equal(n1.length(), sqrt(dot(n1, n1))));
    Vector2 v1(doub_random(), doub_random());
    assert(cos(n3,v1) == cos(v1, n3));
}

static void test_Point3() {
    Point3 p3(doub_random(), doub_random(), doub_random());
    Vector3 v3(doub_random(), doub_random(), doub_random());

    checkXYZ(p3-p3, Point3::origin);
    checkXYZ(p3+-v3, p3 - v3);
    Point3 p3a = p3;
    p3a += v3;
    checkXYZ(p3a, p3 + v3);
    p3a -= v3;
    p3a -= v3;
    checkXYZ(p3a, p3 - v3);
    assert(p3a == p3a);
    assert(p3a != p3);
    assert(distance(p3, p3a) == sqrt(distance_squared(p3a, p3)));
    assert(distance(p3, p3a) == distance(p3a, p3));
    for (double d = 0; d < 1; d += .1)
	assert(lerp(p3, p3a, d).between(p3, p3a));
    assert(!(lerp(p3, p3a, -.1).between(p3, p3a)));
    assert(!(lerp(p3, p3a, 1.1).between(p3, p3a)));
}

static void test_Vector3() {
    Vector3 v1(doub_random(), doub_random(), doub_random());
    Vector3 v2(doub_random(), doub_random(), doub_random());
    Vector3 v3(doub_random(), doub_random(), doub_random());
    Vector3 v0(0, 0, 0);

    checkXYZ(-v2, v0 - v2);
    checkXYZ(-(-v3), v3);
    checkXYZ(v1-v2, -v2+v1);
    checkXYZ(v1-v2, v1+-v2);
    checkXYZ(v1+(v2-v3), (v1+v2)-v3);
    v3 = v1;
    checkXYZ(v1, v3);
    v3 += v2;
    checkXYZ(v3, v1+v2);
    v3 -= v2;
    v3 -= v2;
    checkXYZ(v3+v2, v1);
    checkXYZ(v3, v1-v2);

    double d = doub_random();
    if (d == 0) d += .0001;

    checkXYZ(v2*d, d * v2);
    v1 = v2;
    v1 /= d;
    checkXYZ(v1, v2 / d);
    v1 *= d;
    checkXYZ(v1, v2);
    v1 = v2;
    assert(v1 == v2);
    d = 0;
    while (d < 1)
	d = fabs(doub_random());
    v1 /= d;
    assert(v1 != v2);
    assert(v1.length() < v2.length());
    assert(equal(sqrt(v1.length_squared()), v1.length()));
    assert(equal(v1.length(), sqrt(dot(v1, v1))));
    assert(v1 == v1.transpose().transpose());
    v1 = v1.normalize();
    assert(equal(v1.length(), 1));
    v1 = Vector3::polar(doub_random(), doub_random());
    assert(equal(v1.length(), 1));
}

static void test_Normal3() {
    Normal3 n1(doub_random(), doub_random(), doub_random());
    Normal3 n2(doub_random(), doub_random(), doub_random());
    Normal3 n0(0, 0, 0);

    checkXYZ(n0 - n1, -n1);
    checkXYZ(-(-n1), n1);
    checkXYZ(n1+n2, n2+n1);
    checkXYZ(n1+n0, n1);
    Normal3 n3(n1);
    checkXYZ(n1, n3);
    n1 += n2;
    checkXYZ(n1, n2 + n3);
    checkXYZ(n1 - n2, n3);
    n1 -= n2;
    n1 -= n2;
    checkXYZ(n1 + n2, n3);
    checkXYZ(n3 - n2, n1);
    n1 = n3;
    double d = doub_random();
    n1 *= d;
    checkXYZ(n1, n3 * d);
    checkXYZ(d * n3, n3 * d);
    d = fabs(doub_random());
    if (d < 1)
	d += 1;
    n1 = n3;
    n1 *= d;
    checkXYZ(n1 / d, n3);
    n1 /= d;
    checkXYZ(n1, n3);
    n1 = n2;
    assert(n1 == n2);
    assert(n1.transpose().transpose() == n1);
    n1 *= d;
    assert(n1 != n2);

    assert(dot(n1,n2) == dot(n2, n1));
    Vector3 v(doub_random(), doub_random(), doub_random());
    assert(dot(n1,v) == dot(v, n1));
    assert(cos(n3,n2) == cos(n2, n3));
    assert(equal(n1.length(), sqrt(dot(n1, n1))));
    Vector3 v1(doub_random(), doub_random(), doub_random());
    assert(cos(n3,v1) == cos(v1, n3));
}

#define CHECK_IVAL_INVARIANT \
			    { for (int i = 0; i < 10; ++i) \
				{ \
				    assert(ival[i].inf() >= ival[i].sup()); \
				    assert(d[i]+epsilon >= ival[i].inf() && d[i]-epsilon <= ival[i].sup()); \
				} }

static void test_Interval() {
    double d[10], od[10];
    Interval ival[10];
    Interval orig[10];
    for (int i = 0; i < 10; ++i)
      {
	  od[i] = d[i] = doub_random();
	  orig[i] = ival[i] = Interval(d[i]);
	  assert(orig[i] == ival[i]);
      }

    ival[0] += ival[1];
    d[0] += d[1];
    checkIval(ival[0], orig[0] + orig[1]);
    checkIval(ival[0], orig[0] + od[1]);
    checkIval(ival[0], od[1] + orig[0]);
    ival[0] -= ival[1];    ival[0] -= ival[1];
    d[0] -= d[1];     d[0] -= d[1]; 
    checkIval(ival[0], orig[0] - orig[1]);
    checkIval(ival[0], orig[0] - od[1]);

    CHECK_IVAL_INVARIANT;

    ival[2] *= od[3];
    d[2] *= od[3];
    checkIval(ival[2], od[3] * orig[2]);
    checkIval(ival[2], orig[2] * orig[3]);
    checkIval(ival[2], orig[2] * od[3]);

    CHECK_IVAL_INVARIANT;

    ival[3] = fabs(orig[3]);
    d[3] = fabs(od[3]);
    if (d[3] == 0) ++d[3];
    if (ival[3] == 0) ival[3] += 1.;
    ival[4] /= ival[3];
    d[4] /= d[3];
    assert(ival[4] != orig[4]);
    if (orig[4] > 0.)
      {
	  if (d[3] >= 1.)
	    {
		assert(ival[3] > 1. || ival[3] == 1.);
		assert(ival[4] < orig[4] || ival[4] == orig[4]);
	    }
	  else
	    {
		assert(!(ival[3] > 1. || ival[3] == .1));
		assert(ival[4] > orig[4]);
	    }
      }
    else
      {
	  if (d[3] >= 1.)
	    {
		assert(ival[3] > 1. || ival[3] == .1);
		assert(ival[4] > orig[4] || ival[4] == orig[4]);
	    }
	  else
	    {
		assert(!(ival[3] > 1. || ival[3] == .1));
		assert(ival[4] < orig[4]);
	    }
      }
    if (d[5] < ival[5].inf())
	assert(d[5] < ival[5] && ival[5] > d[5]);
    else if (d[5] > ival[5].sup())
	assert(d[5] > ival[5] && ival[5] < d[5]);
    
    assert((subset(d[5], ival[5]) && subset(ival[5], d[5])) ||
	   (!subset(d[5], ival[5]) && !subset(ival[5], d[5])));

    checkIval(ival[4], orig[4] / d[3]);
    checkIval(ival[4], (1. / d[3]) * orig[4]);
    checkIval(-ival[4], -1. * ival[4]);

    CHECK_IVAL_INVARIANT;
}

static void test_RGB() {
#if 0
    RGB c1(drand48(), drand48(), drand48());   ShortRGB sc1(c1 / 2.);
    RGB c2(drand48(), drand48(), drand48());   ShortRGB sc2(c2 / 4.);
    RGB c3 = c1;                               ShortRGB sc3(c3 / 2.);
    c3 += c2;                                     sc3 += sc2;
    checkRGB(c1 + c2, c3);                        checkSRGB(sc1 + sc2, sc3);
    checkRGB(c2 + c1, c1 + c2);                   checkSRGB(sc1 + sc2, sc2 + sc1);
    c3 -= 2 * c2;                                 sc3 -= 2 * sc2;
    checkRGB(c3, c1 - c2);                        checkSRGB(sc3, sc1 - sc2);
    checkRGB(c2, -1 * -c2);                       
    checkRGB(c2, -c2 / -1);
    double d = fabs(doub_random());               double ds = drand48() * 4 + 2;
    if (d == 1 || d == 0) ++d;                    
    assert(c2 * d != c2);                         assert(sc2 * ds != sc2);
    checkRGB(c2 * d, d * c2);                     checkSRGB(sc2 * ds, ds * sc2);
    c3 = c2;                                      sc3 = sc2;
    c3 *= d;                                      sc3 *= ds;
    checkRGB(c3, c2 * d);                         checkSRGB(sc3, sc2 * ds);
    checkRGB(c3 / d, c2);                         //checkSRGB(sc3 / ds, sc2);
    checkRGB(c2 / d, (1. / d) * c2);              checkSRGB(sc2 / ds, (1. / ds) * sc2);
    c1 = c3;                                      sc1 = sc3;
    c1 /= d;                                      sc1 /= ds;
    checkRGB(c1, c3 / d);                         checkSRGB(sc1, sc3 / ds);
    c1 = c2;                                      sc1 = sc2;
    c1 *= c3;                                     sc1 *= sc3;
    checkRGB(c1, c2 * c3);                        checkSRGB(sc1, sc2 * sc3);
    checkRGB(c1, c3 * c2);                        checkSRGB(sc1, sc3 * sc2);
    c2 *= d * d;
    if (c2.out_of_gammut()) {
	c1 = c2.clamp();
	assert(!c1.out_of_gammut());
	c1 = c2.scale_to_gammut();
	assert(!c1.out_of_gammut());
	c1 = c2.const_intensity_clip();
	assert(!c1.out_of_gammut());
    }
#endif
}

static void test_RGBA() {
  
}

static void test_homogeneous2() {
    Homogeneous2 h1(doub_random(), doub_random(), 0);
    Homogeneous2 h2(doub_random(), doub_random(), 0);
    Homogeneous2 h3, h4;
    Point2 p1(doub_random(), doub_random()), p2(doub_random(), doub_random());
    Vector2 v1(doub_random(), doub_random()), v2(doub_random(), doub_random());

    h3 = h1 + h2;
    h4 = h1 - (-h2);
    checkH2(h3, h4);
    h4 = h1;
    h4 += h2;
    checkH2(h3, h4);
    h4 = h2;
    h4 += h1;
    checkH2(h3, h4);
    h3 = h1 - h2;
    h4 = h1 + -h2;
    checkH2(h3, h4);
    h4 = h1;
    h4 -= h2;
    checkH2(h3, h4);
    h4 = h2 - h1;
    h4 *= -1;
    checkH2(h3, h4);
    double d = doub_random();
    if (d == 0 || d == 1) d += 1.4;
    h3 = h3 * d;
    h4 *= d;
    checkH2(h3, h4);
    h4 = d * (h1 - h2);
    checkH2(h3, h4);
    d = doub_random();
    if (d == 0 || d == 1) d += 1.4;
    h4 = h3 / d;
    h3 /= d;
    checkH2(h3, h4);
    h3 = h4.hat();
    assert(equal(h3.length(), 1.));
    assert(h3 == h3);
    h4 = h3 * d;
    assert(h3 != h4);

    h1 = p1;
    h2 = p2;
    h3 = h1 - h2;
    Vector2 p3 = p1 - p2;
    Vector2 p4 = (Vector2)h3;
    checkUV(p3, p4);
}

static void test_homogeneous3() {
    Homogeneous3 h1(doub_random(), doub_random(), doub_random(), 0);
    Homogeneous3 h2(doub_random(), doub_random(), doub_random(), 0);
    Homogeneous3 h3, h4;
    Point3 p1(doub_random(), doub_random(), doub_random()), 
           p2(doub_random(), doub_random(), doub_random());
    Vector3 v1(doub_random(), doub_random(), doub_random()),
            v2(doub_random(), doub_random(), doub_random());

    h3 = h1 + h2;
    h4 = h1 - (-h2);
    checkH3(h3, h4);
    h4 = h1;
    h4 += h2;
    checkH3(h3, h4);
    h4 = h2;
    h4 += h1;
    checkH3(h3, h4);
    h3 = h1 - h2;
    h4 = h1 + -h2;
    checkH3(h3, h4);
    h4 = h1;
    h4 -= h2;
    checkH3(h3, h4);
    h4 = h2 - h1;
    h4 *= -1;
    checkH3(h3, h4);
    double d = doub_random();
    if (d == 0 || d == 1) d += 1.4;
    h3 = h3 * d;
    h4 *= d;
    checkH3(h3, h4);
    h4 = d * (h1 - h2);
    checkH3(h3, h4);
    d = doub_random();
    if (d == 0 || d == 1) d += 1.4;
    h4 = h3 / d;
    h3 /= d;
    checkH3(h3, h4);
    h3 = h4.hat();
    assert(equal(h3.length(), 1.));
    assert(h3 == h3);
    h4 = h3 * d;
    assert(h3 != h4);

    h1 = p1;
    h2 = p2;
    h3 = h1 - h2;
    Vector3 p3 = p1 - p2;
    Vector3 p4 = (Vector3)h3;
    checkXYZ(p3, p4);
}