%% Corke chap11


%%
clear; 
close all; 
clc;

%%
% The Toolbox allows us to create a model of a central-perspective camera.
% For example returns an instance of a CentralCamera object with a 15 mm
% lens. By default the camera is at the origin of the world frame with its
% optical axis pointing in the world z-direction as shown in Fig. 11.2.

cam = CentralCamera('focal', 0.015);
cam
P = [0.3, 0.4, 3.0]';
cam.project(P)
cam.project(P, 'Tcam', transl(-0.5, 0, 0) )

%%

cam = CentralCamera('focal', 0.015, 'pixel', 10e-6, ...
'resolution',[1280 1024], 'centre', [640 512], 'name', 'mycamera')

cam.project(P)
cam.C
cam.K
cam.fov() * 180/pi
P = mkgrid(3, 0.2, 'T', transl(0, 0, 1.0));
P(:,1:4)
cam.project(P)
cam.plot(P)
F = getframe(cam.figure)

%%

figure()
imshow(F.cdata)

%%

Tcam = transl(-1,0,0.5)*troty(0.9);
cam.plot(P, 'Tcam', Tcam)
F = getframe(cam.figure)
figure()
imshow(F.cdata)

%%

cam.project([1 0 0 0]', 'Tcam', Tcam)
p = cam.plot(P, 'Tcam', Tcam);
p(:,1:4)
cube = mkcube(0.2, 'T', transl([0, 0, 1]) );
cam.plot(cube);

%%

[X,Y,Z] = mkcube(0.2, 'T', transl([0, 0, 1.0]), 'edge');
cam.mesh(X, Y, Z)
F = getframe(cam.figure)
figure()
imshow(F.cdata)

%%

cam.clf
cam.T = transl(-1,0,0.5)*troty(0.8);
cam.mesh(X, Y, Z, 'Tcam', Tcam);
F = getframe(cam.figure)
figure()
imshow(F.cdata)

%%

cam.delete
P = mkcube(0.2);
T_unknown = transl(0.1, 0.2, 1.5) * rpy2tr(0.1, 0.2, 0.3);
cam_unknown = CentralCamera('focal', 0.015, ...
'pixel',10e-6, 'resolution', [1280 1024], 'centre', [512 512], ...
'noise',0.05);
p = cam_unknown.project(P, 'Tobj', T_unknown);
C = camcald(P, p)
est = invcamcal(C)
est.f/est.rho(1)
cam_unknown.f/cam_unknown.rho(2)
T_unknown*est.T


figure()
plot_sphere(P, 0.03, 'r')

%%

trplot(eye(4,4), 'frame', 'T', 'color', 'b', 'length', 0.3)

%%

est.plot_camera()

