clear all
close all
clc

RGBDisp = videoinput('kinect',1,'RGB_640x480');
DEPTHDisp = videoinput('kinect',2,'Depth_640x480');
preview(DEPTHDisp)
pause(16)

RGBFrameFlipped = flip(getsnapshot(RGBDisp),2);
DEPTHFrameFlipped = flip(getsnapshot(DEPTHDisp),2);

RGBFrame1 = getsnapshot(RGBDisp);
DEPTHFrame1 = getsnapshot(DEPTHDisp);

ptCloudOriginal = pcfromkinect(DEPTHDisp, DEPTHFrame1, RGBFrame1);

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

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

% MOSTAR NUVEM DE PONTOS
% PointCloudBrowser = figure("Name",'Point Cloud Browser','NumberTitle','off');
% pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down");
% title('Full Point Cloud')
% xlabel('X(m)')
% ylabel('Y(m)')
% zlabel('Z(m)')


% MOSTAR NUVEM DE PONTOS
figure("Name",'Point Cloud Browser','NumberTitle','off')
pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down");
title('Full Point Cloud')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')


%% DEFINIR UMA ROI CUBICA PARA ASSUMIR A TARGET AREA
% ESPECIFICAR O STEP SIZE
stepSize = 100;

% AMOSTRA A NUVEM DE PONTOS DE ENTRADA E ARMAZENA AS COORDENADAS DE PONTOS COMO UM OBJETO DE NUVEM DE PONTOS
indices = 1:stepSize:ptCloudRot.Count;
tempPtCloud = select(ptCloudRot,indices);

% REMOVER OS PONTOS INVALIDOS DENTRO DA AMOSTRA ANTERIOR
[tempPtCloud,validIndices] = removeInvalidPoints(tempPtCloud);

% OBTER AS COORDENADAS DE LOCALIZAÇAO 3D DOS PONTOS DA NUVEM DE PONTOS
worldPoints = tempPtCloud.Location;

% CONVERTER E ENCONTRAR AS COORDENADAS DOS PONTOS 2D CORRESPONDENTES AOS PONTOS 3D DA NUVEM
[Y,X] = ind2sub([size(ptCloudRot.Location,1),size(ptCloudRot.Location,2)],indices);
imagePoints = [X(validIndices)' Y(validIndices)'];

% ESTIMATIVA DA MATRIZ DE PROJEÇAO DA CAMARA A PARTIR DA IMAGEM E DAS COORDENADAS DOS PONTOS DE LOCALIZAÇAO 3D
camMatrix = estimateCameraMatrix(imagePoints,worldPoints);

% DEFINIR A ROI CUBICA PARA A TARGET AREA
roi = [-0.5 0.5 0 0.415 1 1.8];

% ENCONTRAR OS INDICES DOS PONTOS DA NUVEM QUE ESTAO DENTRO DA ROI CUBICA
indices = findPointsInROI(ptCloudRot,roi,camMatrix);

% SELECIONAR OS PONTOS DA NUVEM QUE SE ENCONTRAM DENTRO DA ROI
ptcloudB = select(ptCloudRot,indices);

% MOSTRAR A NUVEM DE PONTOS COM A TARGER AREA
figure;
pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down")
hold on
pcshow(ptcloudB.Location,'r',"VerticalAxis","Y",VerticalAxisDir="Down")
title('Full Point Cloud with Target Area')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

% DEFINIÇÃO DOS PONTOS DOS VERTICES DO CUBO DA ROI
p1 = [roi(1) roi(4) roi(6)]';
p2 = [roi(1) roi(4) roi(5)]';
p3 = [roi(2) roi(4) roi(5)]';
p4 = [roi(2) roi(4) roi(6)]';
p5 = [roi(1) roi(3) roi(6)]';
p6 = [roi(1) roi(3) roi(5)]';
p7 = [roi(2) roi(3) roi(5)]';
p8 = [roi(2) roi(3) roi(6)]';

% DEFINIR AS FACES DO CUBO ATRAVES DOS VERTICES ANTERIORES
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];

% CORES DAS FACES
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
% DEFINIR A DISTANCIA MAXIMA PONTO-PLANO PARA A MONTAGEM DO PLANO
maxDistance = 0.0005;     % (m)

% DEFINIR O VETOR NORMAL AO PLANO A DEFINIR (NESTE CASO VETOR PREPENDICULAR AO PLANO Y)
referenceVector = [0,1,0];

