def __init__(self, respond = None, regressors = None, intercept = False, D = None, d = None, G = None, a = None, b = None, **args):
        """Input: paras where they are expected to be tuple or dictionary"""
        ECRegression.__init__(self,respond, regressors, intercept, D, d, **args)

        if self.intercept and G != None:
            self.G = scipy.zeros((self.n, self.n))
            self.G[1:, 1:] = G
        elif self.intercept and G == None :
            self.G = scipy.identity(self.n)
            self.G[0, 0] = 0.0
        elif not self.intercept and G != None:
            self.G = G
        else:
            self.G = scipy.identity(self.n)
            
        if self.intercept:
            self.a = scipy.zeros((self.n, 1))
            self.a[1:] = a            
            self.b = scipy.zeros((self.n, 1))
            self.b[1:] = b
        else:
            if a is None:
                self.a = scipy.matrix( scipy.zeros((self.n,1)))
            else: self.a = a
            if b is None:
                self.b = scipy.matrix( scipy.ones((self.n,1)))
            else: self.b = b

 def test_scipy_svd(self):
     U,D,Vt = svd(self.A)
     
     D = array([[D[0],0],[0,D[1]]],'d')
     
     self.assertAlmostEqual( sum(sum(abs(dot(U,U.transpose())- identity(2)))), 0.0, 5)
     self.assertAlmostEqual( sum(sum(abs(dot(Vt,Vt.transpose())- identity(2)))), 0.0, 5)
     self.assertAlmostEqual( sum(sum(abs(dot(U,dot(D,Vt)) - self.A))), 0.0, 5)

    def find_object_frame_and_bounding_box(self, point_cloud):
        
        #leaving point cloud in the cluster frame
        cluster_frame = point_cloud.header.frame_id
        self.base_frame = cluster_frame
        (points, cluster_to_base_frame) = transform_point_cloud(self.tf_listener, point_cloud, self.base_frame)

        #run PCA on all 3 dimensions 
        (shifted_points, xyz_mean) = self.mean_shift_xyz(points)
        directions = self.pca(shifted_points[0:3, :])

        #convert the points to object frame:
        #rotate all the points to be in the frame of the eigenvectors (should already be centered around xyz_mean)
        rotmat = scipy.matrix(scipy.identity(4))
        rotmat[0:3,0:3] = directions
        object_points = rotmat**-1 * shifted_points

        #remove outliers from the cluster
        #object_points = self.remove_outliers(object_points)

        #find the object bounding box in the new object frame as [[xmin, ymin, zmin], [xmax, ymax, zmax]] (coordinates of opposite corners)
        object_bounding_box = [[0]*3 for i in range(2)]
        object_bounding_box_dims = [0]*3
        for dim in range(3):
            object_bounding_box[0][dim] = object_points[dim,:].min()
            object_bounding_box[1][dim] = object_points[dim,:].max()
            object_bounding_box_dims[dim] = object_bounding_box[1][dim] - object_bounding_box[0][dim]

        #now shift the object frame and bounding box so that the center is the center of the object
        offset_mat = scipy.mat(scipy.identity(4))
        for i in range(3):
            offset = object_bounding_box[1][i] - object_bounding_box_dims[i]/2.  #center
            object_bounding_box[0][i] -= offset   #mins
            object_bounding_box[1][i] -= offset   #maxes
            object_points[i, :] -= offset
            offset_mat[i,3] = offset
        rotmat = rotmat * offset_mat

        #record the transforms from object frame to base frame and to the original cluster frame,
        #broadcast the object frame to tf, and draw the object frame in rviz
        unshift_mean = scipy.identity(4)
        for i in range(3):
            unshift_mean[i,3] = xyz_mean[i]
        object_to_base_frame = unshift_mean*rotmat
        object_to_cluster_frame = cluster_to_base_frame**-1 * object_to_base_frame

        #broadcast the object frame to tf
        (object_frame_pos, object_frame_quat) = mat_to_pos_and_quat(object_to_cluster_frame)
        self.tf_broadcaster.sendTransform(object_frame_pos, object_frame_quat, rospy.Time.now(), "object_frame", cluster_frame) 

        return (object_points, object_bounding_box_dims, object_bounding_box, object_to_base_frame, object_to_cluster_frame)

