⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 transform3.cc

📁 一个用MATLAB语言编写的摄像机标定工具箱,内容丰富
💻 CC
📖 第 1 页 / 共 2 页
字号:
🔍
12 
//
// transform3.cc
//
// $Id: transform3.cc,v 1.1.1.1 2001/02/28 00:28:40 cstolte Exp $
//

#include <sgl/transform3.h>
#include <sgl/mathfunc.h>

template <class T>
inline void swap(T& a, T& b) {
    T tmp = a;
    a = b;
    b = tmp;
}

const Matrix3 Matrix3::unit(1, 0, 0, 0, 1, 0, 0, 0, 1);
const Transform3 Transform3::unit(Matrix3::unit);

//
// Matrix3
//

Matrix3::Matrix3(double a00, double a01, double a02,
		 double a10, double a11, double a12,
		 double a20, double a21, double a22) {
  T[0][0] = a00;  T[0][1] = a01;  T[0][2] = a02;
  T[1][0] = a10;  T[1][1] = a11;  T[1][2] = a12;
  T[2][0] = a20;  T[2][1] = a21;  T[2][2] = a22;
}

std::ostream &
operator<<(std::ostream &os, const Matrix3 &m) {
    os << "[[" << m.T[0][0] << ' ' << m.T[0][1] << ' ' << m.T[0][2] << "]" << std::endl;
    os << " [" << m.T[1][0] << ' ' << m.T[1][1] << ' ' << m.T[1][2] << "]" << std::endl;
    os << " [" << m.T[2][0] << ' ' << m.T[2][1] << ' ' << m.T[2][2] << "]]" << std::endl;
    return os;
}

Matrix3
Matrix3::operator*(const Matrix3 &m) const {
    Matrix3 ret(*this);
    ret *= m;
    return ret;
}

Matrix3
Matrix3::operator+(const Matrix3 &m) const {
    Matrix3 ret(*this);
    ret += m;
    return ret;
}

Matrix3
Matrix3::operator-(const Matrix3 &m) const {
    Matrix3 ret(*this);
    ret -= m;
    return ret;
}

Matrix3 &
Matrix3::operator*=(const Matrix3 &m) {
    double tmp[3][3];
    tmp[0][0] = m.T[0][0] * T[0][0] + m.T[0][1] * T[1][0] + m.T[0][2] * T[2][0];
    tmp[0][1] = m.T[0][0] * T[0][1] + m.T[0][1] * T[1][1] + m.T[0][2] * T[2][1];
    tmp[0][2] = m.T[0][0] * T[0][2] + m.T[0][1] * T[1][2] + m.T[0][2] * T[2][2];
    tmp[1][0] = m.T[1][0] * T[0][0] + m.T[1][1] * T[1][0] + m.T[1][2] * T[2][0];
    tmp[1][1] = m.T[1][0] * T[0][1] + m.T[1][1] * T[1][1] + m.T[1][2] * T[2][1];
    tmp[1][2] = m.T[1][0] * T[0][2] + m.T[1][1] * T[1][2] + m.T[1][2] * T[2][2];
    tmp[2][0] = m.T[2][0] * T[0][0] + m.T[2][1] * T[1][0] + m.T[2][2] * T[2][0];
    tmp[2][1] = m.T[2][0] * T[0][1] + m.T[2][1] * T[1][1] + m.T[2][2] * T[2][1];
    tmp[2][2] = m.T[2][0] * T[0][2] + m.T[2][1] * T[1][2] + m.T[2][2] * T[2][2];

    for (int i = 0; i < 3; ++i)
	for (int j = 0; j < 3; ++j)
	    T[i][j] = tmp[i][j];
    return *this;
}

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

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

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

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

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

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

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

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

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

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

Matrix3 &
Matrix3::invert() {
    double tmp[3][3];
    double d = determinant();

    tmp[0][0] = (T[1][1] * T[2][2] - T[1][2] * T[2][1]) / d;
    tmp[0][1] = (T[1][0] * T[2][2] - T[1][2] * T[2][0]) / -d;
    tmp[0][2] = (T[1][0] * T[2][1] - T[1][1] * T[2][0]) / d;

    tmp[1][0] = (T[0][1] * T[2][2] - T[0][2] * T[2][1]) / -d;
    tmp[1][1] = (T[0][0] * T[2][2] - T[0][2] * T[2][0]) / d;
    tmp[1][2] = (T[0][0] * T[2][1] - T[0][1] * T[2][0]) / -d;

    tmp[2][0] = (T[0][1] * T[1][2] - T[0][2] * T[1][1]) / d;
    tmp[2][1] = (T[0][0] * T[1][2] - T[0][2] * T[1][0]) / -d;
    tmp[2][2] = (T[0][0] * T[1][1] - T[0][1] * T[1][0]) / d;

    for (int i = 0; i < 3; ++i)
	for (int j = 0; j < 3; ++j)
	    // note that tmp should be transposed before assigning
	    // to *this, so just do it here...
	    T[i][j] = tmp[j][i];
    return *this;
}

