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

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

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 | |

24 | template<class T> |

25 | class Matrix |

26 | { |

27 | public: |

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 | |

128 | private: |

129 | |

130 | unsigned int m_Rows; |

131 | unsigned int m_Cols; |

132 | |

133 | T *m_Data; |

134 | |

135 | }; |

136 | |

137 | template<class T> |

138 | Matrix<T>::Matrix(unsigned int r, unsigned int c) : |

139 | m_Rows(r), |

140 | m_Cols(c) |

141 | { |

142 | m_Data=new T[r*c]; |

143 | } |

144 | |

145 | template<class T> |

146 | Matrix<T>::Matrix() : |

147 | m_Rows(0), |

148 | m_Cols(0), |

149 | m_Data(NULL) |

150 | { |

151 | } |

152 | |

153 | template<class T> |

154 | Matrix<T>::Matrix(unsigned int r, unsigned int c, float *data) : |

155 | m_Rows(r), |

156 | m_Cols(c), |

157 | m_Data(data) |

158 | { |

159 | } |

160 | |

161 | template<class T> |

162 | Matrix<T>::~Matrix() |

163 | { |

164 | delete[] m_Data; |

165 | } |

166 | |

167 | template<class T> |

168 | Matrix<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 | |

176 | template<class T> |

177 | Matrix<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 | |

192 | template<class T> |

193 | void 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 | |

205 | template<class T> |

206 | void 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 | |

217 | template<class T> |

218 | bool 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 | |

231 | template<class T> |

232 | Matrix<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 | |

245 | void matrix_inverse(const float *Min, float *Mout, int actualsize); |

246 | |

247 | template<class T> |

248 | Matrix<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 | |

256 | template<class T> |

257 | Matrix<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 | |

273 | template<class T> |

274 | Matrix<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 | |

290 | template<class T> |

291 | Matrix<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 | |

311 | template<class T> |

312 | Vector<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 | |

332 | template<class T> |

333 | Vector<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 | |

353 | template<class T> |

354 | Matrix<T> &Matrix<T>::operator+=(const Matrix &other) |

355 | { |

356 | (*this)=(*this)+other; |

357 | return *this; |

358 | } |

359 | |

360 | template<class T> |

361 | Matrix<T> &Matrix<T>::operator-=(const Matrix &other) |

362 | { |

363 | (*this)=(*this)-other; |

364 | return *this; |

365 | } |

366 | |

367 | template<class T> |

368 | Matrix<T> &Matrix<T>::operator*=(const Matrix &other) |

369 | { |

370 | (*this)=(*this)*other; |

371 | return *this; |

372 | } |

373 | |

374 | template<class T> |

375 | bool 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 |

395 | template<class T> |

396 | Vector<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 | |

407 | template<class T> |

408 | Vector<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 | |

419 | template<class T> |

420 | void 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 | |

430 | template<class T> |

431 | void 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 |

442 | template<class T> |

443 | void 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 |

470 | template<class T> |

471 | void 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 | |

497 | template<class T> |

498 | Matrix<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 | |

515 | template<class T> |

516 | Matrix<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 | |

533 | template<class T> |

534 | void Matrix<T>::NormaliseRows() |

535 | { |

536 | for(unsigned int i=0; i<m_Rows; i++) |

537 | { |

538 | SetRowVector(i,GetRowVector(i).Normalised()); |

539 | } |

540 | } |

541 | |

542 | template<class T> |

543 | void Matrix<T>::NormaliseCols() |

544 | { |

545 | for(unsigned int i=0; i<m_Cols; i++) |

546 | { |

547 | SetColVector(i,GetColVector(i).Normalised()); |

548 | } |

549 | } |

550 | |

551 | template<class T> |

552 | void 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 | |

561 | template<class T> |

562 | void 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 | |

572 | template<class T> |

573 | void 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.