
clear all;close all;clc

addpath pcl

pcd_folder = '/home/jorge/Desktop/exp1_small/var_1_1/pcd/short/';
test_file = fullfile(pcd_folder,'000005.pcd');



pcd = loadpcd(test_file);

pcd(isinf(pcd))=NaN;

x = pcd(:,:,1);
y = pcd(:,:,2);
z = pcd(:,:,3);
r = pcd(:,:,4);
g = pcd(:,:,5);
b = pcd(:,:,6);

x=x(:);
y=y(:);
z=z(:);
r=r(:);
g=g(:);
b=b(:);


figure('color',[1 1 1],'position',[80 20 950 950])
hold on

colors=[];
colors(:,1)=r;
colors(:,2)=g;
colors(:,3)=b;


scatter(x,y,z,colors)


% h = plot3(x,y,z,'o');
% set(h,'Color',colors')


% scatter3(X,Y,Z)