testplane.cc

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

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

#include <sgl.h>
#include <sgl/args.h>
#include <unistd.h>
#include <stdlib.h>
#include <assert.h>
#include <iostream>
using namespace std;

static double dr()
{
    return -100. + 200. * drand48();
}

int main(int argc, char *argv[])
{
    int seed = getpid();
    int times = 100000;
    ArgParser ap;
    ap('n', "num-times", &times, Optional);
    ap('s', "seed", &seed, Optional);
    ap.process("testplane", argc, argv);

    cerr << "Starting test plane. Seed = " << seed << ", iterations = " <<
	times << endl;
    srand48(seed);

    // do some stuff we can check by hand
    Plane px(Point3(1,0,0), Normal3(1, 0, 0));
    Plane py(Point3(0,1,0), Normal3(0, 1, 0));
    Plane pz(Point3(0,0,1), Normal3(0, 0, 1));

    Point3 ip;
    Vector3 id;
    assert(px.line(py, &ip, &id));
    cerr << "px/py: " << ip << "\t" << id << endl;
    assert(px.line(pz, &ip, &id));
    cerr << "px/pz: " << ip << "\t" << id << endl;
    assert(pz.line(py, &ip, &id));
    cerr << "pz/py: " << ip << "\t" << id << endl;

    for (int i = 0; i < times; ++i) {
	assert(px.coincident(Point3(1., dr(), dr())));
	assert(px.above(Point3(100.1 + dr(), dr(), dr())));
	assert(px.below(Point3(dr() - 100.1, dr(), dr())));

	assert(py.coincident(Point3(dr(), 1., dr())));
	assert(py.above(Point3(dr(), 100.1 + dr(), dr())));
	assert(py.below(Point3(dr(), dr() - 100.1, dr())));

	assert(pz.coincident(Point3(dr(), dr(), 1.)));
	assert(pz.above(Point3(dr(), dr(), 100.1 + dr())));
	assert(pz.below(Point3(dr(), dr(), dr() - 100.1)));

	Point3 p(dr(), dr(), dr());
	assert(equal(px.distance(p), sqrt(sqr(p.x - 1))));
	assert(equal(py.distance(p), sqrt(sqr(p.y - 1))));
	assert(equal(pz.distance(p), sqrt(sqr(p.z - 1))));

	Plane pr(Point3(dr(), dr(), dr()), Normal3(dr(), dr(), dr()));
	pr.distance(p);

	Plane pr2(Point3(dr(), dr(), dr()), Normal3(dr(), dr(), dr()));
	if (pr.line(pr2, &ip, &id)) {
//CO	    cerr << pr << '\t' << pr2 << endl;
//CO	    cerr << ip << '\t' << id << endl;
//CO	    cerr << pr.distance(ip) << endl;
//CO	    cerr << pr2.distance(ip) << endl;

	    Point3 ip2 = ip + dr() * id;

//CO	    cerr << pr.distance(ip2) << endl;
//CO	    cerr << pr2.distance(ip2) << endl;

	    assert(pr.coincident(ip));
	    assert(pr2.coincident(ip));
	    assert(pr.coincident(ip2));
	    assert(pr2.coincident(ip2));
	}
    }

    cerr << "passed" << endl;

    return 0;
}