def homog_2d(xi, theta):
    """
    Computes a 3x3 homogeneous transformation matrix given a 2D twist and a 
    joint displacement
    
    Args:
    xi - (3,) ndarray: the 2D twist
    (2,1,3)
    theta: the joint displacement
    0.658
    ret_desired = np.array([[-0.3924, -0.9198,  0.1491],
                         [ 0.9198, -0.3924,  1.2348],
                         [ 0.    ,  0.    ,  1.    ]])
    Returns:
    g - (3,3) ndarray: the resulting homogeneous transformation matrix
    """
    if not xi.shape == (3,):
        raise TypeError('xi must be a 3-vector')

    #YOUR CODE HERE
    g = np.eye(3)
    r = rotation_2d(xi[2]*theta)
    w = xi[2]
    p1 = np.array([[1 - cos(w*theta), sin(w*theta)], [-sin(w*theta), 1 - cos(w*theta)]]) 
    p2 = np.array([[0, -1], [1, 0]])
    p3 = np.array([[xi[0]/w], [xi[1]/w]])
    p = linalg.dot(linalg.dot(p1,p2),p3)
    g[0:2,0:2] = r
    g[0:2,2] = np.transpose(p)
    #print g
    #print omega
    #g = expm(omega*theta)
    return g
def fit_allgasdata( E, conservative=True ) :
	A = [] # design matrix
	Y = [] # target values for the fit
	for e in E :
		for d in e['data'] :
			A.append( [ e['D47eq']/d['sD47'], d['d47']/d['sD47'], 1./d['sD47'] ] )
			Y.append( d['D47'] / d['sD47'] )
	A,Y = numpy.array(A), numpy.array(Y)
	f = linalg.lstsq(A,Y.T)[0] # best-fit parameters
	CM = linalg.inv(linalg.dot(A.T,A)) # covariance matrix of fit parameters
	if conservative :
		# Scale up uncertainties in the fit parameters if the goodnes-of-fit is worse than average.
		# To some extent, this helps account for external errors in the gas line data.
		chi2 = sum( ( Y - linalg.dot( A, f ) )**2 )
		nf = sum([len(e['data']) for e in E]) -3
		if chi2 > nf :
			CM = CM * chi2 / nf
	return f,CM
def fit_gaslines( E, conservative=True ) :
	A = [] # design matrix
	Y = [] # target values for the fit
	nE = len(E)
	N = sum([len(e['data']) for e in E])
	r = 0
	for e in E :
		v = [0.]*nE
		v[r] = 1.
		r = r + 1
		for d in e['data'] :
			A.append( scipy.array( [ d['d47'] ] + v ) / d['sD47'] )
			Y.append( d['D47'] / d['sD47'] )
	A,Y = scipy.array(A), scipy.array(Y)
	f = linalg.lstsq(A,Y.T)[0] # best-fit parameters
	C = linalg.inv(linalg.dot(A.T,A)) # covariance matrix of fit parameters
	if conservative :
		# Scale up uncertainties in the fit parameters if the goodnes-of-fit is worse than average.
		# To some extent, this helps account for external errors in the gas line data.
		chi2 = sum( ( Y - linalg.dot( A, f ) )**2 )
		nf = N-3
		if chi2 > nf :
			C = C * chi2 / nf
	return f,C
	Dc = E[1]['D47eq']
	a,b,c =f

	u = (y - a*x - b) / (c-b)
	dudx = -a/(c-b)
	dudy = 1/(c-b)
	duda = -x/(c-b)
	dudb = (u-1)/(c-b)
	dudc = -u/(c-b)

	g = scipy.array([ dudx, dudy, duda, dudb, dudc ]).T
	Cov = scipy.zeros((5,5))
	Cov[1,1] = sy**2
	Cov[2:,2:] = C

	su = (linalg.dot( g.T, linalg.dot( Cov, g ) ))**.5
	s['D47corrected'] = E[0]['D47eq'] + (E[1]['D47eq']-E[0]['D47eq']) * u
	s['sD47corrected'] = (E[1]['D47eq']-E[0]['D47eq']) * su

	# Although the above formula holds true for linear combinations of normal variables
	# (Tellinghuisen, 2001) [http://dx.doi.org/10.1021/jp003484u],
	# Our Monte-Carlo simulations showed that distributions of D47 values
	# estimated using this code are undistinguishable from normal distributions,
	# so that 95% confidence limits correspond to +/- 2*sD47corrected.
	# These limits account for internal and external uncertainties on gas line data,
	# and for internal errors in sample measurements, but not for external uncertainties in the latter.
	# It is thus always recommended to check external reproducibility for
	# duplicate measurements of homogeneous samples.

	print '%s	%.4f	%.4f	%s' %( s['id'], s['D47corrected'], s['sD47corrected'], s['label'] )
	sy = 0
	z = x / a - b/a * y - c/a
	s['trueD47'] = z	
	
	dzdx = a ** -1
	dzdy = -b / a
	dzda = -(x-b*y-c) * a ** -2
	dzdb = -y / a
	dzdc = -a ** -1

	v = numpy.array([ dzdx, dzdy, dzda, dzdb, dzdc ])
	C = numpy.zeros((5,5))
	C[0,0] = sx ** 2
	C[1,1] = sy ** 2
	C[2:,2:] = CM
	sz = (linalg.dot( v, linalg.dot( C, v.T ) ))**.5
	s['strueD47'] = sz
 
	# Although the above formula holds true for linear combinations of normal variables
	# (Tellinghuisen, 2001) [http://dx.doi.org/10.1021/jp003484u],
	# Our Monte-Carlo simulations showed that distributions of D47 values
	# estimated using this code are undistinguishable from normal distributions,
	# so that 95% confidence limits correspond to +/- 2*sD47corrected.
	# These limits account for internal and external uncertainties on gas line data,
	# and for internal errors in sample measurements, but not for external uncertainties in the latter.
	# It is thus always recommended to check external reproducibility for
	# duplicate measurements of homogeneous samples.
	
	print '%s	%.4f	%.4f	%s' %( s['id'], s['trueD47'], s['strueD47'], s['label'] )

# xmin = numpy.floor(