def synth_meas_watson_SH_cyl_neuman_PGSE( self, x, grad_dirs, G, delta, smalldel, fibredir, roots ):
        d=x[0]
        R=x[1]
        kappa=x[2]

        l_q = grad_dirs.shape[0]

        # Parallel component
        LePar = self.cyl_neuman_le_par_PGSE(d, G, delta, smalldel)

        # Perpendicular component
        LePerp = self.cyl_neuman_le_perp_PGSE(d, R, G, delta, smalldel, roots)

        ePerp = np.exp(LePerp)

        # Compute the Legendre weighted signal
        Lpmp = LePerp - LePar
        lgi = self.legendre_gaussian_integral(Lpmp, 6)

        # Compute the spherical harmonic coefficients of the Watson's distribution
        coeff = self.watson_SH_coeff(kappa)
        coeffMatrix = matlib.repmat(coeff, l_q, 1)

        # Compute the dot product between the symmetry axis of the Watson's distribution
        # and the gradient direction
        #
        # For numerical reasons, cosTheta might not always be between -1 and 1
        # Due to round off errors, individual gradient vectors in grad_dirs and the
        # fibredir are never exactly normal.  When a gradient vector and fibredir are
        # essentially parallel, their dot product can fall outside of -1 and 1.
        #
        # BUT we need make sure it does, otherwise the legendre function call below
        # will FAIL and abort the calculation!!!
        #
        cosTheta = np.dot(grad_dirs,fibredir)
        badCosTheta = abs(cosTheta)>1
        cosTheta[badCosTheta] = cosTheta[badCosTheta]/abs(cosTheta[badCosTheta])

        # Compute the SH values at cosTheta
        sh = np.zeros(coeff.shape)
        shMatrix = matlib.repmat(sh, l_q, 1)
        for i in range(7):
            shMatrix[:,i] = np.sqrt((i+1 - .75)/np.pi)
            # legendre function returns coefficients of all m from 0 to l
            # we only need the coefficient corresponding to m = 0
            # WARNING: make sure to input ROW vector as variables!!!
            # cosTheta is expected to be a COLUMN vector.
            tmp = np.zeros((l_q))
            for pol_i in range(l_q):
                tmp[pol_i] = scipy.special.lpmv(0, 2*i, cosTheta[pol_i])
            shMatrix[:,i] = shMatrix[:,i]*tmp

        E = np.sum(lgi*coeffMatrix*shMatrix, 1)
        # with the SH approximation, there will be no guarantee that E will be positive
        # but we need to make sure it does!!! replace the negative values with 10% of
        # the smallest positive values
        E[E<=0] = np.min(E[E>0])*0.1
        E = 0.5*E*ePerp

        return E

def con2vert(A, b):
    """
    Convert sets of constraints to a list of vertices (of the feasible region).
    If the shape is open, con2vert returns False for the closed property.
    """
    # Python implementation of con2vert.m by Michael Kleder (July 2005),
    #  available: http://www.mathworks.com/matlabcentral/fileexchange/7894
    #  -con2vert-constraints-to-vertices
    # Author: Michael Kelder (Original)
    #         Andre Campher (Python implementation)
    c = linalg.lstsq(mat(A), mat(b))[0]
    btmp = mat(b)-mat(A)*c
    D = mat(A)/matlib.repmat(btmp, 1, A.shape[1])

    fmatv = qhull(D, "Ft") #vertices on facets

    G  = zeros((fmatv.shape[0], D.shape[1]))
    for ix in range(0, fmatv.shape[0]):
        F = D[fmatv[ix, :], :].squeeze()
        G[ix, :] = linalg.lstsq(F, ones((F.shape[0], 1)))[0].transpose()

    V = G + matlib.repmat(c.transpose(), G.shape[0], 1)
    ux = uniqm(V)

    eps = 1e-13
    Av = dot(A, ux.T)
    bv = tile(b, (1, ux.shape[0]))
    closed = sciall(Av - bv <= eps)

    return ux, closed

