close all;clear all;clc

crop = CropPositives('/home/data/matlab_rosado_full_p2_1/wide_left',...
    '/home/data/matlab_rosado_full_p2_1/wide_left_groundtruth_detections');

%% Auto bounding box with vicon data

clear all;close all;clc

viconData = 'data/data_in_labeling.mat';
imagesPath = '/home/data/matlab_rosado_full_p2_1/wide_left';
pointsPath = 'data/extrinsic_calibration_points.mat';
% calibrationPath = 'data/xb3_left_right_stereo_calibration_p2.mat';
calibrationPath = 'data/xb3_left_post_rectified_parameters_calibration_2_1.mat';

load(calibrationPath)

calibrationMatching = XB3ViconGeometricTemporalCalibration(viconData,imagesPath,pointsPath,cameraParams);

%% Segment the pedestrian cloud using the bounding boxes, just testing
clear all;close all;clc

imageNumber = 50;

imageName = sprintf('%06d.png',imageNumber);
path = fullfile('/home/data/matlab_rosado_full_p2_1/wide_left',imageName);

imshow(path)

boundingBoxFileName = sprintf('%06d.txt',imageNumber);
boundingBoxPath = fullfile('/home/data/matlab_rosado_full_p2_1/wide_left_groundtruth_detections',boundingBoxFileName);

bb = dlmread(boundingBoxPath);

p1 = bb(1:2);
width = bb(3)-bb(1);
height = bb(4)-bb(2);

rectangle('position',[p1 width height],'edgecolor',[0.3 0 0.6],'linewidth',2)

addpath 'pcl';

pcdName = sprintf('%06d.pcd',imageNumber);
pcdPath = fullfile('/home/data/matlab_rosado_full_p2_1/pcd/wide',pcdName);

p = loadpcd(pcdPath);

bb = round(bb);

segment = p(bb(2):bb(4),bb(1):bb(3),:);

imshow(segment(:,:,3))

