00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00024
00025 #include <limits>
00026 #include <iostream>
00027
00028 #include <qvmath.h>
00029 #include <qvdefines.h>
00030 #include <qvmatrixalgebra.h>
00031
00032 #include <QString>
00033 #include <QVMatrix>
00034
00035
00037
00038
00039 QVMatrix::QVMatrix():
00040 cols(1), rows(1), data(new QBlasDataBuffer(cols*rows))
00041 {
00042 operator()(0,0) = 1;
00043 }
00044
00045 QVMatrix::QVMatrix(const QVMatrix &matrix):
00046 cols(matrix.cols), rows(matrix.rows), data(matrix.data)
00047 { }
00048
00049 QVMatrix::QVMatrix(const int rows, const int cols, const double *data):
00050 cols(cols), rows(rows), data(new QBlasDataBuffer(cols*rows))
00051 {
00052 if (data != NULL)
00053 {
00054 const int n = cols*rows;
00055 double *matrixData = getWriteData();
00056 for(int i = 0; i < n; i++)
00057 matrixData[i] = data[i];
00058 }
00059
00060
00061
00062 }
00063
00064 QVMatrix::QVMatrix(const int rows, const int cols, const QVVector &data):
00065 cols(cols), rows(rows), data(new QBlasDataBuffer(cols*rows))
00066 {
00067 const int n = cols*rows;
00068 double *matrixData = getWriteData();
00069 for(int i = 0; i < n; i++)
00070 matrixData[i] = data[i];
00071 }
00072
00073
00074 QVMatrix::QVMatrix(const int rows, const int cols, const double value):
00075 cols(cols), rows(rows), data(new QBlasDataBuffer(cols*rows))
00076 {
00077 set(value);
00078 }
00079
00080 QVMatrix::QVMatrix(const QVQuaternion &quaternion): cols(4), rows(4), data(new QBlasDataBuffer(cols*rows))
00081 {
00082 std::cout << "DEPRECATED: QVMatrix(QVQuaternion). Use QVQuaternion::toRotationMatrix instead" << std::endl;
00083
00084 *this = QVMatrix::identity(4);
00085
00086 const double norm = quaternion.norm2(),
00087 x = quaternion[0] / norm,
00088 y = quaternion[1] / norm,
00089 z = quaternion[2] / norm,
00090 w = quaternion[3] / norm;
00091
00092 operator()(0,0) = 1.0 - 2.0 * (y*y + z*z);
00093 operator()(0,1) = 2.0 * (x*y - z*w);
00094 operator()(0,2) = 2.0 * (z*x + y*w);
00095
00096 operator()(1,0) = 2.0 * (x*y + z*w);
00097 operator()(1,1) = 1.0 - 2.0 * (z*z + x*x);
00098 operator()(1,2) = 2.0 * (y*z - x*w);
00099
00100 operator()(2,0) = 2.0 * (z*x - y*w);
00101 operator()(2,1) = 2.0 * (y*z + x*w);
00102 operator()(2,2) = 1.0 - 2.0 * (y*y + x*x);
00103 }
00104
00105 QVMatrix::QVMatrix(const QVVector &vector, const bool rowVector):
00106 cols(rowVector?vector.size():1), rows(rowVector?1:vector.size()), data(new QBlasDataBuffer(cols*rows))
00107 {
00108 if (rowVector)
00109 setRow(0, vector);
00110 else
00111 setCol(0, vector);
00112 }
00113
00114 QVMatrix::QVMatrix(const QList<QVVector> &vectorList): cols(vectorList.at(0).size()), rows(vectorList.size()), data(new QBlasDataBuffer(cols*rows))
00115 {
00116 for (int n = 0; n < getRows(); n++)
00117 {
00118 Q_ASSERT(vectorList.at(n).size() == getCols());
00119 setRow(n, vectorList.at(n));
00120 }
00121 }
00122
00123 QVMatrix::QVMatrix(const QList< QVector<double> > &vectorList): cols(vectorList.at(0).size()), rows(vectorList.size()), data(new QBlasDataBuffer(cols*rows))
00124 {
00125 for (int n = 0; n < getRows(); n++)
00126 {
00127 Q_ASSERT(vectorList.at(n).size() == getCols());
00128 setRow(n, vectorList.at(n));
00129 }
00130 }
00131
00132 QVMatrix::QVMatrix(const gsl_matrix *matrix): cols(matrix->size2), rows(matrix->size1), data(new QBlasDataBuffer(cols*rows))
00133 {
00134 for(int i = 0; i < rows; i++)
00135 for(int j = 0; j < cols; j++)
00136 operator()(i, j) = gsl_matrix_get(matrix, i, j);
00137 }
00138
00139 QVMatrix::QVMatrix(const QList<QPointF> &pointList): cols(2), rows(pointList.size()), data(new QBlasDataBuffer(cols*rows))
00140 {
00141 for (int n = 0; n < getRows(); n++)
00142 setRow(n, pointList.at(n));
00143 }
00144
00145 #ifdef OPENCV
00146 QVMatrix::QVMatrix(const CvMat *cvMatrix): cols(cvMatrix->cols), rows(cvMatrix->rows), data(new QBlasDataBuffer(cols*rows))
00147 {
00148 for (int i = 0; i < rows; i++)
00149 for (int j = 0; j < cols; j++)
00150 this->operator()(i,j) = cvmGet(cvMatrix, i, j);
00151 }
00152
00153 CvMat *QVMatrix::toCvMat(const int cvMatType) const
00154 {
00155 Q_ASSERT( (cvMatType == CV_32F) || (cvMatType == CV_64F) );
00156
00157 CvMat *result = cvCreateMat(rows, cols, cvMatType);
00158
00159 for (int i = 0; i < rows; i++)
00160 for (int j = 0; j < cols; j++)
00161 cvmSet(result, i, j, this->operator()(i,j));
00162 return result;
00163 }
00164
00165 #endif
00166
00168 QVMatrix & QVMatrix::operator=(const QVMatrix &matrix)
00169 {
00170 cols = matrix.cols;
00171 rows = matrix.rows;
00172 data = matrix.data;
00173
00174 return *this;
00175 }
00176
00178
00179 QVVector QVMatrix::operator*(const QVVector &vector) const
00180 {
00181 Q_ASSERT(vector.size() == getCols());
00182 return dotProduct(vector.toColumnMatrix()).getCol(0);
00183 }
00184
00186
00187 bool QVMatrix::equals(const QVMatrix &matrix) const
00188 {
00189
00190 if (cols != matrix.cols || rows != matrix.rows)
00191 return false;
00192
00193
00194 const double *data1 = getReadData(),
00195 *data2 = matrix.getReadData();
00196
00197 for (int i = 0; i < getDataSize(); i++)
00198 if (data1[i] != data2[i])
00199 return false;
00200
00201 return true;
00202 }
00203
00204 QVMatrix QVMatrix::dotProduct(const QVMatrix &matrix) const
00205 {
00206 const int cols1 = cols,
00207 rows1 = rows,
00208 cols2 = matrix.cols,
00209 rows2 = matrix.rows;
00210
00211 Q_ASSERT(cols1 == rows2);
00212
00213 if (cols1 != rows2)
00214 {
00215 std::cout << "ERROR: tried to multiply matrices with incompatible sizes at QVMatrix::dotProduct(const QVMatrix &matrix)." << std::endl
00216 << "\tMatrix 1 dimentions:\t" << rows1 << "x" << cols1 << std::endl
00217 << "\tMatrix 2 dimentions:\t" << rows2 << "x" << cols2 << std::endl;
00218 exit(1);
00219 }
00220
00221 QVMatrix result(rows1, cols2);
00222
00223 const int k = cols1, m = rows1, n = cols2;
00224
00225 cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
00226 m, n, k, 1.0,
00227 getReadData(), k,
00228 matrix.getReadData(), n, 0.0,
00229 result.getWriteData(), n);
00230
00231 return result;
00232 }
00233
00234 QVMatrix QVMatrix::elementProduct(const QVMatrix &matrix) const
00235 {
00236 const int cols1 = cols, rows1 = rows, cols2 = matrix.cols, rows2 = matrix.rows;
00237
00238 Q_ASSERT(rows1 == rows2);
00239 Q_ASSERT(cols1 == cols2);
00240
00241 if (cols1 != cols2 || rows1 != rows2)
00242 {
00243 std::cout << "ERROR: tried to multiply matrices with incompatible sizes at QVMatrix::dotProduct(const QVMatrix &matrix)." << std::endl
00244 << "\tMatrix 1 dimentions:\t" << rows1 << "x" << cols1 << std::endl
00245 << "\tMatrix 2 dimentions:\t" << rows2 << "x" << cols2 << std::endl;
00246 exit(1);
00247 }
00248
00249 QVMatrix result(rows1, cols2);
00250
00252 for (int i = 0; i < cols1; i++)
00253 for (int j = 0; j < cols1; j++)
00254 result(i,j) = this->operator()(i,j) * matrix(i,j);
00255
00256 return result;
00257 }
00258
00259 QVMatrix QVMatrix::matrixDivide(const QVMatrix &matrix) const
00260 {
00261 Q_ASSERT(matrix.getCols() >= matrix.getRows());
00262 Q_ASSERT(matrix.getCols() == (*this).getCols());
00263
00264 QVMatrix result(matrix.getRows(), (*this).getRows());
00265
00266 if (matrix.getCols() == matrix.getRows())
00267 solveLinear(matrix.transpose(), result, (*this).transpose());
00268 else
00269 solveOverDetermined(matrix.transpose(), result, (*this).transpose());
00270
00271 return result.transpose();
00272 }
00273
00274 QVMatrix QVMatrix::inverse() const
00275 {
00277 return pseudoInverse(*this);
00278 }
00279
00280 double QVMatrix::det() const
00281 {
00282 return determinant(*this);
00283 }
00284
00285 QVMatrix QVMatrix::transpose() const
00286 {
00287 const int rows = getRows(), cols = getCols();
00288
00289 QVMatrix result(cols, rows);
00290
00291 const double *matrixData = getReadData();
00292 double *resultData = result.getWriteData();
00293
00295 for (int i = 0; i < rows; i++)
00296 for (int j = 0; j < cols; j++)
00297 resultData[j*rows+i] = matrixData[i*cols+j];
00298
00299 return result;
00300 }
00301
00302 void QVMatrix::set(const double value)
00303 {
00304 double *resultData = getWriteData();
00305 const int dataSize = getDataSize();
00306
00308 for (int i = 0; i < dataSize; i++)
00309 resultData[i] = value;
00310 }
00311
00312 QVMatrix QVMatrix::addition(const QVMatrix &matrix) const
00313 {
00314 Q_ASSERT(cols == matrix.cols || rows == matrix.rows);
00315
00316 QVMatrix result = *this;
00317
00318 const double *matrixData = matrix.getReadData();
00319 double *resultData = result.getWriteData();
00320 const int dataSize = matrix.getDataSize();
00321
00323
00324 for (int i = 0; i < dataSize; i++)
00325 resultData[i] += matrixData[i];
00326
00327 return result;
00328 }
00329
00330 QVMatrix QVMatrix::substract(const QVMatrix &matrix) const
00331 {
00332
00333 Q_ASSERT(cols == matrix.cols || rows == matrix.rows);
00334
00335 QVMatrix result = *this;
00336
00337 const double *matrixData = matrix.getReadData();
00338 double *resultData = result.getWriteData();
00339 const int dataSize = matrix.getDataSize();
00340
00342
00343 for (int i = 0; i < dataSize; i++)
00344 resultData[i] -= matrixData[i];
00345
00346 return result;
00347 }
00348
00349 QVMatrix QVMatrix::scalarProduct(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 QVMatrix QVMatrix::scalarDivide(const double value) const
00365 {
00366 QVMatrix result = *this;
00367
00368 double *resultData = result.getWriteData();
00369 const int dataSize = getDataSize();
00370
00372
00373 for (int i = 0; i < dataSize; i++)
00374 resultData[i] /= value;
00375
00376 return result;
00377 }
00378
00379 QVMatrix QVMatrix::horizontalAppend(const QVMatrix &matrix) const
00380 {
00381 const int cols1 = cols, rows1 = rows, cols2 = matrix.cols, rows2 = matrix.rows;
00382
00383 Q_ASSERT(rows1 == rows2);
00384
00385 if (rows1 != rows2)
00386 {
00387 std::cout << "ERROR: tried to append horizontally matrices with different row number at QVMatrix::horizontalAppend(const QVMatrix &matrix)."
00388 << std::endl
00389 << "\tMatrix 1 dimentions:\t" << rows1 << "x" << cols1 << std::endl
00390 << "\tMatrix 2 dimentions:\t" << rows2 << "x" << cols2 << std::endl;
00391 exit(1);
00392 }
00393
00394 QVMatrix result(rows1, cols1 + cols2);
00395
00396 for (int i = 0; i < cols1; i++)
00397 result.setCol(i, getCol(i));
00398
00399 for (int i = 0; i < cols2; i++)
00400 result.setCol(cols1 + i, matrix.getCol(i));
00401
00402 return result;
00403 }
00404
00405 QVMatrix QVMatrix::verticalAppend(const QVMatrix &matrix) const
00406 {
00407 const int cols1 = cols, rows1 = rows, cols2 = matrix.cols, rows2 = matrix.rows;
00408
00409 Q_ASSERT(cols1 == cols2);
00410
00411 if (cols1 != cols2)
00412 {
00413 std::cout << "ERROR: tried to append vertically matrices with different row number at QVMatrix::horizontalAppend(const QVMatrix &matrix)."
00414 << std::endl
00415 << "\tMatrix 1 dimentions:\t" << rows1 << "x" << cols1 << std::endl
00416 << "\tMatrix 2 dimentions:\t" << rows2 << "x" << cols2 << std::endl;
00417 exit(1);
00418 }
00419
00420 QVMatrix result(rows1 + rows2, cols2);
00421
00422 for (int i = 0; i < rows1; i++)
00423 result.setRow(i, getRow(i));
00424
00425 for (int i = 0; i < rows2; i++)
00426 result.setRow(rows1 + i, matrix.getRow(i));
00427
00428 return result;
00429 }
00430
00432
00433 double QVMatrix::norm2() const
00434 { return cblas_dnrm2(getDataSize(), getReadData(), 1); };
00435
00436 double QVMatrix::trace() const
00437 {
00438 const int n = MIN(getCols(), getRows());
00439 double accum = 0.0;
00440 for (int i = 0; i < n; i++)
00441 accum += operator()(i,i);
00442 return accum;
00443 }
00444
00445
00446 QVMatrix QVMatrix::rowHomogeneousNormalize() const
00447 {
00448 const int cols = getCols(), rows = getRows();
00449 QVMatrix result(rows, cols);
00450
00451 for (int i = 0; i < rows; i++)
00452 for (int j = 0; j < cols; j++)
00453 result(i,j) = operator()(i,j) / operator()(i,cols-1);
00454 return result;
00455 }
00456
00457 const QVVector QVMatrix::getRow(const int row) const
00458 {
00459 Q_ASSERT(row < getRows());
00460
00461 const int cols = getCols();
00462 QVVector result(cols);
00463 for (int col= 0; col < cols; col++)
00464 result[col] = operator()(row,col);
00465
00466 return result;
00467 }
00468
00469 void QVMatrix::setRow(const int row, QVVector vector)
00470 {
00471 Q_ASSERT(row < getRows());
00472 Q_ASSERT(getCols() == vector.size());
00473
00474 const int cols = getCols();
00475 for (int col= 0; col < cols; col++)
00476 operator()(row,col) = vector[col];
00477 }
00478
00479 void QVMatrix::setRow(const int row, QVector<double> vector)
00480 {
00481 Q_ASSERT(row < getRows());
00482 Q_ASSERT(getCols() == vector.size());
00483
00484 const int cols = getCols();
00485 for (int col= 0; col < cols; col++)
00486 operator()(row,col) = vector[col];
00487 }
00488
00489 const QVVector QVMatrix::getCol(const int col) const
00490 {
00491 Q_ASSERT(col < getCols());
00492
00493 const int rows = getRows();
00494 QVVector result(rows);
00495 for (int row= 0; row < rows; row++)
00496 result[row] = operator()(row,col);
00497
00498 return result;
00499 }
00500
00501 void QVMatrix::setCol(const int col, QVVector vector)
00502 {
00503 Q_ASSERT(col < getCols());
00504 Q_ASSERT(getRows() == vector.size());
00505
00506 const int rows = getRows();
00507 for (int row= 0; row < rows; row++)
00508 operator()(row,col) = vector[row];
00509 }
00510
00511 const QVMatrix QVMatrix::getSubmatrix(const int firstRow, const int lastRow, const int firstCol, const int lastCol) const
00512 {
00513 Q_ASSERT(firstRow >= 0);
00514 Q_ASSERT(firstCol >= 0);
00515 Q_ASSERT(firstRow <= lastRow);
00516 Q_ASSERT(firstCol <= lastCol);
00517 Q_ASSERT(lastRow < getRows());
00518 Q_ASSERT(lastCol < getCols());
00519
00520 QVMatrix result(lastRow - firstRow +1, lastCol - firstCol +1);
00521
00522 for (int row = firstRow; row <= lastRow; row++)
00523 result.setRow(row-firstRow, getRow(row).mid(firstCol, lastCol - firstCol +1));
00524
00525 return result;
00526 }
00527
00529
00530
00531 QVMatrix QVMatrix::identity(const int size)
00532 {
00533 QVMatrix result(size, size);
00534 result.set(0);
00535 for (int i= 0; i < size; i++)
00536 result(i,i) = 1;
00537 return result;
00538 }
00539
00540 QVMatrix QVMatrix::zeros(const int rows, const int cols)
00541 {
00542 QVMatrix result(rows, cols);
00543 result.set(0);
00544 return result;
00545 }
00546
00547 QVMatrix QVMatrix::random(const int rows, const int cols)
00548 {
00549 QVMatrix result(rows, cols);
00550 for (int col = 0; col < cols; col++)
00551 for (int row = 0; row < rows; row++)
00552 result(row,col) = (double)ABS(rand()) / (double)std::numeric_limits<int>::max();
00553 return result;
00554 }
00555
00556 QVMatrix QVMatrix::diagonal(const QVVector &diagonalVector)
00557 {
00558 const int size = diagonalVector.size();
00559 QVMatrix result(size, size);
00560 result.set(0);
00561 for (int i= 0; i < size; i++)
00562 result(i,i) = diagonalVector[i];
00563 return result;
00564 }
00565
00566 QVMatrix QVMatrix::rotationMatrix(const double delta)
00567 {
00568
00569 QVMatrix result = QVMatrix::identity(3);
00570
00571 result(0,0) = cos(delta); result(0,1) = sin(delta);
00572 result(1,0) = -sin(delta); result(1,1) = cos(delta);
00573
00574 return result;
00575 }
00576
00577 QVMatrix QVMatrix::rotationMatrix(const QPointF center, const double angle)
00578 {
00579 return QVMatrix::translationMatrix(center.x(), center.y()) * QVMatrix::rotationMatrix(angle) *
00580 QVMatrix::translationMatrix(-center.x(), -center.y());
00581 }
00582
00583 QVMatrix QVMatrix::translationMatrix(const double x, const double y)
00584
00585 {
00586 QVMatrix result = QVMatrix::identity(3);
00587
00588 result(0,2) = x;
00589 result(1,2) = y;
00590
00591 return result;
00592 }
00593
00594 QVMatrix QVMatrix::scaleMatrix(const double zoom)
00595 {
00596 QVMatrix result = QVMatrix::identity(3);
00597
00598 result(0,0) = zoom;
00599 result(1,1) = zoom;
00600
00601 return result;
00602 }
00603
00604 QVMatrix QVMatrix::rotationMatrix3dZAxis(const double angle)
00605 {
00606 QVMatrix result = identity(4);
00607
00608 result(0,0) = cos(angle);
00609 result(0,1) = sin(angle);
00610
00611 result(1,0) = -sin(angle);
00612 result(1,1) = cos(angle);
00613
00614 return result;
00615 }
00616
00617 QVMatrix QVMatrix::rotationMatrix3dXAxis(const double angle)
00618 {
00619 QVMatrix result = identity(4);
00620
00621 result(1,1) = cos(angle);
00622 result(1,2) = sin(angle);
00623
00624 result(2,1) = -sin(angle);
00625 result(2,2) = cos(angle);
00626
00627 return result;
00628 }
00629
00630 QVMatrix QVMatrix::rotationMatrix3dYAxis(const double angle)
00631 {
00632 QVMatrix result = identity(4);
00633
00634 result(0,0) = cos(angle);
00635 result(0,2) = -sin(angle);
00636
00637 result(2,0) = sin(angle);
00638 result(2,2) = cos(angle);
00639
00640 return result;
00641 }
00642
00643 QVMatrix QVMatrix::translationMatrix3d(const double x, const double y, const double z)
00644 {
00645 QVMatrix result = identity(4);
00646
00647 result(0,3) = x;
00648 result(1,3) = y;
00649 result(2,3) = z;
00650
00651 return result;
00652 }
00653
00654 QVVector QVMatrix::meanCol() const
00655 {
00656 QVVector result = getCol(0);
00657 for (int i = 1; i < getCols(); i++)
00658 result += getCol(i);
00659 return result / getCols();
00660 }
00661
00663
00664 std::ostream& operator << ( std::ostream &os, const QVMatrix &matrix )
00665 {
00666 const int cols = matrix.getCols(), rows = matrix.getRows();
00667
00668 os << "QVMatrix (" << rows << ", " << cols << ")" << std::endl;
00669
00670 const double *data = matrix.getReadData();
00671 os << "[" << std::endl;
00672 for (int i=0; i < rows; i++)
00673 {
00674 os << " [ ";
00675 for (int j = 0; j < cols; j++)
00676 os << qPrintable(QString("%1").arg(data[i*cols + j], -8, 'f', 6)) << " ";
00677 os << "]" << std::endl;
00678 }
00679 os << "]" << std::endl;
00680 return os;
00681 }
00682
00683 std::istream& operator >> ( std::istream &is, QVMatrix &matrix )
00684 {
00685 matrix = QVMatrix();
00686
00687 int cols, rows;
00688 double value;
00689
00690 is.ignore(256, '(');
00691 if (is.eof())
00692 return is;
00693
00694 is >> rows;
00695
00696 is.ignore(256, ',');
00697 if (is.eof())
00698 return is;
00699
00700 is >> cols;
00701
00702 is.ignore(256, '[');
00703 if (is.eof())
00704 return is;
00705
00706 QVMatrix maux(rows, cols);
00707
00708 for (int i = 0; i < rows; i++)
00709 {
00710 is.ignore(256, '[');
00711 if (is.eof())
00712 return is;
00713
00714 for (int j = 0; j < cols; j++)
00715 {
00716 is >> value;
00717 maux(i, j) = value;
00718 }
00719 }
00720
00721 matrix = maux;
00722
00723 return is;
00724 }
00725
00726
00727 uint qHash(const QVMatrix &matrix)
00728 {
00729 const int cols = matrix.getCols(), rows = matrix.getRows();
00730
00731 double accum = 0;
00732 for (int i = 0; i < cols; i++)
00733 for (int j = 0; j < rows; j++)
00734 accum += matrix(i,j) / matrix(cols-i-1, rows-j-1);
00735
00736 return (uint) ((100000 * accum) / ((double) (cols + rows)));
00737 }
00738
00739
00740 #include <iostream>
00741 #include <fstream>
00742
00743 bool writeQVMatrixToFile(const QString fileName, const QVMatrix &matrix)
00744 {
00745 std::ofstream stream;
00746 stream.open(qPrintable(fileName));
00747
00748 if ( stream.fail() )
00749 return false;
00750
00751 stream << matrix;
00752 stream.close();
00753 return true;
00754 }
00755
00756 bool readQVMatrixFromFile(const QString fileName, QVMatrix &matrix)
00757 {
00758 std::ifstream stream;
00759 stream.open(qPrintable(fileName));
00760
00761 if ( stream.fail() )
00762 return false;
00763
00764 stream >> matrix;
00765 stream.close();
00766 return true;
00767 }
00768