def computeGradient( nn_params, input_layer_size, hidden_layer_size, num_labels, X, y, lamda ):
	Theta1, Theta2 = paramRollback( nn_params, input_layer_size, hidden_layer_size, num_labels )
	# print shape(Theta1), shape(Theta2)
	m, n   = shape(X)
	a1, z2, a2, z3, h = feedForward( Theta1, Theta2, X, m )

	yVec = ( (matlib.repmat(arange(1, num_labels+1), m, 1) == matlib.repmat(y, 1, num_labels)) + 0)

	D1 = zeros(shape(Theta1))
	D2 = zeros(shape(Theta2))

	sigma3 = h - yVec.T
	sigma2 = Theta2.T.dot(sigma3) * sigmoidGradient( r_[ ones((1,m)), z2 ] )
	sigma2 = sigma2[1:,:]
	# print shape(sigma2)
	delta1 = sigma2.dot( a1.T )
	delta2 = sigma3.dot( a2.T ) 

	D1[:,1:] = delta1[:,1:]/m + (Theta1[:,1:] * lamda / m)
	D2[:,1:] = delta2[:,1:]/m + (Theta2[:,1:] * lamda / m)
	D1[:,0]  = delta1[:,0]/m
	D2[:,0]  = delta2[:,0]/m 

	grad = array([D1.T.reshape(-1).tolist() + D2.T.reshape(-1).tolist()]).T
	# print D1, shape(D1)
	# print D2, shape(D2)


	return ndarray.flatten(grad)

	def fit(self, X):
		if self.components==0:
			self.components=X.shape[0]

		self.X = X
		N = X.shape[0]
		K = np.zeros((N,N))
		for row in range(N):
			for col in range(N):
				K[row,col] = self._kernel_func(X[row,:], X[col,:])

		self._K_sum = np.sum(K)
		self._K_cached_sumcols = np.sum(K, axis=0)
		K_c = K - repmat(np.reshape(np.sum(K, axis=1), (N,1)), 1, N)/N - repmat(self._K_cached_sumcols, N, 1)/N + self._K_sum/N**2

		# kernel matrix must be symmetric, so using symmetric matrix eigenvalue solver
		self._eigenvalues, self._eigenvectors = eigh(K_c)
		self._eigenvalues = np.real(self._eigenvalues)
		self._eigenvectors = np.real(self._eigenvectors)
		key = np.argsort(self._eigenvalues)
		key = key[::-1]
		self._eigenvalues = self._eigenvalues[key]
		self._eigenvectors = self._eigenvectors[:,key]
		self.X = self.X[key,:]
		self._K_cached_sumcols = self._K_cached_sumcols[key]

def get_lap_matrix_self_tuning(data):
    start_time = time.clock()
    sls2 = -2 * np.mat(data) * np.mat(data.T)
    sls1 = np.mat(np.sum(data ** 2, 1))
    w_matrix = sls2 + repmat(sls1, len(sls1), 1) + repmat(sls1.T, 1, len(sls1))
    w_matrix = np.array(w_matrix)

    sigma = list()
    m = len(w_matrix)
    sort_w_matrix = np.sort(w_matrix)
    for i in range(m):
        sigma.append(np.sqrt(sort_w_matrix[i, 7]))
    """for i in range(m):
        print(i)
        idx = np.argsort(w_matrix[i, :])

        idx = get_k_min_index(w_matrix[i, :].tolist(), 7)
        sigma.append(np.sqrt(w_matrix[i, idx]))
    """

    for row in range(m):
        for col in range(m):
            w_matrix[row][col] /= float(sigma[row] * sigma[col])

    w_matrix = np.exp(-np.mat(w_matrix))
    w_matrix = np.array(w_matrix)
    d_matrix = np.diag(np.sum(w_matrix, 1))
    d_matrix_square_inv = np.linalg.inv(d_matrix ** 0.5)
    dot_matrix = np.dot(d_matrix_square_inv, w_matrix)
    lap_matrix = np.dot(dot_matrix, d_matrix_square_inv)
    end_time = time.clock()

    print("calc self_tuning laplace matrix spends %f seconds" % (end_time - start_time))
    return lap_matrix

