00001
00002 #ifndef vnl_inverse_h_
00003 #define vnl_inverse_h_
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <vnl/vnl_matrix_fixed.h>
00019 #include <vnl/vnl_matrix.h>
00020 #include <vnl/vnl_det.h>
00021 #include <vcl_cassert.h>
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 template <class T>
00035 vnl_matrix_fixed<T,1,1> vnl_inverse(vnl_matrix_fixed<T,1,1> const& m)
00036 {
00037 return vnl_matrix_fixed<T,1,1>(T(1)/m(0,0));
00038 }
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 template <class T>
00052 vnl_matrix_fixed<T,2,2> vnl_inverse(vnl_matrix_fixed<T,2,2> const& m)
00053 {
00054 T det = vnl_det(m);
00055 if (det==0) {
00056 assert(!"Cannot invert 2x2 matrix with zero determinant");
00057 return vnl_matrix_fixed<T,2,2>();
00058 }
00059 det = T(1)/det;
00060 T d[4];
00061 d[0] = m(1,1)*det; d[1] = - m(0,1)*det;
00062 d[3] = m(0,0)*det; d[2] = - m(1,0)*det;
00063 return vnl_matrix_fixed<T,2,2>(d);
00064 }
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077 template <class T>
00078 vnl_matrix_fixed<T,3,3> vnl_inverse(vnl_matrix_fixed<T,3,3> const& m)
00079 {
00080 T det = vnl_det(m);
00081 if (det==0) {
00082 assert(!"Cannot invert 3x3 matrix with zero determinant");
00083 return vnl_matrix_fixed<T,3,3>();
00084 }
00085 det = T(1)/det;
00086 T d[9];
00087 d[0] = (m(1,1)*m(2,2)-m(1,2)*m(2,1))*det;
00088 d[1] = (m(2,1)*m(0,2)-m(2,2)*m(0,1))*det;
00089 d[2] = (m(0,1)*m(1,2)-m(0,2)*m(1,1))*det;
00090 d[3] = (m(1,2)*m(2,0)-m(1,0)*m(2,2))*det;
00091 d[4] = (m(0,0)*m(2,2)-m(0,2)*m(2,0))*det;
00092 d[5] = (m(1,0)*m(0,2)-m(1,2)*m(0,0))*det;
00093 d[6] = (m(1,0)*m(2,1)-m(1,1)*m(2,0))*det;
00094 d[7] = (m(0,1)*m(2,0)-m(0,0)*m(2,1))*det;
00095 d[8] = (m(0,0)*m(1,1)-m(0,1)*m(1,0))*det;
00096 return vnl_matrix_fixed<T,3,3>(d);
00097 }
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110 template <class T>
00111 vnl_matrix_fixed<T,4,4> vnl_inverse(vnl_matrix_fixed<T,4,4> const& m)
00112 {
00113 T det = vnl_det(m);
00114 if (det==0) {
00115 assert(!"Cannot invert 4x4 matrix with zero determinant");
00116 return vnl_matrix_fixed<T,4,4>();
00117 }
00118 det = T(1)/det;
00119 T d[16];
00120 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)
00121 + 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);
00122 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)
00123 - 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);
00124 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)
00125 + 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);
00126 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)
00127 - 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);
00128 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)
00129 - 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);
00130 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)
00131 + 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);
00132 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)
00133 - 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);
00134 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)
00135 + 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);
00136 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)
00137 + 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);
00138 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)
00139 - 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);
00140 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)
00141 + 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);
00142 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)
00143 - 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);
00144 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)
00145 - 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);
00146 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)
00147 + 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);
00148 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)
00149 - 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);
00150 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)
00151 + 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);
00152 return vnl_matrix_fixed<T,4,4>(d)*det;
00153 }
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166 template <class T>
00167 vnl_matrix<T> vnl_inverse(vnl_matrix<T> const& m)
00168 {
00169 assert(m.rows() == m.columns());
00170 assert(m.rows() <= 4);
00171 if (m.rows() == 1)
00172 return vnl_matrix<T>(1,1, T(1)/m(0,0));
00173 else if (m.rows() == 2)
00174 return vnl_inverse(vnl_matrix_fixed<T,2,2>(m)).as_ref();
00175 else if (m.rows() == 3)
00176 return vnl_inverse(vnl_matrix_fixed<T,3,3>(m)).as_ref();
00177 else
00178 return vnl_inverse(vnl_matrix_fixed<T,4,4>(m)).as_ref();
00179 }
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194 template <class T>
00195 vnl_matrix_fixed<T,1,1> vnl_inverse_transpose(vnl_matrix_fixed<T,1,1> const& m)
00196 {
00197 return vnl_matrix_fixed<T,1,1>(T(1)/m(0,0));
00198 }
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213 template <class T>
00214 vnl_matrix_fixed<T,2,2> vnl_inverse_transpose(vnl_matrix_fixed<T,2,2> const& m)
00215 {
00216 T det = vnl_det(m);
00217 if (det==0) {
00218 assert(!"Cannot invert 2x2 matrix with zero determinant");
00219 return vnl_matrix_fixed<T,2,2>();
00220 }
00221 det = T(1)/det;
00222 T d[4];
00223 d[0] = m(1,1)*det; d[2] = - m(0,1)*det;
00224 d[3] = m(0,0)*det; d[1] = - m(1,0)*det;
00225 return vnl_matrix_fixed<T,2,2>(d);
00226 }
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241 template <class T>
00242 vnl_matrix_fixed<T,3,3> vnl_inverse_transpose(vnl_matrix_fixed<T,3,3> const& m)
00243 {
00244 T det = vnl_det(m);
00245 if (det==0) {
00246 assert(!"Cannot invert 3x3 matrix with zero determinant");
00247 return vnl_matrix_fixed<T,3,3>();
00248 }
00249 det = T(1)/det;
00250 T d[9];
00251 d[0] = (m(1,1)*m(2,2)-m(1,2)*m(2,1))*det;
00252 d[3] = (m(2,1)*m(0,2)-m(2,2)*m(0,1))*det;
00253 d[6] = (m(0,1)*m(1,2)-m(0,2)*m(1,1))*det;
00254 d[1] = (m(1,2)*m(2,0)-m(1,0)*m(2,2))*det;
00255 d[4] = (m(0,0)*m(2,2)-m(0,2)*m(2,0))*det;
00256 d[7] = (m(1,0)*m(0,2)-m(1,2)*m(0,0))*det;
00257 d[2] = (m(1,0)*m(2,1)-m(1,1)*m(2,0))*det;
00258 d[5] = (m(0,1)*m(2,0)-m(0,0)*m(2,1))*det;
00259 d[8] = (m(0,0)*m(1,1)-m(0,1)*m(1,0))*det;
00260 return vnl_matrix_fixed<T,3,3>(d);
00261 }
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276 template <class T>
00277 vnl_matrix_fixed<T,4,4> vnl_inverse_transpose(vnl_matrix_fixed<T,4,4> const& m)
00278 {
00279 T det = vnl_det(m);
00280 if (det==0) {
00281 assert(!"Cannot invert 4x4 matrix with zero determinant");
00282 return vnl_matrix_fixed<T,4,4>();
00283 }
00284 det = T(1)/det;
00285 T d[16];
00286 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)
00287 + 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);
00288 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)
00289 - 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);
00290 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)
00291 + 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);
00292 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)
00293 - 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);
00294 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)
00295 - 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);
00296 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)
00297 + 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);
00298 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)
00299 - 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);
00300 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)
00301 + 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);
00302 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)
00303 + 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);
00304 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)
00305 - 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);
00306 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)
00307 + 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);
00308 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)
00309 - 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);
00310 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)
00311 - 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);
00312 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)
00313 + 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);
00314 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)
00315 - 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);
00316 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)
00317 + 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);
00318 return vnl_matrix_fixed<T,4,4>(d)*det;
00319 }
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334 template <class T>
00335 vnl_matrix<T> vnl_inverse_transpose(vnl_matrix<T> const& m)
00336 {
00337 assert(m.rows() == m.columns());
00338 assert(m.rows() <= 4);
00339 if (m.rows() == 1)
00340 return vnl_matrix<T>(1,1, T(1)/m(0,0));
00341 else if (m.rows() == 2)
00342 return vnl_inverse_transpose(vnl_matrix_fixed<T,2,2>(m)).as_ref();
00343 else if (m.rows() == 3)
00344 return vnl_inverse_transpose(vnl_matrix_fixed<T,3,3>(m)).as_ref();
00345 else
00346 return vnl_inverse_transpose(vnl_matrix_fixed<T,4,4>(m)).as_ref();
00347 }
00348
00349 #endif // vnl_inverse_h_