# Path: lfd/rapprentice/clouds.py
# DEFAULT_F = 535.
#     X = (x - cx)*(Z/f)
#     Y = (y - cy)*(Z/f)
#     XYZ = np.empty((480,640,3))
#     Z = XYZ[:,:,2] = depth / 1000. # convert mm -> meters
#     XYZ[:,:,0] = (x - cx)*(Z/f)
#     XYZ[:,:,1] = (y - cy)*(Z/f)
# def xyZ_to_XY(x,y,Z,f=DEFAULT_F):
# def XYZ_to_xy(X,Y,Z,f=DEFAULT_F):
# def depth_to_xyz(depth,f=DEFAULT_F):
# def downsample(xyz, v):


import cloudprocpy
import cv2, numpy as np
    import interactive_roi as ir
from lfd.rapprentice import berkeley_pr2, clouds
    valid_mask = depth > 0
    
    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
    
    z = xyz_w[:,:,2]   
    z0 = xyz_k[:,:,2]
    if DEBUG_PLOTS:
        cv2.imshow("z0",z0/z0.max())
        cv2.imshow("z",z/z.max())
        cv2.imshow("rgb", rgb)
        cv2.waitKey()
    
    height_mask = xyz_w[:,:,2] > .7 # TODO pass in parameter
    
    
    good_mask = red_mask & height_mask & valid_mask
    if DEBUG_PLOTS:
        cv2.imshow("red",red_mask.astype('uint8')*255)
        cv2.imshow("above_table", height_mask.astype('uint8')*255)
        cv2.imshow("red and above table", good_mask.astype('uint8')*255)
        print "press enter to continue"
        cv2.waitKey()
        
        
    

    good_xyz = xyz_w[good_mask]
    


# Path: lfd/rapprentice/clouds.py
# DEFAULT_F = 535.
#     X = (x - cx)*(Z/f)
#     Y = (y - cy)*(Z/f)
#     XYZ = np.empty((480,640,3))
#     Z = XYZ[:,:,2] = depth / 1000. # convert mm -> meters
#     XYZ[:,:,0] = (x - cx)*(Z/f)
#     XYZ[:,:,1] = (y - cy)*(Z/f)
# def xyZ_to_XY(x,y,Z,f=DEFAULT_F):
# def XYZ_to_xy(X,Y,Z,f=DEFAULT_F):
# def depth_to_xyz(depth,f=DEFAULT_F):
# def downsample(xyz, v):


# Path: lfd/rapprentice/cloud_proc_funcs.py
import cloudprocpy
import cv2, numpy as np
    import interactive_roi as ir
from lfd.rapprentice import berkeley_pr2, clouds

    valid_mask = depth > 0
    
    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
    
    z = xyz_w[:,:,2]   
    z0 = xyz_k[:,:,2]
    if DEBUG_PLOTS:
        cv2.imshow("z0",z0/z0.max())
        cv2.imshow("z",z/z.max())
        cv2.imshow("rgb", rgb)
        cv2.waitKey()
    
    height_mask = xyz_w[:,:,2] > .7 # TODO pass in parameter
    
    
    good_mask = red_mask & height_mask & valid_mask
    if DEBUG_PLOTS:
        cv2.imshow("red",red_mask.astype('uint8')*255)
        cv2.imshow("above_table", height_mask.astype('uint8')*255)
        cv2.imshow("red and above table", good_mask.astype('uint8')*255)
        print "press enter to continue"
        cv2.waitKey()
        
        
    

    good_xyz = xyz_w[good_mask]
    


    return clouds.downsample(good_xyz, .01)