def deltas(x, w=9):
    """
    Calculate the deltas (derivatives) of a sequence
    Use a W-point window (W odd, default 9) to calculate deltas using a
    simple linear slope.  This mirrors the delta calculation performed
    in feacalc etc.  Each row of X is filtered separately.

    Notes:
    x is your data matrix where each feature corresponds to a row (so you may have to transpose the data you
    pass as an argument) and then transpose the output of the function).

    :param x: x is your data matrix where each feature corresponds to a row.
        (so you may have to transpose the data you pass as an argument)
        and then transpose the output of the function)
    :param w: window size, defaults to 9
    :return: derivatives of a sequence
    """
    # compute the shape of the input
    num_row, num_cols = x.shape

    # define window shape
    hlen = w // 2  # floor integer divide
    w = 2 * hlen + 1  # odd number
    win = np.arange(hlen, -hlen - 1, -1, dtype=np.float32)

    # pad the data by repeating the first and last columns
    a = matlab.repmat(x[:, 1], 1, hlen).reshape((num_row, hlen), order='F')
    b = matlab.repmat(x[:, -1], 1, hlen).reshape((num_row, hlen), order='F')
    xx = np.concatenate((a, x, b), axis=1)

    # apply the delta filter, see matlab 1D filter
    d = signal.lfilter(win, 1, xx, 1)

    # trim the edges
    return d[:, hlen*2: hlen*2 + num_cols]

    def env_init(self):
        """
            Based on the levin model, the dispersion probability is initialized.
        """
        self.dispersionModel = InvasiveUtility.Levin
        notDirectedG = networkx.Graph(self.simulationParameterObj.graph)
        adjMatrix = adjacency_matrix(notDirectedG)

        edges = self.simulationParameterObj.graph.edges()
        simulationParameterObj = self.simulationParameterObj
        if self.dispersionModel == InvasiveUtility.Levin:
            parameters = InvasiveUtility.calculatePath(notDirectedG,adjMatrix, edges, simulationParameterObj.downStreamRate,
                simulationParameterObj.upStreamRate)
            C = (1 - simulationParameterObj.upStreamRate * simulationParameterObj.downStreamRate) / (
                (1 - 2 * simulationParameterObj.upStreamRate) * (1 - simulationParameterObj.downStreamRate))
            self.dispertionTable = np.dot(1 / C, parameters)
            self.germinationObj = GerminationDispersionParameterClass(1, 1)
        #calculating the worst case fully invaded rivers cost
        worst_case = repmat(1, 1, self.simulationParameterObj.nbrReaches * self.simulationParameterObj.habitatSize)[0]
        cost_state_unit = InvasiveUtility.get_unit_invaded_reaches(worst_case,
            self.simulationParameterObj.habitatSize) * self.actionParameterObj.costPerReach
        stateCost = cost_state_unit + InvasiveUtility.get_invaded_reaches(
            worst_case) * self.actionParameterObj.costPerTree
        stateCost = stateCost + InvasiveUtility.get_empty_slots(worst_case) * self.actionParameterObj.emptyCost
        costAction = InvasiveUtility.get_budget_cost_actions(repmat(3, 1, self.simulationParameterObj.nbrReaches)[0],
            worst_case, self.actionParameterObj)
        networkx.adjacency_matrix(self.simulationParameterObj.graph)
        return "VERSION RL-Glue-3.0 PROBLEMTYPE non-episodic DISCOUNTFACTOR " + str(
            self.discountFactor) + " OBSERVATIONS INTS (" + str(
            self.simulationParameterObj.nbrReaches * self.simulationParameterObj.habitatSize) + " 1 3) ACTIONS INTS (" + str(
            self.simulationParameterObj.nbrReaches) + " 1 4) REWARDS (" + str(self.Bad_Action_Penalty)+" "+str(
            -1 * (costAction + stateCost)) + ") EXTRA "+str(self.simulationParameterObj.graph.edges()) + " BUDGET "+str(self.actionParameterObj.budget) +" by Majid Taleghan."