def IsingHamiltonian_old(n, h, J, g):
    ### Construct Hamiltonian ###
    Z = sp.matrix([[1,0],[0,-1]])
    X = sp.matrix([[0,1],[1,0]])
    I = sp.identity(2)
    alpha = sp.zeros((2**n,2**n))
    beta = sp.zeros((2**n,2**n))
    delta = sp.zeros((2**n,2**n))
    matrices = []
    # Calculate alpha
    for i in range(0,n):
        for m in range(0,n-1):
            matrices.append(I)
        matrices.insert(i, Z)
        temp = matrices[0]
        matrices.pop(0)
        while (len(matrices) != 0):
            temp = sp.kron(temp, matrices[0])
            matrices.pop(0)
        alpha = alpha + temp*h[i]
    temp = 0
    # Calculate beta
    for i in range(0,n):
        for j in range(0,n):
            if (i != j):
                for m in range(0,n-2):
                    matrices.append(I)
                matrices.insert(i, Z)
                matrices.insert(j, Z)
                temp = matrices[0]
                matrices.pop(0)
                while (len(matrices) != 0):
                    temp = sp.kron(temp, matrices[0])
                    matrices.pop(0)
                beta = beta + temp*J[i,j]
    beta = beta + g*sp.identity(2**n)
    temp = 0
    # Calculate delta                                                            
    for i in range(0,n) :
        for m in range(0,n-1):
            matrices.append(I)
        matrices.insert(i, X)
        temp = matrices[0]
        matrices.pop(0)
        while (len(matrices) != 0):
            temp = sp.kron(temp, matrices[0])
            matrices.pop(0)
        delta += temp
    return [alpha, beta, delta]

	def computeProjectionVectors( self, P, L, U ) :	
		eK = matrix( identity( self.dim, float64 )[ 0: ,( self.dim - 1 ) ] ).T
		U = matrix(U, float64)
		U[ self.dim - 1, self.dim - 1 ] = 1.0
		# Sergio: I added this exception because in rare cases, the matrix
		# U is singular, which gives rise to a LinAlgError.
		try: 
			x1 = matrix( solve( U, eK ), float64 )
		except LinAlgError:
			print "Matrix U was singular, so we input a fake x1\n"
			print "U: ", U
			x1 = matrix(ones(self.dim))

		#print "x1", x1
		del U

		LT = matrix( L, float64, copy=False ).T
		PT = matrix( P, float64, copy=False ).T

		x2 = matrix( solve( LT*PT, eK ), float64 )
		del L
		del P
		del LT
		del PT
		del eK

		return ( x1, x2 )

def AlphaBetaCoeffs_old(n, a, b):
    " Construct the alpha and beta coefficient matrices. "
    Z = sp.matrix([[1,0],[0,-1]])
    I = sp.identity(2)
    alpha = sp.zeros((2**n,2**n))
    beta = sp.zeros((2**n,2**n))
    m1 = []
    m2 = []
    for i in range(0,n):
        for m in range(0,n-1): m1.append(I)
        m1.insert(i, Z)
        temp1 = m1[0]
        m1.pop(0)

        while (len(m1) != 0):
            temp1 = sp.kron(temp1, m1[0])
            m1.pop(0)
        alpha += temp1*a[i]
        for j in range(i+1, n):
            for m in range(0, n-2): m2.append(I)
            m2.insert(i, Z)
            m2.insert(j, Z)
            temp2 = m2[0]
            m2.pop(0)
            while (len(m2) != 0):
                temp2 = sp.kron(temp2, m2[0])
                m2.pop(0)
            beta += (temp2)*b[i,j]
    return [alpha, beta]

