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 <cstddef>
00037 # include <opencv/cv.h>
00038 #endif
00039
00040 namespace fawkes
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
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
00117
00118
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
00136
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
00151 Matrix::~Matrix()
00152 {
00153 if (m_own_memory) free(m_data);
00154 }
00155
00156
00157
00158
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
00169
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
00186
00187
00188
00189
00190
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
00199
00200
00201
00202
00203
00204
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
00230
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)
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
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
00291
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
00310
00311
00312
00313
00314
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
00331 for (unsigned int col = 0; col < m_num_cols; ++col)
00332 {
00333
00334
00335 float factor = 1.f / data(col, col);
00336 i.mult_row(col, factor);
00337 this->mult_row(col, factor);
00338
00339
00340
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
00359
00360
00361 Matrix
00362 Matrix::get_inverse() const
00363 {
00364 Matrix res(*this);
00365 res.invert();
00366
00367 return res;
00368 }
00369
00370
00371
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
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
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
00408
00409
00410
00411
00412
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
00438
00439
00440
00441
00442
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
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
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
00483
00484
00485
00486
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
00501
00502
00503
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
00529
00530
00531
00532
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
00568
00569
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
00623
00624
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
00655
00656
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
00673
00674
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
00688
00689
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
00706
00707
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
00721
00722
00723
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
00747
00748
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
00770
00771
00772
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
00797
00798
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
00820
00821
00822
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
00843
00844
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
00861
00862
00863
00864
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
00885
00886
00887
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 }