source: foam/trunk/vision/src/Matrix.h @ 97

Revision 97, 12.7 KB checked in by dave, 10 years ago (diff)

added the eigen spaces and the rest of the data associated with this, faces, etc

Line 
1// Copyright (C) 2009 foam
2//
3// This program is free software; you can redistribute it and/or modify
4// it under the terms of the GNU General Public License as published by
5// the Free Software Foundation; either version 2 of the License, or
6// (at your option) any later version.
7//
8// This program is distributed in the hope that it will be useful,
9// but WITHOUT ANY WARRANTY; without even the implied warranty of
10// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11// GNU General Public License for more details.
12//
13// You should have received a copy of the GNU General Public License
14// along with this program; if not, write to the Free Software
15// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
16
17#include <assert.h>
18#include <iostream>
19#include "Vector.h"
20
21#ifndef FOAM_MATRIX
22#define FOAM_MATRIX
23
24template<class T>
25class Matrix
26{
27public:
28        Matrix();
29        Matrix(unsigned int r, unsigned int c);
30        ~Matrix();
31        Matrix(const Matrix &other);
32        Matrix(unsigned int r, unsigned int c, float *data);
33
34        // Row proxy classes to allow matrix[r][c] notation
35        class Row
36        {
37        public:
38                Row(Matrix *owner, unsigned int r)
39                {
40                        m_Data=&owner->GetRawData()[r*owner->GetCols()];
41                        m_Cols=owner->GetCols();
42                }
43               
44                T &operator[](unsigned int c)
45                {
46                        assert(c<m_Cols);
47                        return m_Data[c];
48                }
49               
50        private:
51                T *m_Data;
52                unsigned int m_Cols;
53        };
54
55        class ConstRow
56        {
57        public:
58                ConstRow(const Matrix *owner, unsigned int r)
59                {
60                        m_Data=&owner->GetRawDataConst()[r*owner->GetCols()];
61                        m_Cols=owner->GetCols();
62                }
63               
64                const T &operator[](unsigned int c) const
65                {
66                        assert(c<m_Cols);
67                        return m_Data[c];
68                }
69               
70        private:
71                const T *m_Data;
72                unsigned int m_Cols;
73        };
74
75       
76        Row operator[](unsigned int r)
77        {
78                assert(r<m_Rows);
79                return Row(this,r);
80        }
81
82        ConstRow operator[](unsigned int r) const
83        {
84                assert(r<m_Rows);
85                return ConstRow(this,r);
86        }
87 
88        unsigned int GetRows() const { return m_Rows; }
89        unsigned int GetCols() const { return m_Cols; }
90        T *GetRawData() { return m_Data; }
91        const T *GetRawDataConst() const { return m_Data; }
92        Vector<T> GetRowVector(unsigned int r) const;
93        Vector<T> GetColVector(unsigned int c) const;
94        void SetRowVector(unsigned int r, const Vector<T> &row);
95        void SetColVector(unsigned int c, const Vector<T> &col);
96        void NormaliseRows();
97        void NormaliseCols();
98
99        void Print() const;
100        void SetAll(T s);
101        void Zero() { SetAll(0); }
102        bool IsInf();
103        Matrix Transposed() const;
104        Matrix Inverted() const;
105
106        Matrix &operator=(const Matrix &other);
107        Matrix operator+(const Matrix &other) const;
108        Matrix operator-(const Matrix &other) const;
109        Matrix operator*(const Matrix &other) const;
110        Vector<T> operator*(const Vector<T> &other) const;
111        Vector<T> VecMulTransposed(const Vector<T> &other) const;
112        Matrix &operator+=(const Matrix &other);
113        Matrix &operator-=(const Matrix &other);
114        Matrix &operator*=(const Matrix &other);
115        bool operator==(const Matrix &other) const;
116
117        void SortRows(Vector<T> &v);
118        void SortCols(Vector<T> &v);
119       
120        Matrix CropRows(unsigned int s, unsigned int e);
121        Matrix CropCols(unsigned int s, unsigned int e);
122
123        void Save(FILE *f) const;
124        void Load(FILE *f);
125
126        static void RunTests();
127       
128private:
129
130        unsigned int m_Rows;
131        unsigned int m_Cols;
132       
133        T *m_Data;
134       
135};
136
137template<class T>
138Matrix<T>::Matrix(unsigned int r, unsigned int c) :
139m_Rows(r),
140m_Cols(c)
141{
142        m_Data=new T[r*c];
143}
144
145template<class T>
146Matrix<T>::Matrix() :
147m_Rows(0),
148m_Cols(0),
149m_Data(NULL)
150{
151}
152
153template<class T>
154Matrix<T>::Matrix(unsigned int r, unsigned int c, float *data) :
155m_Rows(r),
156m_Cols(c),
157m_Data(data)
158{
159}
160
161template<class T>
162Matrix<T>::~Matrix()
163{
164        delete[] m_Data;
165}
166
167template<class T>
168Matrix<T>::Matrix(const Matrix &other)
169{
170        m_Rows = other.m_Rows;
171        m_Cols = other.m_Cols;
172        m_Data=new T[m_Rows*m_Cols];
173        memcpy(m_Data,other.m_Data,m_Rows*m_Cols*sizeof(T));
174}
175
176template<class T>
177Matrix<T> &Matrix<T>::operator=(const Matrix &other)
178{
179        if (m_Data!=NULL)
180        {
181                delete[] m_Data;
182        }
183       
184        m_Rows = other.m_Rows;
185        m_Cols = other.m_Cols;
186        m_Data=new T[m_Rows*m_Cols];
187        memcpy(m_Data,other.m_Data,m_Rows*m_Cols*sizeof(T));
188       
189        return *this;
190}
191
192template<class T>
193void Matrix<T>::Print() const
194{
195        for (unsigned int i=0; i<m_Rows; i++)
196        {
197                for (unsigned int j=0; j<m_Cols; j++)
198                {
199                        std::cerr<<(*this)[i][j]<<" ";
200                }
201                std::cerr<<std::endl;
202        }
203}
204
205template<class T>
206void Matrix<T>::SetAll(T s)
207{
208        for (unsigned int i=0; i<m_Rows; i++)
209        {
210                for (unsigned int j=0; j<m_Cols; j++)
211                {
212                        (*this)[i][j]=s;
213                }
214        }
215}
216
217template<class T>
218bool Matrix<T>::IsInf()
219{
220        for (unsigned int i=0; i<m_Rows; i++)
221        {
222                for (unsigned int j=0; j<m_Cols; j++)
223                {
224                        if (isinf((*this)[i][j])) return true;
225                        if (isnan((*this)[i][j])) return true;
226                }
227        }
228        return false;
229}
230
231template<class T>
232Matrix<T> Matrix<T>::Transposed() const
233{
234        Matrix<T> copy(m_Cols,m_Rows);
235        for (unsigned int i=0; i<m_Rows; i++)
236        {
237                for (unsigned int j=0; j<m_Cols; j++)
238                {
239                        copy[j][i]=(*this)[i][j];
240                }
241        }
242        return copy;
243}
244
245void matrix_inverse(const float *Min, float *Mout, int actualsize);
246
247template<class T>
248Matrix<T> Matrix<T>::Inverted() const
249{
250        assert(m_Rows==m_Cols); // only works for square matrices
251        Matrix<T> ret(m_Rows,m_Cols);
252        matrix_inverse(GetRawDataConst(),ret.GetRawData(),m_Rows);
253        return ret;
254}
255
256template<class T>
257Matrix<T> Matrix<T>::operator+(const Matrix &other) const
258{
259        assert(m_Rows==other.m_Rows);
260        assert(m_Cols==other.m_Cols);
261       
262        Matrix<T> ret(m_Rows,m_Cols);
263        for (unsigned int i=0; i<m_Rows; i++)
264        {
265                for (unsigned int j=0; j<m_Cols; j++)
266                {
267                        ret[i][j]=(*this)[i][j]+other[i][j];
268                }
269        }
270        return ret;
271}
272
273template<class T>
274Matrix<T> Matrix<T>::operator-(const Matrix &other) const
275{
276        assert(m_Rows==other.m_Rows);
277        assert(m_Cols==other.m_Cols);
278       
279        Matrix<T> ret(m_Rows,m_Cols);
280        for (unsigned int i=0; i<m_Rows; i++)
281        {
282                for (unsigned int j=0; j<m_Cols; j++)
283                {
284                        ret[i][j]=(*this)[i][j]-other[i][j];
285                }
286        }
287        return ret;
288}
289
290template<class T>
291Matrix<T> Matrix<T>::operator*(const Matrix &other) const
292{
293        assert(m_Cols==other.m_Rows);
294       
295        Matrix<T> ret(m_Rows,other.m_Cols);
296       
297        for (unsigned int i=0; i<m_Rows; i++)
298        {
299                for (unsigned int j=0; j<other.m_Cols; j++)
300                {
301                        ret[i][j]=0;
302                        for (unsigned int k=0; k<m_Cols; k++)
303                        {
304                                ret[i][j]+=(*this)[i][k]*other[k][j];
305                        }
306                }
307        }
308        return ret;
309}
310
311template<class T>
312Vector<T> Matrix<T>::operator*(const Vector<T> &other) const
313{
314        assert(m_Cols==other.Size());
315       
316        Vector<T> ret(m_Rows);
317       
318        for (unsigned int i=0; i<m_Rows; i++)
319        {
320                for (unsigned int j=0; j<other.Size(); j++)
321                {
322                        ret[i]=0;
323                        for (unsigned int k=0; k<m_Cols; k++)
324                        {
325                                ret[i]+=(*this)[i][k]*other[k];
326                        }
327                }
328        }
329        return ret;
330}
331
332template<class T>
333Vector<T> Matrix<T>::VecMulTransposed(const Vector<T> &other) const
334{
335        assert(m_Rows==other.Size());
336       
337        Vector<T> ret(m_Cols);
338       
339        for (unsigned int i=0; i<m_Cols; i++)
340        {
341                for (unsigned int j=0; j<other.Size(); j++)
342                {
343                        ret[i]=0;
344                        for (unsigned int k=0; k<m_Rows; k++)
345                        {
346                                ret[i]+=(*this)[k][i]*other[k];
347                        }
348                }
349        }
350        return ret;
351}
352
353template<class T>
354Matrix<T> &Matrix<T>::operator+=(const Matrix &other)
355{
356        (*this)=(*this)+other;
357        return *this;
358}
359
360template<class T>
361Matrix<T> &Matrix<T>::operator-=(const Matrix &other)
362{
363        (*this)=(*this)-other;
364        return *this;
365}
366
367template<class T>
368Matrix<T> &Matrix<T>::operator*=(const Matrix &other)
369{
370        (*this)=(*this)*other;
371        return *this;
372}
373
374template<class T>
375bool Matrix<T>::operator==(const Matrix &other) const
376{
377        if (m_Rows != other.m_Rows ||
378                m_Cols != other.m_Cols)
379        {
380                return false;
381        }
382       
383        for (unsigned int i=0; i<m_Cols; i++)
384        {
385                for (unsigned int j=0; j<m_Rows; j++)
386                {
387                        if (!feq((*this)[i][j],other[i][j])) return false;
388                }
389        }
390
391        return true;
392}
393
394//todo: use memcpy for these 4 functions
395template<class T>
396Vector<T> Matrix<T>::GetRowVector(unsigned int r) const
397{
398        assert(r<m_Rows);
399        Vector<T> ret(m_Cols);
400        for (unsigned int j=0; j<m_Cols; j++)
401        {
402                ret[j]=(*this)[r][j];
403        }
404        return ret;
405}
406
407template<class T>
408Vector<T> Matrix<T>::GetColVector(unsigned int c) const
409{
410        assert(c<m_Cols);
411        Vector<T> ret(m_Rows);
412        for (unsigned int i=0; i<m_Rows; i++)
413        {
414                ret[i]=(*this)[i][c];
415        }
416        return ret;
417}
418
419template<class T>
420void Matrix<T>::SetRowVector(unsigned int r, const Vector<T> &row)
421{
422        assert(r<m_Rows);
423        assert(row.Size()==m_Cols);
424        for (unsigned int j=0; j<m_Cols; j++)
425        {
426                (*this)[r][j]=row[j];
427        }
428}
429
430template<class T>
431void Matrix<T>::SetColVector(unsigned int c, const Vector<T> &col)
432{
433        assert(c<m_Cols);
434        assert(col.Size()==m_Rows);
435        for (unsigned int i=0; i<m_Rows; i++)
436        {
437                (*this)[i][c]=col[i];
438        }
439}
440
441// sort rows by v
442template<class T>
443void Matrix<T>::SortRows(Vector<T> &v)
444{
445        assert(v.Size()==m_Rows);
446       
447        bool sorted=false;
448        while(!sorted)
449        {
450                sorted=true;
451               
452                for (unsigned int i=0; i<v.Size()-1; i++)
453                {
454                        if (v[i]<v[i+1])
455                        {
456                                sorted=false;
457                                float vtmp = v[i];
458                                v[i]=v[i+1];
459                                v[i+1]=vtmp;
460                               
461                                Vector<float> rtmp = GetRowVector(i);
462                                SetRowVector(i,GetRowVector(i+1));
463                                SetRowVector(i+1,rtmp);
464                        }
465                }
466        }
467}
468
469// sort cols by v
470template<class T>
471void Matrix<T>::SortCols(Vector<T> &v)
472{
473        assert(v.Size()==m_Cols);
474       
475        bool sorted=false;
476        while(!sorted)
477        {
478                sorted=true;
479               
480                for (unsigned int i=0; i<v.Size()-1; i++)
481                {
482                        if (v[i]<v[i+1])
483                        {
484                                sorted=false;
485                                float vtmp = v[i];
486                                v[i]=v[i+1];
487                                v[i+1]=vtmp;
488                               
489                                Vector<float> rtmp = GetColVector(i);
490                                SetColVector(i,GetColVector(i+1));
491                                SetColVector(i+1,rtmp);
492                        }
493                }
494        }
495}
496       
497template<class T>
498Matrix<T> Matrix<T>::CropRows(unsigned int s, unsigned int e)
499{
500        assert(s<e);
501        assert(s<m_Rows);
502        assert(e<=m_Rows);
503       
504        Matrix r(e-s,m_Cols);
505        unsigned int c=0;
506        for(unsigned int i=s; i<e; i++)
507        {
508                r.SetRowVector(c,GetRowVector(i));
509                c++;
510        }
511       
512        return r;
513}
514
515template<class T>
516Matrix<T> Matrix<T>::CropCols(unsigned int s, unsigned int e)
517{
518        assert(s<e);
519        assert(s<m_Cols);
520        assert(e<=m_Cols);
521       
522        Matrix r(m_Rows,e-s);
523        unsigned int c=0;
524        for(unsigned int i=s; i<e; i++)
525        {
526                r.SetColVector(c,GetColVector(i));
527                c++;
528        }
529       
530        return r;
531}
532
533template<class T>
534void Matrix<T>::NormaliseRows()
535{
536        for(unsigned int i=0; i<m_Rows; i++)
537        {
538                SetRowVector(i,GetRowVector(i).Normalised());
539        }
540}
541
542template<class T>
543void Matrix<T>::NormaliseCols()
544{
545        for(unsigned int i=0; i<m_Cols; i++)
546        {
547                SetColVector(i,GetColVector(i).Normalised());
548        }
549}
550
551template<class T>
552void Matrix<T>::Save(FILE* f) const
553{
554        int version = 1;       
555        fwrite(&version,1,sizeof(version),f);
556        fwrite(&m_Rows,1,sizeof(m_Rows),f);
557        fwrite(&m_Cols,1,sizeof(m_Cols),f);
558        fwrite(m_Data,1,sizeof(T)*m_Rows*m_Cols,f);
559}
560
561template<class T>
562void Matrix<T>::Load(FILE* f)
563{
564        int version;   
565        fread(&version,sizeof(version),1,f);
566        fread(&m_Rows,sizeof(m_Rows),1,f);
567        fread(&m_Cols,sizeof(m_Cols),1,f);
568        m_Data=new T[m_Rows*m_Cols];
569        fread(m_Data,sizeof(T)*m_Rows*m_Cols,1,f);
570}
571
572template<class T>
573void Matrix<T>::RunTests()
574{
575        Vector<T>::RunTests();
576
577        Matrix<T> m(10,10);
578        m.SetAll(0);
579        assert(m[0][0]==0);
580        m[5][2]=0.5;
581        assert(m[5][2]==0.5);
582        Matrix<T> om(m);
583        assert(om[5][2]==0.5);
584        Matrix<T> a(2,3);
585        a[0][0]=1; a[0][1]=2; a[0][2]=3;
586        a[1][0]=4; a[1][1]=5; a[1][2]=6;
587        Matrix<T> b(3,1);
588        b[0][0]=3;
589        b[1][0]=1;
590        b[2][0]=2;
591        Matrix<T> c=a*b;
592        assert(c[0][0]==11 && c[1][0]==29);
593       
594        // test matrix * vector
595        Vector<T> d(3);
596        d[0]=3;
597        d[1]=1;
598        d[2]=2;
599        Vector<T> e=a*d;
600        assert(e[0]==11 && e[1]==29);
601       
602        Matrix<T> f=a.CropCols(1,3);
603        assert(f.GetRows()==2 && f.GetCols()==2 && f[0][0]==2);
604        Matrix<T> g=a.CropRows(0,1);
605        assert(g.GetRows()==1 && g.GetCols()==3 && g[0][0]==1);
606       
607        // test matrix invert
608        Matrix<T> h(3,3);
609        h.Zero();
610        h[0][0]=1;
611        h[1][1]=1;
612        h[2][2]=1;
613        Matrix<T> i=h.Inverted();
614        i==h;
615       
616        // some transforms from fluxus
617        Matrix<T> j(4,4);       
618        j[0][0]=1.0;                   
619        j[0][1]=0.0 ;                           
620        j[0][2]=0.0;                           
621        j[0][3]=0.0;                           
622       
623        j[1][0]=0.0                     ;       
624        j[1][1]=0.7071067690849304 ;
625        j[1][2]=0.7071067690849304 ;
626        j[1][3]=0.0                             ;
627       
628        j[2][0]=0.0                             ;
629        j[2][1]=-0.7071067690849304 ;
630        j[2][2]=0.7071067690849304  ;
631        j[2][3]=0.0                             ;
632       
633        j[3][0]=1.0                             ;
634        j[3][1]=2.0                             ;
635        j[3][2]=3.0                             ;
636        j[3][3]=1.0                             ;
637
638        Matrix<T> k(4,4);
639        k[0][0]=1.0                              ;
640        k[0][1]=0.0                              ;
641        k[0][2]=0.0                              ;
642        k[0][3]=0.0                              ;
643
644        k[1][0]=0.0                              ;
645        k[1][1]=0.7071068286895752   ;
646        k[1][2]=-0.7071068286895752  ;
647        k[1][3]=0.0                              ;
648
649        k[2][0]=0.0                              ;
650        k[2][1]=0.7071068286895752   ;
651        k[2][2]=0.7071068286895752   ;
652        k[2][3]=0.0                              ;
653
654        k[3][0]=-0.9999999403953552  ;
655        k[3][1]=-3.535533905029297   ;
656        k[3][2]=-0.7071067690849304  ;
657        k[3][3]=0.9999999403953552       ;
658       
659        assert(j.Inverted()==k);
660               
661        Matrix<float> l(2,2);
662        l[0][0]=3;
663        l[0][1]=3;
664        l[1][0]=0;
665        l[1][1]=0;
666       
667        Matrix<float> n(2,2);
668        n[0][0]=2;
669        n[0][1]=2;
670        n[1][0]=0;
671        n[1][1]=0;
672       
673        n*=l;
674       
675        Matrix<float> o(4,4);
676        o.Zero();
677        o[0][0]=1;
678        o[1][1]=1;
679        o[2][2]=1;
680        o[3][3]=1;
681       
682        j*=k;
683        assert(j==o);
684
685        {
686                Matrix<float> a(2,3);
687                Matrix<float> b(3,2);
688               
689                a[0][0]=1; a[0][1]=2; a[0][2]=3;
690                a[1][0]=4; a[1][1]=5; a[1][2]=6;
691
692                b[0][0]=2; b[0][1]=3;
693                b[1][0]=-1; b[1][1]=1;
694                b[2][0]=1; b[2][1]=2;
695               
696                Matrix<float> result(2,2);
697                result[0][0]=3; result[0][1]=11;
698                result[1][0]=9; result[1][1]=29;
699               
700                assert(a*b==result);
701        }
702       
703}
704
705#endif
Note: See TracBrowser for help on using the repository browser.