% DETETAR O PLANO E EXTRAI-LO DA NUVEM DE PONTOS
[~,~,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 E SEM O PLANO DO CHAO
figure("Name",'Object Cloud Viewer','NumberTitle','off');
pcshow(pclimpa,"VerticalAxis","Y",VerticalAxisDir="Down")
title('Object Cloud Point')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')





%% roi
% CONVERTER MATRIX FULL EM MATRIX DE 3xCOLUNAS COM AS COORDENADAS X Y E Z RESPECTIVAMENTE
NovaMatrizNuvemTotal = [reshape(ptCloudRot.Location(:,:,1),[],1) reshape(ptCloudRot.Location(:,:,2),[],1) reshape(ptCloudRot.Location(:,:,3),[],1)];

% COMPARAR MATRIZ FULL E MATRIX DA TARGET AREA E RETIRAR OS INDICES DOS PONTOS DO SEU INTERIOR
[RowNova01,~] = find(NovaMatrizNuvemTotal(:,1)<roi(2) & NovaMatrizNuvemTotal(:,1)>roi(1) & NovaMatrizNuvemTotal(:,2)<roi(4) & NovaMatrizNuvemTotal(:,2)>roi(3) & NovaMatrizNuvemTotal(:,3)>roi(5) & NovaMatrizNuvemTotal(:,3)<roi(6));

% CONVERTER OS INDICES EM LINHA COLUNA DENTRO DA RESOLUÇAO DO FRAME
[ROWNOVA1,COLNOVA1] = ind2sub([480 640],RowNova01);

% ENCONTRAR OS MAX E MIN DE COLUNA E LINHA PARA FORMATAR IMAGEM
% maxrow3 = max(ROWNOVA1)+10;
% minrow3 = min(ROWNOVA1)-10;
% maxcol3 = max(COLNOVA1)+10;
% mincol3 = min(COLNOVA1)-10;

maxrow3 = max(ROWNOVA1);
minrow3 = min(ROWNOVA1);
maxcol3 = max(COLNOVA1);
mincol3 = min(COLNOVA1);

% RETIRAR UM FRAME DA CAMARA RGB E DEPTH
DEPTHFrameFlipped2 = flip(getsnapshot(DEPTHDisp),2);
RGBFrameFlipped2 = flip(getsnapshot(RGBDisp),2);

% "RECORTAR" IMAGEM RGB E DEPTH PELOS MAX/MIN COLUNA/LINHA
RGB_ROI = RGBFrameFlipped2(minrow3:maxrow3,mincol3:maxcol3,:);
DEPTH_ROI = DEPTHFrameFlipped2((minrow3:maxrow3)-10,(mincol3:maxcol3)+15,:);

% ABRIR PLOT PARA CRIAR A ZONA ROI
figroi = figure("Name",'Region of Interest Selection','NumberTitle','off');
imshow(RGB_ROI)
% xlim([mincol3 maxcol3])
% ylim([minrow3 maxrow3])

h1 = drawrectangle;

% ESPERAR PELO USER DEFINIR A ROI PARA SE OBTER A SUA POSIÇAO
% position1 = wait(h1);
wait(h1)

% % RECORTAR A IMAGEM PELA POSIÇAO DEFINIDA ANTERIORMENTE
% ROI_RGB_cropped = imcrop(RGB_ROI, h1.Position);
% 
% % RECORTAR A IMAGEM DEPTH PELA MESMA POSIÇAO QUE A RGB
% ROI_DEPTH_cropped =  imcrop(DEPTH_ROI, h1.Position);
% 
% % FECHAR A JANELA DA ROI DEFINIDA
% % close(figure(figroi))
% 
% figure
% subplot(1,2,1)
% imshow(ROI_RGB_cropped)
% subplot(1,2,2)
% imshow(ROI_DEPTH_cropped,[])

% c1 = (480 - size(ROI_RGB_cropped,1))/2;
% c2 = (640 - size(ROI_RGB_cropped,2))/2;
% 
% if isinteger(c1) == 0
%     c1 = floor(c1)+1;
% end
% 
% if isinteger(c2) == 0
%     c2 = floor(c2)+1;
% end
% 
% 
% bbbb = padarray(ROI_RGB_cropped,[c1 c2]);
% 
% if size(bbbb,1)~=480 && size(bbbb,2)~=640 
%     bbbb = imresize(bbbb,[480 640]);
% end
% 
% 
% c3 = (480 - size(ROI_DEPTH_cropped,1))/2;
% c4 = (640 - size(ROI_DEPTH_cropped,2))/2;
% 
% if isinteger(c3) == 0
%     c3 = floor(c3)+1;
% end
% 
% if isinteger(c4) == 0
%     c4 = floor(c4)+1;
% end
% 
% 
% cccc = padarray(ROI_DEPTH_cropped,[c3 c4]);
% 
% if size(cccc,1)~=480 && size(cccc,2)~=640 
%     cccc = imresize(cccc,[480 640]);
% end
% 
% figure
% subplot(1,2,1)
% imshow(bbbb)
% subplot(1,2,2)
% imshow(cccc,[])
% 
% 
% nuvemfinalmentedoobjetoesperoeu = pcfromkinect(DEPTHDisp,cccc,bbbb);
% nuvemfinal1 = pcdenoise(nuvemfinalmentedoobjetoesperoeu,"NumNeighbors",20);
% 
% figure
% pcshow(nuvemfinal1,"VerticalAxis","Y",VerticalAxisDir="Down")
% title('Object Cloud Point')
% xlabel('X(m)')
% ylabel('Y(m)')
% zlabel('Z(m)')
% 
% 
% 
% 
% imresize(DEPTHDisp,[size(ROI_RGB_cropped,1) size(ROI_RGB_cropped,2)])
% 
% 
% 







%% RETIRAR DO OBJETO O INTERIOR DA ROI ANTERIORMENTE DEFINIDA
% ESPECIFICAR O STEP SIZE
stepSize3 = 100;

% AMOSTRA A NUVEM DE PONTOS DE ENTRADA E ARMAZENA AS COORDENADAS DE PONTOS COMO UM OBJETO DE NUVEM DE PONTOS
indices3 = 1:stepSize3:ptCloudRot.Count;
tempPtCloud3 = select(ptCloudRot,indices3);

% REMOVER OS PONTOS INVALIDOS DENTRO DA AMOSTRA ANTERIOR
[tempPtCloud3,validIndices3] = removeInvalidPoints(tempPtCloud3);

% OBTER AS COORDENADAS DE LOCALIZAÇAO 3D DOS PONTOS DA NUVEM DE PONTOS
worldPoints3 = tempPtCloud3.Location;

% CONVERTER E ENCONTRAR AS COORDENADAS DOS PONTOS 2D CORRESPONDENTES AOS PONTOS 3D DA NUVEM
[Y,X] = ind2sub([size(ptCloudRot.Location,1),size(ptCloudRot.Location,2)],indices3);
imagePoints3 = [X(validIndices3)' Y(validIndices3)'];

% ESTIMATIVA DA MATRIZ DE PROJEÇAO DA CAMARA A PARTIR DA IMAGEM E DAS COORDENADAS DOS PONTOS DE LOCALIZAÇAO 3D
camMatrix3 = estimateCameraMatrix(imagePoints3,worldPoints3);

% DEFINIR A ROI CUBICA PARA A TARGET AREA
distbetween_x = norm(max(pclimpa.Location(:,1))-min(pclimpa.Location(:,1)));
distbetween_y = norm(max(pclimpa.Location(:,2))-min(pclimpa.Location(:,2)));

Vertice1xmin = floor(h1.Vertices(1));
Vertice1xmax = floor(h1.Vertices(4));
Vertice1ymin = floor(h1.Vertices(5));
Vertice1ymax = floor(h1.Vertices(7));


if Vertice1xmin == 0 
    Vertice1xmin = 1;
end

if Vertice1xmax == 0 
    Vertice1xmax = 1;
end

if Vertice1ymin == 0 
    Vertice1ymin = 1;
end

if Vertice1ymax == 0 
    Vertice1ymax = 1;
end


P_ROI_Xmin = min(min(pclimpa.Location(:,1)))+((Vertice1xmin)*distbetween_x)/(size(RGB_ROI,2));
P_ROI_Xmax = min(min(pclimpa.Location(:,1)))+((Vertice1xmax)*distbetween_x)/(size(RGB_ROI,2));
P_ROI_Ymax = min(min(pclimpa.Location(:,2)))+((Vertice1ymax)*distbetween_y)/(size(RGB_ROI,1));
P_ROI_Ymin = min(min(pclimpa.Location(:,2)))+((Vertice1ymin)*distbetween_y)/(size(RGB_ROI,1));

roi3 = [P_ROI_Xmin P_ROI_Xmax P_ROI_Ymin P_ROI_Ymax roi(5) roi(6)];

% ENCONTRAR OS INDICES DOS PONTOS DA NUVEM QUE ESTAO DENTRO DA ROI CUBICA
indices3 = findPointsInROI(ptCloudRot,roi3,camMatrix3);

% SELECIONAR OS PONTOS DA NUVEM QUE SE ENCONTRAM DENTRO DA ROI
ptcloudB3 = select(ptCloudRot,indices3);

% MOSTRAR A NUVEM DE PONTOS COM A TARGER AREA
figure;
pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down")
hold on
pcshow(ptcloudB3.Location,'r',"VerticalAxis","Y",VerticalAxisDir="Down")
title('Full Point Cloud with Target Area')
xlabel('X(m)')
ylabel('Y(m)')
zlabel('Z(m)')

% DEFINIÇÃO DOS PONTOS DOS VERTICES DO CUBO DA ROI
p13 = [roi3(1) roi3(4) roi3(6)]';
p23 = [roi3(1) roi3(4) roi3(5)]';
p33 = [roi3(2) roi3(4) roi3(5)]';
p43 = [roi3(2) roi3(4) roi3(6)]';
p53 = [roi3(1) roi3(3) roi3(6)]';
p63 = [roi3(1) roi3(3) roi3(5)]';
p73 = [roi3(2) roi3(3) roi3(5)]';
p83 = [roi3(2) roi3(3) roi3(6)]';

% DEFINIR AS FACES DO CUBO ATRAVES DOS VERTICES ANTERIORES
A13=[p13 p23 p33 p43;p43 p33 p73 p83;p73 p83 p53 p63;p73 p63 p23 p33;p63 p53 p13 p23;p83 p53 p13 p43];

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

hold off


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


