AliceVision
Photogrammetric Computer Vision Framework
Matrix3x3.hpp
1 // This file is part of the AliceVision project.
2 // Copyright (c) 2017 AliceVision contributors.
3 // This Source Code Form is subject to the terms of the Mozilla Public License,
4 // v. 2.0. If a copy of the MPL was not distributed with this file,
5 // You can obtain one at https://mozilla.org/MPL/2.0/.
6 
7 #pragma once
8 
9 #include <aliceVision/mvsData/Point2d.hpp>
10 #include <aliceVision/mvsData/Point3d.hpp>
11 
12 #include <boost/math/constants/constants.hpp>
13 
14 #include <string>
15 
16 namespace aliceVision {
17 
18 class Matrix3x3
19 {
20  public:
21  union
22  {
23  struct
24  {
25  double m11, m12, m13, m21, m22, m23, m31, m32, m33;
26  };
27  double m[9];
28  };
29 
30  inline Matrix3x3()
31  {
32  m11 = 0.0;
33  m12 = 0.0;
34  m13 = 0.0;
35  m21 = 0.0;
36  m22 = 0.0;
37  m23 = 0.0;
38  m31 = 0.0;
39  m32 = 0.0;
40  m33 = 0.0;
41  }
42 
43  inline Matrix3x3& operator=(const Matrix3x3& m) = default;
44 
45  inline Matrix3x3 operator-(double d) const
46  {
47  Matrix3x3 m;
48  m.m11 = m11 - d;
49  m.m12 = m12 - d;
50  m.m13 = m13 - d;
51  m.m21 = m21 - d;
52  m.m22 = m22 - d;
53  m.m23 = m23 - d;
54  m.m31 = m31 - d;
55  m.m32 = m32 - d;
56  m.m33 = m33 - d;
57 
58  return m;
59  }
60 
61  inline Matrix3x3 operator/(double d) const
62  {
63  Matrix3x3 m;
64  m.m11 = m11 / d;
65  m.m12 = m12 / d;
66  m.m13 = m13 / d;
67  m.m21 = m21 / d;
68  m.m22 = m22 / d;
69  m.m23 = m23 / d;
70  m.m31 = m31 / d;
71  m.m32 = m32 / d;
72  m.m33 = m33 / d;
73 
74  return m;
75  }
76 
77  inline Matrix3x3 operator-() const
78  {
79  Matrix3x3 m;
80  m.m11 = -m11;
81  m.m12 = -m12;
82  m.m13 = -m13;
83  m.m21 = -m21;
84  m.m22 = -m22;
85  m.m23 = -m23;
86  m.m31 = -m31;
87  m.m32 = -m32;
88  m.m33 = -m33;
89 
90  return m;
91  }
92 
93  inline Matrix3x3 operator-(const Matrix3x3& M) const
94  {
95  Matrix3x3 m;
96  m.m11 = m11 - M.m11;
97  m.m12 = m12 - M.m12;
98  m.m13 = m13 - M.m13;
99  m.m21 = m21 - M.m21;
100  m.m22 = m22 - M.m22;
101  m.m23 = m23 - M.m23;
102  m.m31 = m31 - M.m31;
103  m.m32 = m32 - M.m32;
104  m.m33 = m33 - M.m33;
105 
106  return m;
107  }
108 
109  inline Matrix3x3 operator*(const Matrix3x3& _m) const
110  {
111  Matrix3x3 m;
112  m.m11 = m11 * _m.m11 + m12 * _m.m21 + m13 * _m.m31;
113  m.m12 = m11 * _m.m12 + m12 * _m.m22 + m13 * _m.m32;
114  m.m13 = m11 * _m.m13 + m12 * _m.m23 + m13 * _m.m33;
115 
116  m.m21 = m21 * _m.m11 + m22 * _m.m21 + m23 * _m.m31;
117  m.m22 = m21 * _m.m12 + m22 * _m.m22 + m23 * _m.m32;
118  m.m23 = m21 * _m.m13 + m22 * _m.m23 + m23 * _m.m33;
119 
120  m.m31 = m31 * _m.m11 + m32 * _m.m21 + m33 * _m.m31;
121  m.m32 = m31 * _m.m12 + m32 * _m.m22 + m33 * _m.m32;
122  m.m33 = m31 * _m.m13 + m32 * _m.m23 + m33 * _m.m33;
123 
124  return m;
125  }
126 
127  inline Point3d operator*(const Point2d& _p) const
128  {
129  Point3d p;
130  p.x = m11 * _p.x + m12 * _p.y + m13;
131  p.y = m21 * _p.x + m22 * _p.y + m23;
132  p.z = m31 * _p.x + m32 * _p.y + m33;
133  return p;
134  }
135 
136  inline Point2d operator^(const Point2d& _p) const
137  {
138  Point3d p = *this * _p;
139  Point2d pix = Point2d(p.x / p.z, p.y / p.z);
140  return pix;
141  }
142 
143  inline Point3d operator*(const Point3d& _p) const
144  {
145  Point3d p;
146  p.x = m11 * _p.x + m12 * _p.y + m13 * _p.z;
147  p.y = m21 * _p.x + m22 * _p.y + m23 * _p.z;
148  p.z = m31 * _p.x + m32 * _p.y + m33 * _p.z;
149  return p;
150  }
151 
152  inline Matrix3x3 transpose() const
153  {
154  Matrix3x3 m;
155  m.m11 = m11;
156  m.m12 = m21;
157  m.m13 = m31;
158  m.m21 = m12;
159  m.m22 = m22;
160  m.m23 = m32;
161  m.m31 = m13;
162  m.m32 = m23;
163  m.m33 = m33;
164 
165  return m;
166  }
167 
168  inline double deteminant() const
169  {
170  return m11 * m22 * m33 - m11 * m23 * m32 - m12 * m21 * m33 + m12 * m23 * m31 + m13 * m21 * m32 - m13 * m22 * m31;
171  }
172 
173  inline bool isSingular() const { return (deteminant() == 0.0f); }
174 
175  inline double det() const { return m11 * (m33 * m22 - m32 * m23) - m21 * (m33 * m12 - m32 * m13) + m31 * (m23 * m12 - m22 * m13); }
176 
177  inline Matrix3x3 inverse() const
178  {
179  Matrix3x3 m;
180  inverse(m);
181  return m;
182  }
183 
184  inline Point3d mldivide(const Point3d& b)
185  {
186  if (!isSingular())
187  {
188  return inverse() * b;
189  }
190  throw std::runtime_error("Matrix is singular.");
191  }
192 
193  inline void RQ(Matrix3x3& R, Matrix3x3& Q)
194  {
195  /*
196  Matrix3x3 S = transpodeAndEnd1End1();
197 
198 
199  printf("S\n");
200  S.doprintf();
201 
202  S.QR(Q, R);
203 
204  printf("R\n");
205  R.doprintf();
206  printf("Q\n");
207  Q.doprintf();
208 
209 
210  Q = Q.transpodeAndEnd1End1();
211  R = R.transpodeAndEnd1End1();
212 
213  if (Q.deteminant()<0.0f)
214  {
215  R.m11 = -R.m11;
216  R.m21 = -R.m21;
217  R.m31 = -R.m31;
218 
219  Q.m11 = -Q.m11;
220  Q.m12 = -Q.m12;
221  Q.m13 = -Q.m13;
222  };
223  */
224 
225  Point3d a1 = Point3d(m31, m32, m33);
226  Point3d a2 = Point3d(m21, m22, m23);
227  Point3d a3 = Point3d(m11, m12, m13);
228 
229  Point3d u1 = a1;
230  Point3d e1 = u1.normalize();
231  Point3d u2 = a2 - proj(e1, a2);
232  Point3d e2 = u2.normalize();
233  Point3d u3 = a3 - proj(e1, a3) - proj(e2, a3);
234  Point3d e3 = u3.normalize();
235 
236  Q.m11 = e3.x;
237  Q.m12 = e3.y;
238  Q.m13 = e3.z;
239  Q.m21 = e2.x;
240  Q.m22 = e2.y;
241  Q.m23 = e2.z;
242  Q.m31 = e1.x;
243  Q.m32 = e1.y;
244  Q.m33 = e1.z;
245 
246  R.m11 = dot(e1, a1);
247  R.m12 = dot(e1, a2);
248  R.m13 = dot(e1, a3);
249  R.m21 = 0.0f;
250  R.m22 = dot(e2, a2);
251  R.m23 = dot(e2, a3);
252  R.m31 = 0.0f;
253  R.m32 = 0.0f;
254  R.m33 = dot(e3, a3);
255 
256  Matrix3x3 RR = R;
257  R.m11 = RR.m33;
258  R.m12 = RR.m23;
259  R.m13 = RR.m13;
260  R.m21 = RR.m32;
261  R.m22 = RR.m22;
262  R.m23 = RR.m12;
263  R.m31 = RR.m31;
264  R.m32 = RR.m21;
265  R.m33 = RR.m11;
266  }
267 
268  inline bool inverse(Matrix3x3& m) const
269  {
270  double dt = det();
271 
272  if ((fabs(dt) < 0.00000001f) || std::isnan(dt))
273  {
274  return false;
275  }
276 
277  m.m11 = (m33 * m22 - m32 * m23) / dt;
278  m.m12 = -(m33 * m12 - m32 * m13) / dt;
279  m.m13 = (m23 * m12 - m22 * m13) / dt;
280  m.m21 = -(m33 * m21 - m31 * m23) / dt;
281  m.m22 = (m33 * m11 - m31 * m13) / dt;
282  m.m23 = -(m23 * m11 - m21 * m13) / dt;
283  m.m31 = (m32 * m21 - m31 * m22) / dt;
284  m.m32 = -(m32 * m11 - m31 * m12) / dt;
285  m.m33 = (m22 * m11 - m21 * m12) / dt;
286 
287  return true;
288 
289  /*
290  | m11 m12 m13 |-1 | m33m22-m32m23 -(m33m12-m32m13) m23m12-m22m13 |
291  | m21 m22 m23 | = 1/DET * | -(m33m21-m31m23) m33m11-m31m13 -(m23m11-m21m13) |
292  | m31 m32 m33 | | m32m21-m31m22 -(m32m11-m31m12) m22m11-m21m12 |
293 
294  with DET = m11(m33m22-m32m23)-m21(m33m12-m32m13)+m31(m23m12-m22m13)
295  */
296 
297  /*
298  double mem[9];
299  mem[0] = (double)m11; mem[1] = (double)m12; mem[2] = (double)m13;
300  mem[3] = (double)m21; mem[4] = (double)m22; mem[5] = (double)m23;
301  mem[6] = (double)m31; mem[7] = (double)m32; mem[8] = (double)m33;
302 
303  Matrix<double> K(mem, 3, 3);
304  Matrix<double> KK = K;
305  Matrix<double> iK = KK.Inversed();
306 
307 
308  Matrix3x3 m;
309  m.m11 = (double)iK.M(1,1);
310  m.m12 = (double)iK.M(1,2);
311  m.m13 = (double)iK.M(1,3);
312  m.m21 = (double)iK.M(2,1);
313  m.m22 = (double)iK.M(2,2);
314  m.m23 = (double)iK.M(2,3);
315  m.m31 = (double)iK.M(3,1);
316  m.m32 = (double)iK.M(3,2);
317  m.m33 = (double)iK.M(3,3);
318 
319  K.Detach();
320 
321  return m;
322  */
323  }
324 };
325 
326 inline Matrix3x3 diag3x3(double d1, double d2, double d3)
327 {
328  Matrix3x3 m;
329  m.m11 = d1;
330  m.m12 = 0.0;
331  m.m13 = 0.0;
332 
333  m.m21 = 0.0;
334  m.m22 = d2;
335  m.m23 = 0.0;
336 
337  m.m31 = 0.0;
338  m.m32 = 0.0;
339  m.m33 = d3;
340 
341  return m;
342 }
343 
344 } // namespace aliceVision
aliceVision::Point3d
Definition: Point3d.hpp:24
aliceVision
Definition: checkerDetector.cpp:32
aliceVision::Point2d
Definition: Point2d.hpp:14
aliceVision::Matrix3x3
Definition: Matrix3x3.hpp:18
Generated on Tue Jun 2 2026 15:05:44 for AliceVision by   doxygen 1.8.17