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