21 #include <Base/Common/BIASpragma.hh>
23 #include <Base/Common/CompareFloatingPoint.hh>
24 #include "Matrix3x3.hh"
26 #include <Base/Math/Matrix.hh>
27 #include <Base/Math/Matrix3x4.hh>
47 const T a3,
const T a4,
const T a5,
48 const T a6,
const T a7,
const T a8)
50 Data_[0]=a0; Data_[1]=a1; Data_[2]=a2;
51 Data_[3]=a3; Data_[4]=a4; Data_[5]=a5;
52 Data_[6]=a6; Data_[7]=a7; Data_[8]=a8;
65 this->Data_[0] = this->Data_[4] = this->Data_[8] =T(1);
68 BIASERR(
"undefined MatrixInitType");
78 memcpy((
void *)Data_, (
const void *)A.
Data_, 9*
sizeof(T));
87 BIASERR(
"wrong matrix size "<<mat.
num_rows()<<
"x"
91 memcpy((
void *)Data_, (
void *)mat.
GetData(), 9*
sizeof(T));
100 BIASERR(
"wrong matrix size "<<mat.
num_rows()<<
"x"
104 Data_[0] = mat[0][0];
105 Data_[1] = mat[0][1];
106 Data_[2] = mat[0][2];
107 Data_[3] = mat[1][0];
108 Data_[4] = mat[1][1];
109 Data_[5] = mat[1][2];
110 Data_[6] = mat[2][0];
111 Data_[7] = mat[2][1];
112 Data_[8] = mat[2][2];
126 for (
int i=0; i<9; i++)
127 Data_[i] += arg.
Data_[i];
136 for (
int i=0; i<9; i++)
137 Data_[i] -= arg.
Data_[i];
146 register T *src = this->GetData();
147 for (
int i=0; i<9; i++)
157 BIASASSERT(!
Equal(scalar, (T)0.0));
158 register T *src = this->GetData();
159 for (
int i=0; i<9; i++)
172 const T *thisd = Data_;
192 BIASASSERT(vec.
size() == 9 );
200 for (
int i=0; i<9; i++) {
245 const unsigned int & ,
246 const unsigned int & ) {
247 BIASERR(
"not implemented because minus does not make sense for "
256 T *mat=this->GetData();
257 mat[0]=mat[4]=mat[8]=0;
258 mat[1]= -z; mat[2]= y;
259 mat[3]= z; mat[5]=-x;
260 mat[6]= -y; mat[7]= x;
267 BIASERR(
"not implemented because minus does not make sense for "
277 T *mat=this->GetData();
278 mat[0]=mat[4]=mat[8]=0;
279 mat[1]=-vec[2]; mat[2]=vec[1];
280 mat[3]= vec[2]; mat[5]=-vec[0];
281 mat[6]= -vec[1]; mat[7]=vec[0];
290 const T* p =Data_ + 3*row;
302 const T* p =Data_ + col;
314 T* p = Data_ + 3*row;
333 template<> BIASMathBase_EXPORT
337 unsigned int result = 0;
338 for (
int i=0; i<9; i++){
349 double t4 = (*this)[0][0]*(*this)[1][1];
350 double t6 = (*this)[0][0]*(*this)[1][2];
351 double t8 = (*this)[0][1]*(*this)[1][0];
352 double t10 = (*this)[0][2]*(*this)[1][0];
353 double t12 = (*this)[0][1]*(*this)[2][0];
354 double t14 = (*this)[0][2]*(*this)[2][0];
355 return T((t4*(*
this)[2][2]-t6*(*
this)[2][1]-t8*(*
this)[2][2]+
356 t10*(*
this)[2][1]+t12*(*
this)[1][2]-t14*(*
this)[1][1]));
364 BIASERR(
"not implemented because minus does not make sense for "
376 BIASASSERT(x.
GetData()!=Data_);
378 const double t4 = (*this)[0][0]*(*this)[1][1];
379 const double t6 = (*this)[0][0]*(*this)[1][2];
380 const double t8 = (*this)[0][1]*(*this)[1][0];
381 const double t10 = (*this)[0][2]*(*this)[1][0];
382 const double t12 = (*this)[0][1]*(*this)[2][0];
383 const double t14 = (*this)[0][2]*(*this)[2][0];
385 const double det=(t4*(*this)[2][2]-t6*(*this)[2][1]-t8*(*this)[2][2]+t10
386 *(*this)[2][1]+t12*(*this)[1][2]-t14*(*this)[1][1]);
391 const double t17 = 1.0/det;
393 x[0][0] = T(((*
this)[1][1]*(*
this)[2][2]-(*
this)[1][2]*(*
this)[2][1])*t17);
394 x[0][1] = T(-((*
this)[0][1]*(*
this)[2][2]-(*
this)[0][2]*(*
this)[2][1])*t17);
395 x[0][2] = T(-(-(*
this)[0][1]*(*
this)[1][2]+(*
this)[0][2]*(*
this)[1][1])*t17);
396 x[1][0] = T(-((*
this)[1][0]*(*
this)[2][2]-(*
this)[1][2]*(*
this)[2][0])*t17);
397 x[1][1] = T(((*
this)[0][0]*(*
this)[2][2]-t14)*t17);
398 x[1][2] = T(-(t6-t10)*t17);
399 x[2][0] = T(((*
this)[1][0]*(*
this)[2][1]-(*
this)[1][1]*(*
this)[2][0])*t17);
400 x[2][1] = T(-((*
this)[0][0]*(*
this)[2][1]-t12)*t17);
401 x[2][2] = T((t4-t8)*t17);
411 BIASERR(
"not implemented because minus does not make sense for "
425 const double t4 = (*this)[0][0]*(*this)[1][1];
426 const double t6 = (*this)[0][0]*(*this)[1][2];
427 const double t8 = (*this)[0][1]*(*this)[1][0];
428 const double t10 = (*this)[0][2]*(*this)[1][0];
429 const double t12 = (*this)[0][1]*(*this)[2][0];
430 const double t14 = (*this)[0][2]*(*this)[2][0];
432 const double det=(t4*(*this)[2][2]-t6*(*this)[2][1]-t8*(*this)[2][2]+t10
433 *(*this)[2][1]+t12*(*this)[1][2]-t14*(*this)[1][1]);
435 if (det==0.0)
return -1;
437 const double t17 = 1.0/det;
441 tmp[0] = T(((*
this)[1][1]*(*
this)[2][2]-(*
this)[1][2]*(*
this)[2][1])*t17);
442 tmp[1] = T(-((*
this)[0][1]*(*
this)[2][2]-(*
this)[0][2]*(*
this)[2][1])*t17);
443 tmp[2] = T(-(-(*
this)[0][1]*(*
this)[1][2]+(*
this)[0][2]*(*
this)[1][1])*t17);
444 tmp[3] = T(-((*
this)[1][0]*(*
this)[2][2]-(*
this)[1][2]*(*
this)[2][0])*t17);
445 tmp[4] = T(((*
this)[0][0]*(*
this)[2][2]-t14)*t17);
446 tmp[5] = T(-(t6-t10)*t17);
447 tmp[6] = T(((*
this)[1][0]*(*
this)[2][1]-(*
this)[1][1]*(*
this)[2][0])*t17);
448 tmp[7] = T(-((*
this)[0][0]*(*
this)[2][1]-t12)*t17);
449 tmp[8] = T((t4-t8)*t17);
451 memcpy((
void*)(Data_), (
void*)tmp, 9*
sizeof(T));
470 T *destP = (T*) (destmat.
GetData());
471 const T *argP = (T*)(argmat.
GetData());
472 const register T *matP = this->GetData();
474 BIASASSERT(matP!=argP);
475 BIASASSERT(matP!=destP);
478 matP[0] * argP[0] + matP[1] * argP[4] + matP[2] * argP[8];
480 matP[0] * argP[1] + matP[1] * argP[5] + matP[2] * argP[9];
482 matP[0] * argP[2] + matP[1] * argP[6] + matP[2] * argP[10];
484 matP[0] * argP[3] + matP[1] * argP[7] + matP[2] * argP[11];
487 matP[3] * argP[0] + matP[4] * argP[4] + matP[5] * argP[8];
489 matP[3] * argP[1] + matP[4] * argP[5] + matP[5] * argP[9];
491 matP[3] * argP[2] + matP[4] * argP[6] + matP[5] * argP[10];
493 matP[3] * argP[3] + matP[4] * argP[7] + matP[5] * argP[11];
496 matP[6] * argP[0] + matP[7] * argP[4] + matP[8] * argP[8];
498 matP[6] * argP[1] + matP[7] * argP[5] + matP[8] * argP[9];
500 matP[6] * argP[2] + matP[7] * argP[6] + matP[8] * argP[10];
502 matP[6] * argP[3] + matP[7] * argP[7] + matP[8] * argP[11];
510 for (
int i=0; i<9; i++)
511 if (!
Equal(Data_[i], (T)0.0, eps))
return false;
520 if (!
Equal(Data_[0], (T)1.0, eps))
return false;
521 if (!
Equal(Data_[1], (T)0.0, eps))
return false;
522 if (!
Equal(Data_[2], (T)0.0, eps))
return false;
523 if (!
Equal(Data_[3], (T)0.0, eps))
return false;
524 if (!
Equal(Data_[4], (T)1.0, eps))
return false;
525 if (!
Equal(Data_[5], (T)0.0, eps))
return false;
526 if (!
Equal(Data_[6], (T)0.0, eps))
return false;
527 if (!
Equal(Data_[7], (T)0.0, eps))
return false;
528 if (!
Equal(Data_[8], (T)1.0, eps))
return false;
538 for (
int i=0; i<9; i++)
539 if (!
Equal(Data_[i], arg.
Data_[i]))
return false;
546 Load(
const std::string & filename)
548 std::ifstream fs( filename.c_str() );
554 if (!fs.good())
return false;
563 Save(
const std::string & filename)
const
565 std::ofstream fs( filename.c_str() );
571 fs.close();
return false; }
583 for (
int i=0; i<9; i++)
584 if ( (Data_[i])*(Data_[i]) > greatest*greatest )
595 for (
int r=0; r<3; r++){
596 for (
int c=r; c<3; c++){
597 (*this)[c][r] = (*this)[r][c] =
598 T(((*
this)[c][r] + (*
this)[r][c]) / 2.0);
608 register const T *d = this->GetData();
610 if (d[1]>max) max=d[1];
611 if (d[2]>max) max=d[2];
612 if (d[3]>max) max=d[3];
613 if (d[4]>max) max=d[4];
614 if (d[5]>max) max=d[5];
615 if (d[6]>max) max=d[6];
616 if (d[7]>max) max=d[7];
617 if (d[8]>max) max=d[8];
626 register const T *d = this->GetData();
628 if (d[1]<min) min=d[1];
629 if (d[2]<min) min=d[2];
630 if (d[3]<min) min=d[3];
631 if (d[4]<min) min=d[4];
632 if (d[5]<min) min=d[5];
633 if (d[6]<min) min=d[6];
634 if (d[7]<min) min=d[7];
635 if (d[8]<min) min=d[8];
644 register const T *d = this->GetData();
647 if (d[1]>max) max=d[1];
648 if (d[2]>max) max=d[2];
649 if (d[3]>max) max=d[3];
650 if (d[4]>max) max=d[4];
651 if (d[5]>max) max=d[5];
652 if (d[6]>max) max=d[6];
653 if (d[7]>max) max=d[7];
654 if (d[8]>max) max=d[8];
656 if (d[1]<min) min=d[1];
657 if (d[2]<min) min=d[2];
658 if (d[3]<min) min=d[3];
659 if (d[4]<min) min=d[4];
660 if (d[5]<min) min=d[5];
661 if (d[6]<min) min=d[6];
662 if (d[7]<min) min=d[7];
663 if (d[8]<min) min=d[8];
671 BIASERR(
"does not exist for this storaget type");
680 max = min = fabs( Data_[0] );
681 for(
int i=0; i<9; i++) {
682 double ad = fabs(Data_[i]);
683 if (ad<min) min = ad;
684 else if (ad>max) max = ad;
693 max = min = fabs( Data_[0] );
694 for(
int i=0; i<9; i++) {
695 float ad = fabs(Data_[i]);
696 if (ad<min) min = ad;
697 else if (ad>max) max = ad;
706 memcpy((
void *)Data_, (
const void *)mat.
Data_, 9*
sizeof(T));
723 if (tmp.num_cols()!=3 || tmp.num_rows()!=3) {
724 s.setstate(std::ios::badbit | std::ios::failbit);
734 std::ostream& BIAS::operator<<(std::ostream &s, const Matrix3x3<T> &A)
745 template class BIASMathBase_EXPORT BIAS::Matrix3x3<type>; \
746 template BIASMathBase_EXPORT ostream& BIAS::operator<<<type>(ostream& os, const Matrix3x3<type>& mat); \
747 template BIASMathBase_EXPORT istream& BIAS::operator>><type>(istream& is, Matrix3x3<type>& mat);
void GetRow(const unsigned int row, Vector3< T > &r) const
extract one row ('Zeile') from ths matrix (for convenience)
void SetRow(const unsigned int row, const Vector3< T > &r)
MatrixInitType
can be passed to matrix constructors to init the matrix with the most often used values ...
Subscript num_cols() const
void SetColumn(const unsigned int col, const Vector3< T > &c)
void GetAbsMaxMin(T &max, T &min) const
virtual ~Matrix3x3()
destructor
void GetColumn(const unsigned int col, Vector3< T > &r) const
extract one column ('Spalte') from this matrix (for convenience)
bool operator==(const Matrix3x3< T > &arg) const
bool IsIdentity(const T eps=std::numeric_limits< T >::epsilon()) const
Matrix3x3< T > & operator/=(const T &arg)
bool Save(const std::string &fname) const
void Mult(const Vector3< T > &argvec, Vector3< T > &destvec) const
matrix - vector multiplicate this matrix with Vector3, storing the result in destvec calculates: dest...
Matrix3x3< T > & operator-=(const Matrix3x3< T > &arg)
void SetFromRowVectors(const BIAS::Vector3< T > &v0, const BIAS::Vector3< T > &v1, const BIAS::Vector3< T > &v2)
set this matrix from 3 vectors, each representating a row
int GetInverse(Matrix3x3< T > &inv) const
Matrix inversion: inverts this and stores resulty in argument inv.
bool IsZero(const T eps=std::numeric_limits< T >::epsilon()) const
bool Load(const std::string &fname)
T * GetData()
get the pointer to the data array of the matrix (for faster direct memeory access) ...
is a 'fixed size' quadratic matrix of dim.
void SetFromVector(const TNT::Vector< T > &vec)
sets the diagonalelements of this 3x3 Matrix rowwise with the values of the 9 (x1) vector ...
Matrix3x3< T > & operator*=(const Matrix3x3< T > &arg)
woelk 11/2007 (c) www.vision-n.de
class Vector3 contains a Vector of fixed dim.
bool Equal(const T left, const T right, const T eps)
comparison function for floating point values See http://www.boost.org/libs/test/doc/components/test_...
matrix class with arbitrary size, indexing is row major.
Matrix3x3< T > Transpose() const
returns transposed matrix tested 12.06.2002
Matrix3x3< T > & operator=(const Matrix3x3< T > &mat)
assignment operator
T GetDeterminant() const
returns the Determinant |A| of this
void GetMaxMin(T &max, T &min) const
return biggest and smallest entry
int InvertIP()
In place matrix conversion.
Subscript num_rows() const
is a 'fixed size' rectangular matrix of dim.
void SetFromColumnVectors(const BIAS::Vector3< T > &v0, const BIAS::Vector3< T > &v1, const BIAS::Vector3< T > &v2)
set this matrix from 3 vectors each representating a column
void SetAsCrossProductMatrix(const Vector3< T > &vec)
Sets matrix from vector as cross product matrix of this vector.
Matrix3x3()
default constructor
Matrix3x3< T > & operator+=(const Matrix3x3< T > &arg)
T Normalize()
divide this by biggest absolute entry, returns biggest entry
BIASCommon_EXPORT std::istream & operator>>(std::istream &is, BIAS::TimeStamp &ts)
Standard input operator for TimeStamps.