A lite library for operations in linear algebra

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MATRIX_PRIMITIVE.cpp Source File

MATRIX_PRIMITIVE.cpp

00001 #include "MATRIX_PRIMITIVE.h"
00002 
00003 //
00004 // using namespace MATRIX_PRIMITIVE;
00005 //
00006 
00007 // Namespace MATRIX_PRIMITIVE
00008 //////////////////////////
00009 namespace MATRIX_PRIMITIVE
00010 {
00011 // Utilities
00012 ///////////////////////////
00013 void Mat_multiply_Vec(vector<float> &v_out, vector<vector<float> > &m_left, vector<float> &v_right){ // v_out = m_left*v_right
00014     vector<float>::iterator it_out;
00015     vector<float>::iterator it_m_row;
00016     vector<float>::iterator it_v;
00017     //
00018     it_out = v_out.begin();
00019     for (size_t i = 0; i < m_left.size(); ++i){
00020         *it_out = 0.0;
00021         it_m_row = m_left[i].begin();
00022         it_v = v_right.begin();
00023         for (size_t j = 0; j < m_left[i].size(); ++j){
00024             // *it_out += m_left[i][j] * v_right[j];
00025             if (*it_m_row != 0.0 && *it_v != 0.0){
00026                 (*it_out) += (*it_m_row) * (*it_v);
00027             }else{
00028                 // (*it_out) += 0.0
00029             }
00030             // (*it_out) += (*it_m_row) * (*it_v);
00031             //
00032             it_m_row++;
00033             it_v++;
00034         }
00035         it_out++;
00036     }
00037 }
00038 vector<float> Mat_multiply_Vec(vector<vector<float> > &m_left, vector<float> &v_right){ // v_out = m_left*v_right
00039     static vector<float> v_out;
00040     // Size check
00041     if (v_out.size() != m_left.size()){
00042         v_out.resize(m_left.size());
00043     }
00044     // Iterators
00045     vector<float>::iterator it_out;
00046     vector<float>::iterator it_m_row;
00047     vector<float>::iterator it_v;
00048     //
00049     it_out = v_out.begin();
00050     for (size_t i = 0; i < m_left.size(); ++i){
00051         *it_out = 0.0;
00052         it_m_row = m_left[i].begin();
00053         it_v = v_right.begin();
00054         for (size_t j = 0; j < m_left[i].size(); ++j){
00055             // *it_out += m_left[i][j] * v_right[j];
00056             if (*it_m_row != 0.0 && *it_v != 0.0){
00057                 (*it_out) += (*it_m_row) * (*it_v);
00058             }else{
00059                 // (*it_out) += 0.0
00060             }
00061             // (*it_out) += (*it_m_row) * (*it_v);
00062             //
00063             it_m_row++;
00064             it_v++;
00065         }
00066         it_out++;
00067     }
00068     return v_out;
00069 }
00070 
00071 // New function for matrix multiply matrix
00072 vector<vector<float> > Mat_multiply_Mat(vector<vector<float> > &m_left, vector<vector<float> > &m_right){ // m_out = m_left*m_right
00073     static vector<vector<float> > m_out;
00074 
00075     // mxn = (mxk)*(kxn)
00076     size_t M, K, N;
00077     //
00078     M = m_left.size();
00079     K = m_left[0].size();
00080     N = m_right[0].size();
00081 
00082     // Size check
00083     if (m_out.size() != M || m_out[0].size() != N ){
00084         m_out.resize(M, vector<float>( N ) );
00085     }
00086 
00087     // Using indexing
00088     for (size_t i = 0; i < M; ++i){ // row in m_out
00089         for (size_t j = 0; j < N; ++j){ // column in m_out
00090             m_out[i][j] = 0.0;
00091             for (size_t k = 0; k < K; ++k){
00092                 if (m_left[i][k] != 0.0 && m_right[k][j] != 0.0)
00093                     m_out[i][j] += m_left[i][k]*m_right[k][j];
00094             }
00095         }
00096     }
00097 
00098     return m_out;
00099 }
00100 
00101 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
00102 {
00103     static vector<float> v_c;
00104     // Size check
00105     if (v_c.size() != v_a.size()){
00106         v_c.resize(v_a.size());
00107     }
00108     //
00109     for (size_t i = 0; i < v_a.size(); ++i){
00110         if (is_minus){
00111             v_c[i] = v_a[i] - v_b[i];
00112         }else{
00113             v_c[i] = v_a[i] + v_b[i];
00114         }
00115     }
00116     return v_c;
00117 }
00118 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
00119 {
00120     static vector<float> v_c;
00121     // Size check
00122     if (v_c.size() != v_a.size()){
00123         v_c.resize(v_a.size());
00124     }
00125     // for pure negative
00126     if (scale == -1.0){
00127         for (size_t i = 0; i < v_a.size(); ++i){
00128             v_c[i] = -v_a[i];
00129         }
00130         return v_c;
00131     }
00132     // else
00133     for (size_t i = 0; i < v_a.size(); ++i){
00134         v_c[i] = scale*v_a[i];
00135 
00136     }
00137     return v_c;
00138 }
00139 
00140 // Important!
00141 // New function for scale-up a vector
00142 // v_a *= scale
00143 void Get_VectorScaleUp(vector<float> &v_a, float scale) // v_a *= scale
00144 {
00145     // for pure negative
00146     if (scale == -1.0){
00147         for (size_t i = 0; i < v_a.size(); ++i){
00148             v_a[i] = -v_a[i];
00149         }
00150         //
00151     }else{
00152         // else
00153         for (size_t i = 0; i < v_a.size(); ++i){
00154             v_a[i] *= scale;
00155         }
00156         //
00157     }
00158 }
00159 
00160 // Increment
00161 void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
00162     // Size check
00163     if (v_a.size() != v_b.size()){
00164         v_a.resize(v_b.size());
00165     }
00166     //
00167     if (is_minus){ // -=
00168         for (size_t i = 0; i < v_b.size(); ++i){
00169             v_a[i] -= v_b[i];
00170         }
00171     }else{ // +=
00172         for (size_t i = 0; i < v_b.size(); ++i){
00173             v_a[i] += v_b[i];
00174         }
00175     }
00176 
00177 }
00178 /////////////////////////// end Utilities
00179 
00180 
00181 // Matrix inversion, using Gauss method
00182 /////////////////////////////////////////
00183 bool SolveSingularityOnDiag(vector<vector<float> > &M, vector<vector<float> > &M_inv, size_t i){
00184     // Return value: is_singular
00185     //-----------------------------------//
00186     // true: the matrix is singular
00187     // false: the singularity is not yet been found or the matrix is invertible
00188     //-----------------------------------//
00189 
00190 
00191     // For ith-row
00192     //-----------------------------------//
00193     // If the ith element on diagonal is zero, find a row with it ith elemnt non-zero and add to the current row (ith row)
00194     // For both M and M_inv
00195 
00196     size_t n = M.size();
00197     // Search for other non-zero elements
00198     float max_absValue = 0.0;
00199     size_t idx_max = i;
00200 
00201     // The following block of code is wrong.
00202     /*
00203     for (size_t j = 0; j < n; ++j){
00204         if (j != i){ // Other than i
00205             float absValue = fabs(M[j][i]);
00206             if ( absValue > max_absValue){
00207                 max_absValue = absValue;
00208                 idx_max = j;
00209                 //
00210                 // break; // Once found a non-zero element, break it (Not going to find the maximum one)
00211                 //
00212             }
00213         }
00214     }
00215     */
00216 
00217     // It's sufficient and "should" only looking downward
00218     for (size_t j = (i+1); j < n; ++j){
00219         // Below ith-row
00220         float absValue = fabs(M[j][i]);
00221         if ( absValue > max_absValue){
00222             max_absValue = absValue;
00223             idx_max = j;
00224             //
00225             // break; // Once found a non-zero element, break it (Not going to find the maximum one)
00226             //
00227         }
00228         //
00229     }
00230 
00231     //
00232     if (idx_max == i){ // The matrix is singular !!
00233         return true; // is_singular
00234     }
00235 
00236     // Add that row to the current row
00237     Get_VectorIncrement(M[i], M[idx_max], false); // +=
00238     Get_VectorIncrement(M_inv[i], M_inv[idx_max], false); // +=
00239     //
00240     return false; // may be non-singular
00241 }
00242 vector<vector<float> > MatrixInversion(vector<vector<float> > M){
00243     //
00244     size_t n = M.size(); // The size of the square matrix
00245 
00246     // Check if M is a square matrix
00247     if (n != M[0].size()){
00248         return M;
00249     }
00250 
00251     // Output matrix
00252     vector<vector<float> > M_inv(n,vector<float>(n, 0.0));
00253 
00254     // Initialize the identity matrix M_inv
00255     for (size_t i = 0; i < n; ++i){
00256         M_inv[i][i] = 1.0;
00257     }
00258 
00259     //
00260     // cout << "M_inv:\n";
00261     // printMatrix(M_inv);
00262 
00263 
00264     // A row vector
00265     vector<float> row_left;
00266     vector<float> row_right;
00267     float M_diag = 1.0;
00268 
00269     /*
00270     // Check if each element on diagonal is not zero
00271     // If it is zero, find a row with a non-zero element on that column and add that row to the current row
00272     for (size_t i = 0; i < n; ++i){
00273         M_diag = M[i][i];
00274         if (M_diag == 0.0){
00275             // Search for other non-zero elements
00276             // SolveSingularityOnDiag(M, M_inv, i);
00277 
00278             // Search for other non-zero elements
00279             float max_absValue = 0.0;
00280             size_t idx_max = i;
00281             for (size_t j = 0; j < n; ++j){
00282                 if (j != i){ // Other that i
00283                     float absValue = fabs(M[j][i]);
00284                     if ( absValue > max_absValue){
00285                         max_absValue = absValue;
00286                         idx_max = j;
00287                     }
00288                 }
00289             }
00290             // Add that row to the current row
00291             Get_VectorIncrement(M[i], M[idx_max], false); // +=
00292             Get_VectorIncrement(M_inv[i], M_inv[idx_max], false); // +=
00293             //
00294 
00295         }else{
00296             // Fine! Nothing to do.
00297         }
00298     }
00299     */
00300 
00301     //
00302     /*
00303     cout << "M:\n";
00304     printMatrix(M);
00305     //
00306     cout << "M_inv:\n";
00307     printMatrix(M_inv);
00308     */
00309     //
00310 
00311     /*
00312     // Scale each row vector for normalizing each elements on the diagonal to 1.0
00313     for (size_t i = 0; i < n; ++i){
00314         float ratio;
00315         ratio = 1.0/M[i][i];
00316         //
00317         Get_VectorScaleUp(M[i], ratio);
00318         Get_VectorScaleUp(M_inv[i], ratio);
00319         //
00320         // M[i] = Get_VectorScalarMultiply(M[i], ratio);
00321         // M_inv[i] = Get_VectorScalarMultiply(M_inv[i], ratio);
00322         //
00323     }
00324     */
00325 
00326     /*
00327     //
00328     cout << "M:\n";
00329     printMatrix(M);
00330     //
00331     cout << "M_inv:\n";
00332     printMatrix(M_inv);
00333     //
00334     */
00335 
00336     //
00337     // The flag for singularity
00338     bool is_singular = false;
00339     //
00340 
00341     // Eliminate all the other elements except those on the diagonal
00342     for (size_t i = 0; i < n; ++i){ // For each element on diagonal
00343         float ratio;
00344 
00345         // For solving the problem of that diagonal element is zero
00346         if (M[i][i] == 0.0){
00347             is_singular |= SolveSingularityOnDiag(M, M_inv, i);
00348         }
00349 
00350         //
00351         if (is_singular){
00352             // The matrix is singular,
00353             // stop this meaningless process and return.
00354             // break;
00355             return M_inv;
00356         }
00357 
00358         // Normalize the diagonal element in M to 1.0
00359         if (M[i][i] != 1.0){
00360             ratio = 1.0/M[i][i];
00361             Get_VectorScaleUp(M[i], ratio);
00362             Get_VectorScaleUp(M_inv[i], ratio);
00363         }
00364         //
00365 
00366         //
00367         row_left = M[i];
00368         row_right = M_inv[i];
00369         //
00370         // M_diag = M[i][i]; // This has been normalized, which is 1.0
00371         //
00372         for (size_t j = 0; j < n; ++j){ // For the row other than than the current row
00373             if (j != i){ // Not going to do anything with that row itself
00374                 //
00375                 // ratio = M[j][i]/M_diag;
00376                 ratio = M[j][i]; // Because M_diag = 1.0
00377                 Get_VectorIncrement(M[j], Get_VectorScalarMultiply(row_left, ratio), true); // -=
00378                 Get_VectorIncrement(M_inv[j], Get_VectorScalarMultiply(row_right, ratio), true); // -=
00379                 //
00380                 /*
00381                 Get_VectorIncrement(M[j], Get_VectorScalarMultiply(M[i], ratio), true); // -=
00382                 Get_VectorIncrement(M_inv[j], Get_VectorScalarMultiply(M_inv[i], ratio), true); // -=
00383                 */
00384             }
00385         }
00386 
00387         /*
00388         //
00389         cout << "Diagonal element i = " << i << "\n";
00390         //
00391         cout << "M:\n";
00392         printMatrix(M);
00393         //
00394         cout << "M_inv:\n";
00395         printMatrix(M_inv);
00396         */
00397     }
00398     return M_inv;
00399 
00400 }
00401 ///////////////////////////////////////// end Matrix inversion, using Gauss method
00402 
00403 
00404 }
00405 ////////////////////////// end Namespace MATRIX_PRIMITIVE