transform3.cc

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

bool
Transform3::operator!=(const Transform3 &t2) const {
    return (T[0][0] != t2.T[0][0] ||
	    T[0][1] != t2.T[0][1] ||
	    T[0][2] != t2.T[0][2] ||
	    T[1][0] != t2.T[1][0] ||
	    T[1][1] != t2.T[1][1] ||
	    T[1][2] != t2.T[1][2] ||
	    T[2][0] != t2.T[2][0] ||
	    T[2][1] != t2.T[2][1] ||
	    T[2][2] != t2.T[2][2] ||
	    translation != t2.translation);
}

Point3
Transform3::apply(const Point3 &p) const {
    Point3 ret(Matrix3::apply(p));
    return ret += translation;
}

Vector3
Transform3::apply(const Vector3 &v) const {
    return Matrix3::apply(v);
}

Normal3
Transform3::apply(const Normal3 &n, const Transform3 *inv) const {
    return Matrix3::apply(n, inv);
}

Ray3
Transform3::apply(const Ray3 &r) const {
    return Ray3(apply(r.o), apply(r.d));
}

Transform3 translate(double tx, double ty, double tz) {
    return Transform3(Matrix3::unit, Vector3(tx, ty, tz));
}

/*
 * Window Transform maps:
 *		l to -1
 *		r to +1
 *		b to -1
 *		t to +1
 */
//COTransform3&
//COTransform3::window(double l, double r, double b, double t) {
//CO    double sx,sy,dx,dy;
//CO    register double *aptr;
//CO    int register cnt;
//CO
//CO    sx=2./(r-l);
//CO    sy=2./(t-b);
//CO    dx=1.0 - r*sx;
//CO    dy=1.0 - t*sy;
//CO
//CO    aptr=T[0];
//CO    cnt =4;
//CO    do {
//CO        aptr[3] += aptr[0]*dx + aptr[1]*dy;
//CO        aptr[0] *= sx;
//CO        aptr[1] *= sy;
//CO        aptr += 4;
//CO    } while (--cnt);
//CO
//CO    return *this;
//CO}

//
// TransInv3
//

TransInv3::TransInv3(const Transform3 &t) 
  : trans(t) {
      itrans = trans.inverse();
}

TransInv3::TransInv3(const Transform3 &t, const Transform3 &i)
  : trans(t), itrans(i) {
}

Point3
TransInv3::apply(const Point3 &p) const {
    return trans.apply(p);
}

Vector3
TransInv3::apply(const Vector3 &v) const {
    return trans.apply(v);
}

Normal3
TransInv3::apply(const Normal3 &n) const {
    return trans.apply(n, &itrans);
}

Ray3
TransInv3::apply(const Ray3 &r) const {
    return trans.apply(r);
}


Point3
TransInv3::applyInv(const Point3 &p) const {
    return itrans.apply(p);
}

Vector3
TransInv3::applyInv(const Vector3 &v) const {
    return itrans.apply(v);
}

Normal3
TransInv3::applyInv(const Normal3 &n) const {
    return itrans.apply(n, &trans);
}

Ray3
TransInv3::applyInv(const Ray3 &r) const {
    return itrans.apply(r);
}

//
// Projection3
//

Projection3::Projection3(double a00, double a01, double a02, double a03,
			 double a10, double a11, double a12, double a13,
			 double a20, double a21, double a22, double a23,
			 double a30, double a31, double a32, double a33) {
    T[0][0] = a00; T[0][1] = a01; T[0][2] = a02; T[0][3] = a03;
    T[1][0] = a10; T[1][1] = a11; T[1][2] = a12; T[1][3] = a13;
    T[2][0] = a20; T[2][1] = a21; T[2][2] = a22; T[2][3] = a23;
    T[3][0] = a30; T[3][1] = a31; T[3][2] = a32; T[3][3] = a33;
}

Projection3::Projection3(const Matrix3 &m) {
    for (int i = 0; i < 3; ++i)
	for (int j = 0; j < 3; ++j)
	    T[i][j] = m.T[i][j];
    T[0][3] = T[1][3] = T[2][3] = T[3][0] = T[3][1] = T[3][2] = 0;
    T[3][3] = 1;
}

Projection3::Projection3(const Transform3 &t) {
    for (int i = 0; i < 3; ++i)
	for (int j = 0; j < 3; ++j)
	    T[i][j] = t.T[i][j];
    T[0][3] = t.translation.x;
    T[1][3] = t.translation.y;
    T[2][3] = t.translation.z;
    T[3][0] = T[3][1] = T[3][2] = 0;
    T[3][3] = 1;
}

Projection3
Projection3::operator*(const Projection3 &p) const {
    Projection3 ret(*this);
    return ret *= p;
}

Projection3
Projection3::operator+(const Projection3 &p) const {
    Projection3 ret(*this);
    return ret += p;
}

