function [frame,uncertainty] = WP_establish_frame(rgbd,planes,corners,...
    WP_set)
%

MIN_DOT = 0.98;

frame = [];
uncertainty = Inf;

[row,col] = WP_corner_in_data(planes);
W = planes(row-3:row-1,col-3:col-1);
left_up = median(W(:));
W = planes(row-3:row-1,col+1:col+3);
right_up = median(W(:));
W = planes(row+1:row+3,col-3:col+3);
down = median(W(:));
params1 = WP_set(corners(1)).info;
params2 = WP_set(corners(2)).info;
params3 = WP_set(corners(3)).info;
v12 = cross(params1(1:3),params2(1:3));
if abs(dot(v12,params3(1:3)))>MIN_DOT
    e1 = WP_set(right_up).info(1:3);  % world axes in camera frame
    e2 = WP_set(left_up).info(1:3);
    e3 = WP_set(down).info(1:3);
    x = WP_set(right_up).info(4);
    y = WP_set(left_up).info(4);
    z = WP_set(down).info(4);
    alpha = atan2(y,x);
    my_x_angle = alpha + pi/2;
    my_x_dir = [cos(my_x_angle);sin(my_x_angle);0];
    R = CS5320_gen_R(my_x_dir,pi/2-34.69*pi/180);
    my_y_dir = R*[-x;-y;0]/norm([-x,-y,0]);
    R = CS5320_gen_R(my_x_dir,-34.69*pi/180);
    my_z_dir = R*[-x;-y;0]/norm([-x,-y,0]);
else
    return
end
R = [my_x_dir, my_y_dir, my_z_dir];
t = [x;y;z];
T = [R,t; 0 0 0 1]; % complete camera to world transform
frame = eye(4);
f1 = [-x;y;0]/norm([-x;y;0]);
f2 = [-x;-y;0]/norm([-x;-y;0]);
frame(1:3,1:3) = [f1,f2,[0;0;1]];
frame(1:2,4) = [x;y];
uncertainty = max([WP_set(corners(1)).uncertainty,...
    WP_set(corners(2)).uncertainty,...
    WP_set(corners(3)).uncertainty]);
tch = 0;
