1//
2// Copyright 2003 Google, Inc.
3//
4//
5// A simple class to handle 3x3 matrices
6// The aim of this class is to be able to manipulate 3x3 matrices
7// and 3D vectors as naturally as possible and make calculations
8// readable.
9// For that reason, the operators +, -, * are overloaded.
10// (Reading a = a + b*2 - c is much easier to read than
11// a = Sub(Add(a, Mul(b,2)),c) )
12//
13// Please be careful about overflows when using those matrices wth integer types
14// The calculations are carried with VType. eg : if you are using uint8 as the
15// base type, all values will be modulo 256.
16// This feature is necessary to use the class in a more general framework with
17// VType != plain old data type.
18
19#ifndef UTIL_MATH_MATRIX3X3_INL_H__
20#define UTIL_MATH_MATRIX3X3_INL_H__
21#include <iostream>
22using std::ostream;
23using std::cout;
24using std::endl;
25
26
27#include "util/math/mathutil.h"
28#include "util/math/vector3-inl.h"
29#include "util/math/matrix3x3.h"
30#include "base/basictypes.h"
31#include "base/logging.h"
32
33template <class VType>
34class Matrix3x3 {
35 private:
36 VType m_[3][3];
37 public:
38 typedef Matrix3x3<VType> Self;
39 typedef Vector3<VType> MVector;
40 // Initialize the matrix to 0
41 Matrix3x3() {
42 m_[0][2] = m_[0][1] = m_[0][0] = VType();
43 m_[1][2] = m_[1][1] = m_[1][0] = VType();
44 m_[2][2] = m_[2][1] = m_[2][0] = VType();
45 }
46
47 // Constructor explicitly setting the values of all the coefficient of
48 // the matrix
49 Matrix3x3(const VType &m00, const VType &m01, const VType &m02,
50 const VType &m10, const VType &m11, const VType &m12,
51 const VType &m20, const VType &m21, const VType &m22) {
52 m_[0][0] = m00;
53 m_[0][1] = m01;
54 m_[0][2] = m02;
55
56 m_[1][0] = m10;
57 m_[1][1] = m11;
58 m_[1][2] = m12;
59
60 m_[2][0] = m20;
61 m_[2][1] = m21;
62 m_[2][2] = m22;
63 }
64
65 // Copy constructor
66 Matrix3x3(const Self &mb) {
67 m_[0][0] = mb.m_[0][0];
68 m_[0][1] = mb.m_[0][1];
69 m_[0][2] = mb.m_[0][2];
70
71 m_[1][0] = mb.m_[1][0];
72 m_[1][1] = mb.m_[1][1];
73 m_[1][2] = mb.m_[1][2];
74
75 m_[2][0] = mb.m_[2][0];
76 m_[2][1] = mb.m_[2][1];
77 m_[2][2] = mb.m_[2][2];
78 }
79
80 // Casting constructor
81 template <class VType2>
82 static Self Cast(const Matrix3x3<VType2> &mb) {
83 return Self(static_cast<VType>(mb(0, 0)),
84 static_cast<VType>(mb(0, 1)),
85 static_cast<VType>(mb(0, 2)),
86 static_cast<VType>(mb(1, 0)),
87 static_cast<VType>(mb(1, 1)),
88 static_cast<VType>(mb(1, 2)),
89 static_cast<VType>(mb(2, 0)),
90 static_cast<VType>(mb(2, 1)),
91 static_cast<VType>(mb(2, 2)));
92 }
93
94 // Change the value of all the coefficients of the matrix
95 inline Self &
96 Set(const VType &m00, const VType &m01, const VType &m02,
97 const VType &m10, const VType &m11, const VType &m12,
98 const VType &m20, const VType &m21, const VType &m22) {
99 m_[0][0] = m00;
100 m_[0][1] = m01;
101 m_[0][2] = m02;
102
103 m_[1][0] = m10;
104 m_[1][1] = m11;
105 m_[1][2] = m12;
106
107 m_[2][0] = m20;
108 m_[2][1] = m21;
109 m_[2][2] = m22;
110 return (*this);
111 }
112
113 // Copy
114 inline Self& operator=(const Self &mb) {
115 m_[0][0] = mb.m_[0][0];
116 m_[0][1] = mb.m_[0][1];
117 m_[0][2] = mb.m_[0][2];
118
119 m_[1][0] = mb.m_[1][0];
120 m_[1][1] = mb.m_[1][1];
121 m_[1][2] = mb.m_[1][2];
122
123 m_[2][0] = mb.m_[2][0];
124 m_[2][1] = mb.m_[2][1];
125 m_[2][2] = mb.m_[2][2];
126 return (*this);
127 }
128
129 // Compare
130 inline bool operator==(const Self &mb) const {
131 return (m_[0][0] == mb.m_[0][0]) &&
132 (m_[0][1] == mb.m_[0][1]) &&
133 (m_[0][2] == mb.m_[0][2]) &&
134 (m_[1][0] == mb.m_[1][0]) &&
135 (m_[1][1] == mb.m_[1][1]) &&
136 (m_[1][2] == mb.m_[1][2]) &&
137 (m_[2][0] == mb.m_[2][0]) &&
138 (m_[2][1] == mb.m_[2][1]) &&
139 (m_[2][2] == mb.m_[2][2]);
140 }
141
142 // Matrix addition
143 inline Self& operator+=(const Self &mb) {
144 m_[0][0] += mb.m_[0][0];
145 m_[0][1] += mb.m_[0][1];
146 m_[0][2] += mb.m_[0][2];
147
148 m_[1][0] += mb.m_[1][0];
149 m_[1][1] += mb.m_[1][1];
150 m_[1][2] += mb.m_[1][2];
151
152 m_[2][0] += mb.m_[2][0];
153 m_[2][1] += mb.m_[2][1];
154 m_[2][2] += mb.m_[2][2];
155 return (*this);
156 }
157
158 // Matrix subtration
159 inline Self& operator-=(const Self &mb) {
160 m_[0][0] -= mb.m_[0][0];
161 m_[0][1] -= mb.m_[0][1];
162 m_[0][2] -= mb.m_[0][2];
163
164 m_[1][0] -= mb.m_[1][0];
165 m_[1][1] -= mb.m_[1][1];
166 m_[1][2] -= mb.m_[1][2];
167
168 m_[2][0] -= mb.m_[2][0];
169 m_[2][1] -= mb.m_[2][1];
170 m_[2][2] -= mb.m_[2][2];
171 return (*this);
172 }
173
174 // Matrix multiplication by a scalar
175 inline Self& operator*=(const VType &k) {
176 m_[0][0] *= k;
177 m_[0][1] *= k;
178 m_[0][2] *= k;
179
180 m_[1][0] *= k;
181 m_[1][1] *= k;
182 m_[1][2] *= k;
183
184 m_[2][0] *= k;
185 m_[2][1] *= k;
186 m_[2][2] *= k;
187 return (*this);
188 }
189
190 // Matrix addition
191 inline Self operator+(const Self &mb) const {
192 return Self(*this) += mb;
193 }
194
195 // Matrix subtraction
196 inline Self operator-(const Self &mb) const {
197 return Self(*this) -= mb;
198 }
199
200 // Change the sign of all the coefficients in the matrix
201 friend inline Self operator-(const Self &vb) {
202 return Self(-vb.m_[0][0], -vb.m_[0][1], -vb.m_[0][2],
203 -vb.m_[1][0], -vb.m_[1][1], -vb.m_[1][2],
204 -vb.m_[2][0], -vb.m_[2][1], -vb.m_[2][2]);
205 }
206
207 // Matrix multiplication by a scalar
208 inline Self operator*(const VType &k) const {
209 return Self(*this) *= k;
210 }
211
212 friend inline Self operator*(const VType &k, const Self &mb) {
213 return Self(mb)*k;
214 }
215
216 // Matrix multiplication
217 inline Self operator*(const Self &mb) const {
218 return Self(
219 m_[0][0] * mb.m_[0][0] + m_[0][1] * mb.m_[1][0] + m_[0][2] * mb.m_[2][0],
220 m_[0][0] * mb.m_[0][1] + m_[0][1] * mb.m_[1][1] + m_[0][2] * mb.m_[2][1],
221 m_[0][0] * mb.m_[0][2] + m_[0][1] * mb.m_[1][2] + m_[0][2] * mb.m_[2][2],
222
223 m_[1][0] * mb.m_[0][0] + m_[1][1] * mb.m_[1][0] + m_[1][2] * mb.m_[2][0],
224 m_[1][0] * mb.m_[0][1] + m_[1][1] * mb.m_[1][1] + m_[1][2] * mb.m_[2][1],
225 m_[1][0] * mb.m_[0][2] + m_[1][1] * mb.m_[1][2] + m_[1][2] * mb.m_[2][2],
226
227 m_[2][0] * mb.m_[0][0] + m_[2][1] * mb.m_[1][0] + m_[2][2] * mb.m_[2][0],
228 m_[2][0] * mb.m_[0][1] + m_[2][1] * mb.m_[1][1] + m_[2][2] * mb.m_[2][1],
229 m_[2][0] * mb.m_[0][2] + m_[2][1] * mb.m_[1][2] + m_[2][2] * mb.m_[2][2]);
230 }
231
232 // Multiplication of a matrix by a vector
233 inline MVector operator*(const MVector &v) const {
234 return MVector(
235 m_[0][0] * v[0] + m_[0][1] * v[1] + m_[0][2] * v[2],
236 m_[1][0] * v[0] + m_[1][1] * v[1] + m_[1][2] * v[2],
237 m_[2][0] * v[0] + m_[2][1] * v[1] + m_[2][2] * v[2]);
238 }
239
240 // Return the determinant of the matrix
241 inline VType Det(void) const {
242 return m_[0][0] * m_[1][1] * m_[2][2]
243 + m_[0][1] * m_[1][2] * m_[2][0]
244 + m_[0][2] * m_[1][0] * m_[2][1]
245 - m_[2][0] * m_[1][1] * m_[0][2]
246 - m_[2][1] * m_[1][2] * m_[0][0]
247 - m_[2][2] * m_[1][0] * m_[0][1];
248 }
249
250 // Return the trace of the matrix
251 inline VType Trace(void) const {
252 return m_[0][0] + m_[1][1] + m_[2][2];
253 }
254
255 // Return a pointer to the data array for interface with other libraries
256 // like opencv
257 VType* Data() {
258 return reinterpret_cast<VType*>(m_);
259 }
260 const VType* Data() const {
261 return reinterpret_cast<const VType*>(m_);
262 }
263
264 // Return matrix element (i,j) with 0<=i<=2 0<=j<=2
265 inline VType &operator()(const int i, const int j) {
266 DCHECK(i >= 0);
267 DCHECK(i < 3);
268 DCHECK(j >= 0);
269 DCHECK(j < 3);
270 return m_[i][j];
271 }
272 inline VType operator()(const int i, const int j) const {
273 DCHECK(i >= 0);
274 DCHECK(i < 3);
275 DCHECK(j >= 0);
276 DCHECK(j < 3);
277 return m_[i][j];
278 }
279
280 // Return matrix element (i/3,i%3) with 0<=i<=8 (access concatenated rows).
281 inline VType &operator[](const int i) {
282 DCHECK(i >= 0);
283 DCHECK(i < 9);
284 return reinterpret_cast<VType*>(m_)[i];
285 }
286 inline VType operator[](const int i) const {
287 DCHECK(i >= 0);
288 DCHECK(i < 9);
289 return reinterpret_cast<const VType*>(m_)[i];
290 }
291
292 // Return the transposed matrix
293 inline Self Transpose(void) const {
294 return Self(m_[0][0], m_[1][0], m_[2][0],
295 m_[0][1], m_[1][1], m_[2][1],
296 m_[0][2], m_[1][2], m_[2][2]);
297 }
298
299 // Return the transposed of the matrix of the cofactors
300 // (Useful for inversion for example)
301 inline Self ComatrixTransposed(void) const {
302 return Self(
303 m_[1][1] * m_[2][2] - m_[2][1] * m_[1][2],
304 m_[2][1] * m_[0][2] - m_[0][1] * m_[2][2],
305 m_[0][1] * m_[1][2] - m_[1][1] * m_[0][2],
306
307 m_[1][2] * m_[2][0] - m_[2][2] * m_[1][0],
308 m_[2][2] * m_[0][0] - m_[0][2] * m_[2][0],
309 m_[0][2] * m_[1][0] - m_[1][2] * m_[0][0],
310
311 m_[1][0] * m_[2][1] - m_[2][0] * m_[1][1],
312 m_[2][0] * m_[0][1] - m_[0][0] * m_[2][1],
313 m_[0][0] * m_[1][1] - m_[1][0] * m_[0][1]);
314 }
315 // Matrix inversion
316 inline Self Inverse(void) const {
317 VType det = Det();
318 CHECK(det != 0) << " Can't inverse. Determinant = 0.";
319 return (1/det) * ComatrixTransposed();
320 }
321
322 // Return the vector 3D at row i
323 inline MVector Row(const int i) const {
324 DCHECK(i >= 0);
325 DCHECK(i < 3);
326 return MVector(m_[i][0], m_[i][1], m_[i][2]);
327 }
328
329 // Return the vector 3D at col i
330 inline MVector Col(const int i) const {
331 DCHECK(i >= 0);
332 DCHECK(i < 3);
333 return MVector(m_[0][i], m_[1][i], m_[2][i]);
334 }
335
336 // Create a matrix from 3 row vectors
337 static inline Self FromRows(const MVector &v1,
338 const MVector &v2,
339 const MVector &v3) {
340 Self temp;
341 temp.Set(v1[0], v1[1], v1[2],
342 v2[0], v2[1], v2[2],
343 v3[0], v3[1], v3[2]);
344 return temp;
345 }
346
347 // Create a matrix from 3 column vectors
348 static inline Self FromCols(const MVector &v1,
349 const MVector &v2,
350 const MVector &v3) {
351 Self temp;
352 temp.Set(v1[0], v2[0], v3[0],
353 v1[1], v2[1], v3[1],
354 v1[2], v2[2], v3[2]);
355 return temp;
356 }
357
358 // Set the vector in row i to be v1
359 void SetRow(int i, const MVector &v1) {
360 DCHECK(i >= 0);
361 DCHECK(i < 3);
362 m_[i][0] = v1[0];
363 m_[i][1] = v1[1];
364 m_[i][2] = v1[2];
365 }
366
367 // Set the vector in column i to be v1
368 void SetCol(int i, const MVector &v1) {
369 DCHECK(i >= 0);
370 DCHECK(i < 3);
371 m_[0][i] = v1[0];
372 m_[1][i] = v1[1];
373 m_[2][i] = v1[2];
374 }
375
376 // Return a matrix M close to the original but verifying MtM = I
377 // (useful to compensate for errors in a rotation matrix)
378 Self Orthogonalize() const {
379 MVector r1, r2, r3;
380 r1 = Row(0).Normalize();
381 r2 = (Row(2).CrossProd(r1)).Normalize();
382 r3 = (r1.CrossProd(r2)).Normalize();
383 return FromRows(r1, r2, r3);
384 }
385
386 // Return the identity matrix
387 static inline Self Identity(void) {
388 Self temp;
389 temp.Set(1, 0, 0,
390 0, 1, 0,
391 0, 0, 1);
392 return temp;
393 }
394
395 // Return a matrix full of zeros
396 static inline Self Zero(void) {
397 return Self();
398 }
399
400 // Return a diagonal matrix with the coefficients in v
401 static inline Self Diagonal(const MVector &v) {
402 return Self(v[0], VType(), VType(),
403 VType(), v[1], VType(),
404 VType(), VType(), v[2]);
405 }
406
407 // Return the matrix vvT
408 static Self Sym3(const MVector &v) {
409 return Self(
410 v[0]*v[0], v[0]*v[1], v[0]*v[2],
411 v[1]*v[0], v[1]*v[1], v[1]*v[2],
412 v[2]*v[0], v[2]*v[1], v[2]*v[2]);
413 }
414 // Return a matrix M such that:
415 // for each u, M * u = v.CrossProd(u)
416 static Self AntiSym3(const MVector &v) {
417 return Self(VType(), -v[2], v[1],
418 v[2], VType(), -v[0],
419 -v[1], v[0], VType());
420 }
421
422 static Self Rodrigues(const MVector &rot) {
423 Self R;
424 VType theta = rot.Norm();
425 MVector w = rot.Normalize();
426 Self Wv = Self::AntiSym3(w);
427 Self I = Self::Identity();
428 Self A = Self::Sym3(w);
429 R = (1 - cos(theta)) * A + sin(theta) * Wv + cos(theta) * I;
430 return R;
431 }
432
433 // Returns v.Transpose() * (*this) * u
434 VType MulBothSides(const MVector &v, const MVector &u) const {
435 return ((*this) * u).DotProd(v);
436 }
437
438 // Use the 3x3 matrix as a projective transform for 2d points
439 Vector2<VType> Project(const Vector2<VType> &v) const {
440 MVector temp = (*this) * MVector(v[0], v[1], 1);
441 return Vector2<VType>(temp[0] / temp[2], temp[1] / temp[2]);
442 }
443
444 // Return the Frobenius norm of the matrix: sqrt(sum(aij^2))
445 VType FrobeniusNorm() const {
446 VType sum = VType();
447 for (int i = 0; i < 3; i++) {
448 for (int j = 0; j < 3; j++) {
449 sum += m_[i][j] * m_[i][j];
450 }
451 }
452 return sqrt(sum);
453 }
454
455 // Finds the eigen values of the matrix. Return the number of real eigenvalues
456 // found
457 int EigenValues(MVector *eig_val) {
458 long double r1, r2, r3; // NOLINT
459 // characteristic polynomial is
460 // x^3 + (a11*a22+a22*a33+a33*a11)*x^2 - trace(A)*x - det(A)
461 VType a = -Trace();
462 VType b = m_[0][0]*m_[1][1] + m_[1][1]*m_[2][2] + m_[2][2]*m_[0][0]
463 - m_[1][0]*m_[0][1] - m_[2][1]*m_[1][2] - m_[0][2]*m_[2][0];
464 VType c = -Det();
465 bool res = MathUtil::RealRootsForCubic(a, b, c, &r1, &r2, &r3);
466 (*eig_val)[0] = r1;
467 if (res) {
468 (*eig_val)[1] = r2;
469 (*eig_val)[2] = r3;
470 return 3;
471 }
472 return 1;
473 }
474
475 // Finds the eigen values and associated eigen vectors of a
476 // symmetric positive definite 3x3 matrix,eigen values are
477 // sorted in decreasing order, eig_val corresponds to the
478 // columns of the eig_vec matrix.
479 // Note: The routine will only use the lower diagonal part
480 // of the matrix, i.e.
481 // | a00, |
482 // | a10, a11, |
483 // | a20, a21, a22 |
484 void SymmetricEigenSolver(MVector *eig_val,
485 Self *eig_vec ) {
486 // Compute characteristic polynomial coefficients
487 double c2 = -(m_[0][0] + m_[1][1] + m_[2][2]);
488 double c1 = -(m_[1][0] * m_[1][0] - m_[0][0] * m_[1][1]
489 - m_[0][0] * m_[2][2] - m_[1][1] * m_[2][2]
490 + m_[2][0] * m_[2][0] + m_[2][1] * m_[2][1]);
491 double c0 = -(m_[0][0] * m_[1][1] * m_[2][2] - m_[2][0]
492 * m_[2][0] * m_[1][1] - m_[1][0] * m_[1][0]
493 * m_[2][2] - m_[0][0] * m_[2][1] * m_[2][1]
494 + 2 * m_[1][0] * m_[2][0] * m_[2][1]);
495
496 // Root finding
497 double q = (c2*c2-3*c1)/9.0;
498 double r = (2*c2*c2*c2-9*c2*c1+27*c0)/54.0;
499 // Assume R^3 <Q^3 so there are three real roots
500 if (q < 0) q = 0;
501 double sqrt_q = -2.0 * sqrt(q);
502 double theta = acos(r / sqrt(q * q * q));
503 double c2_3 = c2 / 3;
504 (*eig_val)[0] = sqrt_q * cos(theta / 3.0) - c2_3;
505 (*eig_val)[1] = sqrt_q * cos((theta + 2.0 * M_PI)/3.0) - c2_3;
506 (*eig_val)[2] = sqrt_q * cos((theta - 2.0 * M_PI)/3.0) - c2_3;
507
508 // Sort eigen value in decreasing order
509 Vector3<int> d_order = eig_val->ComponentOrder();
510 (*eig_val) = MVector((*eig_val)[d_order[2]],
511 (*eig_val)[d_order[1]],
512 (*eig_val)[d_order[0]]);
513 // Compute eigenvectors
514 for (int i = 0; i < 3; ++i) {
515 MVector r1 , r2 , r3 , e1 , e2 , e3;
516 r1[0] = m_[0][0] - (*eig_val)[i];
517 r2[0] = m_[1][0];
518 r3[0] = m_[2][0];
519 r1[1] = m_[1][0];
520 r2[1] = m_[1][1] - (*eig_val)[i];
521 r3[1] = m_[2][1];
522 r1[2] = m_[2][0];
523 r2[2] = m_[2][1];
524 r3[2] = m_[2][2] - (*eig_val)[i];
525
526 e1 = r1.CrossProd(r2);
527 e2 = r2.CrossProd(r3);
528 e3 = r3.CrossProd(r1);
529
530 // Make e2 and e3 point in the same direction as e1
531 if (e2.DotProd(e1) < 0) e2 = -e2;
532 if (e3.DotProd(e1) < 0) e3 = -e3;
533 MVector e = (e1 + e2 + e3).Normalize();
534 eig_vec->SetCol(i,e);
535 }
536 }
537
538 // Return true is one of the elements of the matrix is NaN
539 bool IsNaN() const {
540 for ( int i = 0; i < 3; ++i ) {
541 for ( int j = 0; j < 3; ++j ) {
542 if ( isnan(m_[i][j]) ) {
543 return true;
544 }
545 }
546 }
547 return false;
548 }
549
550 friend std::ostream &operator <<(std::ostream &out, const Self &mb) {
551 int i, j;
552 for (i = 0; i < 3; i++) {
553 if (i ==0) {
554 out << "[";
555 } else {
556 out << " ";
557 }
558 for (j = 0; j < 3; j++) {
559 out << mb(i, j) << " ";
560 }
561 if (i == 2) {
562 out << "]";
563 } else {
564 out << endl;
565 }
566 }
567 return out;
568 }
569};
570
571// TODO(user): Matrix3x3<T> does not actually satisfy the definition of a
572// POD type even when T is a POD. Pretending that Matrix3x3<T> is a POD
573// probably won't cause immediate problems, but eventually this should be fixed.
574PROPAGATE_POD_FROM_TEMPLATE_ARGUMENT(Matrix3x3);
575
576#endif // UTIL_MATH_MATRIX3X3_INL_H__
577