Projection3
Projection3::operator-(const Projection3 &p) const {
    Projection3 ret(*this);
    return ret -= p;
}

Projection3 &
Projection3::operator*=(const Projection3 &p2) {
    Projection3 old(*this);
    for (int i = 0; i < 4; ++i)
	for (int j = 0; j < 4; ++j) {
	    T[i][j] = 0;
	    for (int k = 0; k < 4; ++k) 
		T[i][j] += p2.T[i][k] * old.T[k][j]; 
	}
    return *this;
}

Projection3 &
Projection3::operator+=(const Projection3 &p) {
    T[0][0] += p.T[0][0]; T[0][1] += p.T[0][1]; T[0][2] += p.T[0][2]; T[0][3] += p.T[0][3];
    T[1][0] += p.T[1][0]; T[1][1] += p.T[1][1]; T[1][2] += p.T[1][2]; T[1][3] += p.T[1][3];
    T[2][0] += p.T[2][0]; T[2][1] += p.T[2][1]; T[2][2] += p.T[2][2]; T[2][3] += p.T[2][3];
    T[3][0] += p.T[3][0]; T[3][1] += p.T[3][1]; T[3][2] += p.T[3][2]; T[3][3] += p.T[3][3];
    return *this;
}

Projection3 & Projection3::operator-=(const Projection3 &p) {
    T[0][0] -= p.T[0][0]; T[0][1] -= p.T[0][1]; T[0][2] -= p.T[0][2]; T[0][3] -= p.T[0][3];
    T[1][0] -= p.T[1][0]; T[1][1] -= p.T[1][1]; T[1][2] -= p.T[1][2]; T[1][3] -= p.T[1][3];
    T[2][0] -= p.T[2][0]; T[2][1] -= p.T[2][1]; T[2][2] -= p.T[2][2]; T[2][3] -= p.T[2][3];
    T[3][0] -= p.T[3][0]; T[3][1] -= p.T[3][1]; T[3][2] -= p.T[3][2]; T[3][3] -= p.T[3][3];
    return *this;
}

Projection3 &
Projection3::identity() {
    T[0][0] = 1; T[0][1] = 0; T[0][2] = 0; T[0][3] = 0;
    T[1][0] = 0; T[1][1] = 1; T[1][2] = 0; T[1][3] = 0;
    T[2][0] = 0; T[2][1] = 0; T[2][2] = 1; T[2][3] = 0;
    T[3][0] = 0; T[3][1] = 0; T[3][2] = 0; T[3][3] = 1;
    return *this;
}

Projection3 &
Projection3::zero() {
    T[0][0] = 0; T[0][1] = 0; T[0][2] = 0; T[0][3] = 0;
    T[1][0] = 0; T[1][1] = 0; T[1][2] = 0; T[1][3] = 0;
    T[2][0] = 0; T[2][1] = 0; T[2][2] = 0; T[2][3] = 0;
    T[3][0] = 0; T[3][1] = 0; T[3][2] = 0; T[3][3] = 0;
    return *this;
}

Projection3 &
Projection3::perspective(double fov, double aspect, 
			 double znear, double zfar) {
    return *this *= ::perspective(fov, aspect, znear, zfar);
}

Projection3 &
Projection3::frustum(double l, double r, double t,
		     double b, double n, double f) {
    return *this *= ::frustum(l, r, t, b, n, f);
}

Projection3 &
Projection3::orthox() {
    return *this *= ::orthox();
}

Projection3 &
Projection3::orthoy() {
    return *this *= ::orthoy();
}

Projection3 &
Projection3::orthoz() {
    return *this *= ::orthoz();
}

Projection3 &
Projection3::scale(double sx, double sy, double sz) {
    return *this *= ::scale(sx, sy, sz);
}

Projection3 &
Projection3::translate(double tx, double ty, double tz) {
    return *this *= ::translate(tx, ty, tz);
}

Projection3 &
Projection3::rotatex(double theta) {
    return *this *= ::rotatex(theta);
}

Projection3 &
Projection3::rotatey(double theta) {
    return *this *= ::rotatey(theta);
}

Projection3 &
Projection3::rotatez(double theta) {
    return *this *= ::rotatez(theta);
}

Projection3 &
Projection3::rotate(double theta, const Vector3 &axis) {
    return *this *= ::rotate(theta, axis);
}

Projection3 &
Projection3::transpose() {
    swap(T[0][1], T[1][0]);
    swap(T[0][2], T[2][0]);
    swap(T[0][3], T[3][0]);
    swap(T[1][2], T[2][1]);
    swap(T[1][3], T[3][1]);
    swap(T[2][3], T[3][2]);
    return *this;
}

