matrix.cc

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

//
// matrix.cc
//
// $Id: matrix.cc,v 1.1.1.1 2001/02/28 00:28:37 cstolte Exp $
//

#include <math.h>
#include <ctype.h>
#include <assert.h>
#include <sgl/matrix.h>
#include <sgl/mathfunc.h>

int InverseMatrix3x3(double mat[3][3], double inv[3][3]) {
  inv[0][0] = mat[1][1]*mat[2][2] - mat[1][2]*mat[2][1];
  inv[1][0] = mat[1][0]*mat[2][2] - mat[1][2]*mat[2][0];
  inv[2][0] = mat[1][0]*mat[2][1] - mat[1][1]*mat[2][0];

  inv[0][1] = mat[0][1]*mat[2][2] - mat[0][2]*mat[2][1];
  inv[1][1] = mat[0][0]*mat[2][2] - mat[0][2]*mat[2][0];
  inv[2][1] = mat[0][0]*mat[2][1] - mat[0][1]*mat[2][0];

  inv[0][2] = mat[0][1]*mat[1][2] - mat[0][2]*mat[1][1];
  inv[1][2] = mat[0][0]*mat[1][2] - mat[0][2]*mat[1][0];
  inv[2][2] = mat[0][0]*mat[1][1] - mat[0][1]*mat[1][0];

  double d = mat[0][0]*inv[0][0] - mat[0][1]*inv[1][0] + mat[0][2]*inv[2][0];

  assert(!zero(d));

  inv[0][0] /= d;
  inv[0][2] /= d;
  inv[1][1] /= d;
  inv[2][0] /= d;
  inv[2][2] /= d;

  d = -d;

  inv[0][1] /= d;
  inv[1][0] /= d;
  inv[1][2] /= d;
  inv[2][1] /= d;

  return 1;
}

#ifdef __GNUG__

template <int sz, class T> 
Vec<sz, T>::Vec(const T &d) {
    for (int i = 0; i < sz; ++i)
	v[i] = d;
}

template <int sz, class T> Vec<sz, T> 
Vec<sz, T>::operator-() const {
    Vec<sz, T> ret;
    for (int i = 0; i < sz; ++i)
	ret.v[i] = -v[i];
    return ret;
}

template <int sz, class T> Vec<sz, T> 
Vec<sz, T>::operator+(const Vec<sz, T> &v2) const {
    Vec<sz, T> ret(*this);
    for (int i = 0; i < sz; ++i)
	ret.v[i] += v2.v[i];
    return ret;
}

template <int sz, class T> Vec<sz, T> &
Vec<sz, T>::operator+=(const Vec<sz, T> &v2) {
    for (int i = 0; i < sz; ++i)
	v[i] += v2.v[i];
    return *this;
}

template <int sz, class T> Vec<sz, T> 
Vec<sz, T>::operator-(const Vec<sz, T> &v2) const {
    Vec<sz, T> ret(*this);
    for (int i = 0; i < sz; ++i)
	ret.v[i] -= v2.v[i];
    return ret;
}

template <int sz, class T> Vec<sz, T> &
Vec<sz, T>::operator-=(const Vec<sz, T> &v2) {
    for (int i = 0; i < sz; ++i)
	v[i] -= v2.v[i];
    return *this;
}

template <int sz, class T> Vec<sz, T> 
Vec<sz, T>::operator*(double d) const {
    Vec<sz, T> ret(*this);
    for (int i = 0; i < sz; ++i)
	ret.v[i] *= d;
    return ret;
}

template <int sz, class T> Vec<sz, T> &
Vec<sz, T>::operator*=(double d) {
    for (int i = 0; i < sz; ++i)
	v[i] *= d;
    return *this;
}

template <int sz, class T> Vec<sz, T> 
Vec<sz, T>::operator/(double d) const {
    Vec<sz, T> ret(*this);
    for (int i = 0; i < sz; ++i)
	ret.v[i] /= d;
    return ret;
}

template <int sz, class T> Vec<sz, T> &
Vec<sz, T>::operator/=(double d) {
    for (int i = 0; i < sz; ++i)
	v[i] /= d;
    return *this;
}

template <int sz, class T> Vec<sz, T> 
Vec<sz, T>::operator*(const Vec<sz, T> &v2) const {
    Vec<sz, T> ret(*this);
    return ret *= v2;
}

template <int sz, class T> Vec<sz, T> &
Vec<sz, T>::operator*=(const Vec<sz, T> &v2) {
    for (int i = 0; i < sz; ++i)
	v[i] *= v2.v[i];
    return *this;
}

template <int sz, class T> Vec<sz, T> 
Vec<sz, T>::operator/(const Vec<sz, T> &v2) const {
    Vec<sz, T> ret(*this);
    return ret /= v2;
}

template <int sz, class T> Vec<sz, T> &
Vec<sz, T>::operator/=(const Vec<sz, T> &v2) {
    for (int i = 0; i < sz; ++i)
	v[i] /= v2.v[i];
    return *this;
}

