609 lines
14 KiB
C++
609 lines
14 KiB
C++
/****************************************************************************
|
|
* VCGLib o o *
|
|
* Visual and Computer Graphics Library o o *
|
|
* _ O _ *
|
|
* Copyright(C) 2004 \/)\/ *
|
|
* Visual Computing Lab /\/| *
|
|
* ISTI - Italian National Research Council | *
|
|
* \ *
|
|
* All rights reserved. *
|
|
* *
|
|
* This program is free software; you can redistribute it and/or modify *
|
|
* it under the terms of the GNU General Public License as published by *
|
|
* the Free Software Foundation; either version 2 of the License, or *
|
|
* (at your option) any later version. *
|
|
* *
|
|
* This program is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
|
* for more details. *
|
|
* *
|
|
****************************************************************************/
|
|
/****************************************************************************
|
|
History
|
|
|
|
$Log: not supported by cvs2svn $
|
|
Revision 1.15 2006/09/25 23:05:29 ganovelli
|
|
added constructor from matrix44 excluding a row and colum
|
|
|
|
Revision 1.14 2006/06/22 08:00:05 ganovelli
|
|
bug in operator + with MatrixxDig
|
|
|
|
Revision 1.13 2006/01/20 16:41:44 pietroni
|
|
added operators:
|
|
operator -= ( const Matrix33Diag<S> &p )
|
|
Matrix33 operator - ( const Matrix33Diag<S> &p )
|
|
Matrix33 operator + ( const Matrix33 &m )
|
|
Matrix33 operator + ( const Matrix33Diag<S> &p )
|
|
|
|
Revision 1.12 2005/11/14 10:28:25 cignoni
|
|
Changed Invert -> FastInvert for the function based on the maple expansion
|
|
|
|
Revision 1.11 2005/10/13 15:45:23 ponchio
|
|
Changed a Zero in SetZero in WeightedCrossCovariance() (again)
|
|
|
|
Revision 1.10 2005/10/05 17:06:12 pietroni
|
|
corrected sintax error on singular value decomposition
|
|
|
|
Revision 1.9 2005/09/29 09:53:58 ganovelli
|
|
added inverse by SVD
|
|
|
|
Revision 1.8 2005/06/10 14:51:54 cignoni
|
|
Changed a Zero in SetZero in WeightedCrossCovariance()
|
|
|
|
Revision 1.7 2005/06/10 11:46:49 pietroni
|
|
Added Norm Function
|
|
|
|
Revision 1.6 2005/06/07 14:29:56 ganovelli
|
|
changed from Matrix33Ide to MatrixeeDiag
|
|
|
|
Revision 1.5 2005/05/23 15:05:26 ganovelli
|
|
Matrix33Diag Added: it implements diagonal matrix. Added only operator += in Matrix33
|
|
|
|
Revision 1.4 2005/04/11 14:11:22 pietroni
|
|
changed swap to math::Swap in Traspose Function
|
|
|
|
Revision 1.3 2004/10/18 15:03:02 fiorin
|
|
Updated interface: all Matrix classes have now the same interface
|
|
|
|
Revision 1.2 2004/07/13 06:48:26 cignoni
|
|
removed uppercase references in include
|
|
|
|
Revision 1.1 2004/05/28 13:09:05 ganovelli
|
|
created
|
|
|
|
Revision 1.1 2004/05/28 13:00:39 ganovelli
|
|
created
|
|
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
#ifndef __VCGLIB_MATRIX33_H
|
|
#define __VCGLIB_MATRIX33_H
|
|
|
|
#include <stdio.h>
|
|
#include <vcg/math/lin_algebra.h>
|
|
#include <vcg/math/matrix44.h>
|
|
#include <vcg/space/point3.h>
|
|
#include <vector>
|
|
|
|
namespace vcg {
|
|
|
|
template <class S>
|
|
class Matrix33Diag:public Point3<S>{
|
|
public:
|
|
|
|
/** @name Matrix33
|
|
Class Matrix33Diag.
|
|
This is the class for definition of a diagonal matrix 3x3.
|
|
@param S (Templete Parameter) Specifies the ScalarType field.
|
|
*/
|
|
Matrix33Diag(const S & p0,const S & p1,const S & p2):Point3<S>(p0,p1,p2){};
|
|
Matrix33Diag(const Point3<S>&p ):Point3<S>(p){};
|
|
};
|
|
|
|
template<class S>
|
|
/** @name Matrix33
|
|
Class Matrix33.
|
|
This is the class for definition of a matrix 3x3.
|
|
@param S (Templete Parameter) Specifies the ScalarType field.
|
|
*/
|
|
class Matrix33
|
|
{
|
|
public:
|
|
typedef S ScalarType;
|
|
|
|
/// Default constructor
|
|
inline Matrix33() {}
|
|
|
|
/// Copy constructor
|
|
Matrix33( const Matrix33 & m )
|
|
{
|
|
for(int i=0;i<9;++i)
|
|
a[i] = m.a[i];
|
|
}
|
|
|
|
/// create from array
|
|
Matrix33( const S * v )
|
|
{
|
|
for(int i=0;i<9;++i) a[i] = v[i];
|
|
}
|
|
|
|
/// create from Matrix44 excluding row and column k
|
|
Matrix33 (const Matrix44<S> & m, const int & k)
|
|
{
|
|
int i,j, r=0, c=0;
|
|
for(i = 0; i< 4;++i)if(i!=k){c=0;
|
|
for(j=0; j < 4;++j)if(j!=k)
|
|
{ (*this)[r][c] = m[i][j]; ++c;}
|
|
++r;
|
|
}
|
|
}
|
|
|
|
/// Number of columns
|
|
inline unsigned int ColumnsNumber() const
|
|
{
|
|
return 3;
|
|
};
|
|
|
|
/// Number of rows
|
|
inline unsigned int RowsNumber() const
|
|
{
|
|
return 3;
|
|
};
|
|
|
|
/// Assignment operator
|
|
Matrix33 & operator = ( const Matrix33 & m )
|
|
{
|
|
for(int i=0;i<9;++i)
|
|
a[i] = m.a[i];
|
|
return *this;
|
|
}
|
|
|
|
|
|
|
|
/// Operatore di indicizzazione
|
|
inline S * operator [] ( const int i )
|
|
{
|
|
return a+i*3;
|
|
}
|
|
/// Operatore const di indicizzazione
|
|
inline const S * operator [] ( const int i ) const
|
|
{
|
|
return a+i*3;
|
|
}
|
|
|
|
|
|
/// Modificatore somma per matrici 3x3
|
|
Matrix33 & operator += ( const Matrix33 &m )
|
|
{
|
|
for(int i=0;i<9;++i)
|
|
a[i] += m.a[i];
|
|
return *this;
|
|
}
|
|
|
|
/// Modificatore somma per matrici 3x3
|
|
Matrix33 & operator += ( const Matrix33Diag<S> &p )
|
|
{
|
|
a[0] += p[0];
|
|
a[4] += p[1];
|
|
a[8] += p[2];
|
|
return *this;
|
|
}
|
|
|
|
/// Modificatore sottrazione per matrici 3x3
|
|
Matrix33 & operator -= ( const Matrix33 &m )
|
|
{
|
|
for(int i=0;i<9;++i)
|
|
a[i] -= m.a[i];
|
|
return *this;
|
|
}
|
|
|
|
/// Modificatore somma per matrici 3x3
|
|
Matrix33 & operator -= ( const Matrix33Diag<S> &p )
|
|
{
|
|
a[0] -= p[0];
|
|
a[4] -= p[1];
|
|
a[8] -= p[2];
|
|
return *this;
|
|
}
|
|
|
|
/// Modificatore divisione per scalare
|
|
Matrix33 & operator /= ( const S &s )
|
|
{
|
|
for(int i=0;i<9;++i)
|
|
a[i] /= s;
|
|
return *this;
|
|
}
|
|
|
|
|
|
/// Modificatore prodotto per matrice
|
|
Matrix33 operator * ( const Matrix33< S> & t ) const
|
|
{
|
|
Matrix33<S> r;
|
|
|
|
int i,j;
|
|
for(i=0;i<3;++i)
|
|
for(j=0;j<3;++j)
|
|
r[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j];
|
|
|
|
return r;
|
|
}
|
|
|
|
/// Modificatore prodotto per matrice
|
|
void operator *=( const Matrix33< S> & t )
|
|
{
|
|
int i,j;
|
|
for(i=0;i<3;++i)
|
|
for(j=0;j<3;++j)
|
|
(*this)[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j];
|
|
|
|
}
|
|
|
|
/// Dot product with a diagonal matrix
|
|
Matrix33 operator * ( const Matrix33Diag< S> & t ) const
|
|
{
|
|
Matrix33<S> r;
|
|
|
|
int i,j;
|
|
for(i=0;i<3;++i)
|
|
for(j=0;j<3;++j)
|
|
r[i][j] = (*this)[i][j]*t[j];
|
|
|
|
return r;
|
|
}
|
|
|
|
/// Dot product modifier with a diagonal matrix
|
|
void operator *=( const Matrix33Diag< S> & t )
|
|
{
|
|
int i,j;
|
|
for(i=0;i<3;++i)
|
|
for(j=0;j<3;++j)
|
|
(*this)[i][j] = (*this)[i][j]*t[j];
|
|
}
|
|
|
|
/// Modificatore prodotto per costante
|
|
Matrix33 & operator *= ( const S t )
|
|
{
|
|
for(int i=0;i<9;++i)
|
|
a[i] *= t;
|
|
return *this;
|
|
}
|
|
|
|
/// Operatore prodotto per costante
|
|
Matrix33 operator * ( const S t )
|
|
{
|
|
Matrix33<S> r;
|
|
for(int i=0;i<9;++i)
|
|
r.a[i] = a[i]* t;
|
|
|
|
return r;
|
|
}
|
|
|
|
/// Operatore sottrazione per matrici 3x3
|
|
Matrix33 operator - ( const Matrix33 &m )
|
|
{
|
|
Matrix33<S> r;
|
|
for(int i=0;i<9;++i)
|
|
r.a[i] = a[i] - m.a[i];
|
|
|
|
return r;
|
|
}
|
|
|
|
/// Operatore sottrazione di matrici 3x3 con matrici diagonali
|
|
Matrix33 operator - ( const Matrix33Diag<S> &p )
|
|
{
|
|
Matrix33<S> r=a;
|
|
r[0][0] -= p[0];
|
|
r[1][1] -= p[1];
|
|
r[2][2] -= p[2];
|
|
return r;
|
|
}
|
|
|
|
/// Operatore sottrazione per matrici 3x3
|
|
Matrix33 operator + ( const Matrix33 &m )
|
|
{
|
|
Matrix33<S> r;
|
|
for(int i=0;i<9;++i)
|
|
r.a[i] = a[i] + m.a[i];
|
|
|
|
return r;
|
|
}
|
|
|
|
/// Operatore addizione di matrici 3x3 con matrici diagonali
|
|
Matrix33 operator + ( const Matrix33Diag<S> &p )
|
|
{
|
|
Matrix33<S> r=(*this);
|
|
r[0][0] += p[0];
|
|
r[1][1] += p[1];
|
|
r[2][2] += p[2];
|
|
return r;
|
|
}
|
|
|
|
/** Operatore per il prodotto matrice-vettore.
|
|
@param v A point in $R^{3}$
|
|
@return Il vettore risultante in $R^{3}$
|
|
*/
|
|
Point3<S> operator * ( const Point3<S> & v ) const
|
|
{
|
|
Point3<S> t;
|
|
|
|
t[0] = a[0]*v[0] + a[1]*v[1] + a[2]*v[2];
|
|
t[1] = a[3]*v[0] + a[4]*v[1] + a[5]*v[2];
|
|
t[2] = a[6]*v[0] + a[7]*v[1] + a[8]*v[2];
|
|
return t;
|
|
}
|
|
|
|
void OuterProduct(Point3<S> const &p0, Point3<S> const &p1) {
|
|
Point3<S> row;
|
|
row = p1*p0[0];
|
|
a[0] = row[0];a[1] = row[1];a[2] = row[2];
|
|
row = p1*p0[1];
|
|
a[3] = row[0]; a[4] = row[1]; a[5] = row[2];
|
|
row = p1*p0[2];
|
|
a[6] = row[0];a[7] = row[1];a[8] = row[2];
|
|
}
|
|
|
|
void SetZero() {
|
|
for(int i=0;i<9;++i) a[i] =0;
|
|
}
|
|
void SetIdentity() {
|
|
for(int i=0;i<9;++i) a[i] =0;
|
|
a[0]=a[4]=a[8]=1.0;
|
|
}
|
|
|
|
void Rotate(S angle, const Point3<S> & axis )
|
|
{
|
|
angle = angle*(S)3.14159265358979323846/180;
|
|
S c = cos(angle);
|
|
S s = sin(angle);
|
|
S q = 1-c;
|
|
Point3<S> t = axis;
|
|
t.Normalize();
|
|
a[0] = t[0]*t[0]*q + c;
|
|
a[1] = t[0]*t[1]*q - t[2]*s;
|
|
a[2] = t[0]*t[2]*q + t[1]*s;
|
|
a[3] = t[1]*t[0]*q + t[2]*s;
|
|
a[4] = t[1]*t[1]*q + c;
|
|
a[5] = t[1]*t[2]*q - t[0]*s;
|
|
a[6] = t[2]*t[0]*q -t[1]*s;
|
|
a[7] = t[2]*t[1]*q +t[0]*s;
|
|
a[8] = t[2]*t[2]*q +c;
|
|
}
|
|
/// Funzione per eseguire la trasposta della matrice
|
|
Matrix33 & Transpose()
|
|
{
|
|
math::Swap(a[1],a[3]);
|
|
math::Swap(a[2],a[6]);
|
|
math::Swap(a[5],a[7]);
|
|
return *this;
|
|
}
|
|
|
|
/// Funzione per costruire una matrice diagonale dati i tre elem.
|
|
Matrix33 & SetDiagonal(S *v)
|
|
{int i,j;
|
|
for(i=0;i<3;i++)
|
|
for(j=0;j<3;j++)
|
|
if(i==j) (*this)[i][j] = v[i];
|
|
else (*this)[i][j] = 0;
|
|
return *this;
|
|
}
|
|
|
|
|
|
/// Assegna l'n-simo vettore colonna
|
|
void SetColumn(const int n, S* v){
|
|
assert( (n>=0) && (n<3) );
|
|
a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2];
|
|
};
|
|
|
|
/// Assegna l'n-simo vettore riga
|
|
void SetRow(const int n, S* v){
|
|
assert( (n>=0) && (n<3) );
|
|
int m=n*3;
|
|
a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2];
|
|
};
|
|
|
|
/// Assegna l'n-simo vettore colonna
|
|
void SetColumn(const int n, const Point3<S> v){
|
|
assert( (n>=0) && (n<3) );
|
|
a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2];
|
|
};
|
|
|
|
/// Assegna l'n-simo vettore riga
|
|
void SetRow(const int n, const Point3<S> v){
|
|
assert( (n>=0) && (n<3) );
|
|
int m=n*3;
|
|
a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2];
|
|
};
|
|
|
|
/// Restituisce l'n-simo vettore colonna
|
|
Point3<S> GetColumn(const int n) const {
|
|
assert( (n>=0) && (n<3) );
|
|
Point3<S> t;
|
|
t[0]=a[n]; t[1]=a[n+3]; t[2]=a[n+6];
|
|
return t;
|
|
};
|
|
|
|
/// Restituisce l'n-simo vettore riga
|
|
Point3<S> GetRow(const int n) const {
|
|
assert( (n>=0) && (n<3) );
|
|
Point3<S> t;
|
|
int m=n*3;
|
|
t[0]=a[m]; t[1]=a[m+1]; t[2]=a[m+2];
|
|
return t;
|
|
};
|
|
|
|
|
|
|
|
/// Funzione per il calcolo del determinante
|
|
S Determinant() const
|
|
{
|
|
return a[0]*(a[4]*a[8]-a[5]*a[7]) -
|
|
a[1]*(a[3]*a[8]-a[5]*a[6]) +
|
|
a[2]*(a[3]*a[7]-a[4]*a[6]) ;
|
|
}
|
|
|
|
// Warning, this Inversion code can be HIGHLY NUMERICALLY UNSTABLE!
|
|
// In most case you are advised to use the Invert() method based on SVD decomposition.
|
|
|
|
Matrix33 & FastInvert()
|
|
{
|
|
// Maple produsse:
|
|
S t4 = a[0]*a[4];
|
|
S t6 = a[0]*a[5];
|
|
S t8 = a[1]*a[3];
|
|
S t10 = a[2]*a[3];
|
|
S t12 = a[1]*a[6];
|
|
S t14 = a[2]*a[6];
|
|
S t17 = 1/(t4*a[8]-t6*a[7]-t8*a[8]+t10*a[7]+t12*a[5]-t14*a[4]);
|
|
S a0 = a[0];
|
|
S a1 = a[1];
|
|
S a3 = a[3];
|
|
S a4 = a[4];
|
|
a[0] = (a[4]*a[8]-a[5]*a[7])*t17;
|
|
a[1] = -(a[1]*a[8]-a[2]*a[7])*t17;
|
|
a[2] = (a1 *a[5]-a[2]*a[4])*t17;
|
|
a[3] = -(a[3]*a[8]-a[5]*a[6])*t17;
|
|
a[4] = (a0 *a[8]-t14 )*t17;
|
|
a[5] = -(t6 - t10)*t17;
|
|
a[6] = (a3 *a[7]-a[4]*a[6])*t17;
|
|
a[7] = -(a[0]*a[7]-t12)*t17;
|
|
a[8] = (t4-t8)*t17;
|
|
|
|
return *this;
|
|
}
|
|
|
|
void show(FILE * fp)
|
|
{
|
|
for(int i=0;i<3;++i)
|
|
printf("| %g \t%g \t%g |\n",a[3*i+0],a[3*i+1],a[3*i+2]);
|
|
}
|
|
|
|
// return the Trace of the matrix i.e. the sum of the diagonal elements
|
|
S Trace() const
|
|
{
|
|
return a[0]+a[4]+a[8];
|
|
}
|
|
|
|
/*
|
|
compute the matrix generated by the product of a * b^T
|
|
*/
|
|
void ExternalProduct(const Point3<S> &a, const Point3<S> &b)
|
|
{
|
|
for(int i=0;i<3;++i)
|
|
for(int j=0;j<3;++j)
|
|
(*this)[i][j] = a[i]*b[j];
|
|
}
|
|
|
|
/* Compute the Frobenius Norm of the Matrix
|
|
*/
|
|
ScalarType Norm()
|
|
{
|
|
ScalarType SQsum=0;
|
|
for(int i=0;i<3;++i)
|
|
for(int j=0;j<3;++j)
|
|
SQsum += a[i]*a[i];
|
|
return (math::Sqrt(SQsum));
|
|
}
|
|
|
|
/*
|
|
It compute the cross covariance matrix of two set of 3d points P and X;
|
|
it returns also the barycenters of P and X.
|
|
fonte:
|
|
|
|
Besl, McKay
|
|
A method for registration o f 3d Shapes
|
|
IEEE TPAMI Vol 14, No 2 1992
|
|
|
|
*/
|
|
template <class STLPOINTCONTAINER >
|
|
void CrossCovariance(const STLPOINTCONTAINER &P, const STLPOINTCONTAINER &X,
|
|
Point3<S> &bp, Point3<S> &bx)
|
|
{
|
|
SetZero();
|
|
assert(P.size()==X.size());
|
|
bx.Zero();
|
|
bp.Zero();
|
|
Matrix33<S> tmp;
|
|
typename std::vector <Point3<S> >::const_iterator pi,xi;
|
|
for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
|
|
bp+=*pi;
|
|
bx+=*xi;
|
|
tmp.ExternalProduct(*pi,*xi);
|
|
(*this)+=tmp;
|
|
}
|
|
bp/=P.size();
|
|
bx/=X.size();
|
|
(*this)/=P.size();
|
|
tmp.ExternalProduct(bp,bx);
|
|
(*this)-=tmp;
|
|
}
|
|
|
|
template <class STLPOINTCONTAINER, class STLREALCONTAINER>
|
|
void WeightedCrossCovariance(const STLREALCONTAINER & weights,
|
|
const STLPOINTCONTAINER &P,
|
|
const STLPOINTCONTAINER &X,
|
|
Point3<S> &bp,
|
|
Point3<S> &bx)
|
|
{
|
|
SetZero();
|
|
assert(P.size()==X.size());
|
|
bx.Zero();
|
|
bp.Zero();
|
|
Matrix33<S> tmp;
|
|
typename std::vector <Point3<S> >::const_iterator pi,xi;
|
|
typename STLREALCONTAINER::const_iterator pw;
|
|
|
|
for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
|
|
bp+=(*pi);
|
|
bx+=(*xi);
|
|
}
|
|
bp/=P.size();
|
|
bx/=X.size();
|
|
|
|
for(pi=P.begin(),xi=X.begin(),pw = weights.begin();pi!=P.end();++pi,++xi,++pw){
|
|
|
|
tmp.ExternalProduct(((*pi)-(bp)),((*xi)-(bp)));
|
|
|
|
(*this)+=tmp*(*pw);
|
|
}
|
|
}
|
|
|
|
private:
|
|
S a[9];
|
|
};
|
|
|
|
template <class S>
|
|
void Invert(Matrix33<S> &m)
|
|
{
|
|
Matrix33<S> v;
|
|
Point3<typename Matrix33<S>::ScalarType> e;
|
|
SingularValueDecomposition(m,&e[0],v);
|
|
e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2];
|
|
m.Transpose();
|
|
m = v * Matrix33Diag<S>(e) * m;
|
|
}
|
|
|
|
template <class S>
|
|
Matrix33<S> Inverse(const Matrix33<S>&m)
|
|
{
|
|
Matrix33<S> v,m_copy=m;
|
|
Point3<S> e;
|
|
SingularValueDecomposition(m_copy,&e[0],v);
|
|
m_copy.Transpose();
|
|
e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2];
|
|
return v * Matrix33Diag<S>(e) * m_copy;
|
|
}
|
|
|
|
///
|
|
typedef Matrix33<short> Matrix33s;
|
|
typedef Matrix33<int> Matrix33i;
|
|
typedef Matrix33<float> Matrix33f;
|
|
typedef Matrix33<double> Matrix33d;
|
|
|
|
} // end of namespace
|
|
|
|
#endif
|