%Calibracao Extrinseca-TOP LEFT
close all 
clear all
clc

load('calibrationSession.mat')
cameraParams=calibrationSession.CameraParameters;
worldPoints=calibrationSession.CameraParameters.WorldPoints;

img_from_camera=(imread('frame0000.jpg'));
imagePoints = detectCheckerboardPoints(img_from_camera);

%Deteção da origem do xadrez
% figure(1)
% imshow(img_from_camera)
% hold on
% plot(imagePoints(1,1),imagePoints(1,2),'r*')
% origin_image=[imagePoints(1,1),imagePoints(1,2)];


[rotation, translation]=extrinsics(imagePoints, calibrationSession.CameraParameters.WorldPoints,calibrationSession.CameraParameters);
transformC_X=[rotation',translation';0 0 0 1];
newWorldPoints=pointsToWorld(cameraParams,rotation,translation,imagePoints);

%Diferença entre posições no mundo (erro)
% figure(2)
% plot(worldPoints(:,1),worldPoints(:,2),'gx');
% hold on
% plot(newWorldPoints(:,1),newWorldPoints(:,2),'ro');

%Posição da câmara em relação ao xadrez:
[orientation, location] = extrinsicsToCameraPose(rotation, ...
  translation);
% figure (3)
% plotCamera('Location',location,'Orientation',orientation,'Size',20);
% hold on
% pcshow([worldPoints,zeros(size(worldPoints,1),1)], ...
%   'VerticalAxisDir','down','MarkerSize',40);

%----------------------------------------------------------------
%Transformação xadrez-moving_axis(em mm)
% rotationX_MA=[-1 0 0 
%                0 1 0
%                0 0 -1];
rotationX_MA=[0 -1 0 
              -1 0 0
              0 0 -1];
translationX_MA=[0;4970;51];
%translationX_MA=[0;-4970;-51];
transformX_MA=[rotationX_MA translationX_MA; 0 0 0 1];

%-------------------------------------------------------------------
%Transformação Câmara-Moving_Axis
trasnformaC_MA=transformC_X*transformX_MA;
%Inversa:
transformMA_C=Transformacao_inversa(trasnformaC_MA);

%------------------------------------------------------------------
%valores ROS-launch file
X=transformMA_C(1,end);
Y=transformMA_C(2,end);
Z=transformMA_C(3,end);

roll=atan2(transformMA_C(2,1),transformMA_C(1,1));
pitch=atan2(-transformMA_C(3,1),sqrt((transformMA_C(3,2)^2)+(transformMA_C(3,3)^2)));
yaw=atan2(transformMA_C(3,2),transformMA_C(3,3));

tf_ros=[X,Y,Z,yaw,pitch,roll];