vnl_inverse.h
Go to the documentation of this file.
1 // This is core/vnl/vnl_inverse.h
2 #ifndef vnl_inverse_h_
3 #define vnl_inverse_h_
4 //:
5 // \file
6 // \brief Calculates inverse of a small vnl_matrix_fixed (not using svd)
7 // \author Peter Vanroose
8 // \date 22 October 2002
9 //
10 // \verbatim
11 // Modifications
12 // 19 April 2003 - PVr - added interface for vnl_matrix<T>
13 // 19 April 2004 - PVr - made 4x4 implementation a bit more robust (but still incomplete)
14 // 18 June 2004 - PVr - finally completed 4x4 implementation
15 // 19 June 2004 - PVr - added vnl_inverse_transpose() methods
16 // \endverbatim
17 
18 #include <vnl/vnl_matrix_fixed.h>
19 #include <vnl/vnl_matrix.h>
20 #include <vnl/vnl_det.h>
21 #include <cassert>
22 #ifdef _MSC_VER
23 # include <vcl_msvc_warnings.h>
24 #endif
25 #include "vnl/vnl_export.h"
26 
27 //: Calculates inverse of a small vnl_matrix_fixed (not using svd)
28 // This allows you to write e.g.
29 //
30 // x = vnl_inverse(A) * b;
31 //
32 // Note that this function is inlined (except for the call to vnl_det()),
33 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
34 // since that one is using svd.
35 //
36 // \relatesalso vnl_matrix_fixed
37 
38 template <class T>
39 vnl_matrix_fixed<T,1,1> vnl_inverse(vnl_matrix_fixed<T,1,1> const& m)
40 {
41  return vnl_matrix_fixed<T,1,1>(T(1)/m(0,0));
42 }
43 
44 //: Calculates inverse of a small vnl_matrix_fixed (not using svd)
45 // This allows you to write e.g.
46 //
47 // x = vnl_inverse(A) * b;
48 //
49 // Note that this function is inlined (except for the call to vnl_det()),
50 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
51 // since that one is using svd.
52 //
53 // \relatesalso vnl_matrix_fixed
54 
55 template <class T>
56 vnl_matrix_fixed<T,2,2> vnl_inverse(vnl_matrix_fixed<T,2,2> const& m)
57 {
58  T det = vnl_det(m);
59  if (det==0) {
60  assert(!"Cannot invert 2x2 matrix with zero determinant");
61  return vnl_matrix_fixed<T,2,2>();
62  }
63  det = T(1)/det;
64  T d[4];
65  d[0] = m(1,1)*det; d[1] = - m(0,1)*det;
66  d[3] = m(0,0)*det; d[2] = - m(1,0)*det;
67  return vnl_matrix_fixed<T,2,2>(d);
68 }
69 
70 //: Calculates inverse of a small vnl_matrix_fixed (not using svd)
71 // This allows you to write e.g.
72 //
73 // x = vnl_inverse(A) * b;
74 //
75 // Note that this function is inlined (except for the call to vnl_det()),
76 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
77 // since that one is using svd.
78 //
79 // \relatesalso vnl_matrix_fixed
80 
81 template <class T>
82 vnl_matrix_fixed<T,3,3> vnl_inverse(vnl_matrix_fixed<T,3,3> const& m)
83 {
84  T det = vnl_det(m);
85  if (det==0) {
86  assert(!"Cannot invert 3x3 matrix with zero determinant");
87  return vnl_matrix_fixed<T,3,3>();
88  }
89  det = T(1)/det;
90  T d[9];
91  d[0] = (m(1,1)*m(2,2)-m(1,2)*m(2,1))*det;
92  d[1] = (m(2,1)*m(0,2)-m(2,2)*m(0,1))*det;
93  d[2] = (m(0,1)*m(1,2)-m(0,2)*m(1,1))*det;
94  d[3] = (m(1,2)*m(2,0)-m(1,0)*m(2,2))*det;
95  d[4] = (m(0,0)*m(2,2)-m(0,2)*m(2,0))*det;
96  d[5] = (m(1,0)*m(0,2)-m(1,2)*m(0,0))*det;
97  d[6] = (m(1,0)*m(2,1)-m(1,1)*m(2,0))*det;
98  d[7] = (m(0,1)*m(2,0)-m(0,0)*m(2,1))*det;
99  d[8] = (m(0,0)*m(1,1)-m(0,1)*m(1,0))*det;
100  return vnl_matrix_fixed<T,3,3>(d);
101 }
102 
103 //: Calculates inverse of a small vnl_matrix_fixed (not using svd)
104 // This allows you to write e.g.
105 //
106 // x = vnl_inverse(A) * b;
107 //
108 // Note that this function is inlined (except for the call to vnl_det()),
109 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
110 // since that one is using svd.
111 //
112 // \relatesalso vnl_matrix_fixed
113 
114 template <class T>
115 vnl_matrix_fixed<T,4,4> vnl_inverse(vnl_matrix_fixed<T,4,4> const& m)
116 {
117  T det = vnl_det(m);
118  if (det==0) {
119  assert(!"Cannot invert 4x4 matrix with zero determinant");
120  return vnl_matrix_fixed<T,4,4>();
121  }
122  det = T(1)/det;
123  T d[16];
124  d[0] = m(1,1)*m(2,2)*m(3,3) - m(1,1)*m(2,3)*m(3,2) - m(2,1)*m(1,2)*m(3,3)
125  + m(2,1)*m(1,3)*m(3,2) + m(3,1)*m(1,2)*m(2,3) - m(3,1)*m(1,3)*m(2,2);
126  d[1] = -m(0,1)*m(2,2)*m(3,3) + m(0,1)*m(2,3)*m(3,2) + m(2,1)*m(0,2)*m(3,3)
127  - m(2,1)*m(0,3)*m(3,2) - m(3,1)*m(0,2)*m(2,3) + m(3,1)*m(0,3)*m(2,2);
128  d[2] = m(0,1)*m(1,2)*m(3,3) - m(0,1)*m(1,3)*m(3,2) - m(1,1)*m(0,2)*m(3,3)
129  + m(1,1)*m(0,3)*m(3,2) + m(3,1)*m(0,2)*m(1,3) - m(3,1)*m(0,3)*m(1,2);
130  d[3] = -m(0,1)*m(1,2)*m(2,3) + m(0,1)*m(1,3)*m(2,2) + m(1,1)*m(0,2)*m(2,3)
131  - m(1,1)*m(0,3)*m(2,2) - m(2,1)*m(0,2)*m(1,3) + m(2,1)*m(0,3)*m(1,2);
132  d[4] = -m(1,0)*m(2,2)*m(3,3) + m(1,0)*m(2,3)*m(3,2) + m(2,0)*m(1,2)*m(3,3)
133  - m(2,0)*m(1,3)*m(3,2) - m(3,0)*m(1,2)*m(2,3) + m(3,0)*m(1,3)*m(2,2);
134  d[5] = m(0,0)*m(2,2)*m(3,3) - m(0,0)*m(2,3)*m(3,2) - m(2,0)*m(0,2)*m(3,3)
135  + m(2,0)*m(0,3)*m(3,2) + m(3,0)*m(0,2)*m(2,3) - m(3,0)*m(0,3)*m(2,2);
136  d[6] = -m(0,0)*m(1,2)*m(3,3) + m(0,0)*m(1,3)*m(3,2) + m(1,0)*m(0,2)*m(3,3)
137  - m(1,0)*m(0,3)*m(3,2) - m(3,0)*m(0,2)*m(1,3) + m(3,0)*m(0,3)*m(1,2);
138  d[7] = m(0,0)*m(1,2)*m(2,3) - m(0,0)*m(1,3)*m(2,2) - m(1,0)*m(0,2)*m(2,3)
139  + m(1,0)*m(0,3)*m(2,2) + m(2,0)*m(0,2)*m(1,3) - m(2,0)*m(0,3)*m(1,2);
140  d[8] = m(1,0)*m(2,1)*m(3,3) - m(1,0)*m(2,3)*m(3,1) - m(2,0)*m(1,1)*m(3,3)
141  + m(2,0)*m(1,3)*m(3,1) + m(3,0)*m(1,1)*m(2,3) - m(3,0)*m(1,3)*m(2,1);
142  d[9] = -m(0,0)*m(2,1)*m(3,3) + m(0,0)*m(2,3)*m(3,1) + m(2,0)*m(0,1)*m(3,3)
143  - m(2,0)*m(0,3)*m(3,1) - m(3,0)*m(0,1)*m(2,3) + m(3,0)*m(0,3)*m(2,1);
144  d[10]= m(0,0)*m(1,1)*m(3,3) - m(0,0)*m(1,3)*m(3,1) - m(1,0)*m(0,1)*m(3,3)
145  + m(1,0)*m(0,3)*m(3,1) + m(3,0)*m(0,1)*m(1,3) - m(3,0)*m(0,3)*m(1,1);
146  d[11]= -m(0,0)*m(1,1)*m(2,3) + m(0,0)*m(1,3)*m(2,1) + m(1,0)*m(0,1)*m(2,3)
147  - m(1,0)*m(0,3)*m(2,1) - m(2,0)*m(0,1)*m(1,3) + m(2,0)*m(0,3)*m(1,1);
148  d[12]= -m(1,0)*m(2,1)*m(3,2) + m(1,0)*m(2,2)*m(3,1) + m(2,0)*m(1,1)*m(3,2)
149  - m(2,0)*m(1,2)*m(3,1) - m(3,0)*m(1,1)*m(2,2) + m(3,0)*m(1,2)*m(2,1);
150  d[13]= m(0,0)*m(2,1)*m(3,2) - m(0,0)*m(2,2)*m(3,1) - m(2,0)*m(0,1)*m(3,2)
151  + m(2,0)*m(0,2)*m(3,1) + m(3,0)*m(0,1)*m(2,2) - m(3,0)*m(0,2)*m(2,1);
152  d[14]= -m(0,0)*m(1,1)*m(3,2) + m(0,0)*m(1,2)*m(3,1) + m(1,0)*m(0,1)*m(3,2)
153  - m(1,0)*m(0,2)*m(3,1) - m(3,0)*m(0,1)*m(1,2) + m(3,0)*m(0,2)*m(1,1);
154  d[15]= m(0,0)*m(1,1)*m(2,2) - m(0,0)*m(1,2)*m(2,1) - m(1,0)*m(0,1)*m(2,2)
155  + m(1,0)*m(0,2)*m(2,1) + m(2,0)*m(0,1)*m(1,2) - m(2,0)*m(0,2)*m(1,1);
156  return vnl_matrix_fixed<T,4,4>(d)*det;
157 }
158 
159 //: Calculates inverse of a small vnl_matrix_fixed (not using svd)
160 // This allows you to write e.g.
161 //
162 // x = vnl_inverse(A) * b;
163 //
164 // Note that this function is inlined (except for the call to vnl_det()),
165 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
166 // since that one is using svd.
167 //
168 // \relatesalso vnl_matrix
169 
170 template <class T>
171 vnl_matrix<T> vnl_inverse(vnl_matrix<T> const& m)
172 {
173  assert(m.rows() == m.columns());
174  assert(m.rows() <= 4);
175  if (m.rows() == 1)
176  return vnl_matrix<T>(1,1, T(1)/m(0,0));
177  else if (m.rows() == 2)
178  return vnl_inverse(vnl_matrix_fixed<T,2,2>(m)).as_ref();
179  else if (m.rows() == 3)
180  return vnl_inverse(vnl_matrix_fixed<T,3,3>(m)).as_ref();
181  else
182  return vnl_inverse(vnl_matrix_fixed<T,4,4>(m)).as_ref();
183 }
184 
185 //: Calculates transpose of the inverse of a small vnl_matrix_fixed (not using svd)
186 // This allows you to write e.g.
187 //
188 // x = vnl_inverse_transpose(A) * b;
189 //
190 // Note that this function is inlined (except for the call to vnl_det()),
191 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
192 // since that one is using svd. This is also faster than using
193 //
194 // x = vnl_inverse(A).transpose() * b;
195 //
196 // \relatesalso vnl_matrix_fixed
197 
198 template <class T>
199 vnl_matrix_fixed<T,1,1> vnl_inverse_transpose(vnl_matrix_fixed<T,1,1> const& m)
200 {
201  return vnl_matrix_fixed<T,1,1>(T(1)/m(0,0));
202 }
203 
204 //: Calculates transpose of the inverse of a small vnl_matrix_fixed (not using svd)
205 // This allows you to write e.g.
206 //
207 // x = vnl_inverse_transpose(A) * b;
208 //
209 // Note that this function is inlined (except for the call to vnl_det()),
210 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
211 // since that one is using svd. This is also faster than using
212 //
213 // x = vnl_inverse(A).transpose() * b;
214 //
215 // \relatesalso vnl_matrix_fixed
216 
217 template <class T>
218 vnl_matrix_fixed<T,2,2> vnl_inverse_transpose(vnl_matrix_fixed<T,2,2> const& m)
219 {
220  T det = vnl_det(m);
221  if (det==0) {
222  assert(!"Cannot invert 2x2 matrix with zero determinant");
223  return vnl_matrix_fixed<T,2,2>();
224  }
225  det = T(1)/det;
226  T d[4];
227  d[0] = m(1,1)*det; d[2] = - m(0,1)*det;
228  d[3] = m(0,0)*det; d[1] = - m(1,0)*det;
229  return vnl_matrix_fixed<T,2,2>(d);
230 }
231 
232 //: Calculates transpose of the inverse of a small vnl_matrix_fixed (not using svd)
233 // This allows you to write e.g.
234 //
235 // x = vnl_inverse_transpose(A) * b;
236 //
237 // Note that this function is inlined (except for the call to vnl_det()),
238 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
239 // since that one is using svd. This is also faster than using
240 //
241 // x = vnl_inverse(A).transpose() * b;
242 //
243 // \relatesalso vnl_matrix_fixed
244 
245 template <class T>
246 vnl_matrix_fixed<T,3,3> vnl_inverse_transpose(vnl_matrix_fixed<T,3,3> const& m)
247 {
248  T det = vnl_det(m);
249  if (det==0) {
250  assert(!"Cannot invert 3x3 matrix with zero determinant");
251  return vnl_matrix_fixed<T,3,3>();
252  }
253  det = T(1)/det;
254  T d[9];
255  d[0] = (m(1,1)*m(2,2)-m(1,2)*m(2,1))*det;
256  d[3] = (m(2,1)*m(0,2)-m(2,2)*m(0,1))*det;
257  d[6] = (m(0,1)*m(1,2)-m(0,2)*m(1,1))*det;
258  d[1] = (m(1,2)*m(2,0)-m(1,0)*m(2,2))*det;
259  d[4] = (m(0,0)*m(2,2)-m(0,2)*m(2,0))*det;
260  d[7] = (m(1,0)*m(0,2)-m(1,2)*m(0,0))*det;
261  d[2] = (m(1,0)*m(2,1)-m(1,1)*m(2,0))*det;
262  d[5] = (m(0,1)*m(2,0)-m(0,0)*m(2,1))*det;
263  d[8] = (m(0,0)*m(1,1)-m(0,1)*m(1,0))*det;
264  return vnl_matrix_fixed<T,3,3>(d);
265 }
266 
267 //: Calculates transpose of the inverse of a small vnl_matrix_fixed (not using svd)
268 // This allows you to write e.g.
269 //
270 // x = vnl_inverse_transpose(A) * b;
271 //
272 // Note that this function is inlined (except for the call to vnl_det()),
273 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
274 // since that one is using svd. This is also faster than using
275 //
276 // x = vnl_inverse(A).transpose() * b;
277 //
278 // \relatesalso vnl_matrix_fixed
279 
280 template <class T>
281 vnl_matrix_fixed<T,4,4> vnl_inverse_transpose(vnl_matrix_fixed<T,4,4> const& m)
282 {
283  T det = vnl_det(m);
284  if (det==0) {
285  assert(!"Cannot invert 4x4 matrix with zero determinant");
286  return vnl_matrix_fixed<T,4,4>();
287  }
288  det = T(1)/det;
289  T d[16];
290  d[0] = m(1,1)*m(2,2)*m(3,3) - m(1,1)*m(2,3)*m(3,2) - m(2,1)*m(1,2)*m(3,3)
291  + m(2,1)*m(1,3)*m(3,2) + m(3,1)*m(1,2)*m(2,3) - m(3,1)*m(1,3)*m(2,2);
292  d[4] = -m(0,1)*m(2,2)*m(3,3) + m(0,1)*m(2,3)*m(3,2) + m(2,1)*m(0,2)*m(3,3)
293  - m(2,1)*m(0,3)*m(3,2) - m(3,1)*m(0,2)*m(2,3) + m(3,1)*m(0,3)*m(2,2);
294  d[8] = m(0,1)*m(1,2)*m(3,3) - m(0,1)*m(1,3)*m(3,2) - m(1,1)*m(0,2)*m(3,3)
295  + m(1,1)*m(0,3)*m(3,2) + m(3,1)*m(0,2)*m(1,3) - m(3,1)*m(0,3)*m(1,2);
296  d[12]= -m(0,1)*m(1,2)*m(2,3) + m(0,1)*m(1,3)*m(2,2) + m(1,1)*m(0,2)*m(2,3)
297  - m(1,1)*m(0,3)*m(2,2) - m(2,1)*m(0,2)*m(1,3) + m(2,1)*m(0,3)*m(1,2);
298  d[1] = -m(1,0)*m(2,2)*m(3,3) + m(1,0)*m(2,3)*m(3,2) + m(2,0)*m(1,2)*m(3,3)
299  - m(2,0)*m(1,3)*m(3,2) - m(3,0)*m(1,2)*m(2,3) + m(3,0)*m(1,3)*m(2,2);
300  d[5] = m(0,0)*m(2,2)*m(3,3) - m(0,0)*m(2,3)*m(3,2) - m(2,0)*m(0,2)*m(3,3)
301  + m(2,0)*m(0,3)*m(3,2) + m(3,0)*m(0,2)*m(2,3) - m(3,0)*m(0,3)*m(2,2);
302  d[9] = -m(0,0)*m(1,2)*m(3,3) + m(0,0)*m(1,3)*m(3,2) + m(1,0)*m(0,2)*m(3,3)
303  - m(1,0)*m(0,3)*m(3,2) - m(3,0)*m(0,2)*m(1,3) + m(3,0)*m(0,3)*m(1,2);
304  d[13]= m(0,0)*m(1,2)*m(2,3) - m(0,0)*m(1,3)*m(2,2) - m(1,0)*m(0,2)*m(2,3)
305  + m(1,0)*m(0,3)*m(2,2) + m(2,0)*m(0,2)*m(1,3) - m(2,0)*m(0,3)*m(1,2);
306  d[2] = m(1,0)*m(2,1)*m(3,3) - m(1,0)*m(2,3)*m(3,1) - m(2,0)*m(1,1)*m(3,3)
307  + m(2,0)*m(1,3)*m(3,1) + m(3,0)*m(1,1)*m(2,3) - m(3,0)*m(1,3)*m(2,1);
308  d[6] = -m(0,0)*m(2,1)*m(3,3) + m(0,0)*m(2,3)*m(3,1) + m(2,0)*m(0,1)*m(3,3)
309  - m(2,0)*m(0,3)*m(3,1) - m(3,0)*m(0,1)*m(2,3) + m(3,0)*m(0,3)*m(2,1);
310  d[10]= m(0,0)*m(1,1)*m(3,3) - m(0,0)*m(1,3)*m(3,1) - m(1,0)*m(0,1)*m(3,3)
311  + m(1,0)*m(0,3)*m(3,1) + m(3,0)*m(0,1)*m(1,3) - m(3,0)*m(0,3)*m(1,1);
312  d[14]= -m(0,0)*m(1,1)*m(2,3) + m(0,0)*m(1,3)*m(2,1) + m(1,0)*m(0,1)*m(2,3)
313  - m(1,0)*m(0,3)*m(2,1) - m(2,0)*m(0,1)*m(1,3) + m(2,0)*m(0,3)*m(1,1);
314  d[3] = -m(1,0)*m(2,1)*m(3,2) + m(1,0)*m(2,2)*m(3,1) + m(2,0)*m(1,1)*m(3,2)
315  - m(2,0)*m(1,2)*m(3,1) - m(3,0)*m(1,1)*m(2,2) + m(3,0)*m(1,2)*m(2,1);
316  d[7] = m(0,0)*m(2,1)*m(3,2) - m(0,0)*m(2,2)*m(3,1) - m(2,0)*m(0,1)*m(3,2)
317  + m(2,0)*m(0,2)*m(3,1) + m(3,0)*m(0,1)*m(2,2) - m(3,0)*m(0,2)*m(2,1);
318  d[11]= -m(0,0)*m(1,1)*m(3,2) + m(0,0)*m(1,2)*m(3,1) + m(1,0)*m(0,1)*m(3,2)
319  - m(1,0)*m(0,2)*m(3,1) - m(3,0)*m(0,1)*m(1,2) + m(3,0)*m(0,2)*m(1,1);
320  d[15]= m(0,0)*m(1,1)*m(2,2) - m(0,0)*m(1,2)*m(2,1) - m(1,0)*m(0,1)*m(2,2)
321  + m(1,0)*m(0,2)*m(2,1) + m(2,0)*m(0,1)*m(1,2) - m(2,0)*m(0,2)*m(1,1);
322  return vnl_matrix_fixed<T,4,4>(d)*det;
323 }
324 
325 //: Calculates transpose of the inverse of a small vnl_matrix_fixed (not using svd)
326 // This allows you to write e.g.
327 //
328 // x = vnl_inverse_transpose(A) * b;
329 //
330 // Note that this function is inlined (except for the call to vnl_det()),
331 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
332 // since that one is using svd. This is also faster than using
333 //
334 // x = vnl_inverse(A).transpose() * b;
335 //
336 // \relatesalso vnl_matrix
337 
338 template <class T>
339 vnl_matrix<T> vnl_inverse_transpose(vnl_matrix<T> const& m)
340 {
341  assert(m.rows() == m.columns());
342  assert(m.rows() <= 4);
343  if (m.rows() == 1)
344  return vnl_matrix<T>(1,1, T(1)/m(0,0));
345  else if (m.rows() == 2)
346  return vnl_inverse_transpose(vnl_matrix_fixed<T,2,2>(m)).as_ref();
347  else if (m.rows() == 3)
348  return vnl_inverse_transpose(vnl_matrix_fixed<T,3,3>(m)).as_ref();
349  else
350  return vnl_inverse_transpose(vnl_matrix_fixed<T,4,4>(m)).as_ref();
351 }
352 
353 #endif // vnl_inverse_h_
vnl_matrix_fixed::vnl_inverse
vnl_matrix_fixed< T, 1, 1 > vnl_inverse(vnl_matrix_fixed< T, 1, 1 > const &m)
Calculates inverse of a small vnl_matrix_fixed (not using svd).
Definition: vnl_inverse.h:39
vnl_matrix.h
An ordinary mathematical matrix.
m
#define m
Definition: vnl_vector.h:43
vnl_matrix_fixed::vnl_inverse_transpose
vnl_matrix_fixed< T, 1, 1 > vnl_inverse_transpose(vnl_matrix_fixed< T, 1, 1 > const &m)
Calculates transpose of the inverse of a small vnl_matrix_fixed (not using svd).
Definition: vnl_inverse.h:199
vnl_matrix_fixed
Fixed size, stack-stored, space-efficient matrix.
Definition: vnl_fwd.h:23
vnl_det.h
Direct evaluation of 2x2, 3x3 and 4x4 determinants.
vnl_matrix
An ordinary mathematical matrix.
Definition: vnl_adjugate.h:22
vnl_matrix_fixed.h
fixed size matrix
vnl_matrix_fixed::vnl_det
T vnl_det(vnl_matrix_fixed< T, 1, 1 > const &m)
Determinant of small size matrices.
Definition: vnl_det.h:36
Generated by   doxygen 1.8.15