def fastsvds(M,r): 
    """
    "Fast" but less accurate SVD by computing the SVD of MM^T or M^TM 
    ***IF*** one of the dimensions of M is much smaller than the other. 
    Note. This is numerically less stable, but useful for large hyperspectral 
    images. 

    """

    m,n = M.shape 
    rationmn = 10 # Parameter, should be >= 1

    if m < rationmn*n: 
        MMt = np.dot(M,M.T)
        u,s,v = svds(MMt,r)
        s = np.diag(s)
        v = np.dot(M.T, u) 
        v = np.multiply(v,repmat( (sum(v**2)+1e-16)**(-0.5),n,1)) 
        s = np.sqrt(s) 
    elif n < rationmn*m:
        MtM = np.dot(M.T,M)
        u,s,v = svds(MtM,r) 
        s = np.diag(s)
        u = np.dot(M,v) 
        u = np.multiply(u,repmat( (sum(u**2)+1e-16)**(-0.5),m,1))
        s = np.sqrt(s) 
    else:
        u,s,v = svds(M,r) 
        s = np.diag(s)
    return (u,s,v)

    def __check_and_repmat__(self, attrib, angles):
        """
        Checks whether the attribute is a single value and repeats it into an array if it is
        :rtype: None
        :param attrib: string
        :param angles: np.ndarray
        """
        old_attrib = getattr(self, attrib)

        if type(old_attrib) in [float, int, np.float32, np.float64]:
            new_attrib = matlib.repmat(old_attrib, 1, angles.shape[0])[0]
            setattr(self, attrib, new_attrib)

        elif type(old_attrib) == np.ndarray:
            if old_attrib.ndim == 1:
                if old_attrib.shape in [(3,), (2,), (1,)]:
                    new_attrib = matlib.repmat(old_attrib, angles.shape[0], 1)
                    setattr(self, attrib, new_attrib)
                elif old_attrib.shape == (angles.shape[0],):
                    pass
            else:
                if old_attrib.shape == (angles.shape[0], old_attrib.shape[1]):
                    pass
                else:
                    raise AttributeError(attrib + " with shape: " + str(old_attrib.shape) +
                                         " not compatible with shapes: " + str([(angles.shape[0],),
                                                                                (angles.shape[0], old_attrib.shape[1]),
                                                                                (3,), (2,), (1,)]))

        else:
            raise TypeError(
                "Data type not understood for: geo." + attrib + " with type = " + str(type(getattr(self, attrib))))

def samples_to_cluster_object(data, label):

    m = len(label)
    assert len(data) == len(label)
    Cluster.n_samples = m
    for i in range(m):
        cluster_object = Cluster()
        cluster_object.data = data[i, :]
        cluster_object.cluster = label[i]
        Cluster.samples_list.append(cluster_object)

    start_time = time.clock()
    sls2 = -2 * np.mat(data) * np.mat(data.T)
    sls1 = np.mat(np.sum(data ** 2, 1))
    Cluster.w_matrix = sls2 + repmat(sls1, len(sls1), 1) + repmat(sls1.T, 1, len(sls1))
    Cluster.w_matrix = np.array(Cluster.w_matrix) ** 0.5
    end_time = time.clock()
    print("calc distance matrix' time is %f seconds" % (end_time - start_time))

    outfile = open("distance.txt", "w+")
    print("writing distance to file ...")
    for i in range(m - 1):
        for j in range(i + 1, m):
            Cluster.distances.append(Cluster.w_matrix[i, j])
            str_d = str(i + 1) + "\t" + str(j + 1) + "\t" + str(Cluster.w_matrix[i, j]) + "\n"
            outfile.write(str_d)
    print("write done")

    del cluster_object

