%% CUBE OBJECT DEFINITION
%Cube size; origin at a vertex.
sizex=.8;
sizey=.8;
sizez=.5;
% Definition of 3D points w.r.t. cube frame
P1 = [sizex,  sizey, 0]'; 
P2 = [sizex,  0, 0]'; 
P3 = [0,  sizey, 0]'; 
P4 = [0,  0, 0]'; 
P = [P1 P2 P3 P4]

% Setting object frame (with orientation defined by consecutive rotations
%around fixed axis x,y,z)
Tobject = transl(0, -1, sizez)*rpy2tr(0,0,-10*pi/180);

%% CAMERA SETUP
%
cam = CentralCamera('default');
cam.f=0.008; %default is 0.008
% setting camera frame (with orientation defined by consecutive rotations
%around fixed axis x,y,z)
cam.T = transl(0, 0, 3)*rpy2tr(0, 160*pi/180, 10*pi/180); 
p = cam.plot(P, 'objpose', Tobject ) %with object translated

%% VISUALIZE 3D FRAMES
%
figure
% plot world frame
W = transl(0,0,0);
trplot(W, 'frame', 'W', 'axis', [-3 3 -3 3 -2 4], 'view', [-60 10])
hold on
% plot camera frame 
trplot(cam.T, 'frame', 'C', 'color', 'k')
% plot object frame
trplot(Tobject, 'frame', 'O', 'color', 'r')
% plot Pw, i.e. point P w.r.t. world frame
% Pw=Tobject*[P;1];
Pw=Tobject*cat(1,P,ones(1,size(P,2)));
plot3(Pw(1,:),Pw(2,:),Pw(3,:),'*r')

% Plot the camera and the virtual image plane
zplane = 1.5;
scalecamera = 0.3;
plotCameraImagePlane(cam, zplane, Pw, scalecamera);

%plot the object
cube_plot(Tobject,sizex,sizey,sizez,'r');