def GP_covmat(X1, X2, par, typ = 'SE', sigma = None):
    '''
    Compute covariance matrix with or without white noise for a range of
    GP kernels. Currently implemented:
    - SE (squared exponential 1D, default)
    - SE_ARD (squared exponential with separate length scales for each input dimension)
    - M32 (Matern 32, 1D)
    - QP (quasi-periodic SE, 1D)
    '''
    if typ == 'QP':
        DD = ssp.distance.cdist(X1, X2, 'euclidean')
        K = par[0]**2 * \
            scipy.exp(- (scipy.sin(scipy.pi * DD / par[1]))**2 / 2. / par[2]**2 \
                      - DD**2 / 2. / par[3]**2) 
    if typ == 'Per':
        DD = ssp.distance.cdist(X1, X2, 'euclidean')
        K = par[0]**2 * \
            scipy.exp(- (scipy.sin(scipy.pi * DD / par[1]))**2 / 2. / par[2]**2) 
    elif typ == 'M32':
        DD = ssp.distance.cdist(X1, X2, 'euclidean')
        arg = scipy.sqrt(3) * abs(DD) / par[1]
        K = par[0]**2 * (1 + arg) * scipy.exp(- arg)
    elif typ == 'SE_ARD':
        V = numpy.abs(numpy.matrix( numpy.diag( 1. / numpy.sqrt(2) / par[1:]) ))
        D2 = ssp.distance.cdist(X1 * V, X2 * V, 'sqeuclidean')
        K = par[0]**2 * numpy.exp( -D2 )
    else: # 'SE (radial)'
        D2 = ssp.distance.cdist(X1, X2, 'sqeuclidean')
        K = par[0]**2 * scipy.exp(- D2 / 2. / par[1]**2)
    if sigma != None:
        N = X1.shape[0]
        K += sigma**2 * scipy.identity(N)
    return scipy.matrix(K)

    def process_collision_geometry_for_table(self, firsttable, additional_tables = []):

        table_object = CollisionObject()
        table_object.operation.operation = CollisionObjectOperation.ADD
        table_object.header.frame_id = firsttable.pose.header.frame_id
        table_object.header.stamp = rospy.Time.now()

        #create a box for each table
        for table in [firsttable,]+additional_tables:
            object = Shape()
            object.type = Shape.BOX;
            object.dimensions.append(math.fabs(table.x_max-table.x_min))
            object.dimensions.append(math.fabs(table.y_max-table.y_min))
            object.dimensions.append(0.01)
            table_object.shapes.append(object)
  
        #set the origin of the table object in the middle of the firsttable
        table_mat = self.pose_to_mat(firsttable.pose.pose)
        table_offset = scipy.matrix([(firsttable.x_min + firsttable.x_max)/2.0, (firsttable.y_min + firsttable.y_max)/2.0, 0.0]).T
        table_offset_mat = scipy.matrix(scipy.identity(4))
        table_offset_mat[0:3,3] = table_offset
        table_center = table_mat * table_offset_mat
        origin_pose = self.mat_to_pose(table_center)
        table_object.poses.append(origin_pose)

        table_object.id = "table"
        self.object_in_map_pub.publish(table_object)

 def test_uncorrelated_noscatter(self):
     data = sp.arange(10, dtype=float)
     theory = data/2.0
     C = sp.identity(10)
     out = utils.ampfit(data, C, theory)
     a, s = out['amp'], out['error']
     self.assertAlmostEqual(a, 2)

