import cv2
import pcl
from openni import *
import numpy as np,cv
class Segmentation:

    def Reorganice(self,depth):
        """Transform a depth image into a point cloud with one point for each
        pixel in the image, using the camera transform for a camera
        centred at cx, cy with field of view fx, fy.

        depth is a 2-D ndarray with shape (rows, cols) containing
        depths from 1 to 254 inclusive. The result is a 3-D array with
        shape (rows, cols, 3). Pixels with invalid depth in the input have
        NaN for the z-coordinate in the result.

        """
        Mesh = []
        for i in xrange(0,depth.shape[0]):
            for j in xrange(0,depth.shape[1]):
                Mesh.append((i,j,depth[i][j][0]))
                # print "i: "+i.__str__()+" j: "+j.__str__()+" d: "+depth[i][j][0].__str__()
        return Mesh

    def CreateCloud(self, Depth):
        self.Cloud = pcl.PointCloud()
        Reor= self.Reorganice(Depth)
        self.Cloud.from_list(Reor)
        # depth = self.Reorganice(Depth)
        D = Depth[:,:,0]
        return D

    def PlaneSegmentation(self):
        # fil = self.Cloud.make_passthrough_filter()
        # fil.set_filter_field_name("z")
        # fil.set_filter_limits(10, 35)
        # self.Cloud = fil.filter()
        # print self.Cloud.size
        seg = self.Cloud.make_segmenter_normals(ksearch=100)
        seg.set_optimize_coefficients(True)
        seg.set_model_type(pcl.SACMODEL_PLANE)
        seg.set_normal_distance_weight(0.1)
        seg.set_method_type(pcl.SAC_RANSAC)
        seg.set_max_iterations(200)
        seg.set_distance_threshold(0.01)
        indices, model = seg.segment()
        self.Cloud = self.Cloud.extract(indices, negative=False)
        print self.Cloud.size
        return self.Cloud

    def Clustering(self,cloud):
        None

    def ClustertoImage(self,cloud,cluster):
        None

    def ShowCloud(self,cloud,RGB):
        None

    def __init__(self):
        self.Cloud = pcl.PointCloud()
        self.numSegments = 100
        self.eps = 1e-7
        self.num = 0
        self.n = 0