prob.py
Go to the documentation of this file.
00001 # Probability Utility Functions
00002 import numpy
00003 #import fun
00004 from itertools import *
00005 from numpy.linalg.linalg import det
00006 from numpy.linalg.linalg import inv
00007 from StringIO import StringIO 
00008 
00009 def cov(m):
00010    """
00011    Returns an estimate of the covariance matrix associated with the data m.
00012    m is a dxn matrix where d is the number of dimensions and n is the number of points
00013    """
00014    if (m.__class__ != numpy.matrix):
00015       raise AssertionError
00016    num_el = m.shape[1]
00017    mean   = m.mean(1)
00018    #msub   = m - numpy.tile(mean, (1, num_el))
00019    msub   = (m.T - mean.A[:,0]).T
00020    return (msub * msub.T) / (num_el - 1)
00021 
00022 
00023 def _weight(mat, weights):
00024    """
00025    Deprecated: This is an inefficient implementation.
00026    Returns the result of mat*eye(weights).
00027    """
00028    print "prob._weight: WARNING (Deprecated) This is an inefficient implementation."
00029    for i in xrange(mat.shape[1]):
00030       mat[:,i] = mat[:,i] * weights[0,i]
00031    return mat
00032 
00033 
00034 def cov_w(mat, weights):
00035    """
00036    Returns a weighted covariance matrix.
00037    """
00038    if (mat.__class__ != numpy.matrix):
00039       raise AssertionError
00040    num_el = mat.shape[1]
00041    total_weights = numpy.sum(weights)
00042    #mat_w         = _weight(mat.copy(), weights)
00043    mat_w         = numpy.matrix(mat.A * weights.A)
00044    mean          = numpy.sum(mat_w, axis=1) / total_weights
00045 
00046    #tiled_mean    = numpy.tile(mean, (1, num_el))
00047    #m_sub         = mat - tiled_mean
00048    m_sub         = (mat.T - mean.A[:,0]).T
00049    m_sub_t       = m_sub.T 
00050    #result = (_weight(m_sub, weights) * m_sub_t) / (total_weights - 1)
00051    result = (numpy.matrix(m_sub.A * weights.A) * m_sub_t) / (total_weights - 1)
00052    return result
00053 
00054 
00055 def fit(points):
00056    """
00057    Returns a Gaussian object fit to input points.
00058    """
00059    if (points.__class__ != numpy.matrix):
00060       raise RuntimeError("Param points is not of type matrix")
00061    mean = points.mean(1)
00062    cov = cov(points)
00063    return Gaussian(mean, cov)
00064 
00065 
00066 def fit(points, weights):
00067    """
00068    Returns a Gaussian object fit to the weighted input points.
00069    """
00070    if (points.__class__ != numpy.matrix):
00071       raise RuntimeError("Param points is not of type matrix")
00072    #def wmul(el): 
00073    #   x,w = el
00074    #   return x*w
00075    #paired = izip(fun.points_of_mat(points), fun.points_of_mat(weights))
00076    #mean0 = reduce(lambda x, y: x+y, imap(wmul, paired)) / weights.sum()
00077    mean = (points * numpy.matrix(weights.A[0,:]).T)/weights.sum()
00078    mean = numpy.matrix(mean).T
00079    cov  = cov_w(points, weights)
00080    return Gaussian(mean, cov)
00081 
00082 
00083 class Gaussian(object):
00084     """ 
00085         Class for multidimensional Gaussians 
00086         TODO: make m & v optional
00087     """
00088 
00089     def __init__(self, m, v, dimension=None):
00090        if dimension is not None:
00091           #dx1 where d is the number of dimensions
00092           self.mean = numpy.matrix(numpy.zeros((dimension,1)))
00093           #dxd
00094           self.cov  = numpy.matrix(numpy.eye(dimension))
00095        else:
00096 # forcibly make the mean a NX1 column vector - Advait (for numpy 1.0.3)
00097           sort_shape = numpy.sort(m.shape)
00098           self.mean = numpy.matrix(m).reshape(sort_shape[1], 1)
00099 #          self.mean = m
00100           self.cov = v
00101 
00102     def eval(self,x):
00103        """Evaluate the pdf at a point
00104        x is Nx1"""
00105        pdf = self.pdf()
00106        return pdf(x)
00107 
00108     def pdf(self):
00109        """ Partially applied Gaussian pdf """
00110        dim     = self.mean.shape[0]
00111        const   = 1 / (((2*numpy.pi)**(dim/2.0)) * (det(self.cov)**0.5))
00112        inv_cov = inv(self.cov)
00113        def gauss_pdf(x):
00114           sub = x - self.mean
00115           exponent = -0.5* sub.T * inv_cov * sub
00116           if (numpy.shape(exponent) != (1,1)):
00117              raise AssertionError
00118           return const * (numpy.e ** exponent[0,0])
00119        return gauss_pdf  
00120 
00121 
00122     def pdf_mat(self):
00123        """ Return a partially applied Gaussian pdf that takes in a matrix whose columns are the input vectors"""
00124        dim     = self.mean.shape[0]
00125        const   = 1 / (((2*numpy.pi)**(dim/2.0)) * (det(self.cov)**0.5))
00126        inv_cov = inv(self.cov)
00127        def gauss_pdf_mat(x):
00128           """Partially applied Gaussian pdf that takes in a matrix whose columns are the input vectors"""
00129           sub = x - self.mean
00130           r0 = inv_cov * sub
00131           exponent = -0.5 * numpy.sum(sub.A * r0.A, axis=0)
00132           if (numpy.shape(exponent) != (x.shape[1],)):
00133              raise AssertionError("exponent has the wrong shape, should be (%d,), but is (%d,)" % x.shape[1], exponent.shape[0])
00134           g = const * (numpy.e ** exponent)
00135           return g
00136        return gauss_pdf_mat
00137 
00138 
00139     def sample(self):
00140        """
00141        Returns a sample drawn from the Gaussian pdf.
00142        """
00143        return numpy.matrix(numpy.random.multivariate_normal(self.mean.T.A[0], self.cov.A)).T
00144 
00145     def __sub__(self, other):
00146        """
00147        Compare gaussians using bootlegged frobenius norm
00148        """
00149        mean_diff = numpy.linalg.norm(self.mean - other.mean)
00150        cov_diff  = numpy.linalg.norm(self.cov - other.cov)
00151        return mean_diff + cov_diff
00152 
00153     def __str__(self):
00154        """
00155        Prints the parameters associated with the Gaussian.
00156        """
00157        p = StringIO()
00158        print >>p, "(mean = ", self.mean.T, ", \n     cov = \n", self.cov, ")", 
00159        s = p.getvalue()
00160        return s
00161 
00162 


hrl_lib
Author(s): Cressel Anderson, Travis Deyle, Advait Jain, Hai Nguyen, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 11:34:06