close all
clear all
clc

%% CAMARAS
% LIGAR CAMARA RGB
RGBDisp = videoinput('kinect',1,'RGB_640x480');

% LIGAR CAMARA DEPTH
DEPTHDisp = videoinput('kinect',2,'Depth_640x480');
preview(DEPTHDisp)
pause(16)

% RETIRAR FRAMES DE AMBAS AS CAMARAS 
RGBFrame = getsnapshot(RGBDisp);
DEPTHFrame = getsnapshot(DEPTHDisp);

% AMBOS FRAMES TIVERAM DE SER RODADOS PARA FICAREM NORMAIS
% O FRAME PARA A NUVEM DE PONTOS NÃO PODE SER RODADO
RGBFrameFlipped = flip(RGBFrame,2);
DEPTHFrameFlipped = flip(DEPTHFrame,2);

%% NUVEM DE PONTOS COMPLETA
% EXTRAIR A NUVEM DE PONTOS
ptCloudOriginal = pcfromkinect(DEPTHDisp, DEPTHFrame, RGBFrame);

%% RODAR A NUVEM DE PONTOS PARA FICAR PARALELA COM EIXO DOS XX
% ANGULO DE ROTAÇAO
theta = -0.055;

% MATRIZ DE ROTAÇAO EM X
Rx = [1 0 0; 0 cos(theta) -sin(theta); 0 sin(theta) cos(theta)];
trans = [0, 0, 0];
tform = rigid3d(Rx,trans);

% NOVA NUVEM DE PONTOS RODADA
ptCloudRot = pctransform(ptCloudOriginal,tform);



%% TARGET AREA IDENTIFICATION
stepSize = 100;

indices1 = 1:stepSize:ptCloudRot.Count;
tempPtCloud = select(ptCloudRot,indices1);

[tempPtCloud,validIndices] = removeInvalidPoints(tempPtCloud);

worldPoints = tempPtCloud.Location;

[Y,X] = ind2sub([size(ptCloudRot.Location,1),size(ptCloudRot.Location,2)],indices1);
imagePoints = [X(validIndices)' Y(validIndices)'];

camMatrix = estimateCameraMatrix(imagePoints,worldPoints);

roi1 = [-0.5 0.5 0 0.4 1.2 1.7];

indices1 = findPointsInROI(ptCloudRot,roi1,camMatrix);
ptcloudB = select(ptCloudRot,indices1);

figure
pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down")
hold on
pcshow(ptcloudB.Location,'r',"VerticalAxis","Y",VerticalAxisDir="Down")
title('Point Cloud w/Target Area')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

p1 = [-0.5 0.4 1.7]';
p2 = [-0.5 0.4 1.2]';
p3 = [0.5 0.4 1.2]';
p4 = [0.5 0.4 1.7]';
p5 = [-0.5 0 1.7]';
p6 = [-0.5 0 1.2]';
p7 = [0.5 0 1.2]';
p8 = [0.5 0 1.7]';
A1=[p1 p2 p3 p4;p4 p3 p7 p8;p7 p8 p5 p6;p7 p6 p2 p3;p6 p5 p1 p2;p8 p5 p1 p4];

p=fill3(A1(1,:),A1(2,:),A1(3,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(4,:),A1(5,:),A1(6,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(7,:),A1(8,:),A1(9,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(10,:),A1(11,:),A1(12,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(13,:),A1(14,:),A1(15,:),'--w');
p.FaceAlpha = 0.3;
p=fill3(A1(16,:),A1(17,:),A1(18,:),'--w');
p.FaceAlpha = 0.3;

hold off

%% REMOÇAO DO PLANO DO CHAO E VIZUALIZAÇAO DA NUVEM DE PONTOS DO OBJETO DENTRO DA AREA TARGET

% figure
% pcshow(ptcloudB.Location,'r',"VerticalAxis","Y",VerticalAxisDir="Down")

% SET THE MAX POINT TO PLABE DISTANCE FOR PLANE FITTING
maxDistance7 = 0.005;     % (m)

% SET THE NORMAL VECTOR OF THE PLANE
referenceVector7 = [0,1,0];

% SET THE MAX ANGULAR DISTANCE TO 5 DEGREES
maxAngularDistance7 = 5;

% DETECT THE FIRST PLANE AND EXTRACT IT FROM THE POINT CLOUD
[model7,inlierIndices7,outlierIndices7] = pcfitplane(ptcloudB,maxDistance7,referenceVector7,maxAngularDistance7);
% plane7 = select(ptcloudB,inlierIndices7);
remainPtCloud7 = select(ptcloudB,outlierIndices7);

% SET THE REGION OF INTEREST TO CONSTRAIN THE SEARCH FOR THE SECOND PLANE
% roi7 = [-inf,inf;0,inf;-inf,inf];
% sampleIndices7 = findPointsInROI(ptcloudB,roi7);


pclimpa7 = pcdenoise(remainPtCloud7,"NumNeighbors",100);

figure
pcshow(pclimpa7,"VerticalAxis","Y",VerticalAxisDir="Down")
title('remain pt cloud')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')










%% CONTORNOS
indices=0;
for i=1:size(pclimpa7.Location(:,1))
   indices= [indices find(ptCloudRot.Location(:,:,1)==pclimpa7.Location(i,1) & ptCloudRot.Location(:,:,2)==pclimpa7.Location(i,2) & ptCloudRot.Location(:,:,3)==pclimpa7.Location(i,3))];
end

% LINHAS E COLUNAS DA MATRIZ DO OBJETO SELECIONADO
[row,col] = ind2sub([480 640],indices(2:end));

% CONVERTER IMAGEM RGB EM PRETOS
BlackRGB=zeros(size(RGBFrameFlipped));

% OS PONTOS DO OBJETO COINCIDENTES COM A IMAGEM RGB SÃO CONVERTIDOS A BRANCO

for i=1:size(col,2)
    BlackRGB(row(i),col(i),:)=1;

%     qq(row(i),col(i),:)=RGBFrameFlipped
end






% 
% figure
% imshow(BlackRGB)

% 
bi = imbinarize(im2gray(BlackRGB));
% figure
% imshow(bi);

p=bwperim(bi);
% figure
% imshow(p)

% ENCONTRAR OS LIMITES DO OBJETO
rowmax = max(max(row))+10;
rowmin = min(min(row))-10;
colmax = max(max(col))+10;
colmin = min(min(col))-10;

% FRAME RGB RECORTADO PARA ELIMINAR PERTURBAÇÕES NO FUNDO
RGBCropped = p(rowmin:rowmax,colmin:colmax,:);



% MOSTRAR IMAGEM DO OBJETO RECORTADO
figure
imshow(RGBCropped)





