# 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

DEBUG_PLOTS = False


def extract_red(rgb, depth, T_w_k):
    """
    extract red points and downsample
    """
        
    hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
    h = hsv[:,:,0]
    s = hsv[:,:,1]
    v = hsv[:,:,2]
    
    red_mask = ((h<10) | (h>150)) & (s > 100) & (v > 100)
    
    valid_mask = depth > 0
    

# 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


DEBUG_PLOTS = False


def extract_red(rgb, depth, T_w_k):
    """
    extract red points and downsample
    """
        
    hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
    h = hsv[:,:,0]
    s = hsv[:,:,1]
    v = hsv[:,:,2]
    
    red_mask = ((h<10) | (h>150)) & (s > 100) & (v > 100)
    
    valid_mask = depth > 0
    

    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)