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);

figure
pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down")

%% 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.2 0.2 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.415 1.7]';
p2 = [-0.5 0.415 1.2]';
p3 = [0.5 0.415 1.2]';
p4 = [0.5 0.415 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

%% REMOVER O PLANO DO CHAO
% SET THE MAX POINT TO PLABE DISTANCE FOR PLANE FITTING
maxDistance = 0.0005;     % (m)

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

% DETECT THE FIRST PLANE AND EXTRACT IT FROM THE POINT CLOUD
[~,~,outlierIndices] = pcfitplane(ptcloudB,maxDistance,referenceVector);
remainPtCloud = select(ptcloudB,outlierIndices);

% REMOVER O BARULHO DA NUVEM DE PONTOS (ELIMINAR "IMPUREZAS")
pclimpa = pcdenoise(remainPtCloud,"NumNeighbors",50);

% MOSTRAR A NUVEM DE PONTOS LIMPA
figure
pcshow(pclimpa,"VerticalAxis","Y",VerticalAxisDir="Down")
title('Object Cloud Point')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')


% pontos x e y (retirar z da nuvem do objeto)
xnuvem = pclimpa.Location(:,1);
ynuvem = pclimpa.Location(:,2);

% mostrar objeto em 2D
figure(200)
plot(xnuvem,ynuvem,'.k')
set(gca,'ydir','reverse')
axis off

% retirar frame do plot anterior
f= frame2im(getframe(figure(200)));
I = imresize(f,[480,640]);
figure
imshow(I)
title('frame do plot')

% binarizar imagem e inverter cores
imagembin = imbinarize(im2gray(f));
inverseGrayImage = uint8(255) - im2uint8(imagembin);
figure
imshow(inverseGrayImage)
title('frame binarizado')


% PREENCER ESPAÇOS ABERTOS DO OBJETO
se = strel('disk',5);
j = imclose(inverseGrayImage,se);
figure
imshow(j)

% RETIRAR OS CONTORNOS DO OBJETO
imgedge1 = edge(j,'canny');
figure
imshow(imgedge1)











bw3 = imdilate(inverseGrayImage, [1 1 1; 1 1 1; 1 1 1]);
figure
imshow(bw3)


bw4 =imdilate(bw3, [0 1 0; 1 1 1; 0 1 0]);
figure
imshow(bw4)

% encontrar as edges do objeto
imgedge = edge(bw4,'prewitt');
figure
imshow(imgedge)

% preencher furos
% imgfill = imfill(imgedge,'holes');
% figure
% imshow(imgfill)


total = bwarea(imgedge);
figure
imshow(total)

bw2 = bwperim(imgedge);
figure
imshow(bw2)
