matrix.cpp

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