def decompose( matrix ):
	# Returns the decomposition of a matrix A where
	#
	# Q.A.Q = P.L.U
	#
	# P.L.U is the factoring of Q.A.Q such that L is a lower triangular matrix with 1's
	# on the diagonal and U is an upper triangular matrix; P is the permutation (row-swapping
	# operations) required for this procedure. The permutation matrix Q is chosen such that 
	# the last element of U is its smallest diagnoal element. If A has a zero eigenvalue, 
	# then U's last element will be zero.
	
	dim = matrix.shape[ 0 ]

	# first decomposition
	( P, L, U ) = lu( matrix )
	
 	# detect the smallest element of U
	smallestIndex = findsmallestdiag( U )
	smallest = U[ smallestIndex, smallestIndex ]

	#show( matrix, "M" )
	#show( U, "U" )
	#print "Smallest element is %f at %d" % ( smallest, smallestIndex )

	# is the permutation Q not just the identity matrix?
	Q = identity( dim )
	if smallestIndex+1 != dim :
		# trick: exchange row 'smallestIndex' with row 'dim-1' of the identity matrix
		swaprow( Q, smallestIndex, dim-1 )

	return ( P, L, U, Q )

def kalman_filter(b,
                  V,
                  Phi,
                  y,
                  X,
                 sigma,
                  Sigma,
                  switch = 0,
                  D = None,
                  d = None,
                  G = None,
                  a = None,
                  c = None):
    r"""
    
    .. math::
       :nowrap:

       \begin{eqnarray*}
       \beta_{t|t-1} = \Phi \: \beta_{t-1|t-1}\\
       V_{t|t-1} = \Phi  V_{t-1|t-1} \Phi ^T + \Sigma \\
       e_t = y_t -  X_t \beta_{t|t-1}\\
       K_t =  V_{t|t-1} X_t^T (\sigma + X_t V_{t|t-1} X_t )^{-1}\\
       \beta_{t|t} = \beta_{t|t-1} + K_t e_t\\
       V_{t|t} = (I - K_t X_t^T) V_{t|t-1}\\
       \end{eqnarray*}

    """

    n = scipy.shape(X)[1]
    beta = scipy.empty(scipy.shape(X))
    n = len(b)
    if D is None:
        D = scipy.ones((1, n))
    if d is None:
        d = scipy.matrix(1.)
    if G is None:
        G = scipy.identity(n)
    if a is None:
        a = scipy.zeros((n, 1))
    if c is None:
        c = scipy.ones((n, 1))
#        import code; code.interact(local=locals())
    (b, V) = kalman_predict(b, V, Phi, Sigma)
    for i in xrange(len(X)):
        beta[i] = scipy.array(b).T
        (b, V, e, K) = kalman_upd(b,
                                V,
                                y[i],
                                X[i],
                                sigma,
                                Sigma,
                                switch,
                                D,
                                d,
                                G,
                                a,
                                c)
        (b, V) = kalman_predict(b, V, Phi, Sigma)
    return beta

