PARP Research Group University of Murcia, Spain


src/qvmath/qvmatrix.cpp

Go to the documentation of this file.
00001 /*
00002  *      Copyright (C) 2007, 2008, 2009. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00024 
00025 #include <qvmath.h>
00026 #include <qvdefines.h>
00027 #include <qvmatrixalgebra.h>
00028 
00029 #include <QString>
00030 #include <QVMatrix>
00031 
00032 
00034 // Constructors
00035 
00036 QVMatrix::QVMatrix():
00037         cols(1), rows(1), data(new QBlasDataBuffer(cols*rows))
00038         { }
00039 
00040 QVMatrix::QVMatrix(const QVMatrix &matrix):
00041         cols(matrix.cols), rows(matrix.rows), data(matrix.data)
00042         { }
00043 
00044 QVMatrix::QVMatrix(const int rows, const int cols):
00045         cols(cols), rows(rows), data(new QBlasDataBuffer(cols*rows))
00046         { }
00047 
00048 QVMatrix::QVMatrix(const int rows, const int cols, const double value):
00049         cols(cols), rows(rows), data(new QBlasDataBuffer(cols*rows))
00050         {
00051         set(value);
00052         }
00053 
00054 QVMatrix::QVMatrix(const QVQuaternion &quaternion): cols(4), rows(4), data(new QBlasDataBuffer(cols*rows))
00055         {
00056         *this = QVMatrix::identity(4);
00057 
00058         const double    norm = quaternion.norm2(),
00059                         w = quaternion[0] / norm,
00060                         x = quaternion[1] / norm,
00061                         y = quaternion[2] / norm,
00062                         z = quaternion[3] / norm;
00063 
00064         operator()(0,0) = 1 - 2.0 * (x * x + y * y);
00065         operator()(0,1) = + 2.0 * (w * x - y * z);
00066         operator()(0,2) = + 2.0 * (y * w + x * z);
00067         
00068         operator()(1,0) = + 2.0 * (w * x + y * z);
00069         operator()(1,1) = 1 - 2.0 * (y * y + w * w);
00070         operator()(1,2) = + 2.0 * (x * y - w * z);
00071         
00072         operator()(2,0) = + 2.0 * (y * w - x * z);
00073         operator()(2,1) = + 2.0 * (x * y + w * z);
00074         operator()(2,2) = 1 - 2.0 * (x * x + w * w);
00075 
00076 /*
00077         operator()(0,0) += - 2.0 * (quaternion[1] * quaternion[1] + quaternion[2] * quaternion[2]);
00078         operator()(0,1) += + 2.0 * (quaternion[0] * quaternion[1] - quaternion[2] * quaternion[3]);
00079         operator()(0,2) += + 2.0 * (quaternion[2] * quaternion[0] + quaternion[1] * quaternion[3]);
00080         
00081         operator()(1,0) += + 2.0 * (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]);
00082         operator()(1,1) += - 2.0 * (quaternion[2] * quaternion[2] + quaternion[0] * quaternion[0]);
00083         operator()(1,2) += + 2.0 * (quaternion[1] * quaternion[2] - quaternion[0] * quaternion[3]);
00084         
00085         operator()(2,0) += + 2.0 * (quaternion[2] * quaternion[0] - quaternion[1] * quaternion[3]);
00086         operator()(2,1) += + 2.0 * (quaternion[1] * quaternion[2] + quaternion[0] * quaternion[3]);
00087         operator()(2,2) += - 2.0 * (quaternion[1] * quaternion[1] + quaternion[0] * quaternion[0]);*/
00088         }
00089 
00090 QVMatrix::QVMatrix(const QVVector &vector, const bool rowVector):
00091         cols(rowVector?vector.size():1), rows(rowVector?1:vector.size()), data(new QBlasDataBuffer(cols*rows))
00092         {
00093         if (rowVector)
00094                 setRow(0, vector);
00095         else
00096                 setCol(0, vector);
00097         }
00098 
00099 QVMatrix::QVMatrix(const QList<QVVector> &vectorList): cols(vectorList.at(0).size()), rows(vectorList.size()), data(new QBlasDataBuffer(cols*rows))
00100         {
00101         for (int n = 0; n < getRows(); n++)
00102                 {
00103                 Q_ASSERT(vectorList.at(n).size() == getCols());
00104                 setRow(n, vectorList.at(n));
00105                 }
00106         }
00107 
00108 QVMatrix::QVMatrix(const QList< QVector<double> > &vectorList): cols(vectorList.at(0).size()), rows(vectorList.size()), data(new QBlasDataBuffer(cols*rows))
00109         {
00110         for (int n = 0; n < getRows(); n++)
00111                 {
00112                 Q_ASSERT(vectorList.at(n).size() == getCols());
00113                 setRow(n, vectorList.at(n));
00114                 }
00115         }
00116 
00117 QVMatrix::QVMatrix(const gsl_matrix *matrix): cols(matrix->size2), rows(matrix->size1), data(new QBlasDataBuffer(cols*rows))
00118         {
00119         for(int i = 0; i < rows; i++)
00120                 for(int j = 0; j < cols; j++)
00121                         operator()(i, j) = gsl_matrix_get(matrix, i, j);
00122         }
00123 
00124 QVMatrix::QVMatrix(const QList<QPointF> &pointList): cols(2), rows(pointList.size()), data(new QBlasDataBuffer(cols*rows))
00125         {
00126         for (int n = 0; n < getRows(); n++)
00127                 setRow(n, pointList.at(n));
00128         }
00129 
00130 #ifdef OPENCV
00131 QVMatrix::QVMatrix(const CvMat *cvMatrix): cols(cvMatrix->cols), rows(cvMatrix->rows), data(new QBlasDataBuffer(cols*rows))
00132         {
00133         for (int i = 0; i < rows; i++)
00134                         for (int j = 0; j < cols; j++)
00135                                 this->operator()(i,j) = cvmGet(cvMatrix, i, j);
00136         }
00137 
00138 CvMat *QVMatrix::toCvMat(const int cvMatType) const
00139         {
00140         Q_ASSERT( (cvMatType == CV_32F) || (cvMatType == CV_64F) );
00141 
00142         CvMat *result = cvCreateMat(rows, cols, cvMatType);
00143 
00144         for (int i = 0; i < rows; i++)
00145                         for (int j = 0; j < cols; j++)
00146                                 cvmSet(result, i, j, this->operator()(i,j));
00147         return result;
00148         }
00149 
00150 #endif
00151 
00153 QVMatrix & QVMatrix::operator=(const QVMatrix &matrix)
00154         {
00155         cols = matrix.cols;
00156         rows = matrix.rows;
00157         data = matrix.data;
00158 
00159         return *this;
00160         }
00161 
00163 
00164 QVVector QVMatrix::operator*(const QVVector &vector) const
00165         {
00166         Q_ASSERT(vector.size() == getCols());
00167         return dotProduct(vector.toColumnMatrix()).getCol(0);
00168         }
00169 
00171 
00172 bool QVMatrix::equals(const QVMatrix &matrix) const
00173         {
00174         // Check valences for both matrixs are equivalent
00175         if (cols != matrix.cols || rows != matrix.rows)
00176                 return false;
00177 
00178         // Check data in both matrixs is equal
00179         const double    *data1 = getReadData(),
00180                         *data2 = matrix.getReadData();
00181 
00182         for (int i = 0; i < getDataSize(); i++)
00183                 if (data1[i] != data2[i])
00184                         return false;
00185 
00186         return true;
00187         }
00188 
00189 QVMatrix QVMatrix::dotProduct(const QVMatrix &matrix) const
00190         {
00191         const int       cols1 = cols,
00192                         rows1 = rows,
00193                         cols2 = matrix.cols,
00194                         rows2 = matrix.rows;
00195 
00196         Q_ASSERT(cols1 == rows2);
00197 
00198         if (cols1 != rows2)
00199                 {
00200                 std::cout << "ERROR: tried to multiply matrices with incompatible sizes at QVMatrix::dotProduct(const QVMatrix &matrix)." << std::endl
00201                         << "\tMatrix 1 dimentions:\t" << rows1 << "x" << cols1 << std::endl
00202                         << "\tMatrix 2 dimentions:\t" << rows2 << "x" << cols2 << std::endl;
00203                 exit(1);
00204                 }
00205 
00206         QVMatrix result(rows1, cols2);
00207 
00208         const int k = cols1, m = rows1, n = cols2;
00209  
00210         cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
00211                         m, n, k, 1.0,
00212                         getReadData(), k,
00213                         matrix.getReadData(), n, 0.0,
00214                         result.getWriteData(), n);
00215 
00216         return result;
00217         }
00218 
00219 QVMatrix QVMatrix::elementProduct(const QVMatrix &matrix) const
00220         {
00221         const int cols1 = cols, rows1 = rows, cols2 = matrix.cols, rows2 = matrix.rows;
00222 
00223         Q_ASSERT(rows1 == rows2);
00224         Q_ASSERT(cols1 == cols2);
00225 
00226         if (cols1 != cols2 || rows1 != rows2)
00227                 {
00228                 std::cout << "ERROR: tried to multiply matrices with incompatible sizes at QVMatrix::dotProduct(const QVMatrix &matrix)." << std::endl
00229                         << "\tMatrix 1 dimentions:\t" << rows1 << "x" << cols1 << std::endl
00230                         << "\tMatrix 2 dimentions:\t" << rows2 << "x" << cols2 << std::endl;
00231                 exit(1);
00232                 }
00233 
00234         QVMatrix result(rows1, cols2);
00235 
00237         for (int i = 0; i < cols1; i++)
00238                 for (int j = 0; j < cols1; j++)
00239                         result(i,j) = this->operator()(i,j) * matrix(i,j);
00240 
00241         return result;
00242         }
00243 
00244 QVMatrix QVMatrix::matrixDivide(const QVMatrix &matrix) const
00245         {
00246         Q_ASSERT(matrix.getCols() >= matrix.getRows());
00247         Q_ASSERT(matrix.getCols() == (*this).getCols());
00248 
00249         QVMatrix result(matrix.getRows(), (*this).getRows());
00250 
00251         if (matrix.getCols() == matrix.getRows())
00252                 solveLinear(matrix.transpose(), result, (*this).transpose());
00253         else
00254                 solveOverDetermined(matrix.transpose(), result, (*this).transpose());
00255 
00256         return result.transpose();
00257         }
00258 
00259 QVMatrix QVMatrix::inverse() const
00260         {
00262         return pseudoInverse(*this);
00263         }
00264 
00265 double QVMatrix::det() const
00266         {
00267         return determinant(*this);
00268         }
00269 
00270 QVMatrix QVMatrix::transpose() const
00271         {
00272         const int rows = getRows(), cols = getCols();
00273 
00274         QVMatrix result(cols, rows);
00275 
00276         const double    *matrixData = getReadData();
00277         double          *resultData = result.getWriteData();
00278 
00280         for (int i = 0; i < rows; i++)
00281                 for (int j = 0; j < cols; j++)
00282                         resultData[j*rows+i] = matrixData[i*cols+j];
00283 
00284         return result;
00285         }
00286 
00287 void QVMatrix::set(const double value)
00288         {
00289         double          *resultData = getWriteData();
00290         const int       dataSize = getDataSize();
00291 
00293         for (int i = 0; i < dataSize; i++)
00294                 resultData[i] = value;
00295         }
00296 
00297 QVMatrix QVMatrix::addition(const QVMatrix &matrix) const
00298         {
00299         Q_ASSERT(cols == matrix.cols || rows == matrix.rows);
00300 
00301         QVMatrix result = *this;
00302 
00303         const double    *matrixData = matrix.getReadData();
00304         double          *resultData = result.getWriteData();
00305         const int       dataSize = matrix.getDataSize();
00306 
00308 
00309         for (int i = 0; i < dataSize; i++)
00310                 resultData[i] += matrixData[i];
00311 
00312         return result;
00313         }
00314 
00315 QVMatrix QVMatrix::substract(const QVMatrix &matrix) const
00316         {
00317         // Check valences for both matrixs are equivalent
00318         Q_ASSERT(cols == matrix.cols || rows == matrix.rows);
00319 
00320         QVMatrix result = *this;
00321 
00322         const double    *matrixData = matrix.getReadData();
00323         double          *resultData = result.getWriteData();
00324         const int       dataSize = matrix.getDataSize();
00325 
00327 
00328         for (int i = 0; i < dataSize; i++)
00329                 resultData[i] -= matrixData[i];
00330 
00331         return result;
00332         }
00333 
00334 QVMatrix QVMatrix::scalarProduct(const double value) const
00335         {
00336         QVMatrix result = *this;
00337 
00338         double          *resultData = result.getWriteData();
00339         const int       dataSize = getDataSize();
00340 
00342 
00343         for (int i = 0; i < dataSize; i++)
00344                 resultData[i] *= value;
00345 
00346         return result;
00347         }
00348 
00349 QVMatrix QVMatrix::scalarDivide(const double value) const
00350         {
00351         QVMatrix result = *this;
00352 
00353         double          *resultData = result.getWriteData();
00354         const int       dataSize = getDataSize();
00355 
00357 
00358         for (int i = 0; i < dataSize; i++)
00359                 resultData[i] /= value;
00360 
00361         return result;
00362         }
00363 
00364 double QVMatrix::norm2() const
00365         { return cblas_dnrm2(getDataSize(), getReadData(), 1); };
00366 
00367 
00368 QVMatrix QVMatrix::rowHomogeneousNormalize() const
00369         {
00370         const int cols = getCols(), rows = getRows();
00371         QVMatrix result(rows, cols);
00372 
00373         for (int i = 0; i < rows; i++)
00374                 for (int j = 0; j < cols; j++)
00375                         result(i,j) = operator()(i,j) / operator()(i,cols-1);
00376         return result;
00377         }
00378 
00379 const QVVector QVMatrix::getRow(const int row) const
00380         {
00381         Q_ASSERT(row < getRows());
00382 
00383         const int cols = getCols();
00384         QVVector result(cols);
00385         for (int col= 0; col < cols; col++)
00386                 result[col] = operator()(row,col);
00387 
00388         return result;
00389         }
00390 
00391 void QVMatrix::setRow(const int row, QVVector vector)
00392         {
00393         Q_ASSERT(row < getRows());
00394         Q_ASSERT(getCols() == vector.size());
00395 
00396         const int cols = getCols();
00397         for (int col= 0; col < cols; col++)
00398                 operator()(row,col) = vector[col];
00399         }
00400 
00401 void QVMatrix::setRow(const int row, QVector<double> vector)
00402         {
00403         Q_ASSERT(row < getRows());
00404         Q_ASSERT(getCols() == vector.size());
00405 
00406         const int cols = getCols();
00407         for (int col= 0; col < cols; col++)
00408                 operator()(row,col) = vector[col];
00409         }
00410 
00411 const QVVector QVMatrix::getCol(const int col) const
00412         {
00413         Q_ASSERT(col < getCols());
00414 
00415         const int rows = getRows();
00416         QVVector result(rows);
00417         for (int row= 0; row < rows; row++)
00418                 result[row] = operator()(row,col);
00419 
00420         return result;
00421         }
00422 
00423 void QVMatrix::setCol(const int col, QVVector vector)
00424         {
00425         Q_ASSERT(col < getCols());
00426         Q_ASSERT(getRows() == vector.size());
00427 
00428         const int rows = getRows();
00429         for (int row= 0; row < rows; row++)
00430                 operator()(row,col) = vector[row];
00431         }
00432 
00433 const QVMatrix QVMatrix::getSubmatrix(const int firstRow, const int lastRow, const int firstCol, const int lastCol) const
00434         {
00435 
00436         Q_ASSERT(firstRow >= 0);
00437         Q_ASSERT(firstCol >= 0);
00438         Q_ASSERT(firstRow <= lastRow);
00439         Q_ASSERT(firstCol <= lastCol);
00440         Q_ASSERT(lastRow < getRows());
00441         Q_ASSERT(lastCol < getCols());
00442 
00443         QVMatrix result(lastRow - firstRow +1, lastCol - firstCol +1);
00444 
00445         for (int row = firstRow; row <= lastRow; row++)
00446                 for (int col = firstCol; col < lastCol; col++)
00447                         result(row - firstRow, col - firstCol) = operator()(row,col);
00448 
00449         return result;
00450         }
00451         
00453 // Misc matrices
00454 
00455 QVMatrix QVMatrix::identity(const int size)
00456         {
00457         QVMatrix result(size, size);
00458         result.set(0);
00459         for (int i= 0; i < size; i++)
00460                 result(i,i) = 1;
00461         return result;
00462         }
00463 
00464 QVMatrix QVMatrix::zeros(const int rows, const int cols)
00465         {
00466         QVMatrix result(rows, cols);
00467         result.set(0);
00468         return result;
00469         }
00470 
00471 QVMatrix QVMatrix::random(const int rows, const int cols)
00472         {
00473         QVMatrix result(rows, cols);
00474         for (int col = 0; col < cols; col++)
00475                 for (int row = 0; row < rows; row++)
00476                         result(row,col) = (double)ABS(rand()) / (double)std::numeric_limits<int>::max();
00477         return result;
00478         }
00479 
00480 QVMatrix QVMatrix::diagonal(const QVVector &diagonalVector)
00481         {
00482         const int size = diagonalVector.size();
00483         QVMatrix result(size, size);
00484         result.set(0);
00485         for (int i= 0; i < size; i++)
00486                 result(i,i) = diagonalVector[i];
00487         return result;
00488         }
00489 
00490 QVMatrix QVMatrix::rotationMatrix(const double delta)
00491         {
00492         // Rotation[delta_] := {{Cos[delta], Sin[delta], 0}, {-Sin[delta], Cos[delta], 0}, {0, 0, 1}};
00493         QVMatrix result = QVMatrix::identity(3);
00494 
00495         result(0,0) = cos(delta);       result(0,1) = sin(delta);
00496         result(1,0) = -sin(delta);      result(1,1) = cos(delta);
00497 
00498         return result;
00499         }
00500 
00501 QVMatrix QVMatrix::traslationMatrix(const double x, const double y)
00502         // Translation[x_, y_] := {{1, 0, x}, {0, 1, y}, {0, 0, 1}};
00503         {
00504         QVMatrix result = QVMatrix::identity(3);
00505 
00506         result(0,2) = x;
00507         result(1,2) = y;
00508 
00509         return result;
00510         }
00511 
00512 QVMatrix QVMatrix::scaleMatrix(const double zoom)
00513         {
00514         QVMatrix result = QVMatrix::identity(3);
00515 
00516         result(0,0) = zoom;
00517         result(1,1) = zoom;
00518 
00519         return result;
00520         }
00521 
00522 QVMatrix QVMatrix::rotationMatrix3dZAxis(const double angle)
00523         {
00524         QVMatrix result = identity(4);
00525 
00526         result(0,0) = cos(angle);
00527         result(0,1) = sin(angle);
00528 
00529         result(1,0) = -sin(angle);
00530         result(1,1) = cos(angle);
00531 
00532         return result;
00533         }
00534 
00535 QVMatrix QVMatrix::rotationMatrix3dXAxis(const double angle)
00536         {
00537         QVMatrix result = identity(4);
00538 
00539         result(1,1) = cos(angle);
00540         result(1,2) = sin(angle);
00541 
00542         result(2,1) = -sin(angle);
00543         result(2,2) = cos(angle);
00544 
00545         return result;
00546         }
00547 
00548 QVMatrix QVMatrix::rotationMatrix3dYAxis(const double angle)
00549         {
00550         QVMatrix result = identity(4);
00551 
00552         result(0,0) = cos(angle);
00553         result(0,2) = -sin(angle);
00554 
00555         result(2,0) = sin(angle);
00556         result(2,2) = cos(angle);
00557 
00558         return result;
00559         }
00560 
00561 QVMatrix QVMatrix::traslationMatrix3d(const double x, const double y, const double z)
00562         {
00563         QVMatrix result = identity(4);
00564 
00565         result(0,3) = x;
00566         result(1,3) = y;
00567         result(2,3) = z;
00568 
00569         return result;
00570         }
00571 
00572 QVVector QVMatrix::meanCol() const
00573         {
00574         QVVector result = getCol(0);
00575         for (int i = 1; i < getCols(); i++)
00576                 result += getCol(i);
00577         return result / getCols();
00578         }
00579 
00581 
00582 std::ostream& operator << ( std::ostream &os, const QVMatrix &matrix )
00583         {
00584         const int cols = matrix.getCols(), rows = matrix.getRows();
00585 
00586         os << "QVMatrix (" << rows << ", " << cols << ")" << std::endl;
00587 
00588         const double *data = matrix.getReadData();
00589         os << "[" << std::endl;
00590         for (int i=0; i < rows; i++)
00591                 {
00592                 os << "    [ ";
00593                 for (int j = 0; j < cols; j++)
00594                         os << qPrintable(QString("%1").arg(data[i*cols + j], -8, 'f', 6)) << " ";
00595                 os << "]" << std::endl;
00596                 }
00597         os << "]" << std::endl;
00598         return os;
00599         }
00600 
00601 std::istream& operator >> ( std::istream &is, QVMatrix &matrix )
00602         {
00603         int cols, rows;
00604         double value;
00605 
00606         is.ignore(256, '(');
00607         is >> rows;
00608         is.ignore(256, ',');
00609         is >> cols;
00610         is.ignore(256, '[');
00611         QVMatrix maux(rows, cols);
00612 
00613         for (int i = 0; i < rows; i++)
00614                 {
00615                 is.ignore(256, '[');
00616                 for (int j = 0; j < cols; j++)
00617                         {
00618                         is >> value;
00619                         maux(i, j) = value;
00620                         }
00621                 }
00622 
00623         matrix = maux;
00624 
00625         return is;
00626         }
00627 
00628 uint qHash(const QVMatrix &matrix)
00629         {
00630         const int cols = matrix.getCols(), rows = matrix.getRows();
00631 
00632         double accum = 0;
00633         for (int i = 0; i < cols; i++)
00634                 for (int j = 0; j < rows; j++)
00635                         accum += matrix(i,j) / matrix(cols-i-1, rows-j-1);
00636 
00637         return (uint) ((100000 * accum) / ((double) (cols + rows)));
00638         }
00639 
00640 



QVision framework. PARP research group, copyright 2007, 2008.