def trueFeatureStats(T, R, fMap, discountFactor, stateProp=1, MAT_LIMIT=1e8):
    """ Gather the statistics needed for LSTD,
    assuming infinite data (true probabilities).
    Option: if stateProp is  < 1, then only a proportion of all 
    states will be seen as starting state for transitions """
    dim = len(fMap)
    numStates = len(T)
    statMatrix = zeros((dim, dim))
    statResidual = zeros(dim)
    ss = range(numStates)
    repVersion = False
    
    if stateProp < 1:
        ss = random.sample(ss, int(numStates * stateProp))
    elif dim * numStates**2 < MAT_LIMIT:
        repVersion = True
    
    # two variants, depending on how large we can afford our matrices to become.        
    if repVersion:    
        tmp1 = tile(fMap, (numStates,1,1))
        tmp2 = transpose(tmp1, (2,1,0))
        tmp3 = tmp2 - discountFactor * tmp1            
        tmp4 = tile(T, (dim,1,1))
        tmp4 *= transpose(tmp1, (1,2,0))
        statMatrix = tensordot(tmp3, tmp4, axes=[[0,2], [1,2]]).T
        statResidual = dot(R, dot(fMap, T).T)
    else:
        for sto in ss:
            tmp = fMap - discountFactor * repmat(fMap[:, sto], numStates, 1).T
            tmp2 = fMap * repmat(T[:, sto], dim, 1)
            statMatrix += dot(tmp2, tmp.T)             
            statResidual += R[sto] * dot(fMap, T[:, sto])
    return statMatrix, statResidual

 def dlnprob(self, theta):
     
     if self.batchsize > 0:
         batch = [ i % self.N for i in range(self.iter * self.batchsize, (self.iter + 1) * self.batchsize) ]
         ridx = self.permutation[batch]
         self.iter += 1
     else:
         ridx = np.random.permutation(self.X.shape[0])
         
     Xs = self.X[ridx, :]
     Ys = self.Y[ridx]
     
     w = theta[:, :-1]  # logistic weights
     alpha = np.exp(theta[:, -1])  # the last column is logalpha
     d = w.shape[1]
     
     wt = np.multiply((alpha / 2), np.sum(w ** 2, axis=1))
     
     coff = np.matmul(Xs, w.T)
     y_hat = 1.0 / (1.0 + np.exp(-1 * coff))
     
     dw_data = np.matmul(((nm.repmat(np.vstack(Ys), 1, theta.shape[0]) + 1) / 2.0 - y_hat).T, Xs)  # Y \in {-1,1}
     dw_prior = -np.multiply(nm.repmat(np.vstack(alpha), 1, d) , w)
     dw = dw_data * 1.0 * self.X.shape[0] / Xs.shape[0] + dw_prior  # re-scale
     
     dalpha = d / 2.0 - wt + (self.a0 - 1) - self.b0 * alpha + 1  # the last term is the jacobian term
     
     return np.hstack([dw, np.vstack(dalpha)])  # % first order derivative 

def julia_sets(complex_number, n, maxIter=30, a=-1, b=1):
    """
    This function computes the Julia set of the comlex number passed in
    according to the function f(x) = x**2  + c.

    Inputs:
	complex_number: The comlpex number for which you would like to find
			the Julia set.
	n: The size of the square (n x n) matrix that will contain plot
	   information.
	maxIter: The maximum number of iterations to pass x though f(x).
	a: The lower bound for both the real and complex axes.
	b: The upper bound for both the real and complex axes.

    Outputs:
	plot: There is no output that can be stored to a variable. When this
	      function is called a plot is automatically generated.
    """

    if n % 2 != 0:
	print 'Value Error: We need n to be an even number'
	return

    X = sp.zeros((n,n), dtype = 'complex64')
    Xj = sp.zeros((n,n), dtype = 'complex64')
    x = sp.linspace(a,b,n)
    xj = x * 1j

    X = repmat(x, n, 1)
    Xj = repmat(xj, n, 1)

    start_grid =  X + Xj.T
    answer_grid = sp.zeros((n,n), dtype = 'complex64')

    func = lambda x: x**2 + complex_number

    for i in range(n):
	for j in range(n):
	    x = start_grid[i,j]
	    for m in range(maxIter):
		x = func(x)
		m += 1
	    answer_grid[i,j] = x

    for i in range(n):
	for j in range(n):
	    if math.isnan(answer_grid[i,j]):
		answer_grid[i,j] = 0

    E = sp.zeros((n,n), dtype = 'complex64')
    for i in range(n):
	for j in range(n):
	    E[i,j] = sp.exp(-abs(answer_grid[i,j]))

    E = -1* sp.array(E, dtype = float)
    plt.imshow(E)

 def compute_distance_matrix(self):
     X = self.X
     y = self.y
     self.EDM = distance_utils.calcDistanceMatrixFastEuclidean(X)
     N,d = X.shape
     ind = repmat(array(range(N)),N,1)
     self.EDM.setfield(ind, dtype=int16)
     Y = repmat(y, N, 1)
     flag = (Y==Y.T)
     self.EDM.setfield(flag, dtype=bool, offset=2)