Projection3 &
Projection3::invert() {
    double d = determinant();
    assert(d != 0);
    adjoint();
    for (int i = 0; i < 4; ++i)
	for (int j = 0; j < 4; ++j)
	    T[i][j] /= d;
    return *this;
}

double
Projection3::determinant() const {
    double ans;
    double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;

    /* assign to individual variable names to aid selecting */
    /*  correct elements */

    a1 = T[0][0]; b1 = T[0][1]; 
    c1 = T[0][2]; d1 = T[0][3];

    a2 = T[1][0]; b2 = T[1][1]; 
    c2 = T[1][2]; d2 = T[1][3];

    a3 = T[2][0]; b3 = T[2][1]; 
    c3 = T[2][2]; d3 = T[2][3];

    a4 = T[3][0]; b4 = T[3][1]; 
    c4 = T[3][2]; d4 = T[3][3];

    ans = a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4)
        - b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4)
        + c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4)
        - d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);

    return ans;
}

double
Projection3::det3x3(double a1, double a2, double a3, 
		    double b1, double b2, double b3,
		    double c1, double c2, double c3) {
    return (a1 * (b2 * c3 - b3 * c2)
	    - b1 * (a2 * c3 - a3 * c2)
	    + c1 * (a2 * b3 - a3 * b2));
}

Projection3 &
Projection3::adjoint() {
    double a1, a2, a3, a4, b1, b2, b3, b4;
    double c1, c2, c3, c4, d1, d2, d3, d4;

    /* assign to individual variable names to aid  */
    /* selecting correct values  */

    a1 = T[0][0]; b1 = T[0][1]; 
    c1 = T[0][2]; d1 = T[0][3];

    a2 = T[1][0]; b2 = T[1][1]; 
    c2 = T[1][2]; d2 = T[1][3];

    a3 = T[2][0]; b3 = T[2][1];
    c3 = T[2][2]; d3 = T[2][3];

    a4 = T[3][0]; b4 = T[3][1]; 
    c4 = T[3][2]; d4 = T[3][3];

    /* row column labeling reversed since we transpose rows & columns */

    T[0][0]  =   det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
    T[1][0]  = - det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
    T[2][0]  =   det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
    T[3][0]  = - det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
        
    T[0][1]  = - det3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
    T[1][1]  =   det3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
    T[2][1]  = - det3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
    T[3][1]  =   det3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
        
    T[0][2]  =   det3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
    T[1][2]  = - det3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
    T[2][2]  =   det3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
    T[3][2]  = - det3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
        
    T[0][3]  = - det3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
    T[1][3]  =   det3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
    T[2][3]  = - det3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
    T[3][3]  =   det3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3);

    return *this;
}

Point3 
Projection3::project(const Point3 &p) const {
    double w = T[3][0] * p.x + T[3][1] * p.y + T[3][2] * p.z + T[3][3];
    assert(w != 0);
    Point3 ret(T[0][0] * p.x + T[0][1] * p.y + T[0][2] * p.z + T[0][3],
	       T[1][0] * p.x + T[1][1] * p.y + T[1][2] * p.z + T[1][3],
	       T[2][0] * p.x + T[2][1] * p.y + T[2][2] * p.z + T[2][3]);
    ret.x /= w; ret.y /= w; ret.z /= w;
    return ret;
}

Vector3 
Projection3::project(const Vector3 &v) const {
#if 0
    double w = T[3][0] * v.x + T[3][1] * v.y + T[3][2] * v.z + T[3][3];
    assert(w != 0);
    Vector3 ret(T[0][0] * v.x + T[0][1] * v.y + T[0][2] * v.z + T[0][3],
		T[1][0] * v.x + T[1][1] * v.y + T[1][2] * v.z + T[1][3],
		T[2][0] * v.x + T[2][1] * v.y + T[2][2] * v.z + T[2][3]);
    return ret /= w;
#endif
    return Vector3(T[0][0] * v.x + T[0][1] * v.y + T[0][2] * v.z,
		   T[1][0] * v.x + T[1][1] * v.y + T[1][2] * v.z,
		   T[2][0] * v.x + T[2][1] * v.y + T[2][2] * v.z);
}

