Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * matrix.cpp - A matrix class 00004 * 00005 * Created: Wed Sep 26 13:54:12 2007 00006 * Copyright 2007-2009 Daniel Beck <beck@kbsg.rwth-aachen.de> 00007 * 2009 Masrur Doostdar <doostdar@kbsg.rwth-aachen.de> 00008 * 2009 Christof Rath <c.rath@student.tugraz.at> 00009 * 00010 ****************************************************************************/ 00011 00012 /* This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU General Public License as published by 00014 * the Free Software Foundation; either version 2 of the License, or 00015 * (at your option) any later version. A runtime exception applies to 00016 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00017 * 00018 * This program is distributed in the hope that it will be useful, 00019 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00020 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00021 * GNU Library General Public License for more details. 00022 * 00023 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00024 */ 00025 00026 #include "matrix.h" 00027 #include "vector.h" 00028 00029 #include <core/exceptions/software.h> 00030 00031 #include <cstdlib> 00032 #include <cstdio> 00033 #include <algorithm> 00034 00035 #ifdef HAVE_OPENCV 00036 # include <cstddef> 00037 # include <opencv/cv.h> 00038 #endif 00039 00040 namespace fawkes 00041 { 00042 00043 /** @class Matrix <geometry/matrix.h> 00044 * A general matrix class. 00045 * It provides all the operations that are commonly used with a matrix, but has 00046 * been optimized with typical robotic applications in mind. That meas especially 00047 * that the chose data type is single-precision float and the class has been 00048 * optimized for small matrices (up to about 10x10). 00049 * @author Daniel Beck 00050 * @author Masrur Doostdar 00051 * @author Christof Rath 00052 */ 00053 00054 /** @fn inline unsigned int Matrix::num_rows() const 00055 * Return the number of rows in the Matrix 00056 * @return the number of rows 00057 */ 00058 00059 /** @fn inline unsigned int Matrix::num_cols() const 00060 * Return the number of columns in the Matrix 00061 * @return the number of columns 00062 */ 00063 00064 /** @fn inline float* Matrix::get_data() 00065 * Returns the data pointer 00066 * @return the data pointer 00067 */ 00068 00069 /** @fn inline const float* Matrix::get_data() const 00070 * Returns the const data pointer 00071 * @return the data pointer 00072 */ 00073 00074 /** @fn float Matrix::data(unsigned int row, unsigned int col) const 00075 * (Read-only) Access to matrix data without index check. 00076 * With this operator it is possible to access a specific 00077 * element of the matrix. 00078 * Make sure the indices are correct, there is no sanity 00079 * check! 00080 * @param row the row of the element 00081 * @param col the column of the element 00082 * @return the value of the specified element 00083 */ 00084 00085 /** @fn float& Matrix::data(unsigned int row, unsigned int col) 00086 * (RW) Access to matrix data without index check. 00087 * see the read-only access operator for operational details 00088 * Make sure the indizes are correct, there is no sanity 00089 * check! 00090 * @param row the row of the element 00091 * @param col the column of the element 00092 * @return a reference to the specified element 00093 */ 00094 00095 /** Constructor. 00096 * @param num_rows number of rows 00097 * @param num_cols number of columns 00098 * @param data array containing elements of the matrix in row-by-row-order 00099 * @param manage_own_memory if true, the Matrix will manage its memory on its own, else it 00100 * will not allocate new memory but works with the provided array 00101 */ 00102 Matrix::Matrix(unsigned int num_rows, unsigned int num_cols, 00103 float *data, bool manage_own_memory) 00104 { 00105 m_num_rows = num_rows; 00106 m_num_cols = num_cols; 00107 m_num_elements = m_num_rows * m_num_cols; 00108 00109 if (!m_num_elements) printf("WTF?\n"); 00110 00111 if (data == NULL || manage_own_memory) 00112 { 00113 m_data = (float*) malloc(sizeof(float) * m_num_elements); 00114 m_own_memory = true; 00115 00116 /* It showed that for arrays up to approx. 1000 elements an optimized for-loop 00117 * is faster than a memcpy() call. It is assumed that the same is true for 00118 * memset(). */ 00119 if (data != NULL) 00120 { 00121 for (unsigned int i = 0; i < m_num_elements; ++i) m_data[i] = data[i]; 00122 } 00123 else 00124 { 00125 for (unsigned int i = 0; i < m_num_elements; ++i) m_data[i] = 0.f; 00126 } 00127 } 00128 else 00129 { 00130 m_data = data; 00131 m_own_memory = false; 00132 } 00133 } 00134 00135 /** Copy-constructor. 00136 * @param tbc matrix to be copied 00137 */ 00138 Matrix::Matrix(const Matrix &tbc) 00139 { 00140 m_num_rows = tbc.m_num_rows; 00141 m_num_cols = tbc.m_num_cols; 00142 m_num_elements = tbc.m_num_elements; 00143 00144 m_own_memory = true; 00145 00146 m_data = (float*) malloc(sizeof(float) * m_num_elements); 00147 for (unsigned int i = 0; i < m_num_elements; ++i) m_data[i] = tbc.m_data[i]; 00148 } 00149 00150 /** Destructor. */ 00151 Matrix::~Matrix() 00152 { 00153 if (m_own_memory) free(m_data); 00154 } 00155 00156 /** Determines the dimensions of the matrix. 00157 * @param num_cols pointer to an unsigned int to where the number of columns is copied to 00158 * @param num_rows pointer to an unsigned int to where the number of rows is copied to 00159 */ 00160 void 00161 Matrix::size(unsigned int &num_rows, unsigned int &num_cols) const 00162 { 00163 num_rows = this->num_rows(); 00164 num_cols = this->num_cols(); 00165 } 00166 00167 00168 /** Sets the diagonal elements to 1.0 and all other to 0.0. 00169 * @return a reference to the matrix object 00170 */ 00171 Matrix & 00172 Matrix::id() 00173 { 00174 for (unsigned int row = 0; row < num_rows(); ++row) 00175 { 00176 for (unsigned int col = 0; col < num_cols(); ++col) 00177 { 00178 data(row, col) = (row == col) ? 1.0 : 0.0; 00179 } 00180 } 00181 00182 return *this; 00183 } 00184 00185 /** Creates a quadratic matrix with dimension size and sets the diagonal elements to 1.0. 00186 * All other elements are set to 0.0. 00187 * @param size dimension of the matrix 00188 * @param data_buffer if != NULL the given float array will be used as data internal data store 00189 * (the object will not perform any memory management in this case) 00190 * @return the id matrix object 00191 */ 00192 Matrix 00193 Matrix::get_id(unsigned int size, float *data_buffer) 00194 { 00195 return get_diag(size, 1.f, data_buffer); 00196 } 00197 00198 /** Creates a quadratic matrix with dimension size and sets the diagonal elements to value. 00199 * All other elements are set to 0.0. 00200 * @param size dimension of the matrix 00201 * @param value of the elements of the main diagonal 00202 * @param data_buffer if != NULL the given float array will be used as data internal data store 00203 * (the object will not perform any memory management in this case) 00204 * @return the diag matrix object 00205 */ 00206 Matrix 00207 Matrix::get_diag(unsigned int size, float value, float *data_buffer) 00208 { 00209 Matrix res(size, size, data_buffer, data_buffer == NULL); 00210 00211 if (data_buffer != NULL) 00212 { 00213 unsigned int diag_elem = 0; 00214 for (unsigned int i = 0; i < size * size; ++i) 00215 { 00216 if (i == diag_elem) 00217 { 00218 diag_elem += size + 1; 00219 data_buffer[i] = value; 00220 } 00221 else data_buffer[i] = 0.f; 00222 } 00223 } 00224 else for (unsigned int i = 0; i < size; ++i) res.data(i, i) = value; 00225 00226 return res; 00227 } 00228 00229 /** Transposes the matrix. 00230 * @return a reference to the matrix object now containing the transposed matrix 00231 */ 00232 Matrix & 00233 Matrix::transpose() 00234 { 00235 #ifdef HAVE_OPENCV 00236 if (m_num_cols == m_num_rows) 00237 { 00238 CvMat cvmat = cvMat(m_num_rows, m_num_cols, CV_32FC1, m_data); 00239 cvTranspose(&cvmat, &cvmat); 00240 00241 return *this; 00242 } 00243 #endif 00244 if (m_num_cols == m_num_rows) // Perform a in-place transpose 00245 { 00246 for (unsigned int row = 0; row < m_num_rows - 1; ++row) 00247 { 00248 for (unsigned int col = row + 1; col < m_num_cols; ++col) 00249 { 00250 float &a = data(row, col); 00251 float &b = data(col, row); 00252 float t = a; 00253 a = b; 00254 b = t; 00255 } 00256 } 00257 } 00258 else // Could not find a in-place transpose, so we use a temporary data array 00259 { 00260 float *new_data = (float*) malloc(sizeof(float) * m_num_elements); 00261 float *cur = new_data; 00262 00263 for (unsigned int col = 0; col < m_num_cols; ++col) 00264 { 00265 for (unsigned int row = 0; row < m_num_rows; ++row) 00266 { 00267 *cur++ = data(row, col); 00268 } 00269 } 00270 00271 unsigned int cols = m_num_cols; 00272 m_num_cols = m_num_rows; 00273 m_num_rows = cols; 00274 00275 if (m_own_memory) 00276 { 00277 free(m_data); 00278 m_data = new_data; 00279 } 00280 else 00281 { 00282 for (unsigned int i = 0; i < m_num_elements; ++i) m_data[i] = new_data[i]; 00283 free(new_data); 00284 } 00285 } 00286 00287 return *this; 00288 } 00289 00290 /** Computes a matrix that is the transposed of this matrix. 00291 * @return a matrix that is the transposed of this matrix 00292 */ 00293 Matrix 00294 Matrix::get_transpose() const 00295 { 00296 Matrix res(m_num_cols, m_num_rows); 00297 float *cur = res.get_data(); 00298 00299 for (unsigned int col = 0; col < m_num_cols; ++col) 00300 { 00301 for (unsigned int row = 0; row < m_num_rows; ++row) 00302 { 00303 *cur++ = data(row, col); 00304 } 00305 } 00306 return res; 00307 } 00308 00309 /** Inverts the matrix. 00310 * The algorithm that is implemented for computing the inverse 00311 * of the matrix is the Gauss-Jordan-Algorithm. Hereby, the block- 00312 * matrix (A|I) consisting of the matrix to be inverted (A) and the 00313 * identity matrix (I) is transformed into (I|A^(-1)). 00314 * @return a reference to the matrix object which contains now the inverted matrix 00315 */ 00316 Matrix & 00317 Matrix::invert() 00318 { 00319 if (m_num_rows != m_num_cols) 00320 { 00321 throw fawkes::Exception("Matrix::invert(): Trying to compute inverse of non-quadratic matrix!"); 00322 } 00323 00324 #ifdef HAVE_OPENCV 00325 CvMat cvmat = cvMat(m_num_rows, m_num_cols, CV_32FC1, m_data); 00326 cvInv(&cvmat, &cvmat, CV_LU); 00327 #else 00328 Matrix i = Matrix::get_id(m_num_rows); 00329 00330 // for each column... 00331 for (unsigned int col = 0; col < m_num_cols; ++col) 00332 { 00333 // ...multiply the row by the inverse of the element 00334 // on the diagonal... 00335 float factor = 1.f / data(col, col); 00336 i.mult_row(col, factor); 00337 this->mult_row(col, factor); 00338 00339 // ...and subtract that row multiplied by the elements 00340 // in the current column from all other rows. 00341 for (unsigned int row = 0; row < m_num_rows; ++row) 00342 { 00343 if (row != col) 00344 { 00345 float factor2 = data(row, col); 00346 i.sub_row(row, col, factor2); 00347 this->sub_row(row, col, factor2); 00348 } 00349 } 00350 } 00351 00352 overlay(0, 0, i); 00353 #endif 00354 00355 return *this; 00356 } 00357 00358 /** Computes a matrix that is the inverse of this matrix. 00359 * @return a matrix that is the inverse of this matrix 00360 */ 00361 Matrix 00362 Matrix::get_inverse() const 00363 { 00364 Matrix res(*this); 00365 res.invert(); 00366 00367 return res; 00368 } 00369 00370 /** Computes the determinant of the matrix. 00371 * @return the determinant 00372 */ 00373 float 00374 Matrix::det() const 00375 { 00376 if (m_num_rows != m_num_cols) 00377 { 00378 throw fawkes::Exception("Matrix::det(): The determinant can only be calculated for quadratic matrices."); 00379 } 00380 00381 #ifdef HAVE_OPENCV 00382 CvMat cvmat = cvMat(m_num_rows, m_num_cols, CV_32FC1, m_data); 00383 00384 return (float)cvDet(&cvmat); 00385 #else 00386 Matrix tmp_matrix(*this); 00387 float result = 1.f; 00388 00389 // compute the upper triangular matrix 00390 for (unsigned int col = 0; col < m_num_cols; ++col) 00391 { 00392 float diag_elem = tmp_matrix.data(col, col); 00393 result *= diag_elem; 00394 00395 // multiply n-th row by m(n,n)^{-1} 00396 tmp_matrix.mult_row(col, (1.f / diag_elem)); 00397 for (unsigned int row = col + 1; row < m_num_rows; ++row) 00398 { 00399 tmp_matrix.sub_row(row, col, tmp_matrix.data(row, col)); 00400 } 00401 } 00402 00403 return result; 00404 #endif 00405 } 00406 00407 /** Returns a submatrix of the matrix. 00408 * @param row the row in the original matrix of the top-left element in the submatrix 00409 * @param col the column in the original matrix of the top-left element in the submatrix 00410 * @param num_rows the number of rows of the submatrix 00411 * @param num_cols the number of columns of the submatrix 00412 * @return the submatrix 00413 */ 00414 Matrix 00415 Matrix::get_submatrix(unsigned int row, unsigned int col, 00416 unsigned int num_rows, unsigned int num_cols) const 00417 { 00418 if ((m_num_rows < row + num_rows) || (m_num_cols < col + num_cols)) 00419 { 00420 throw fawkes::OutOfBoundsException("Matrix::get_submatrix(): The current matrix doesn't contain a submatrix of the requested dimension at the requested position."); 00421 } 00422 00423 Matrix res(num_rows, num_cols); 00424 float *res_data = res.get_data(); 00425 00426 for (unsigned int r = 0; r < num_rows; ++r) 00427 { 00428 for (unsigned int c = 0; c < num_cols; ++c) 00429 { 00430 *res_data++ = data(row + r, col + c); 00431 } 00432 } 00433 00434 return res; 00435 } 00436 00437 /** Overlays another matrix over this matrix. 00438 * @param row the top-most row from which onwards the the elements are 00439 * exchanged for corresponding elements in the given matrix 00440 * @param col the left-most column from which onwards the the elements 00441 * are exchanged for corresponding elements in the given matrix 00442 * @param over the matrix to be overlaid 00443 */ 00444 void 00445 Matrix::overlay(unsigned int row, unsigned int col, const Matrix &over) 00446 { 00447 unsigned int max_row = std::min(m_num_rows, over.m_num_rows + row); 00448 unsigned int max_col = std::min(m_num_cols, over.m_num_cols + col); 00449 00450 for (unsigned int r = row; r < max_row; ++r) 00451 { 00452 for (unsigned int c = col; c < max_col; ++c) 00453 { 00454 data(r, c) = over.data(r - row, c - col); 00455 } 00456 } 00457 } 00458 00459 /** (Read-only) Access-operator. 00460 * With this operator it is possible to access a specific 00461 * element of the matrix. (First element is at (0, 0) 00462 * @param row the row of the element 00463 * @param col the column of the element 00464 * @return the value of the specified element 00465 */ 00466 /* Not True: To conform with the mathematical 00467 * fashion of specifying the elements of a matrix the top 00468 * left element of the matrix is accessed with (1, 1) 00469 * (i.e., numeration starts with 1 and not with 0). 00470 */ 00471 float 00472 Matrix::operator()(unsigned int row, unsigned int col) const 00473 { 00474 if (row >= m_num_rows || col >= m_num_cols) 00475 { 00476 throw fawkes::OutOfBoundsException("Matrix::operator() The requested element is not within the dimension of the matrix."); 00477 } 00478 00479 return data(row, col); 00480 } 00481 00482 /** (RW) Access operator. 00483 * see the read-only access operator for operational details 00484 * @param row the row of the element 00485 * @param col the column of the element 00486 * @return a reference to the specified element 00487 */ 00488 float & 00489 Matrix::operator()(unsigned int row, 00490 unsigned int col) 00491 { 00492 if (row >= m_num_rows || col >= m_num_cols) 00493 { 00494 throw fawkes::OutOfBoundsException("Matrix::operator() The requested element (%d, %d) is not within the dimension of the %dx%d matrix."); 00495 } 00496 00497 return data(row, col); 00498 } 00499 00500 /** Assignment operator. 00501 * Copies the data form the rhs Matrix to the lhs Matrix. 00502 * @param m the rhs Matrix 00503 * @return a reference to this Matrix 00504 */ 00505 Matrix & 00506 Matrix::operator=(const Matrix &m) 00507 { 00508 if (m_num_elements != m.m_num_elements) 00509 { 00510 if (!m_own_memory) 00511 { 00512 throw fawkes::OutOfBoundsException("Matrix::operator=(): The rhs matrix has not the same number of elements. This isn't possible if not self managing memory."); 00513 } 00514 00515 m_num_elements = m.m_num_elements; 00516 free(m_data); 00517 m_data = (float*) malloc(sizeof(float) * m_num_elements); 00518 } 00519 00520 m_num_rows = m.m_num_rows; 00521 m_num_cols = m.m_num_cols; 00522 00523 for (unsigned int i = 0; i < m_num_elements; ++i) m_data[i] = m.m_data[i]; 00524 00525 return *this; 00526 } 00527 00528 /** Matrix multiplication operator. 00529 * (Matrix)a.operator*((Matrix)b) computes a * b; 00530 * i.e., the 2nd matrix is right-multiplied to the 1st matrix 00531 * @param rhs the right-hand-side matrix 00532 * @return the product of the two matrices (a * b) 00533 */ 00534 Matrix 00535 Matrix::operator*(const Matrix &rhs) const 00536 { 00537 if (m_num_cols != rhs.m_num_rows) 00538 { 00539 throw fawkes::Exception("Matrix::operator*(...): Dimension mismatch: a %d x %d matrix can't be multiplied " 00540 "with a %d x %d matrix.\n", 00541 m_num_rows, m_num_cols, rhs.num_rows(), rhs.num_cols()); 00542 } 00543 00544 unsigned int res_rows = m_num_rows; 00545 unsigned int res_cols = rhs.m_num_cols; 00546 00547 Matrix res(res_rows, res_cols); 00548 00549 for (unsigned int r = 0; r < res_rows; ++r) 00550 { 00551 for (unsigned int c = 0; c < res_cols; ++c) 00552 { 00553 float t = 0.0f; 00554 00555 for (unsigned int i = 0; i < m_num_cols; ++i) 00556 { 00557 t += data(r, i) * rhs.data(i, c); 00558 } 00559 00560 res.data(r, c) = t; 00561 } 00562 } 00563 00564 return res; 00565 } 00566 00567 /** Combined matrix-multipliation and assignement operator. 00568 * @param rhs the right-hand-side Matrix 00569 * @return a reference to the Matrix that contains the result of the multiplication 00570 */ 00571 Matrix & 00572 Matrix::operator*=(const Matrix &rhs) 00573 { 00574 if (m_num_cols != rhs.m_num_rows) 00575 { 00576 throw fawkes::Exception("Matrix::operator*(...): Dimension mismatch: a %d x %d matrix can't be multiplied " 00577 "with a %d x %d matrix.\n", 00578 m_num_rows, m_num_cols, rhs.num_rows(), rhs.num_cols()); 00579 } 00580 00581 unsigned int res_rows = m_num_rows; 00582 unsigned int res_cols = rhs.m_num_cols; 00583 unsigned int res_num_elem = res_rows * res_cols; 00584 00585 if (!m_own_memory && (m_num_elements != res_num_elem)) 00586 { 00587 throw fawkes::Exception("Matrix::operator*=(): The resulting matrix has not the same number of elements. This doesn't work if not self managing memory."); 00588 } 00589 00590 float *new_data = (float*) malloc(sizeof(float) * res_num_elem); 00591 float *cur = new_data; 00592 00593 for (unsigned int r = 0; r < res_rows; ++r) 00594 { 00595 for (unsigned int c = 0; c < res_cols; ++c) 00596 { 00597 float t = 0.0f; 00598 00599 for (unsigned int i = 0; i < m_num_cols; ++i) 00600 { 00601 t += data(r, i) * rhs.data(i, c); 00602 } 00603 00604 *cur++ = t; 00605 } 00606 } 00607 00608 if (m_own_memory) 00609 { 00610 free(m_data); 00611 m_data = new_data; 00612 } 00613 else 00614 { 00615 for (unsigned int i = 0; i < m_num_elements; ++i) m_data[i] = new_data[i]; 00616 free(new_data); 00617 } 00618 00619 return *this; 00620 } 00621 00622 /** Multiply the matrix with given vector. 00623 * @param v a vector 00624 * @return the result of the matrix-vector multiplication 00625 */ 00626 Vector 00627 Matrix::operator*(const Vector &v) const 00628 { 00629 unsigned int cols = v.size(); 00630 00631 if (m_num_cols != cols) 00632 { 00633 throw fawkes::Exception("Matrix::operator*(...): Dimension mismatch: a %d x %d matrix can't be multiplied " 00634 "with a vector of length %d.\n", num_rows(), num_cols(), cols); 00635 } 00636 00637 Vector res(m_num_rows); 00638 const float *vector_data = v.data_ptr(); 00639 00640 for (unsigned int r = 0; r < num_rows(); ++r) 00641 { 00642 float row_result = 0.f; 00643 00644 for (unsigned int c = 0; c < cols; ++c) 00645 { 00646 row_result += data(r, c) * vector_data[c]; 00647 } 00648 res[r] = row_result; 00649 } 00650 00651 return res; 00652 } 00653 00654 /** Mulitply every element of the matrix with the given scalar. 00655 * @param f a scalar 00656 * @return the result of the multiplication 00657 */ 00658 Matrix 00659 Matrix::operator*(const float &f) const 00660 { 00661 Matrix res(*this); 00662 float *data = res.get_data(); 00663 00664 for (unsigned int i = 0; i < res.m_num_elements; ++i) 00665 { 00666 data[i] *= f; 00667 } 00668 00669 return res; 00670 } 00671 00672 /** Combined scalar multiplication and assignment operator. 00673 * @param f a scalar 00674 * @return reference to the result 00675 */ 00676 Matrix & 00677 Matrix::operator*=(const float &f) 00678 { 00679 for (unsigned int i = 0; i < m_num_elements; ++i) 00680 { 00681 m_data[i] *= f; 00682 } 00683 00684 return *this; 00685 } 00686 00687 /** Divide every element of the matrix with the given scalar. 00688 * @param f a scalar 00689 * @return the result of the divison 00690 */ 00691 Matrix 00692 Matrix::operator/(const float &f) const 00693 { 00694 Matrix res(*this); 00695 float *data = res.get_data(); 00696 00697 for (unsigned int i = 0; i < res.m_num_elements; ++i) 00698 { 00699 data[i] /= f; 00700 } 00701 00702 return res; 00703 } 00704 00705 /** Combined scalar division and assignment operator. 00706 * @param f a scalar 00707 * @return reference to the result 00708 */ 00709 Matrix & 00710 Matrix::operator/=(const float &f) 00711 { 00712 for (unsigned int i = 0; i < m_num_elements; ++i) 00713 { 00714 m_data[i] /= f; 00715 } 00716 00717 return *this; 00718 } 00719 00720 /** Addition operator. 00721 * Adds the corresponding elements of the two matrices. 00722 * @param rhs the right-hand-side matrix 00723 * @return the resulting matrix 00724 */ 00725 Matrix 00726 Matrix::operator+(const Matrix &rhs) const 00727 { 00728 if ((m_num_rows != rhs.m_num_rows) || (m_num_cols != rhs.m_num_cols)) 00729 { 00730 throw fawkes::Exception("Matrix::operator+(...): Dimension mismatch: a %d x %d matrix can't be added to a %d x %d matrix\n", 00731 num_rows(), num_cols(), rhs.num_rows(), rhs.num_cols()); 00732 } 00733 00734 Matrix res(*this); 00735 const float *rhs_d = rhs.get_data(); 00736 float *res_d = res.get_data(); 00737 00738 for (unsigned int i = 0; i < m_num_elements; ++i) 00739 { 00740 res_d[i] += rhs_d[i]; 00741 } 00742 00743 return res; 00744 } 00745 00746 /**Add-assign operator. 00747 * @param rhs the right-hand-side matrix 00748 * @return a reference to the resulting matrix (this) 00749 */ 00750 Matrix & 00751 Matrix::operator+=(const Matrix &rhs) 00752 { 00753 if ((m_num_rows != rhs.m_num_rows) || (m_num_cols != rhs.m_num_cols)) 00754 { 00755 throw fawkes::Exception("Matrix::operator+(...): Dimension mismatch: a %d x %d matrix can't be added to a %d x %d matrix\n", 00756 num_rows(), num_cols(), rhs.num_rows(), rhs.num_cols()); 00757 } 00758 00759 const float *rhs_d = rhs.get_data(); 00760 00761 for (unsigned int i = 0; i < m_num_elements; ++i) 00762 { 00763 m_data[i] += rhs_d[i]; 00764 } 00765 00766 return *this; 00767 } 00768 00769 /** Subtraction operator. 00770 * Subtracts the corresponding elements of the two matrices. 00771 * @param rhs the right-hand-side matrix 00772 * @return the resulting matrix 00773 */ 00774 Matrix 00775 Matrix::operator-(const Matrix &rhs) const 00776 { 00777 if ((num_rows() != rhs.num_rows()) || (num_cols() != rhs.num_cols())) 00778 { 00779 throw fawkes::Exception("Matrix::operator-(...): Dimension mismatch: a %d x %d matrix can't be subtracted from a %d x %d matrix\n", 00780 num_rows(), num_cols(), rhs.num_rows(), rhs.num_cols()); 00781 } 00782 00783 Matrix res(*this); 00784 00785 const float *rhs_d = rhs.get_data(); 00786 float *res_d = res.get_data(); 00787 00788 for (unsigned int i = 0; i < m_num_elements; ++i) 00789 { 00790 res_d[i] -= rhs_d[i]; 00791 } 00792 00793 return res; 00794 } 00795 00796 /**Subtract-assign operator. 00797 * @param rhs the right-hand-side matrix 00798 * @return a reference to the resulting matrix (this) 00799 */ 00800 Matrix & 00801 Matrix::operator-=(const Matrix &rhs) 00802 { 00803 if ((num_rows() != rhs.num_rows()) || (num_cols() != rhs.num_cols())) 00804 { 00805 throw fawkes::Exception("Matrix::operator-=(...): Dimension mismatch: a %d x %d matrix can't be subtracted from a %d x %d matrix\n", 00806 num_rows(), num_cols(), rhs.num_rows(), rhs.num_cols()); 00807 } 00808 00809 const float *rhs_d = rhs.get_data(); 00810 00811 for (unsigned int i = 0; i < m_num_elements; ++i) 00812 { 00813 m_data[i] -= rhs_d[i]; 00814 } 00815 00816 return *this; 00817 } 00818 00819 /** Comparison operator. 00820 * @param rhs the right-hand-side Matrix 00821 * @return true if every element of this matrix is equal to the 00822 * corresponding element of the other matrix 00823 */ 00824 bool 00825 Matrix::operator==(const Matrix &rhs) const 00826 { 00827 if ((num_rows() != rhs.num_rows()) || (num_cols() != rhs.num_cols())) 00828 { 00829 return false; 00830 } 00831 00832 const float *rhs_d = rhs.get_data(); 00833 00834 for (unsigned int i = 0; i < m_num_elements; ++i) 00835 { 00836 if (m_data[i] != rhs_d[i]) return false; 00837 } 00838 00839 return true; 00840 } 00841 00842 /** Changes the matrix by multiplying a row with a factor. 00843 * @param row the row 00844 * @param factor the factor 00845 */ 00846 void 00847 Matrix::mult_row(unsigned int row, float factor) 00848 { 00849 if (row >= m_num_rows) 00850 { 00851 throw fawkes::OutOfBoundsException("Matrix::mult_row(...)", row, 0, m_num_rows); 00852 } 00853 00854 for (unsigned int col = 0; col < m_num_cols; ++col) 00855 { 00856 data(row, col) *= factor; 00857 } 00858 } 00859 00860 /** For two rows A and B and a factor f, A is changed to A - f*B. 00861 * @param row_a the row that is changed 00862 * @param row_b the row that is substracted from row_a 00863 * @param factor the factor by which every element of row_b is multiplied before it is 00864 * substracted from row_a 00865 */ 00866 void 00867 Matrix::sub_row(unsigned int row_a, unsigned int row_b, float factor) 00868 { 00869 if (row_a >= m_num_rows) 00870 { 00871 throw fawkes::OutOfBoundsException("Matrix::sub_row(...) row_a", row_a, 0, m_num_rows); 00872 } 00873 if (row_b >= m_num_rows) 00874 { 00875 throw fawkes::OutOfBoundsException("Matrix::sub_row(...) row_b", row_b, 0, m_num_rows); 00876 } 00877 00878 for (unsigned int col = 0; col < m_num_cols; ++col) 00879 { 00880 data(row_a, col) -= factor * data(row_b, col); 00881 } 00882 } 00883 00884 /** Print matrix to standard out. 00885 * @param name a name that is printed before the content of the matrix (not required) 00886 * @param col_sep a string used to separate columns (defaults to '\\t') 00887 * @param row_sep a string used to separate rows (defaults to '\\n') 00888 */ 00889 void 00890 Matrix::print_info(const char *name, const char *col_sep, const char *row_sep) const 00891 { 00892 if (name) 00893 { 00894 printf("%s:\n", name); 00895 } 00896 00897 for (unsigned int r = 0; r < num_rows(); ++r) 00898 { 00899 printf((r == 0 ? "[" : " ")); 00900 for (unsigned int c = 0; c < num_cols(); ++c) 00901 { 00902 printf("%f", (*this)(r, c)); 00903 if (c+1 < num_cols()) 00904 { 00905 if (col_sep) printf("%s", col_sep); 00906 else printf("\t"); 00907 } 00908 } 00909 if (r+1 < num_rows()) 00910 { 00911 if (row_sep) printf("%s", row_sep); 00912 else printf("\n"); 00913 } 00914 else printf("]\n\n"); 00915 } 00916 } 00917 00918 } // end namespace fawkes