function [rgbd,rgb_im,map] = WP_rgbd(T_robot,T_camera,world)
%

PLANE = 1;  % world type
INT_DIST_THRESH = 0.00001;

MIN_RANGE = 0.1;
MAX_RANGE = 10;
MIN_X = -1;
MAX_X = 1;
MIN_Y = -1;
MAX_Y = 1;
STEP = 0.03;  % 0.08
NUM_ROWS = length([MIN_X:STEP:MAX_X]);
NUM_COLS = length([MIN_Y:STEP:MAX_Y]);

rgbd = zeros(NUM_ROWS,NUM_COLS,5);
map = [0,0,0];

T = T_robot*T_camera;

num_objects = length(world);

z = 0;
dir_c = [0;0;1;1];
dir_z = T*dir_c;
origin_c = T*[0;0;0;1];
dir_w = dir_z - origin_c;
origin_c3 = origin_c(1:3)';
wb = waitbar(0,'Range Finder');
for row = 1:NUM_ROWS
    waitbar(row/NUM_ROWS);
    y = MAX_Y - STEP*(row-1);
    for col = 1:NUM_COLS
        x = MIN_X + STEP*(col-1);
        pt_c = [x;y;z;1];
        pt_w = T*pt_c;
        line = [pt_w(1:3)';pt_w(1:3)'+dir_w(1:3)'];
        for obj = 1:num_objects
            if world(obj).type==PLANE
                params = world(obj).params;
                a = params(1);
                b = params(2);
                c = params(3);
                d = params(4);
                plane = WP_find3pts(a,b,c,d);
                [li,ls] = cv_int_line_plane(line,plane,INT_DIST_THRESH);
                pt_dist = norm(pt_w(1:3)'-li);
                if ls==1
                    if dot(dir_w(1:3)',[a,b,c])<0
                        if rgbd(row,col,5)==0|rgbd(row,col,5)>pt_dist
                            rgbd(row,col,1) = li(1); % World Frame info
                            rgbd(row,col,2) = li(2);
                            rgbd(row,col,3) = li(3);
                            rgbd(row,col,1) = x;  % Camera Frame info
                            rgbd(row,col,2) = y;
                            rgbd(row,col,3) = pt_dist;
                            rgbd(row,col,5) = pt_dist;
                            rgb = world(obj).rgb;
                            index = find(map(:,1)==rgb(1)...
                                &map(:,2)==rgb(2)&map(:,3)==rgb(3));
                            if isempty(index)
                                map = [map; rgb];
                                index = length(map(:,1));
                            end
                            rgbd(row,col,4) = index;
                        end
                    end
                end
            end
        end
    end
end
close(wb);
rgb_im = ind2rgb(rgbd(:,:,4),map);
tch = 0;

