function [WP_set,planes] = WP_rgbd2WP(rgbd)
%

MIN_PTS4PLANE = 10;
MIN_THRESH = 0.1;
NUM_UNUSED = 21;
WP = 4;
PLANE = 2;

WP_set = [];
planes = [];

if isempty(rgbd)
    return
end

[num_rows,num_cols,dummy] = size(rgbd);
num_pixels = num_rows*num_cols;
% channels 1-3 are x,y,z;
% channel 4 is color map index
% channel 5 is range value (from camera pixel to surface)

% Extract Planes
unused = ones(num_rows,num_cols);
planes = zeros(num_rows,num_cols);
num_planes = 0;
done = sum(unused(:))<MIN_PTS4PLANE;
while done==0
    OK = filter2(ones(5,5),unused);
    [rows,cols] = find(OK>=NUM_UNUSED);
    index = find(rows>2&rows<num_rows-2&cols>2&cols<num_cols-2);
    if ~isempty(index)
        r = rows(index(1));
        c = cols(index(1));
        x1 = rgbd(r-2,c-2,1);
        y1 = rgbd(r-2,c-2,2);
        z1 = rgbd(r-2,c-2,3);
        x2 = rgbd(r+2,c+2,1);
        y2 = rgbd(r+2,c+2,2);
        z2 = rgbd(r+2,c+2,3);
        x3 = rgbd(r-2,c+2,1);
        y3 = rgbd(r-2,c+2,2);
        z3 = rgbd(r-2,c+2,3);
        x4 = rgbd(r+2,c-2,1);
        y4 = rgbd(r+2,c-2,2);
        z4 = rgbd(r+2,c-2,3);
        pts(1,:) = [x1;y1;z1];
        pts(2,:) = [x2;y2;z2];
        pts(3,:) = [x3;y3;z3];
        pts(4,:) = [x4;y4;z4];
        n = cross(pts(2,:)-pts(1,:),pts(4,:)-pts(3,:));
        n = n/norm(n);
        if n(3)>0
            n = -n;
        end
        d = -n*pts(1,:)';
        dists(1) = abs(n*[x1;y1;z1]+d);
        dists(2) = n*pts(2,:)'+d;
        dists(3) = n*pts(3,:)'+d;
        dists(4) = n*pts(4,:)'+d;
        if max(abs(dists))<MIN_THRESH
            num_planes = num_planes + 1;
            a = n(1);
            b = n(2);
            c = n(3);
            [rows,cols] = find(unused==1);
            num_pts = length(rows);
            avg_err = 0;
            for pp = 1:num_pts
                x = rgbd(rows(pp),cols(pp),1);
                y = rgbd(rows(pp),cols(pp),2);
                z = rgbd(rows(pp),cols(pp),3);
                err = abs(a*x+b*y+c*z+d);
                if err<MIN_THRESH
                    unused(rows(pp),cols(pp)) = 0;
                    planes(rows(pp),cols(pp)) = num_planes;
                    avg_err = avg_err + err;
                end
            end
            avg_err = avg_err/sum(sum(planes==num_planes));
            WP_set(num_planes).name = ['plane ',num2str(num_planes)];
            WP_set(num_planes).id = num_planes;
            WP_set(num_planes).type = WP;
            WP_set(num_planes).subtype = PLANE;
            WP_set(num_planes).info = [a,b,c,d];
            WP_set(num_planes).uncertainty = avg_err;
            WP_set(num_planes).parent = 0;
            WP_set(num_planes).children = 0;
            done = sum(unused(:))<MIN_PTS4PLANE;
        end
    else
        done = 1;
    end
end

% Find convex hull for each planar segment
for s = 1:num_planes
    [rows,cols] = find(planes==s);
    num_pts = length(rows);
    pts = zeros(num_pts,3);
    params = WP_set(s).info;
    v = -params(4)*[params(1);params(2);params(3)];
    alpha = atan2(v(2),v(1));
    f = sqrt(v(1)^2+v(2)^2);
    beta = atan2(f,v(3));
    for p = 1:num_pts
        pts(p,1) = rgbd(rows(p),cols(p),1) - v(1);
        pts(p,2) = rgbd(rows(p),cols(p),2) - v(2);
        pts(p,3) = rgbd(rows(p),cols(p),3) - v(3);
    end
    pts = pts';
    Rz = CS5320_gen_R([0;0;1],-alpha);
    Ry = CS5320_gen_R([0;1;0],-beta);
    pts_p = Ry*Rz*pts;
    pts_ch = pts_p(1:2,:)';
    K = convhull(pts_ch);
    WP_set(s).convhull = K;
    WP_set(s).convhull_pts = [pts(1,K)'+v(1),pts(2,K)'+v(2),pts(3,K)'+v(3)];
end
tch = 0;