template <int sz, class T> bool 
Vec<sz, T>::operator==(const Vec<sz, T> &v2) const {
    for (int i = 0; i < sz; ++i)
	if (v[i] != v2.v[i])
	    return false;
    return true;
}

template <int sz, class T> bool 
Vec<sz, T>::operator!=(const Vec<sz, T> &v2) const {
    for (int i = 0; i < sz; ++i)
	if (v[i] == v2.v[i])
	    return false;
    return true;
}

template <int sz, class T> T
Vec<sz, T>::lengthsq() const {
    T ret(0);
    for (int i = 0; i < sz; ++i) {
	ret += (v[i] * v[i]);
    }
    return ret;
}

template <int sz, class T> T &
Vec<sz, T>::operator[](int i) {
    assert(i >= 0 && i < sz);
    return v[i];
}

template <int sz, class T> const T &
Vec<sz, T>::operator[](int i) const {
    assert(i >= 0 && i < sz);
    return v[i];
}

template <int sz, class T> T
dot(const Vec<sz, T> &v1, const Vec<sz, T> &v2) {
    T ret(0);
    for (int i = 0; i < sz; ++i)
	ret += v1.v[i] * v2.v[i];
    return ret;
}

template <int sz, class T> Vec<sz, T>
MIN(const Vec<sz, T> &v1, const Vec<sz, T> &v2) {
    Vec<sz, T> ret;
    for (int i = 0; i < sz; ++i) {
	if (v1.v[i] <= v2.v[i])
	    ret.v[i] = v1.v[i];
	else
	    ret.v[i] = v2.v[i];
    }
    return ret;
}

template <int sz, class T> Vec<sz, T>
MAX(const Vec<sz, T> &v1, const Vec<sz, T> &v2) {
    Vec<sz, T> ret;
    for (int i = 0; i < sz; ++i) {
	if (v1.v[i] > v2.v[i])
	    ret.v[i] = v1.v[i];
	else
	    ret.v[i] = v2.v[i];
    }
    return ret;
}

template <int sz, class T> Vec<sz, T>
reflection(const Vec<sz, T> &n, const Vec<sz, T> &wi) {
    return -wi + n * 2. * dot(wi, n);
}

template <int sz, class T> bool
refraction(const Vec<sz, T> &n, const Vec<sz, T> &wi, 
	   Vec<sz, T> *wt, double ni, double nt) {
    double eta = ni/nt;
    Vec<sz, T> i = -wi;
    double cosi = -dot(i,n);
    double cost = 1.0 - eta * eta * (1.0 - cosi * cosi);
    if (cost < 0.0)
        return 0;

    *wt = i*eta + n * (eta*cosi - sqrt(cost));
    return 1;
}

template <int sz, class T> std::ostream &
operator<<(std::ostream &os, const Vec<sz, T> &vec) {
    for (int i = 0; i < sz; ++i) {
	os << vec[i];
	if (i != sz-1)
	    os << ' ';
    }
    return os;
}

//////////////////////////////////////////////////

template <int sz, class T>
Mat<sz, T>::Mat(const T &d) {
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    m[i][j] = d;
}

template <int sz, class T>
Mat<sz, T>::Mat(const T v[sz * sz]) {
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    m[i][j] = v[i*sz + j];
}

template <int sz, class T> Mat<sz, T> 
Mat<sz, T>::operator-() const {
    Mat<sz, T> ret;
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    ret.m[i][j] = -m[i][j];
    return ret;
}

template <int sz, class T> Mat<sz, T> 
Mat<sz, T>::operator+(const Mat<sz, T> &m2) const {
    Mat<sz, T> ret(*this);
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    ret.m[i][j] += m2.m[i][j];
    return ret;
}

template <int sz, class T> Mat<sz, T> &
Mat<sz, T>::operator+=(const Mat<sz, T> &m2) {
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    m[i][j] += m2.m[i][j];
    return *this;
}

template <int sz, class T> Mat<sz, T> 
Mat<sz, T>::operator-(const Mat<sz, T> &m2) const {
    Mat<sz, T> ret(*this);
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    ret.m[i][j] -= m2.m[i][j];
    return ret;
}

template <int sz, class T> Mat<sz, T> &
Mat<sz, T>::operator-=(const Mat<sz, T> &m2) {
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    m[i][j] -= m2.m[i][j];
    return *this;
}

template <int sz, class T> Mat<sz, T> 
Mat<sz, T>::operator*(const Mat<sz, T> &m2) const {
    Mat<sz, T> ret(*this);
    return ret *= m2;
}

template <int sz, class T> Mat<sz, T> &
Mat<sz, T>::operator*=(const Mat<sz, T> &m2) {
    Mat<sz, T> old(*this);
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j) {
	    m[i][j] = 0;
	    for (int k = 0; k < sz; ++k)
		m[i][j] += old.m[i][k] * m2.m[k][j];
	}
    return *this;
}

template <int sz, class T> Mat<sz, T> 
Mat<sz, T>::operator*(double d) const {
    Mat<sz, T> ret(*this);
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    ret.m[i][j] *= d;
    return ret;
}