def embedTraversal(cloned, obj,n,suffix):
    for i in range(len(obj)):
        if isinstance(obj[i],Model): 
            cloned.body += [obj[i]]
        elif (isinstance(obj[i],tuple) or isinstance(obj[i],list)) and (
                len(obj[i])==2):
            V,EV = obj[i]
            V = [v+n*[0.0] for v in V]
            cloned.body  += [(V,EV)]
        elif (isinstance(obj[i],tuple) or isinstance(obj[i],list)) and (
                len(obj[i])==3):
            V,FV,EV = obj[i]
            V = [v+n*[0.0] for v in V]
            cloned.body  += [(V,FV,EV)]
        elif isinstance(obj[i],Mat): 
            mat = obj[i]
            d,d = mat.shape

            newMat = scipy.identity(d+n*1)
            for h in range(d-1): 
                for k in range(d-1): 
                    newMat[h,k] = mat[h,k]
                newMat[h,d-1+n*1] = mat[h,d-1]
            cloned.body  +=  [newMat.view(Mat)]

        elif isinstance(obj[i],Struct):
            newObj = Struct()
            newObj.box = hstack((obj[i].box, [n*[0],n*[0]]))
            newObj.name = obj[i].name+suffix
            newObj.category = obj[i].category
            cloned.body  += [embedTraversal(newObj, obj[i], n, suffix)]
    return cloned

    def argmin(self,start=None,tolerance=0.0001,maxit=100,stepsize=1.0):
        xold = start if start is not None else scipy.zeros(self.shape)

        # Initial hessian inverse guess
        B = scipy.identity(self.shape)

        grad=(tolerance+1)*scipy.ones(self.shape)
        for it in xrange(maxit):
            if (it != 0 and numpy.linalg.norm(grad)<tolerance): break

            grad = self.gradient(xold)

            # Search direction
            s = numpy.dot(B,-1*grad)

            # Use scipy line search until implemented here
            a=scipy.optimize.linesearch.line_search_wolfe2(
                self.f,
                self.gradient,
                xold,
                s,
                grad
                )
            s = a[0] * s

            xnew = xold + s
            if numpy.isnan(self.f(xnew)): break

            y = self.gradient(xnew) -grad
            ytb = numpy.dot(y,B)
            by = numpy.dot(B,y)
            B = B + numpy.outer(s,s)/numpy.dot(y,s) - numpy.outer(by,ytb)/numpy.dot(ytb,y)
            xold = xnew
        return xnew

 def solve(self,rhs):
     """ 
     Overrides LinearSolver.solve
     Result contains (solution,status)
         status is always 0, indicating that the method has converged
     """
     if not self.built:
         N = len(self.point.getState()[0])
         Dt = self.point.system.Dt
         dt = self.point.system.dt
         k = int(Dt/dt)
         I = scipy.identity(N)
         
         A = self.point.computeJacobian()
         B = I+dt*A[:N,:N]
         AA = B
         for i in range(k-1):
             AA=scipy.dot(AA,B)
         Matrix = A  # zo blijven extra rijen en kolommen dezelfde als A
         Matrix[:N,:N]= I - AA
         self.Matrix = Matrix
         self.built = True
     else:
         Matrix=self.Matrix
     x=scipy.linalg.solve(Matrix,rhs)
     status = 0
     return (x,status)

 def test_correlated_scatter(self) :
     n = 50
     r = (sp.arange(n, dtype=float) + 10.0*n)/10.0*n
     data = sp.sin(sp.arange(n)) * r 
     amp = 25.0
     theory = data/amp
     # Generate correlated matrix.
     C = random.rand(n, n) # [0, 1) 
     # Raise to high power to make values near 1 rare.
     C = (C**10) * 0.2
     C = (C + C.T)/2.0
     C += sp.identity(n)
     C *= r[:, None]/2.0
     C *= r[None, :]/2.0
     # Generate random numbers in diagonal frame.
     h, R = linalg.eigh(C)
     self.assertTrue(sp.alltrue(h>0))
     rand_vals = random.normal(size=n)*sp.sqrt(h)
     # Rotate back.
     data += sp.dot(R.T, rand_vals)
     out = utils.ampfit(data, C, theory)
     a, s = out['amp'], out['error']
     self.assertTrue(sp.allclose(a, amp, atol=5.0*s, rtol=0))
     # Expect the next line to fail 1/100 trials.
     self.assertFalse(sp.allclose(a, amp, atol=0.01*s, rtol=0))

def generate_gaussians(k):
    """Generate k iid spherical k-dim Gaussians g_1, ..., g_k"""
    mean = [0 for x in range(0, k)]
    covariance = sp.matrix(sp.identity(k), copy=False)
    g = []
    for i in range(0, k):
        tmp = sp.random.multivariate_normal(mean, covariance)
        g.append(tmp)
    return g

