00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
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
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
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
00175 if (cols != matrix.cols || rows != matrix.rows)
00176 return false;
00177
00178
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
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
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
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
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