inverse of a symmetric matrix... taking into account singularity due to numerical error...

inverse.cc

template < class MatrixType >                                                                            
MatrixType inverse(const MatrixType & m)                                                                 
{                                                                                                        
  typedef itk::Vector<typename MatrixType::ValueType, MatrixType::RowDimensions> VectorType;             
  typedef itk::SymmetricEigenAnalysis<MatrixType,VectorType> EigenAnalysisType;                          
  EigenAnalysisType eigenAnalysis;                                                                       
  eigenAnalysis.SetOrderEigenMagnitudes(true); // order by abs val of eigenvals..                        
  eigenAnalysis.SetDimension(MatrixType::RowDimensions);                                                 
  VectorType eigenVals;                                                                                  
  MatrixType eigenVecs;                                                                                  
  eigenAnalysis.ComputeEigenValuesAndVectors( m, eigenVals, eigenVecs );                                 
                                                                                                         
  // determinant:                                                                                        
  float determinant = 1.f;                                                                               
  float epsilon = 10e-4;                                                                                 
  float totalPower = 0.f;                                                                                
  size_t i;                                                                                              
  std::cerr << "eigenvals: ";                                                                            
  for(i=0; i<eigenVals.Size(); ++i)                                                                      
  {                                                                                                      
    if( eigenVals[i] < 0 && fabs(eigenVals[i]) < epsilon )                                               
    {                                                                                                    
      // probably negative due to numerical error                                                        
      // negative determinants cause problems later on                                                   
      // so I flip them here...                                                                          
      eigenVals[i] *= -1;                                                                                
    }                                                                                                    
    std::cerr << eigenVals[i] << ",";                                                                    
    determinant *= eigenVals[i];                                                                         
    totalPower += fabs(eigenVals[i]);                                                                    
  }                                                                                                      
  std::cerr << std::endl;                                                                                
                                                                                                         
  std::cerr << "determinant: " << determinant << std::endl;                                              
                                                                                                         
  // inverse:                                                                                            
  float multiplier = 0.f;                                                                                
  if( fabs(determinant) < epsilon ) // avoid singular matrices                                           
  {                                                                                                      
    float power = 0.f;                                                                                   
    // analysis sorts in reverse order..                                                                 
    for(i=eigenVals.Size(); i>=0; --i)                                                                   
    {                                                                                                    
      power += fabs(eigenVals[i]);                                                                       
      if( power/totalPower > .95 )                                                                       
      {                                                                                                  
        break; // found "most" of the power of the matrix                                                
      }                                                                                                  
    }                                                                                                    
    multiplier = fabs(eigenVals[i]);                                                                     
  }                                                                                                           
  MatrixType inv;                                                                                        
  inv.Fill(0);                                                                                           
  for(size_t i=0; i<eigenVals.Size(); ++i)                                                               
  {                                                                                                      
    if( eigenVals[i] < 0 )                                                                               
    {
      inv[i][i] = 1/(eigenVals[i] - multiplier);
    }
    else
    {
      inv[i][i] = 1/(eigenVals[i] + multiplier);
    }
  }
  // Inv = V' * Sigma^(-1) * V
  inv  = MatrixType(eigenVecs.GetTranspose()) * inv * eigenVecs;
  return inv;
}