def MutInf(x1, x2, Nbins=(20, 20, 20), s=1):
    import numpy as np  # histogramdd, array, transpose

    # define data vectors/arrays
    x = x1[s:]  # time series data from osc_1
    y = x1[:-s]  # time series data from osc_1 delayed by s
    z = x2[:-s]  # time series data from osc_2 delayed by s

    # calculate 3D histogram of size (nx,ny,nz) where nx is the number
    # of bins for x, ny for y, nz for z. This is the tricky part since
    # the size of bins is quite critical. However you could start with
    # say nx=20, i.e. 20 bins evenly spaced between the min and max values
    # of x
    nx, ny, nz = Nbins
    H, edges = np.histogramdd([x, y, z], bins=(nx, ny, nz))

    # add small probability mass everywhere to avoid divide by zeros,
    # e.g. 1e-9 (you can experiment with different values). This part
    # was a bit of a fudge and should be replaced with something more
    # sensible - but I never got round to it
    # H = H + 1e-12

    # renormalise so sums to unity
    P = H / np.sum(H)

    # now sum along the first dimension to create a 2D array of p(y,z)
    P_yz = np.sum(P, axis=0)

    # replicate the 2D array at each element in the first dimension
    # so as to regain a (nx,ny,nz) array (this is purely for computational
    # ease)
    from numpy.matlib import repmat

    P_yz = repmat(P_yz, nx, 1).reshape(nx, ny, nz)

    # now sum along the first and third dimension to give 1D array of p(y)
    P_y = np.sum(P, axis=(0, 2))

    # replicate in both the x and z directions
    P_y = repmat(P_y, nx, nz).reshape(nx, ny, nz)

    P_xy = np.sum(P, axis=2)
    P_xy = repmat(P_xy, nz, 1).reshape(nx, ny, nz)

    # create conditional probability mass functions
    P_x_given_yz = P / P_yz
    P_x_given_y = P_xy / P_y

    # calculate transfer entropy
    logP = np.log(P_x_given_yz / P_x_given_y)
    logP = np.nan_to_num(logP)

    T = np.sum(P * logP)

    return T

def mkfdstencil(x,xbar,k):
#this funtion is sue to create finite diference method matrix
  maxorder            = len(x)
  h_matrix            = repmat(np.transpose(x)-xbar,maxorder,1)
  powerfactor_matrix  = np.transpose(repmat(np.arange(0,maxorder),maxorder,1))
  factorialindex      = np.transpose(repmat(factorial(np.arange(0,maxorder)),maxorder,1))
  taylormatrix        = h_matrix ** powerfactor_matrix /factorialindex
  derivativeindex     = np.zeros(maxorder)
  derivativeindex[k]  = 1 
  u = np.linalg.solve(taylormatrix,derivativeindex)
  return u

def depth_to_local_point_cloud(image, color=None, max_depth=0.9):
    """
    Convert an image containing CARLA encoded depth-map to a 2D array containing
    the 3D position (relative to the camera) of each pixel and its corresponding
    RGB color of an array.
    "max_depth" is used to omit the points that are far enough.
    """
    far = 1000.0  # max depth in meters.
    normalized_depth = depth_to_array(image)

    # (Intrinsic) K Matrix
    k = numpy.identity(3)
    k[0, 2] = image.width / 2.0
    k[1, 2] = image.height / 2.0
    k[0, 0] = k[1, 1] = image.width / \
        (2.0 * math.tan(image.fov * math.pi / 360.0))

    # 2d pixel coordinates
    pixel_length = image.width * image.height
    u_coord = repmat(numpy.r_[image.width-1:-1:-1],
                     image.height, 1).reshape(pixel_length)
    v_coord = repmat(numpy.c_[image.height-1:-1:-1],
                     1, image.width).reshape(pixel_length)
    if color is not None:
        color = color.reshape(pixel_length, 3)
    normalized_depth = numpy.reshape(normalized_depth, pixel_length)

    # Search for pixels where the depth is greater than max_depth to
    # delete them
    max_depth_indexes = numpy.where(normalized_depth > max_depth)
    normalized_depth = numpy.delete(normalized_depth, max_depth_indexes)
    u_coord = numpy.delete(u_coord, max_depth_indexes)
    v_coord = numpy.delete(v_coord, max_depth_indexes)
    if color is not None:
        color = numpy.delete(color, max_depth_indexes, axis=0)

    # pd2 = [u,v,1]
    p2d = numpy.array([u_coord, v_coord, numpy.ones_like(u_coord)])

    # P = [X,Y,Z]
    p3d = numpy.dot(numpy.linalg.inv(k), p2d)
    p3d *= normalized_depth * far

    # Formating the output to:
    # [[X1,Y1,Z1,R1,G1,B1],[X2,Y2,Z2,R2,G2,B2], ... [Xn,Yn,Zn,Rn,Gn,Bn]]
    if color is not None:
        # numpy.concatenate((numpy.transpose(p3d), color), axis=1)
        return sensor.PointCloud(
            image.frame_number,
            numpy.transpose(p3d),
            color_array=color)
    # [[X1,Y1,Z1],[X2,Y2,Z2], ... [Xn,Yn,Zn]]
    return sensor.PointCloud(image.frame_number, numpy.transpose(p3d))