def ellipsoid(R=np.array([[2, 0, 0],[0, 1, 0],[0, 0, 1] ]),position=(0,0,0),thetares=20,phires=20,color=(0,0,1),opacity=1,tessel=0):

    ''' Create a ellipsoid actor.    
    Stretch a unit sphere to make it an ellipsoid under a 3x3 translation matrix R 
    
    R=sp.array([[2, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1] ])
    '''
    
    Mat=sp.identity(4)
    Mat[0:3,0:3]=R
       
    '''
    Mat=sp.array([[2, 0, 0, 0],
                             [0, 1, 0, 0],
                             [0, 0, 1, 0],
                             [0, 0, 0,  1]  ])
    '''
    mat=vtk.vtkMatrix4x4()
    
    for i in sp.ndindex(4,4):
        
        mat.SetElement(i[0],i[1],Mat[i])
    
    radius=1
    sphere = vtk.vtkSphereSource()
    sphere.SetRadius(radius)
    sphere.SetLatLongTessellation(tessel)
   
    sphere.SetThetaResolution(thetares)
    sphere.SetPhiResolution(phires)
    
    trans=vtk.vtkTransform()
    
    trans.Identity()
    #trans.Scale(0.3,0.9,0.2)
    trans.SetMatrix(mat)
    trans.Update()
    
    transf=vtk.vtkTransformPolyDataFilter()
    transf.SetTransform(trans)
    transf.SetInput(sphere.GetOutput())
    transf.Update()
    
    spherem = vtk.vtkPolyDataMapper()
    spherem.SetInput(transf.GetOutput())
    
    spherea = vtk.vtkActor()
    spherea.SetMapper(spherem)
    spherea.SetPosition(position)
    spherea.GetProperty().SetColor(color)
    spherea.GetProperty().SetOpacity(opacity)
    #spherea.GetProperty().SetRepresentationToWireframe()
    
    return spherea

def get_algebraic_page_rank(transition_matrix, dumping_factor=0.85):
    """Computes the page ranks in an algebraic way"""
    n = transition_matrix.shape[0]
    m_a = transition_matrix
    d = dumping_factor

    ls = la.inv(sp.identity(n) - d*m_a)
    rs = sp.matrix([(1-d)/n]*n).reshape(n, 1)

    return sp.dot(ls, rs)

def get_transform(tf_listener, frame1, frame2):
    temp_header = Header()
    temp_header.frame_id = frame1
    temp_header.stamp = rospy.Time(0)
    try:
        frame1_to_frame2 = tf_listener.asMatrix(frame2, temp_header)
    except:
        rospy.logerr("tf transform was not there between %s and %s"%(frame1, frame2))
        return scipy.matrix(scipy.identity(4))
    return scipy.matrix(frame1_to_frame2)

def kalman_upd(beta,
               V,
               y,
               X,
               s,
               S,
               switch = 0,
               D = None,
               d = None,
               G = None,
               a = None,
               b = None):
    r"""
    This is the update step of kalman filter. 

    .. math::
       :nowrap:

       \begin{eqnarray*}
       e_t &=& y_t -  X_t \beta_{t|t-1} \\
       K_t &=&  V_{t|t-1} X_t^T (\sigma + X_t V_{t|t-1} X_t )^{-1}\\
       \beta_{t|t} &=& \beta_{t|t-1} + K_t e_t\\
       V_{t|t} &=& (I - K_t X_t^T) V_{t|t-1}\\
       \end{eqnarray*}


    
    """
    e = y - X * beta
    K = V * X.T * ( s + X * V * X.T).I
    beta = beta + K * e
    if switch == 1:
        D = scipy.matrix(D)
        d = scipy.matrix(d)
        if DEBUG: print "beta: ", beta
        beta = beta - S * D.T * ( D * S * D.T).I * ( D * beta - d)
        if DEBUG: print "beta: ", beta
    elif switch == 2:
        G = scipy.matrix(G)
        a = scipy.matrix(a)
        b = scipy.matrix(b)
        n = len(beta)
        P = 2* V.I
        q = -2 * V.I.T * beta
        bigG = scipy.empty((2*n, n))
        h = scipy.empty((2*n, 1))
        bigG[:n, :] = -G
        bigG[n:, :] = G
        h[:n, :] = -a
        h[n:, :] = b
        paraset = map(cvxopt.matrix, (P, q, bigG, h, D, d))
        beta = qp(*paraset)['x']
    temp = K*X
    V = (scipy.identity(temp.shape[0]) - temp) * V
    return (beta, V, e, K)