transform2.cc

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

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

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

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

const Matrix2 Matrix2::unit(1, 0, 0, 1);
const Transform2 Transform2::unit(Matrix2::unit);

//
// Matrix2
//

Matrix2::Matrix2(double a00, double a01, double a10, double a11) {
    T[0][0] = a00;    T[0][1] = a01;
    T[1][0] = a10;    T[1][1] = a11;
}

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

Matrix2 
Matrix2::operator*(const Matrix2 &m2) const {
    Matrix2 ret(*this);
    return ret *= m2;
}

Matrix2 
Matrix2::operator+(const Matrix2 &m2) const {
    Matrix2 ret(*this);
    return ret += m2;
}

Matrix2 
Matrix2::operator-(const Matrix2 &m2) const {
    Matrix2 ret(*this);
    return ret -= m2;
}

Matrix2 &
Matrix2::operator*=(const Matrix2 &m2) {
    Matrix2 m1(*this);
    T[0][0] = m2.T[0][0] * m1.T[0][0] + m2.T[0][1] * m1.T[1][0];
    T[0][1] = m2.T[0][0] * m1.T[0][1] + m2.T[0][1] * m1.T[1][1];
    T[1][0] = m2.T[1][0] * m1.T[0][0] + m2.T[1][1] * m1.T[1][0];
    T[1][1] = m2.T[1][0] * m1.T[0][1] + m2.T[1][1] * m1.T[1][1];
    return *this;
}

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

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

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

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


Matrix2 &
Matrix2::rotate(double theta) {
    return *this *= ::rotate(theta);
}

Matrix2 &
Matrix2::scale(double su, double sv) {
    return *this *= ::scale(su, sv);
}

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

Matrix2 &
Matrix2::invert() {
    double d = determinant();
    swap(T[0][0], T[1][1]);
    T[0][0] /= d;      T[0][1] /= -d;
    T[1][0] /= -d;     T[1][1] /= d;
    return *this;
}

Matrix2 &
Matrix2::shearu(double a) {
    return *this *= ::shearu(a);
}

Matrix2 &
Matrix2::shearv(double a) {
    return *this *= ::shearv(a);
}

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

bool
Matrix2::operator==(const Matrix2 &m) const {
    return (T[0][0] == m.T[0][0] && T[0][1] == m.T[0][1] &&
	    T[1][0] == m.T[1][0] && T[1][1] == m.T[1][1]);
}

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

Point2 
Matrix2::apply(const Point2 &p) const {
    return Point2(T[0][0] * p.u + T[0][1] * p.v,
		  T[1][0] * p.u + T[1][1] * p.v);
}

Vector2 
Matrix2::apply(const Vector2 &v) const {
    return Vector2(T[0][0] * v.u + T[0][1] * v.v,
		   T[1][0] * v.u + T[1][1] * v.v);
}

Normal2 
Matrix2::apply(const Normal2 &n, const Matrix2 *inv) const {
    if (inv)
	return Normal2(inv->T[0][0] * n.u + inv->T[1][0] * n.v,
		       inv->T[0][1] * n.u + inv->T[1][1] * n.v);
    else {
	Matrix2 inv1 = inverse();
	return Normal2(inv1.T[0][0] * n.u + inv1.T[1][0] * n.v,
		       inv1.T[0][1] * n.u + inv1.T[1][1] * n.v);
    }
}

Normal2
Matrix2::row(int i) const {
    return Normal2(T[i][0], T[i][1]);
}

Vector2
Matrix2::column(int i) const {
    return Vector2(T[0][i], T[1][i]);
}

Matrix2
identityM2() {
    return Matrix2(1, 0,
		   0, 1);
}

Matrix2
rotate(double theta) {
    double s = sin(theta);
    double c = cos(theta);
    return Matrix2( c, -s,
		    s,  c);
}

Matrix2
scale(double su, double sv) {
    return Matrix2(su, 0,
		   0,  sv);
}

Matrix2
shearu(double s) {
    return Matrix2(1, s,
		   0, 1);
}

Matrix2
shearv(double s) {
    return Matrix2(1, 0,
		   s, 1);
}

//
// Transform2
// 

Transform2::Transform2(const Matrix2 &m) 
  : Matrix2(m), translation(0,0) {
}

Transform2::Transform2(const Matrix2 &m, const Vector2 &t)
  : Matrix2(m), translation(t) {
}

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

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

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

Transform2 &
Transform2::operator*=(const Transform2 &t) {
    Matrix2 tmp_m(t.T[0][0] * T[0][0] + t.T[0][1] * T[1][0],
		  t.T[0][0] * T[0][1] + t.T[0][1] * T[1][1],
		  t.T[1][0] * T[0][0] + t.T[1][1] * T[1][0],
		  t.T[1][0] * T[0][1] + t.T[1][1] * T[1][1]);
    translation = Vector2(dot(translation, t.row(0)),
			  dot(translation, t.row(1)));
    translation += t.translation;

    T[0][0] = tmp_m.T[0][0];
    T[0][1] = tmp_m.T[0][1];
    T[1][0] = tmp_m.T[1][0];
    T[1][1] = tmp_m.T[1][1];
    return *this;
}

Transform2 &
Transform2::operator*=(const Matrix2 &m) {
    Matrix2 tmp_m(m.T[0][0] * T[0][0] + m.T[0][1] * T[1][0],
		  m.T[0][0] * T[0][1] + m.T[0][1] * T[1][1],
		  m.T[1][0] * T[0][0] + m.T[1][1] * T[1][0],
		  m.T[1][0] * T[0][1] + m.T[1][1] * T[1][1]);
    translation = Vector2(dot(translation, m.row(0)),
			  dot(translation, m.row(1)));
    T[0][0] = tmp_m.T[0][0];
    T[0][1] = tmp_m.T[0][1];
    T[1][0] = tmp_m.T[1][0];
    T[1][1] = tmp_m.T[1][1];
    return *this;
}

Transform2 &
Transform2::identity() {
    Matrix2::identity();
    translation = Vector2(0,0);
    return *this;
}

Transform2 &
Transform2::translate(double tu, double tv) {
    return *this *= ::translate(tu, tv);
}

Transform2 &
Transform2::rotate(double angle) {
    return *this *= ::rotate(angle);
}

Transform2 &
Transform2::scale(double su, double sv) {
    return *this *= ::scale(su, sv);
}

Transform2 &
Transform2::invert() {
    double d = determinant();
    swap(T[0][0], T[1][1]);
    T[0][1] = -T[0][1];
    T[1][0] = -T[1][0];
    T[0][0] /= d;   T[0][1] /= d;
    T[1][0] /= d;   T[1][1] /= d;
    translation = Vector2(-dot(translation, row(0)),
			  -dot(translation, row(1)));
    return *this;
}

Point2 
Transform2::apply(const Point2 &p) const {
    Point2 ret(Matrix2::apply(p));
    return ret += translation;
}

Vector2 
Transform2::apply(const Vector2 &v) const {
    return Matrix2::apply(v);
}

Normal2 
Transform2::apply(const Normal2 &n, const Transform2 *inv) const {
    return Matrix2::apply(n, inv);
}

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

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

Transform2
translate(double tu, double tv) {
    return Transform2(Matrix2::unit, Vector2(tu, tv));
}

Transform2
operator*(const Matrix2 &mat, const Transform2 &tr)
{
    Transform2 ret(mat);
    return ret *= tr;
}