double
Matrix3::determinant() const {
    return ((T[0][0] * (T[1][1] * T[2][2] - T[1][2] * T[2][1])) -
	    (T[0][1] * (T[1][0] * T[2][2] - T[1][2] * T[2][0])) +
	    (T[0][2] * (T[1][0] * T[2][1] - T[1][1] * T[2][0])));
}

double 
Matrix3::determinant(const Matrix3 &adj) const {
    return T[0][0] * adj.T[0][0] 
    	+ T[0][1] * adj.T[1][0] 
	+ T[0][2] * adj.T[2][0];
}

Matrix3 
Matrix3::adjoint() const {
    Matrix3 ret;
    double sign = 1;
    for (u_int i = 0; i < 3; ++i) {
	for (u_int j = 0; j < 3; ++j) {
	    ret.T[i][j] = cofactor(i, j) * sign; // save the transpose
	    sign = -sign;
	}
    }
    return ret;
}

Point3
Matrix3::apply(const Point3 &p) const {
    return Point3(T[0][0] * p.x + T[0][1] * p.y + T[0][2] * p.z,
		  T[1][0] * p.x + T[1][1] * p.y + T[1][2] * p.z,
		  T[2][0] * p.x + T[2][1] * p.y + T[2][2] * p.z);
}

Vector3
Matrix3::apply(const Vector3 &v) const {
    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
Matrix3::apply(const Normal3 &n, const Matrix3 *inv) const {
    if (inv) {
	return Normal3(n.x * inv->T[0][0] + n.y * inv->T[1][0] + n.z * inv->T[2][0],
		       n.x * inv->T[0][1] + n.y * inv->T[1][1] + n.z * inv->T[2][1],
		       n.x * inv->T[0][2] + n.y * inv->T[1][2] + n.z * inv->T[2][2]);
    }
    else {
	Matrix3 inv1 = inverse();
	// skip transpose of inverse and take dot with columns
	return Normal3(n.x * inv1.T[0][0] + n.y * inv1.T[1][0] + n.z * inv1.T[2][0],
		       n.x * inv1.T[0][1] + n.y * inv1.T[1][1] + n.z * inv1.T[2][1],
		       n.x * inv1.T[0][2] + n.y * inv1.T[1][2] + n.z * inv1.T[2][2]);
    }
}

bool
Matrix3::operator==(const Matrix3 &m2) const {
    return (T[0][0] == m2.T[0][0] &&
	    T[0][1] == m2.T[0][1] &&
	    T[0][2] == m2.T[0][2] &&
	    T[1][0] == m2.T[1][0] &&
	    T[1][1] == m2.T[1][1] &&
	    T[1][2] == m2.T[1][2] &&
	    T[2][0] == m2.T[2][0] &&
	    T[2][1] == m2.T[2][1] &&
	    T[2][2] == m2.T[2][2]);
}

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

double
Matrix3::cofactor(int i, int j) const {
    assert(i >= 0 && i < 3);
    assert(j >= 0 && j < 3);

    switch (i) {
      case 0:
	switch (j) {
	  case 0:
	    return T[1][1] * T[2][2] - T[1][2] * T[2][1];
	  case 1:
	    return T[1][0] * T[2][2] - T[1][2] * T[2][0];
	  case 2:
	    return T[1][0] * T[2][1] - T[1][1] * T[2][0];
	}
      case 1:
	switch (j) {
	  case 0:
	    return T[0][1] * T[2][2] - T[0][2] * T[2][1];
	  case 1:
	    return T[0][0] * T[2][2] - T[0][2] * T[2][0];
	  case 2:
	    return T[0][0] * T[2][1] - T[0][1] * T[2][0];
	}
      case 2:
	switch(j) {
	  case 0:
	    return T[0][1] * T[1][2] - T[0][2] * T[1][1];
	  case 1:
	    return T[0][0] * T[1][2] - T[0][2] * T[1][0];
	  case 2:
	    return T[0][0] * T[1][1] - T[0][1] * T[1][0];
	}
    }
    return 0; // notreached
}

Matrix3 identityM3() {
    return Matrix3(1, 0, 0,
		   0, 1, 0,
		   0, 0, 1);
}

Matrix3 rotatex(double theta) {
    double s = sin(theta);
    double c = cos(theta);
    return Matrix3(1, 0, 0,
		   0, c, -s,
		   0, s, c);
}

Matrix3 rotatey(double theta) {
    double s = sin(theta);
    double c = cos(theta);
    return Matrix3(c, 0, s,
		   0, 1, 0,
		  -s, 0, c);
}

Matrix3 rotatez(double theta) {
    double s = sin(theta);
    double c = cos(theta);
    return Matrix3(c, -s, 0,
		   s,  c, 0,
		   0,  0, 1);
}

Matrix3 rotate(double theta, const Vector3 &a) {
    Vector3 axis = a.hat();
    double s = sin(theta);
    double c = cos(theta);

    return Matrix3((axis.x * axis.x + (1. - axis.x * axis.x) * c),
		   (axis.x * axis.y * (1 - c) - axis.z * s),
		   (axis.x * axis.z * (1 - c) + axis.y * s),

		   (axis.x * axis.y * (1 - c) + axis.z * s),
		   (axis.y * axis.y + (1 - axis.y * axis.y) * c),
		   (axis.y * axis.z * (1 - c) - axis.x * s),

		   (axis.x * axis.z * (1 - c) - axis.y * s),
		   (axis.y * axis.z * (1 - c) + axis.x * s),
		   (axis.z * axis.z + (1 - axis.z * axis.z) * c));
}

Matrix3 scale(double sx, double sy, double sz) {
    return Matrix3(sx, 0,  0,
		   0, sy,  0,
		   0,  0, sz);
}

//
// Transform3
//

Transform3::Transform3(const Matrix3 &m) 
  : Matrix3(m), translation(0, 0, 0) {
}

Transform3::Transform3(const Matrix3 &m, const Vector3 &t) 
  : Matrix3(m), translation(t) {
}

std::ostream &
operator<<(std::ostream &os, const Transform3 &tr) {
    os << (const Matrix3 &)tr << std::endl << tr.translation;
    return os;
}

Transform3
Transform3::operator*(const Transform3 &t) const {
    Transform3 ret(*this);
    ret *= t;
    return ret;
}

Transform3 &
Transform3::operator*=(const Transform3 &t) {
    double tmp[3][3];
    tmp[0][0] = t.T[0][0] * T[0][0] + t.T[0][1] * T[1][0] + t.T[0][2] * T[2][0];
    tmp[0][1] = t.T[0][0] * T[0][1] + t.T[0][1] * T[1][1] + t.T[0][2] * T[2][1];
    tmp[0][2] = t.T[0][0] * T[0][2] + t.T[0][1] * T[1][2] + t.T[0][2] * T[2][2];
    tmp[1][0] = t.T[1][0] * T[0][0] + t.T[1][1] * T[1][0] + t.T[1][2] * T[2][0];
    tmp[1][1] = t.T[1][0] * T[0][1] + t.T[1][1] * T[1][1] + t.T[1][2] * T[2][1];
    tmp[1][2] = t.T[1][0] * T[0][2] + t.T[1][1] * T[1][2] + t.T[1][2] * T[2][2];
    tmp[2][0] = t.T[2][0] * T[0][0] + t.T[2][1] * T[1][0] + t.T[2][2] * T[2][0];
    tmp[2][1] = t.T[2][0] * T[0][1] + t.T[2][1] * T[1][1] + t.T[2][2] * T[2][1];
    tmp[2][2] = t.T[2][0] * T[0][2] + t.T[2][1] * T[1][2] + t.T[2][2] * T[2][2];

    for (int i = 0; i < 3; ++i)
	for (int j = 0; j < 3; ++j)
	    T[i][j] = tmp[i][j];

    double tx = t.translation.x +
	translation.x * t.T[0][0] + translation.y * t.T[0][1] +
	    translation.z * t.T[0][2];
    double ty = t.translation.y +
	translation.x * t.T[1][0] + translation.y * t.T[1][1] +
	    translation.z * t.T[1][2];
    double tz = t.translation.z +
	translation.x * t.T[2][0] + translation.y * t.T[2][1] +
	    translation.z * t.T[2][2];
    translation.x = tx;
    translation.y = ty;
    translation.z = tz;

    return *this;
}

Transform3
operator*(const Matrix3 &mat, const Transform3 &tr)
{
    Transform3 ret(mat);
    return ret *= tr;
}

Transform3 
Transform3::operator*(const Matrix3 &m) const {
    Transform3 ret(*this);
    return ret *= m;
}

Transform3 &
Transform3::operator*=(const Matrix3 &m) {
    double tmp[3][3];
    tmp[0][0] = m.T[0][0] * T[0][0] + m.T[0][1] * T[1][0] + m.T[0][2] * T[2][0];
    tmp[0][1] = m.T[0][0] * T[0][1] + m.T[0][1] * T[1][1] + m.T[0][2] * T[2][1];
    tmp[0][2] = m.T[0][0] * T[0][2] + m.T[0][1] * T[1][2] + m.T[0][2] * T[2][2];
    tmp[1][0] = m.T[1][0] * T[0][0] + m.T[1][1] * T[1][0] + m.T[1][2] * T[2][0];
    tmp[1][1] = m.T[1][0] * T[0][1] + m.T[1][1] * T[1][1] + m.T[1][2] * T[2][1];
    tmp[1][2] = m.T[1][0] * T[0][2] + m.T[1][1] * T[1][2] + m.T[1][2] * T[2][2];
    tmp[2][0] = m.T[2][0] * T[0][0] + m.T[2][1] * T[1][0] + m.T[2][2] * T[2][0];
    tmp[2][1] = m.T[2][0] * T[0][1] + m.T[2][1] * T[1][1] + m.T[2][2] * T[2][1];
    tmp[2][2] = m.T[2][0] * T[0][2] + m.T[2][1] * T[1][2] + m.T[2][2] * T[2][2];
    translation = 
	Vector3(translation.x * m.T[0][0] + translation.y * m.T[0][1] +
		translation.z * m.T[0][2],
		translation.x * m.T[1][0] + translation.y * m.T[1][1] +
		translation.z * m.T[1][2],
		translation.x * m.T[2][0] + translation.y * m.T[2][1] +
		translation.z * m.T[2][2]);

    for (int i = 0; i < 3; ++i)
	for (int j = 0; j < 3; ++j)
	    T[i][j] = tmp[i][j];
    return *this;
}

Transform3 &
Transform3::identity() {
    translation = Vector3(0,0,0);
    Matrix3::identity();
    return *this;
}

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

Transform3 &
Transform3::rotatex(double angle) {
    return *this *= ::rotatex(angle);
}

Transform3 &
Transform3::rotatey(double angle) {
    return *this *= ::rotatey(angle);
}

Transform3 &
Transform3::rotatez(double angle) {
    return *this *= ::rotatez(angle);
}

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

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

Transform3 &
Transform3::invert() {
    Matrix3::invert();
    translation = Vector3(-(translation.x * T[0][0] + translation.y * T[0][1] +
			    translation.z * T[0][2]),
			  -(translation.x * T[1][0] + translation.y * T[1][1] +
			    translation.z * T[1][2]),
			  -(translation.x * T[2][0] + translation.y * T[2][1] +
			    translation.z * T[2][2]));
    return *this;
}

Transform3&
Transform3::camera(const Point3 &at, const Point3 &from,
		   const Point3 &up) {
    // apply rotation and translation to convert to a coordinate system
    // with the from point at the origin, the at point on the positive
    // z axis, and the up point on the positive y axis

    Vector3 Z = (at - from).normalize();
    Vector3 X = (cross(up - from, Z)).normalize();
    Vector3 Y = (cross(Z, X)).normalize();

    Matrix3 m(X.x, X.y, X.z,
	      Y.x, Y.y, Y.z,
	      Z.x, Z.y, Z.z);

    Vector3 v(-dot(from - Point3::origin, X),
	      -dot(from - Point3::origin, Y),
	      -dot(from - Point3::origin, Z));

    return *this *= Transform3(m, v);
}

Transform3&
Transform3::perspective(double fov, double aspect,
			double /*znear*/, double zfar) {
    // apply differential scaling to convert the above coordinate system
    // into the canonical perspective coordinate system for the given field
    // of view, aspect ratio, and near and far planes.
    // (Foley et al, chapter 6)

    double cotfov = tan(fov / 2.0);
    if (cotfov != 0.0)
	cotfov = 1.0 / cotfov;

    Matrix3 tmp(cotfov / aspect, 0.0,      0.0,
		0.0,             cotfov,   0.0,
		0.0,             0.0,      -1.0 / zfar);

    return *this *= tmp;
}

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);
}
12

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -