core/vnl/vnl_inverse.h
Go to the documentation of this file.
00001 // This is core/vnl/vnl_inverse.h
00002 #ifndef vnl_inverse_h_
00003 #define vnl_inverse_h_
00004 //:
00005 // \file
00006 // \brief Calculates inverse of a small vnl_matrix_fixed (not using svd)
00007 // \author Peter Vanroose
00008 // \date   22 October 2002
00009 //
00010 // \verbatim
00011 //  Modifications
00012 //   19 April 2003 - PVr - added interface for vnl_matrix<T>
00013 //   19 April 2004 - PVr - made 4x4 implementation a bit more robust (but still incomplete)
00014 //   18 June  2004 - PVr - finally completed 4x4 implementation
00015 //   19 June  2004 - PVr - added vnl_inverse_transpose() methods
00016 // \endverbatim
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 //: Calculates inverse of a small vnl_matrix_fixed (not using svd)
00024 //  This allows you to write e.g.
00025 //
00026 //  x = vnl_inverse(A) * b;
00027 //
00028 // Note that this function is inlined (except for the call to vnl_det()),
00029 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
00030 // since that one is using svd.
00031 //
00032 //  \relatesalso vnl_matrix_fixed
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 //: Calculates inverse of a small vnl_matrix_fixed (not using svd)
00041 //  This allows you to write e.g.
00042 //
00043 //  x = vnl_inverse(A) * b;
00044 //
00045 // Note that this function is inlined (except for the call to vnl_det()),
00046 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
00047 // since that one is using svd.
00048 //
00049 //  \relatesalso vnl_matrix_fixed
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 //: Calculates inverse of a small vnl_matrix_fixed (not using svd)
00067 //  This allows you to write e.g.
00068 //
00069 //  x = vnl_inverse(A) * b;
00070 //
00071 // Note that this function is inlined (except for the call to vnl_det()),
00072 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
00073 // since that one is using svd.
00074 //
00075 //  \relatesalso vnl_matrix_fixed
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 //: Calculates inverse of a small vnl_matrix_fixed (not using svd)
00100 //  This allows you to write e.g.
00101 //
00102 //  x = vnl_inverse(A) * b;
00103 //
00104 // Note that this function is inlined (except for the call to vnl_det()),
00105 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
00106 // since that one is using svd.
00107 //
00108 //  \relatesalso vnl_matrix_fixed
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 //: Calculates inverse of a small vnl_matrix_fixed (not using svd)
00156 //  This allows you to write e.g.
00157 //
00158 //  x = vnl_inverse(A) * b;
00159 //
00160 // Note that this function is inlined (except for the call to vnl_det()),
00161 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
00162 // since that one is using svd.
00163 //
00164 //  \relatesalso vnl_matrix
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 //: Calculates transpose of the inverse of a small vnl_matrix_fixed (not using svd)
00182 //  This allows you to write e.g.
00183 //
00184 //  x = vnl_inverse_transpose(A) * b;
00185 //
00186 // Note that this function is inlined (except for the call to vnl_det()),
00187 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
00188 // since that one is using svd.  This is also faster than using
00189 //
00190 //  x = vnl_inverse(A).transpose() * b;
00191 //
00192 //  \relatesalso vnl_matrix_fixed
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 //: Calculates transpose of the inverse of a small vnl_matrix_fixed (not using svd)
00201 //  This allows you to write e.g.
00202 //
00203 //  x = vnl_inverse_transpose(A) * b;
00204 //
00205 // Note that this function is inlined (except for the call to vnl_det()),
00206 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
00207 // since that one is using svd.  This is also faster than using
00208 //
00209 //  x = vnl_inverse(A).transpose() * b;
00210 //
00211 //  \relatesalso vnl_matrix_fixed
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 //: Calculates transpose of the inverse of a small vnl_matrix_fixed (not using svd)
00229 //  This allows you to write e.g.
00230 //
00231 //  x = vnl_inverse_transpose(A) * b;
00232 //
00233 // Note that this function is inlined (except for the call to vnl_det()),
00234 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
00235 // since that one is using svd.  This is also faster than using
00236 //
00237 //  x = vnl_inverse(A).transpose() * b;
00238 //
00239 //  \relatesalso vnl_matrix_fixed
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 //: Calculates transpose of the inverse of a small vnl_matrix_fixed (not using svd)
00264 //  This allows you to write e.g.
00265 //
00266 //  x = vnl_inverse_transpose(A) * b;
00267 //
00268 // Note that this function is inlined (except for the call to vnl_det()),
00269 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
00270 // since that one is using svd.  This is also faster than using
00271 //
00272 //  x = vnl_inverse(A).transpose() * b;
00273 //
00274 //  \relatesalso vnl_matrix_fixed
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 //: Calculates transpose of the inverse of a small vnl_matrix_fixed (not using svd)
00322 //  This allows you to write e.g.
00323 //
00324 //  x = vnl_inverse_transpose(A) * b;
00325 //
00326 // Note that this function is inlined (except for the call to vnl_det()),
00327 // which makes it much faster than the vnl_matrix_inverse class in vnl/algo
00328 // since that one is using svd.  This is also faster than using
00329 //
00330 //  x = vnl_inverse(A).transpose() * b;
00331 //
00332 //  \relatesalso vnl_matrix
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_