def polyFeatures(X, p):
	m, n   = shape(X)
	powers = matlib.repmat(range(1, p+1), m, 1)
	Xrep   = matlib.repmat(X, 1, p)
	# print shape(powers), shape(Xrep)
	X_poly = Xrep ** powers
	# print shape(X_poly)
	# print X_poly
	# test   = (ones((12,8))*2) ** powers
	# print test

	return X_poly

    def backcor(self,n,y,ord_cus,s,fct):
        # Rescaling
        N = len(n)
        index = np.argsort(n)
        n=np.array([n[i] for i in index])
        y=np.array([y[i] for i in index])
        maxy = max(y)
        dely = (maxy-min(y))/2.
        n = 2. * (n-n[N-1]) / float(n[N-1]-n[0]) + 1.
        n=n[:,np.newaxis]
        y = (y-maxy)/dely + 1

        # Vandermonde matrix
        p = np.array(range(ord_cus+1))[np.newaxis,:]
        T = repmat(n,1,ord_cus+1) ** repmat(p,N,1)
        Tinv = pinv(np.transpose(T).dot(T)).dot(np.transpose(T))

        # Initialisation (least-squares estimation)
        a = Tinv.dot(y)
        z = T.dot(a)

        # Other variables
        alpha = 0.99 * 1/2     # Scale parameter alpha
        it = 0                 # Iteration number
        zp = np.ones((N,1))         # Previous estimation

        # LEGEND
        while np.sum((z-zp)**2)/np.sum(zp**2) > 1e-10:

            it = it + 1        # Iteration number
            zp = z             # Previous estimation
            res = y - z        # Residual

            # Estimate d
            if fct=='sh':
                d = (res*(2*alpha-1)) * (abs(res)<s) + (-alpha*2*s-res) * (res<=-s) + (alpha*2*s-res) * (res>=s)
            elif fct=='ah':
                d = (res*(2*alpha-1)) * (res<s) + (alpha*2*s-res) * (res>=s)
            elif fct=='stq':
                d = (res*(2*alpha-1)) * (abs(res)<s) - res * (abs(res)>=s)
            elif fct=='atq':
                d = (res*(2*alpha-1)) * (res<s) - res * (res>=s)
            else:
                pass

            # Estimate z
            a = Tinv.dot(y+d)   # Polynomial coefficients a
            z = T.dot(a)            # Polynomial

        z=np.array([(z[list(index).index(i)]-1)*dely+maxy for i in range(len(index))])

        return z,a,it,ord_cus,s,fct

def cocktail(data1, data2):
	global BITWIDTH
	dt1 = data1 / float(1<<(BITWIDTH-1))
	dt2 = data2 / float(1<<(BITWIDTH-1))
	xx = [dt1, dt2]
	p = xx - np.transpose(matlib.repmat(np.mean(xx, 1), np.shape(xx)[1], 1))
	yy = np.dot(linalg.sqrtm(np.linalg.inv(np.cov(xx))), p)
	rr = matlib.repmat(np.sum(yy*yy, 0), np.shape(yy)[0], 1)
	w, s, v = np.linalg.svd(np.dot(rr*yy, np.transpose(yy)))
	f = np.transpose(np.dot(np.transpose(xx), w)) #w is the unmixing matrix
	f[0] *= float(1<<(BITWIDTH-1))
	f[1] *= float(1<<(BITWIDTH-1))
	return f