00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
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
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
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
00116
00117
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
00135
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
00150 Matrix::~Matrix()
00151 {
00152 if (m_own_memory) free(m_data);
00153 }
00154
00155
00156
00157
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
00168
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
00185
00186
00187
00188
00189
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
00198
00199
00200
00201
00202
00203
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
00229
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)
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
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
00290
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
00309
00310
00311
00312
00313
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
00330 for (unsigned int col = 0; col < m_num_cols; ++col)
00331 {
00332
00333
00334 float factor = 1.f / data(col, col);
00335 i.mult_row(col, factor);
00336 this->mult_row(col, factor);
00337
00338
00339
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
00358
00359
00360 Matrix
00361 Matrix::get_inverse() const
00362 {
00363 Matrix res(*this);
00364 res.invert();
00365
00366 return res;
00367 }
00368
00369
00370
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
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
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
00407
00408
00409
00410
00411
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
00437
00438
00439
00440
00441
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
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
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
00482
00483
00484
00485
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
00500
00501
00502
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
00528
00529
00530
00531
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
00567
00568
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
00622
00623
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
00654
00655
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
00672
00673
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
00687
00688
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
00705
00706
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
00720
00721
00722
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
00746
00747
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
00769
00770
00771
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
00796
00797
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
00819
00820
00821
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
00842
00843
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
00860
00861
00862
00863
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
00884
00885
00886
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 }