dmatrix.h
Go to the documentation of this file.
1 #ifndef DMATRIX_HXX
2 #define DMATRIX_HXX
3 
4 #include <iostream>
5 #include <exception>
6 namespace GMapping {
7 
8 class DNotInvertibleMatrixException: public std::exception {};
9 class DIncompatibleMatrixException: public std::exception {};
10 class DNotSquareMatrixException: public std::exception {};
11 
12 template <class X> class DMatrix {
13  public:
14  DMatrix(int n=0,int m=0);
15  ~DMatrix();
16 
17  DMatrix(const DMatrix&);
18  DMatrix& operator=(const DMatrix&);
19 
20  X * operator[](int i) {
21  if ((*shares)>1) detach();
22  return mrows[i];
23  }
24 
25  const X * operator[](int i) const { return mrows[i]; }
26 
27  const X det() const;
28  DMatrix inv() const;
29  DMatrix transpose() const;
30  DMatrix operator*(const DMatrix&) const;
31  DMatrix operator+(const DMatrix&) const;
32  DMatrix operator-(const DMatrix&) const;
33  DMatrix operator*(const X&) const;
34 
35  int rows() const { return nrows; }
36  int columns() const { return ncols; }
37 
38  void detach();
39 
40  static DMatrix I(int);
41 
42  protected:
43  int nrows,ncols;
44  X * elems;
45  X ** mrows;
46 
47  int * shares;
48 };
49 
50 template <class X> DMatrix<X>::DMatrix(int n,int m) {
51  if (n<1) n=1;
52  if (m<1) m=1;
53  nrows=n;
54  ncols=m;
55  elems=new X[nrows*ncols];
56  mrows=new X* [nrows];
57  for (int i=0;i<nrows;i++) mrows[i]=elems+ncols*i;
58  for (int i=0;i<nrows*ncols;i++) elems[i]=X(0);
59  shares=new int;
60  (*shares)=1;
61 }
62 
63 template <class X> DMatrix<X>::~DMatrix() {
64  if (--(*shares)) return;
65  delete [] elems;
66  delete [] mrows;
67  delete shares;
68 }
69 
70 template <class X> DMatrix<X>::DMatrix(const DMatrix& m) {
71  shares=m.shares;
72  elems=m.elems;
73  nrows=m.nrows;
74  ncols=m.ncols;
75  mrows=m.mrows;
76  (*shares)++;
77 }
78 
79 template <class X> DMatrix<X>& DMatrix<X>::operator=(const DMatrix& m) {
80  if (!--(*shares)) {
81  delete [] elems;
82  delete [] mrows;
83  delete shares;
84  }
85  shares=m.shares;
86  elems=m.elems;
87  nrows=m.nrows;
88  ncols=m.ncols;
89  mrows=m.mrows;
90  (*shares)++;
91  return *this;
92 }
93 
94 template <class X> DMatrix<X> DMatrix<X>::inv() const {
95  if (nrows!=ncols) throw DNotInvertibleMatrixException();
96  DMatrix<X> aux1(*this),aux2(I(nrows));
97  aux1.detach();
98  for (int i=0;i<nrows;i++) {
99  int k=i;
100  for (;k<nrows&&aux1.mrows[k][i]==X(0);k++);
101  if (k>=nrows) throw DNotInvertibleMatrixException();
102  X val=aux1.mrows[k][i];
103  for (int j=0;j<nrows;j++) {
104  aux1.mrows[k][j]=aux1.mrows[k][j]/val;
105  aux2.mrows[k][j]=aux2.mrows[k][j]/val;
106  }
107  if (k!=i) {
108  for (int j=0;j<nrows;j++) {
109  X tmp=aux1.mrows[k][j];
110  aux1.mrows[k][j]=aux1.mrows[i][j];
111  aux1.mrows[i][j]=tmp;
112  tmp=aux2.mrows[k][j];
113  aux2.mrows[k][j]=aux2.mrows[i][j];
114  aux2.mrows[i][j]=tmp;
115  }
116  }
117  for (int j=0;j<nrows;j++)
118  if (j!=i) {
119  X tmp=aux1.mrows[j][i];
120  for (int l=0;l<nrows;l++) {
121  aux1.mrows[j][l]=aux1.mrows[j][l]-tmp*aux1.mrows[i][l];
122  aux2.mrows[j][l]=aux2.mrows[j][l]-tmp*aux2.mrows[i][l];
123  }
124  }
125  }
126  return aux2;
127 }
128 
129 template <class X> const X DMatrix<X>::det() const {
130  if (nrows!=ncols) throw DNotSquareMatrixException();
131  DMatrix<X> aux(*this);
132  X d=X(1);
133  aux.detach();
134  for (int i=0;i<nrows;i++) {
135  int k=i;
136  for (;k<nrows&&aux.mrows[k][i]==X(0);k++);
137  if (k>=nrows) return X(0);
138  X val=aux.mrows[k][i];
139  for (int j=0;j<nrows;j++) {
140  aux.mrows[k][j]/=val;
141  }
142  d=d*val;
143  if (k!=i) {
144  for (int j=0;j<nrows;j++) {
145  X tmp=aux.mrows[k][j];
146  aux.mrows[k][j]=aux.mrows[i][j];
147  aux.mrows[i][j]=tmp;
148  }
149  d=-d;
150  }
151  for (int j=i+1;j<nrows;j++){
152  X tmp=aux.mrows[j][i];
153  if (!(tmp==X(0)) ){
154  for (int l=0;l<nrows;l++) {
155  aux.mrows[j][l]=aux.mrows[j][l]-tmp*aux.mrows[i][l];
156  }
157  //d=d*tmp;
158  }
159  }
160  }
161  return d;
162 }
163 
164 template <class X> DMatrix<X> DMatrix<X>::transpose() const {
165  DMatrix<X> aux(ncols, nrows);
166  for (int i=0; i<nrows; i++)
167  for (int j=0; j<ncols; j++)
168  aux[j][i]=mrows[i][j];
169  return aux;
170 }
171 
172 template <class X> DMatrix<X> DMatrix<X>::operator*(const DMatrix<X>& m) const {
173  if (ncols!=m.nrows) throw DIncompatibleMatrixException();
174  DMatrix<X> aux(nrows,m.ncols);
175  for (int i=0;i<nrows;i++)
176  for (int j=0;j<m.ncols;j++){
177  X a=0;
178  for (int k=0;k<ncols;k++)
179  a+=mrows[i][k]*m.mrows[k][j];
180  aux.mrows[i][j]=a;
181  }
182  return aux;
183 }
184 
185 template <class X> DMatrix<X> DMatrix<X>::operator+(const DMatrix<X>& m) const {
186  if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
187  DMatrix<X> aux(nrows,ncols);
188  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]+m.elems[i];
189  return aux;
190 }
191 
192 template <class X> DMatrix<X> DMatrix<X>::operator-(const DMatrix<X>& m) const {
193  if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
194  DMatrix<X> aux(nrows,ncols);
195  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]-m.elems[i];
196  return aux;
197 }
198 
199 template <class X> DMatrix<X> DMatrix<X>::operator*(const X& e) const {
200  DMatrix<X> aux(nrows,ncols);
201  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]*e;
202  return aux;
203 }
204 
205 template <class X> void DMatrix<X>::detach() {
206  DMatrix<X> aux(nrows,ncols);
207  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i];
208  operator=(aux);
209 }
210 
211 template <class X> DMatrix<X> DMatrix<X>::I(int n) {
212  DMatrix<X> aux(n,n);
213  for (int i=0;i<n;i++) aux[i][i]=X(1);
214  return aux;
215 }
216 
217 template <class X> std::ostream& operator<<(std::ostream& os, const DMatrix<X> &m) {
218  os << "{";
219  for (int i=0;i<m.rows();i++) {
220  if (i>0) os << ",";
221  os << "{";
222  for (int j=0;j<m.columns();j++) {
223  if (j>0) os << ",";
224  os << m[i][j];
225  }
226  os << "}";
227  }
228  return os << "}";
229 }
230 
231 }; //namespace GMapping
232 #endif
GMapping::DMatrix::det
const X det() const
Definition: dmatrix.h:129
GMapping::DMatrix::operator-
DMatrix operator-(const DMatrix &) const
Definition: dmatrix.h:192
GMapping::operator-
point< T > operator-(const point< T > &p1, const point< T > &p2)
Definition: point.h:25
GMapping::DMatrix::operator*
DMatrix operator*(const DMatrix &) const
Definition: dmatrix.h:172
GMapping::DMatrix::operator[]
const X * operator[](int i) const
Definition: dmatrix.h:25
GMapping::DMatrix::operator+
DMatrix operator+(const DMatrix &) const
Definition: dmatrix.h:185
GMapping::DMatrix::rows
int rows() const
Definition: dmatrix.h:35
GMapping::DMatrix::I
static DMatrix I(int)
Definition: dmatrix.h:211
GMapping::DMatrix::operator[]
X * operator[](int i)
Definition: dmatrix.h:20
GMapping::DMatrix::columns
int columns() const
Definition: dmatrix.h:36
GMapping::operator*
point< T > operator*(const point< T > &p, const T &v)
Definition: point.h:30
GMapping::operator+
point< T > operator+(const point< T > &p1, const point< T > &p2)
Definition: point.h:20
GMapping::DMatrix::transpose
DMatrix transpose() const
Definition: dmatrix.h:164
GMapping::DMatrix::operator=
DMatrix & operator=(const DMatrix &)
Definition: dmatrix.h:79
GMapping::DMatrix::inv
DMatrix inv() const
Definition: dmatrix.h:94
GMapping::DMatrix::DMatrix
DMatrix(int n=0, int m=0)
Definition: dmatrix.h:50
n
#define n
Definition: eig3.cpp:11

openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20