Normal3
Projection3::project(const Normal3 &n, const Projection3 *inv) const {
#if 0
    if (inv) {
	// skip transpose and use rows instead of columns
	Normal3 ret(inv->T[0][0] * n.x + inv->T[1][0] * n.y + inv->T[2][0] * n.z +
		    inv->T[3][0],
		    inv->T[0][1] * n.x + inv->T[1][1] * n.y + inv->T[2][1] * n.z +
		    inv->T[3][1],
		    inv->T[0][2] * n.x + inv->T[1][2] * n.y + inv->T[2][2] * n.z +
		    inv->T[3][2]);
	double w = inv->T[0][3] * n.x + inv->T[1][3] * n.y + inv->T[2][3] * n.z + 
	    inv->T[3][3];
	assert(w != 0);
	return ret /= w;
    }
    else {
	Projection3 inv1 = inverse();
	Normal3 ret(inv1.T[0][0] * n.x + inv1.T[1][0] * n.y + inv1.T[2][0] * n.z +
		    inv1.T[3][0],
		    inv1.T[0][1] * n.x + inv1.T[1][1] * n.y + inv1.T[2][1] * n.z +
		    inv1.T[3][1],
		    inv1.T[0][2] * n.x + inv1.T[1][2] * n.y + inv1.T[2][2] * n.z +
		    inv1.T[3][2]);
	double w = inv1.T[0][3] * n.x + inv1.T[1][3] * n.y + inv1.T[2][3] * n.z +
	    inv1.T[3][3];
	assert(w != 0);
	return ret /= w;
    }
#endif
    if (inv) {
	// skip transpose and use rows instead of columns
	return Normal3(inv->T[0][0] * n.x + inv->T[1][0] * n.y + inv->T[2][0] * n.z, 
		       inv->T[0][1] * n.x + inv->T[1][1] * n.y + inv->T[2][1] * n.z,
		       inv->T[0][2] * n.x + inv->T[1][2] * n.y + inv->T[2][2] * n.z);
    }
    else {
	Projection3 inv1 = inverse();
	return Normal3(inv1.T[0][0] * n.x + inv1.T[1][0] * n.y + inv1.T[2][0] * n.z,
		       inv1.T[0][1] * n.x + inv1.T[1][1] * n.y + inv1.T[2][1] * n.z,
		       inv1.T[0][2] * n.x + inv1.T[1][2] * n.y + inv1.T[2][2] * n.z);
    }
}

Homogeneous3
Projection3::project(const Homogeneous3 &h) const {
    return Homogeneous3(h.x * T[0][0] + h.y * T[0][1] + 
			    h.z * T[0][2] + h.d * T[0][3],
			h.x * T[1][0] + h.y * T[1][1] + 
			    h.z * T[1][2] + h.d * T[1][3],
			h.x * T[2][0] + h.y * T[2][1] + 
			    h.z * T[2][2] + h.d * T[2][3],
			h.x * T[3][0] + h.y * T[3][1] + 
			    h.z * T[3][2] + h.d * T[3][3]);
}

bool
Projection3::operator==(const Projection3 &p) const {
    for (int i = 0; i < 4; ++i)
	for (int j = 0; j < 4; ++j)
	    if (T[i][j] != p.T[i][j])
		return false;
    return true;
}

bool
Projection3::operator!=(const Projection3 &p) const {
    for (int i = 0; i < 4; ++i)
	for (int j = 0; j < 4; ++j)
	    if (T[i][j] != p.T[i][j])
		return true;
    return false;
}

std::ostream &
operator<<(std::ostream &os, const Projection3 &p) {
    for (int i = 0; i < 4; ++i) {
	os << '[';
	for (int j = 0; j < 4; ++j) {
	    os << p.T[i][j];
	    if (j != 3)
		os << ' ';
	    else
		os << ']' << std::endl;
	}
    }
    return os;
}

Projection3 identityP3() {
    return Projection3(1, 0, 0, 0,
		       0, 1, 0, 0,
		       0, 0, 1, 0,
		       0, 0, 0, 1);
}

Projection3
perspective(double fov, double aspect, double znear, double zfar) {
    double cotfov = tan(fov / 2.0);
    if(cotfov != 0.0) {
	cotfov = 1.0 / cotfov;
    }
    return Projection3(
       cotfov / aspect, 0,       0,                      0,
       0,               cotfov,  0,                      0,
       0,               0,      -(zfar+znear)/(zfar-znear), -2*zfar*znear/(zfar-znear),
       0,               0,      -1,                      0);
}

Projection3
frustum(double l, double r, double t,
	double b, double n, double f) {
    double dx = r - l;
    double dy = t - b;
    double dz = f - n;

    return Projection3(2*n / dx,  0,         (r+l)/dx,  0,
		       0,         2*n / dy,  (t+b)/dy,  0,
		       0,         0,        -(f+n)/dz,  -(2*f*n)/dz,
		       0,         0,        -1,         0);
}

Projection3
orthox() {
    return Projection3(0, 0, 0, 0,
		       0, 1, 0, 0,
		       0, 0, 1, 0,
		       0, 0, 0, 1);
}
		       
Projection3
orthoy() {
    return Projection3(1, 0, 0, 0,
		       0, 0, 0, 0,
		       0, 0, 1, 0, 
		       0, 0, 0, 1);
}

Projection3
orthoz() {
    return Projection3(1, 0, 0, 0,
		       0, 1, 0, 0,
		       0, 0, 0, 0,
		       0, 0, 0, 1);
}