template <int sz, class T> Mat<sz, T> &
Mat<sz, T>::operator*=(double d) {
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    m[i][j] *= d;
    return *this;
}

template <int sz, class T> Mat<sz, T>
operator*(double d, const Mat<sz, T> &m) {
    return m * d;
}

template <int sz, class T> Vec<sz, T>
operator*(const Vec<sz, T> &v, const Mat<sz, T> &m) {
    Vec<sz, T> ret(0);
    for (int i = 0; i < sz; ++i) {
	for (int j = 0; j < sz; ++j)
	    ret[i] += v[j] * m[j][i];
    }
    return ret;
}

template <int sz, class T> Mat<sz, T> 
Mat<sz, T>::operator/(double d) const {
    Mat<sz, T> ret(*this);
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    ret.m[i][j] /= d;
    return ret;
}

template <int sz, class T> Mat<sz, T> &
Mat<sz, T>::operator/=(double d) {
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    m[i][j] /= d;
    return *this;
}

template <int sz, class T> bool 
Mat<sz, T>::operator==(const Mat<sz, T> &m2) const {
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    if (m[i][j] != m2.m[i][j])
		return false;
    return true;
}

template <int sz, class T> bool 
Mat<sz, T>::operator!=(const Mat<sz, T> & m2) const {
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j)
	    if (m[i][j] == m2.m[i][j])
		return false;
    return true;
}

template <int sz, class T> Mat<sz, T> &
Mat<sz, T>::identity() {
    for (int i = 0; i < sz; ++i)
	for (int j = 0; j < sz; ++j) {
	    if (i == j)
		m[i][j] = 1;
	    else
		m[i][j] = 0;
	}
    return *this;
}

template <int sz, class T> Mat<sz, T> &
Mat<sz, T>::transpose() {
    for (int i = 0; i < sz; ++i) {
	int j = i+1;
	while (j < sz) {
	    swap(m[i][j], m[j][i]);
	    ++j;
	}
    }
    return *this;
}

template <int sz, class T> Mat<sz, T> &
Mat<sz, T>::adjoint() {
    Mat<sz, T> tmp;
    for (int i = 0; i < sz; ++i) {
	int sign = ((i % 2) == 1) ? -1 : 1;
	for (int j = 0; j < sz; ++j) {
	    Mat<sz, T> tmp2;
	    int kpos = 0;
	    for (int k = 0; k < sz; ++k) {
		if (k != i) {
		    int lpos = 0;
		    for (int l = 0; l < sz; ++l) {
			if (l != j) {
			    tmp2.m[kpos][lpos] = m[k][l];
			    ++lpos;
			}
		    }
		    ++kpos;
		}
	    }
	    tmp.m[i][j] = sign * detHelper(tmp2, sz-1);
            sign = -sign;
	}
    }
    *this = tmp;
    return transpose();
}

template <int sz, class T> Mat<sz, T> &
Mat<sz, T>::invert() {
    double det = determinant();
    adjoint();
    return *this /= det;
}

template <int sz, class T> Mat<sz, T>
Mat<sz, T>::inverse() const {
    Mat<sz, T> ret(*this);
    return ret.invert();
}

template <int sz, class T> T
Mat<sz, T>::determinant() const {
    return detHelper(*this, sz);
}

template <int sz, class T> T
Mat<sz, T>::detHelper(const Mat<sz, T> &mat, int size) {
    if (size == 1)
	return mat.m[0][0];
    if (size == 2)
	return ((mat.m[0][0] * mat.m[1][1]) - (mat.m[1][0] * mat.m[0][1]));
    else {
	Mat<sz, T> tmp;
	T ret(0);
	int sign = 1;
	for (int col = 0; col < size; ++col) {
	    for (int i = 1; i < size; ++i) {
		int jpos = 0;
		for (int j = 0; j < size; ++j) {
		    if (j != col) {
			tmp.m[i-1][jpos] = mat.m[i][j];
			++jpos;
		    }
		}
	    }
	    ret += sign * mat.m[0][col] * detHelper(tmp, size-1);
	    sign = -sign;
	}
	return ret;
    }
}

template <int sz, class T> Vec<sz, T> 
Mat<sz, T>::column(int i) {
    assert(i >= 0 && i < sz);
    Vec<sz, T> ret;
    for (int j = 0; j < sz; ++j)
	ret[j] = m[j][i];
    return ret;
}

template <int sz, class T> Vec<sz, T> &
Mat<sz, T>::row(int i) {
    assert(i >= 0 && i < sz);
    return m[i];
}

template <int sz, class T> const Vec<sz, T> &
Mat<sz, T>::row(int i) const {
    assert(i >= 0 && i < sz);
    return m[i];
}

template <int sz, class T> std::ostream &
operator<<(std::ostream &os, const Mat<sz, T> &m) {
    for (int i = 0; i < sz; ++i) {
	for (int j = 0; j < sz; ++j) {
	    os << m[i][j];
	    if (j != sz-1)
		os << ' ';
	}
	if (i != sz - 1)
	    os << endl;
    }
    return os;
}

#endif // __GNUG__