track3D_part1.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - PIV project (Part 1)                                %
%                                                                        %
%  The transformations (R1,T1,R2,T2) are known, the function             %
%  receives the images and the transformations as inputs and  returns    %
%  the 8 points describing the time trajectories of the enclosing box of %
%  the objects in world coordinates                                      %
%                                                                        %
%  Autores: nº 78486, Luís Rei                                           %
%           nº 78675, Gonçalo Duarte                                     %
%           nº 78761, João Girão                                         %
%                                                                        %
%  Data: 22/12/2017                                                      %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function objects = track3D_part1( imgseq1, imgseq2, cam_params, cam1toW, cam2toW )

% --------------------------------------------------------------------
%                           Variables' declarations and instantiations
% --------------------------------------------------------------------
% Define global variables
global depth_threshold
global label_threshold
global seq_threshold
global gradient_threshold
global peak_thresh

% Define thresholds
seq_threshold = 30; % maximum of 30 images to perform background removal
depth_threshold = 50; % Able to deal with small objects
label_threshold = 500; % Minimum object has 500 pixels
gradient_threshold = 150; % Assumed object is never > 15cm in depth
peak_thresh = 0.03; % SIFT peak threshold parameter
gridStep = 0.005; % Pointcloud downsample grid size parameter
maxPointDistance = 0.200; % Maximum of 20 cm between centroids distance to
% be considered the same object

% Object tracking through frames parameters
alpha = 0.6; % Weight given to the #matches/#features
beta = 0.2;  % Weight given to the color histogram difference
gamma = 0.2; % Weight given to the object eccentricity match

% Initializes 'objects' structure
objects = [];

% Initialize camera parameters
Kdepth = cam_params.Kdepth; % 3x3 matrix
Krgb = cam_params.Krgb;
R = cam_params.R; % Rotation matrix from depth to RGB (extrinsic params)
T = cam_params.T; % Translation from depth to RGB

% Initialize world frame parameters
Rw1 = cam1toW.R; % 3x3  Rotation matrix between 1st camera and world frame
Tw1 = cam1toW.T; % 3x1 Translation vector from camera 1 to the world frame
Rw2 = cam2toW.R; % 3x3  Rotation matrix between 2nd camera and world frame
Tw2 = cam2toW.T; % 3x1 Translation vector from camera 2 to the world frame

% --------------------------------------------------------------------
%                                                           Background
% --------------------------------------------------------------------
[bgd1] = get_bg(imgseq1); % camera 1
[bgd2] = get_bg(imgseq2); % camera 2

% --------------------------------------------------------------------
%                                                Object tracking cycle
% --------------------------------------------------------------------
for i = 1:numel(imgseq1)
    
    % Load Depth and RGB from specified index array structure
    im1 = imread(imgseq1(i).rgb);
    load(imgseq1(i).depth, 'depth_array'); depth1 = depth_array;
    
    im2 = imread(imgseq2(i).rgb);
    load(imgseq2(i).depth, 'depth_array'); depth2 = depth_array;
    clear depth_array;
    
    % --------------------------------------------------------------------
    %                                                           Foreground
    % --------------------------------------------------------------------
    % Camera 1
    [fg1_gray, fg1_label] = get_fg(bgd1, im1, depth1);
    % Camera 2
    [fg2_gray, fg2_label] = get_fg(bgd2, im2, depth2);
    
    % --------------------------------------------------------------------
    %                                          Combine depth + rgb cameras
    % --------------------------------------------------------------------
    if max(fg1_label(:))~=0
        % camera 1
        xyz1 = get_xyzasus(depth1(:),[length(depth1(:,1)) length(depth1(1,:))],1:length(depth1(1,:))*length(depth1(:,1)),Kdepth,1,0);
        rgbd1 = get_rgbd(xyz1, im1, R, T, Krgb);
    end
    if max(fg2_label(:))~=0
        % camera 2
        xyz2 = get_xyzasus(depth2(:),[length(depth2(:,1)) length(depth2(1,:))],1:length(depth2(1,:))*length(depth2(:,1)),Kdepth,1,0);
        rgbd2 = get_rgbd(xyz2, im2, R, T, Krgb);
    end
    
    
    %%
    % Compute pointcloud in 3D to assess objects' similarities in both
    % cameras. Reduce pointcloud size to easen the computational burden.
    
    % List of explored object regions in current camera frame.
    % Assures all objects are scanned and checked their "box" set.
    explored1 = zeros(max(fg1_label(:)),1);
    explored2 = zeros(max(fg2_label(:)),1);
    
    % Auxiliary structures. objs_aux0 for objects in both camera, and aux1
    % and 2 for objects solely in camera 1 and 2, respectively.
    objs_aux0 = []; objs_aux1 = []; objs_aux2 = [];
    count = 0; % Auxiliary variable to index objects
    
    % --------------------------------------------------------------------
    %                 Compare objects identity through centroid comparison
    % --------------------------------------------------------------------
    for j = 1:max(fg1_label(:))
        % For each label at each frame in each camera verify object closeness
        
        % Reset auxiliary variables
        index1 = 0;
        new_xyz1 = [];
        
        % Compute object's pointcloud based on label j
        [row,col] = find(fg1_label == j);
        temp_xyz1 = reshape(xyz1,length(depth1(:,1)),length(depth1(1,:)),3);
        for l = 1:length(row)
            if all(xyz1((col(l)-1)*length(depth1(:,1)) + row(l), :) == [0 0 0])
            else
                index1 = index1 + 1;
                new_xyz1(index1, :) = temp_xyz1(row(l), col(l),:);
            end
        end
        
        % Build translation vector
        TW1 = cat(2, ones(length(new_xyz1),1).*Tw1(1,:), ones(length(new_xyz1),1).*Tw1(2,:), ones(length(new_xyz1),1).*Tw1(3,:));
        
        % Translate xyz from camera 1 to World Frame
        new_xyz1 = (Rw1*new_xyz1' + TW1')';
        
        % Project objects in 3D. Downsampled pointcloud
        p1 =  pcdownsample(pointCloud(new_xyz1),'gridAverage',gridStep);
        
        % Object centroid in camera 1
        centroid1(1) = mean(p1.Location(:,1));
        centroid1(2) = mean(p1.Location(:,2));
        centroid1(3) = mean(p1.Location(:,3));
        
        for k = 1:max(fg2_label(:))
            % Same as before but for camera 2
            
            index2 = 0;
            new_xyz2 = [];
            
            % Compute object's pointcloud based on label k
            [row,col] = find(fg2_label == k);
            temp_xyz2 = reshape(xyz2,length(depth2(:,1)),length(depth2(1,:)),3);
            for l = 1:length(row)
                if all(xyz2((col(l)-1)*length(depth2(:,1)) + row(l), :) == [0 0 0])
                else
                    index2 = index2 + 1;
                    new_xyz2(index2, :) = temp_xyz2(row(l), col(l),:);
                end
            end
            
            % Build translation vectors
            TW2 = cat(2, ones(length(new_xyz2),1).*Tw2(1,:), ones(length(new_xyz2),1).*Tw2(2,:), ones(length(new_xyz2),1).*Tw2(3,:));
            
            % Translate xyz from camera 2 to World Frame
            new_xyz2 = (Rw2*new_xyz2' + TW2')';
            
            % Project objects in 3D. Downsampled pointcloud
            p2 =  pcdownsample(pointCloud(new_xyz2),'gridAverage',gridStep);
            
            % Object centroid in camera 2
            centroid2(1) = mean(p2.Location(:,1));
            centroid2(2) = mean(p2.Location(:,2));
            centroid2(3) = mean(p2.Location(:,3));
            
            % Check pointcloud centroids' distance
            if norm([centroid1(1)-centroid2(1), centroid1(2)-centroid2(2), centroid1(3)-centroid2(3)]) < maxPointDistance
                count = count+1;
                explored1(j) = 1;
                explored2(k) = 1;
                
                if max(p1.XLimits) > max(p2.XLimits)
                    max_coords(1) = max(p2.XLimits)+(max(p1.XLimits)-max(p2.XLimits))/2;
                    max_coords(2) = max(p2.YLimits)+(max(p1.YLimits)-max(p2.YLimits))/2;
                    max_coords(3) = max(p2.ZLimits)+(max(p1.ZLimits)-max(p2.ZLimits))/2;
                else
                    max_coords(1) = max(p1.XLimits)+(max(p2.XLimits)-max(p1.XLimits))/2;
                    max_coords(2) = max(p1.YLimits)+(max(p2.YLimits)-max(p1.YLimits))/2;
                    max_coords(3) = max(p1.ZLimits)+(max(p2.ZLimits)-max(p1.ZLimits))/2;
                end
                
                if min(p1.XLimits) < min(p2.XLimits)
                    min_coords(1) = min(p2.XLimits)+(min(p1.XLimits)-min(p2.XLimits))/2;
                    min_coords(2) = min(p2.YLimits)+(min(p1.YLimits)-min(p2.YLimits))/2;
                    min_coords(3) = min(p2.ZLimits)+(min(p1.ZLimits)-min(p2.ZLimits))/2;
                else
                    min_coords(1) = min(p1.XLimits)+(min(p2.XLimits)-min(p1.XLimits))/2;
                    min_coords(2) = min(p1.YLimits)+(min(p2.YLimits)-min(p1.YLimits))/2;
                    min_coords(3) = min(p1.ZLimits)+(min(p2.ZLimits)-min(p1.ZLimits))/2;
                end
                found = 0;
                
                if found == 0
                    objs_aux0(count).X = [min_coords(1), min_coords(1), max_coords(1), max_coords(1), min_coords(1), min_coords(1), max_coords(1), max_coords(1)]; %#ok<*AGROW> %%#ok<*AGROW>
                    objs_aux0(count).Y = [min_coords(2), max_coords(2), max_coords(2), min_coords(2), min_coords(2), max_coords(2), max_coords(2), min_coords(2)];
                    objs_aux0(count).Z = [max_coords(3), max_coords(3), max_coords(3), max_coords(3), min_coords(3), min_coords(3), min_coords(3), min_coords(3)];
                    objs_aux0(count).frames_tracked = i;
                    objs_aux0(count).label1 = j;
                    objs_aux0(count).label2 = k;
                    objs_aux0(count).ind = count;
                    break
                end
            end
        end
    end
    
    clear new_xyz1; clear new_xyz2; clear temp_xyz1; clear temp_xyz2;
    clear new_rgbd1; clear new_rgbd2;
    clear TW1; clear TW2;
    
    % Unexplored objects are individual to each camera. Set them as
    % different objects (two boxes instead of one).
    if max(fg1_label(:)) ~=0
        [objs_aux1, count] = set_explored(count, i,1,explored1,fg1_label,xyz1, depth1, Rw1, Tw1);
    end
    if max(fg2_label(:)) ~=0
        [objs_aux2, ~] = set_explored(count, i,2,explored2,fg2_label,xyz2, depth2, Rw2, Tw2);
    end
    
    
    %%
    % Calculates object movement through frames
    
    % --------------------------------------------------------------------
    %                              SIFT matches and output structure build
    % --------------------------------------------------------------------
    if (i > 1 && (max(fg1_label(:)) ~= 0 && max(fg1_label_ant(:)) ~= 0) )
        % Retrieve features match matrix.
        matches_matrix1 = get_match_matrix(fg1_label, fg1_label_ant, fg1_gray, fg1_gray_ant);
        
        % Retrieve color similarity matrix. Retrieve eccentricity matching between objects
        [color_matrix1, ecc_matrix1] = get_info_objects(fg1_label_ant,...
            fg1_label, xyz1, depth1, rgbd1);
        
        % Compute stochastic matrix to access object tracking through frames
        objs_match1 = get_object_track(alpha, beta, gamma,...
            matches_matrix1, color_matrix1, ecc_matrix1);
        
        [objects,objs_aux0,objs_aux1] = get_association_frame_cam1(objects, objs_match1,...
            objs_aux0_ant,objs_aux1_ant,objs_aux2_ant, objs_aux0,objs_aux1);
    end
    
    % Same for camera 2
    if (i > 1 && (max(fg2_label(:)) ~= 0 && max(fg2_label_ant(:)) ~= 0) )
        % Retrieve features match matrix.
        matches_matrix2 = get_match_matrix(fg2_label, fg2_label_ant, fg2_gray, fg2_gray_ant);
        
        % Compute stochastic matrix to access object tracking through frames
        [color_matrix2, ecc_matrix2] = get_info_objects(fg2_label_ant,...
            fg2_label, xyz2, depth2, rgbd2);
        
        % Retrieve color similarity matrix. Retrieve eccentricity matching between objects
        objs_match2 = get_object_track(alpha, beta, gamma,...
            matches_matrix2, color_matrix2, ecc_matrix2);
        
        [objects,objs_aux0,objs_aux1,objs_aux2] = get_association_frame_cam2(objects, objs_match2,...
            objs_aux0_ant,objs_aux1_ant,objs_aux2_ant, objs_aux0,objs_aux1,objs_aux2);
    end
    
    % Construct initial objects' structure
    if i > 1
        if ~isempty(objs_aux0) && max(fg1_label_ant(:)) == 0 && max(fg2_label_ant(:)) == 0
            last = length(objects) + 1;
            for k = 1:length(objs_aux0)
                objs_aux0(k).ind = last;
                last = last + 1;
            end
            objects = cat(2, objects, objs_aux0);
        end
        if ~isempty(objs_aux1) && max(fg1_label_ant(:)) == 0
            last = length(objects) + 1;
            for k = 1:length(objs_aux1)
                objs_aux1(k).ind = last;
                last = last + 1;
            end
            objects = cat(2, objects, objs_aux1);
        end
        if ~isempty(objs_aux2) && max(fg2_label_ant(:)) == 0
            last = length(objects) + 1;
            for k = 1:length(objs_aux2)
                objs_aux2(k).ind = last;
                last = last + 1;
            end
            objects = cat(2, objects, objs_aux2);
        end
    else
        if ~isempty(objs_aux0)
            objects = cat(2, objects, objs_aux0);
        end
        if ~isempty(objs_aux1)
            objects = cat(2, objects, objs_aux1);
        end
        if ~isempty(objs_aux2)
            objects = cat(2, objects, objs_aux2);
        end
    end
    
    % --------------------------------------------------------------------
    %                                                    Update foreground
    % --------------------------------------------------------------------
    fg1_gray_ant = fg1_gray; fg2_gray_ant = fg2_gray;       % update gray
    fg1_label_ant = fg1_label; fg2_label_ant = fg2_label;   % update label
    objs_aux0_ant = objs_aux0; % update objects seen in last frame
    objs_aux1_ant = objs_aux1;
    objs_aux2_ant = objs_aux2;
    
end

% Trim final objects structure. Ignore objects seen in only one frame.
objects = trim_objects(objects);

end


%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        AUXILIARY FUNCTIONS                             %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Project image to 3D                                 %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xyz_ret = get_xyzasus(im_vec, im_orig_size, good_inds, K, alpha, beta)
% im_vec - depth image vectorized (Nx1)
% im_orig_size - original image size (HxW) : [H, W]
% good_inds - indexes of the image that are valid, i.e., different from 0.

persistent u;
persistent v;
persistent im_size;
persistent xyz;
persistent z;

Kx = K(1,1);
Cx = K(1,3);
Ky = K(2,2);
Cy = K(2,3);

if isempty(im_size)
    % im_size = size(im);
    im_size = im_orig_size;
    
    u = repmat(1:im_size(2),im_size(1),1);
    u = u(:)-Cx;
    v = repmat((1:im_size(1))',im_size(2),1);
    v = v(:)-Cy;
    xyz = zeros(length(u),3);
end

% tmp = im(:);
xyz(:,3) = double(im_vec)*0.001; % Convert to meters
xyz(good_inds,3) = alpha*xyz(good_inds,3) + beta;
xyz(:,1) = (xyz(:,3)/Kx) .* u ;
xyz(:,2) = (xyz(:,3)/Ky) .* v;

%plot3(x,y,z,'.');axis equal
xyz_ret = xyz;

end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Combine rgb + depth                                 %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function rgbd = get_rgbd(xyz, rgb, R, T, K_rgb)

Kx = K_rgb(1,1);
Cx = K_rgb(1,3);
Ky = K_rgb(2,2);
Cy = K_rgb(2,3);

% Rotate and translate
xyz_rgb = R * xyz';
xyz_rgb = [xyz_rgb(1,:) + T(1); xyz_rgb(2,:) + T(2); xyz_rgb(3,:) + T(3)];

% Define x, y and z
x = xyz_rgb(1,:);
y = xyz_rgb(2,:);
z = xyz_rgb(3,:);

% Project to 2D
u = round(Kx * x./z + Cx);
v = round(Ky * y./z + Cy);

rgb_size = size(rgb);
n_pixels = numel(rgb(:,:,1));

v(v > rgb_size(1)) = 1;
v(v < 1) = 1;
u(u > rgb_size(2)) = 1;
u(u < 1) = 1;

rgb_inds = sub2ind(rgb_size, v, u);

rgbd = zeros(n_pixels,3);
rgb_aux = reshape(rgb,length(rgb(:,1,:))*length(rgb(1,:,:)),3);

rgbd((1:n_pixels)',:) = rgb_aux(rgb_inds,:);

rgbd(xyz(:,1) == 0 & xyz(:,2) == 0 & xyz(:,3) == 0,:) = 0;

rgbd = uint8(reshape(rgbd,rgb_size));

end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Retrieve background from a series of images         %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [bgimd] = get_bg(imgseq)
% imgseq - sequence of images up to seq_threshold

global seq_threshold

% Subsample sequence of images if neccessary
if (length(imgseq) > seq_threshold)
    new_imgseq = imgseq(1:seq_threshold);
else
    new_imgseq = imgseq;
end

% Pre-allocate memory
load(new_imgseq(1).depth);
ims = zeros(length(depth_array(:,1))*length(depth_array(1,:)),numel(new_imgseq));
imsd = ims;

% Construct array with rgb and depth info
for i = 1:numel(new_imgseq)
    im_aux = imread(new_imgseq(i).rgb);
    % make grayscale
    if size(im_aux,3) > 1,
        im = rgb2gray(im_aux);
    else
        im = im_aux;
    end
    
    load(new_imgseq(i).depth);
    ims(:,i) = im(:);
    imsd(:,i) = depth_array(:);
end

% Calculate median
medim = median(double(ims),2);
meddep = median(double(imsd),2);

% Reshape into matrix with desirable size
bgim = (uint8(reshape(medim,[length(depth_array(:,1)) length(depth_array(1,:))])));
bgimd = reshape(meddep,[length(depth_array(:,1)) length(depth_array(1,:))]);

end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Retrieve foreground from an image                   %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [fg_gray, fg_depth_label] = get_fg(bgd, im_rgb, im_depth)
% im_rgb - rgb image matrix (:,:,3)
% im_depth - depth_array matrix
% bg - previously calculated image background
% bg - depth background

% Define thresholds
global depth_threshold
global label_threshold
global gradient_threshold

% make grayscale
if size(im_rgb,3) > 1,
    im_gray = rgb2gray(im_rgb) ;
else
    im_gray = im_rgb ;
end

fg_depth = abs(double(im_depth) - double(bgd))*0.001; % Translate depth foreground to meters
fg_gray = uint8(zeros(length(bgd(:,1)),length(bgd(1,:)))); % initialize fg matrix

% Deal with depth foreground
fg_depth_mask = bwareafilt(abs(double(im_depth) - double(bgd)) > depth_threshold,...
    [label_threshold length(bgd(:,1))*length(bgd(1,:))]);

% Gradient to filter depth discrepancies
[fx, fy] = gradient(double(im_depth));
gradient_mask = (fx.*fx + fy.*fy) < (gradient_threshold)^2;

% Filter foreground through masks
fg_depth_mask(im_depth(:) == 0) = 0; % Ignore invalid values
fg_depth_mask(gradient_mask(:) == 0) = 0; % Ignore depth discrepancies
fg_depth_label = bwlabel(bwareafilt(imopen(imfill(bwareafilt(fg_depth_mask,...
    [label_threshold length(bgd(:,1))*length(bgd(1,:))]),'holes'),strel('disk',4)),...
    [label_threshold length(bgd(:,1))*length(bgd(1,:))]));

% Construct final foreground containing only labelled objects
for m = 1:length(bgd(:,1))
    for n = 1:length(bgd(1,:))
        if fg_depth_label(m, n) > 0
            fg_gray(m, n) = im_gray(m, n);
        end
    end
end
end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Index different objects that were not seen in both  %
%  cameras                                                               %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [objects, count] = set_explored(count, frame,cam_number, explored, label, xyz, depth, Rw, Tw)
% Returns objects' structure with fields specified as in the project guide

objects = []; % Initialize objects' structure
o = 0;
% For each unexplored object, add a line in the structure
for e = 1:length(explored)
    if explored(e) == 0
        index = 0;
        
        % Resize object
        [r,c] = find(label == e);
        temp_xyz = reshape(xyz,length(depth(:,1)),length(depth(1,:)),3);
        for l = 1:length(r)
            if all(xyz((c(l)-1)*length(depth(:,1)) + r(l), :) == [0 0 0])
            else
                index = index + 1;
                new_xyz(index, :) = temp_xyz(r(l), c(l),:);
            end
        end
        
        % Build translation vector
        TW = cat(2, ones(length(new_xyz),1).*Tw(1,:), ones(length(new_xyz),1).*Tw(2,:), ones(length(new_xyz),1).*Tw(3,:));
        
        % Translate xyz from camera 1 to World Frame
        new_xyz = (Rw*new_xyz' + TW')';
        
        % Get XYZ coordinates for the box
        max_coords = max(new_xyz, [], 1);
        min_coords = min(new_xyz, [], 1);
        o = o + 1;
        count = count+1;
        objects(o).X = [min_coords(1), min_coords(1), max_coords(1), max_coords(1), min_coords(1), min_coords(1), max_coords(1), max_coords(1)];
        objects(o).Y = [min_coords(2), max_coords(2), max_coords(2), min_coords(2), min_coords(2), max_coords(2), max_coords(2), min_coords(2)];
        objects(o).Z = [max_coords(3), max_coords(3), max_coords(3), max_coords(3), min_coords(3), min_coords(3), min_coords(3), min_coords(3)];
        objects(o).frames_tracked = frame;
        objects(o).label1 = 0;
        objects(o).label2 = 0;
        objects(o).ind = count;
        if cam_number == 1
            objects(o).label1 = e;
        else
            objects(o).label2 = e;
        end
    end
end

end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Gather information about eccentricity and color of  %
%    the objects in two consecutive frames                               %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [color_matrix, ecc_matrix] = get_info_objects( label_ant, label, xyz, depth, rgbd)

color_matrix = zeros(max(label_ant(:))+1, max(label(:))+1);
ecc_matrix = zeros(max(label_ant(:))+1, max(label(:))+1);

for j = 1:max(label_ant(:))
    index1 = 0;
    new_rgbd_ant = [];
    
    new_xyz_ant = [];
    
    [r,c] = find(label_ant == j);
    temp_xyz = reshape(xyz,length(depth(:,1)),length(depth(1,:)),3);
    for l = 1:length(r)
        if all(xyz((c(l)-1)*length(depth(:,1)) + r(l), :) == [0 0 0])
        else
            index1 = index1 + 1;
            new_rgbd_ant(index1,:) = rgbd(r(l), c(l),:);
            new_xyz_ant(index1, :) = temp_xyz(r(l), c(l),:);
        end
    end
    
    for k = 1:max(label(:))
        index2 = 0;
        new_rgbd = [];
        new_xyz = [];
        
        [r,c] = find(label == k);
        
        temp_xyz = reshape(xyz,length(depth(:,1)),length(depth(1,:)),3);
        for l = 1:length(r)
            if all(xyz((c(l)-1)*length(depth(:,1)) + r(l), :) == [0 0 0])
            else
                index2 = index2 + 1;
                new_rgbd(index2,:) = rgbd(r(l), c(l),:);
                new_xyz(index2, :) = temp_xyz(r(l), c(l),:);
            end
        end
        
        color_matrix(j,k) = get_color_histogram(new_rgbd_ant, new_rgbd);
    end
    ecc_matrix = get_eccentricity(label_ant, label);
end

for ind1 = 1:size(color_matrix,1)-1
    color_matrix(ind1,:) = color_matrix(ind1,:)/sum(color_matrix(ind1,:));
end

end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Compare color histograms of objects                 %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function coef = get_color_histogram(rgbd_ant, rgbd)
% Transforms color histogram differences into qualitative coefficient to be
% used in the feature matching problem

% Color normalization to lower luminosity differences impact
hsvImage = rgb2hsv(rgbd_ant./255);
hsvImage(hsvImage(:) == 1) = 254/255; % Correct black histogram bin going over to white
hueImage = hsvImage(:,1);
c1 = imhist(hueImage);

hsvImage = rgb2hsv(rgbd./255);
hsvImage(hsvImage(:) == 1) = 254/255;
hueImage = hsvImage(:,1);
c2 = imhist(hueImage);

% Negative exponential penalizes big differences.
% Biggest value (1) when the color histograms are the same.
c = abs(c1-c2)/(0.5*(sum(c1) + sum(c2)));
aux = sum(c);
coef = exp(-aux);

end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Retrieve object eccentricity                        %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ecc = get_eccentricity( label_ant, label)
% Outputs eccentricity parameters for object comparison.
aux = regionprops(label_ant,'eccentricity');
s1 = zeros(size(aux,1),1);
for ind_k = 1:size(aux,1)
    s1(ind_k,1) = aux(ind_k).Eccentricity;
end
aux = regionprops(label,'eccentricity');

s2 = zeros(size(aux,1),1);
for ind_k = 1:size(aux,1)
    s2(ind_k,1) = aux(ind_k).Eccentricity;
end

% Build matrix
if length(s1) == 1 && length(s2) == 1
    ecc = [1 0; 0 0];
else
    ecc = ones(length(s1)+1, length(s2)+1);
    for ind_i = 1:length(s1)
        for ind_j = 1:length(s2)
            ecc(ind_i, ind_j) = abs(s1(ind_i)-s2(ind_j));
        end
    end
    for ind_i = 1:length(s1)
        ecc(ind_i, 1:end-1) = ecc(ind_i, 1:end-1)./sum(ecc(ind_i, 1:end-1));
    end
    ecc = ones(length(s1)+1, length(s2)+1) - ecc;
    for ind_i = 1:length(s1)
        ecc(ind_i, 1:end-1) = ecc(ind_i, 1:end-1)./sum(ecc(ind_i, 1:end-1));
    end
end
end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Compute matching matrix                             %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function mm_ret = get_match_matrix(labels, labels_prev, Ia, Ib)

global peak_thresh

% Execute sift on the foreground of the grayscaled images
% Obtain frames and descriptors
[fa, da] = vl_sift(im2single(Ia), 'PeakThresh', peak_thresh);
[fb, db] = vl_sift(im2single(Ib), 'PeakThresh', peak_thresh);

% Obtain matches and scores
[matches, ~] = vl_ubcmatch(da, db);

f1 = round(fb(1:2,matches(2,:))); % Coordinates of the features of the 1st image
f2 = round(fa(1:2,matches(1,:))); % Coordinates of the features of the 2nd image

new_mm = zeros(max(labels_prev(:))+1, max(labels(:))+1);

for i = 1:size(matches,2)
    x = labels_prev(f1(2,i), f1(1,i));
    y = labels(f2(2,i), f2(1,i));
    if x ~= 0 && y ~= 0
        new_mm(x, y) = new_mm(x, y) + 1;
    end
end

for i = 1:size(new_mm, 1)-1
    if sum(new_mm(i,:)) ~= 0
        new_mm(i,1:end-1) = new_mm(i,1:end-1)./sum(new_mm(i,1:end-1));
    end
    new_mm(i, end) = 1 - sum(new_mm(i,1:end-1));
end

mm_ret = new_mm;
end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Compute object tracking matrix                      %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function track = get_object_track(alpha, beta, gamma, match, color, ecc)

track = zeros(size(match));

% For each region in previous frame, calculate percentage of being certain
% region in the next frame
for i = 1:length(match(:,1))
    track(i,:) = (alpha*match(i,:) + beta*color(i,:) + gamma*ecc(i,:));
    if ~all(track(i,:) == zeros(size(track(i,:))))
        track(i,end) = 1 - sum(track(i,1:end-1));
    end
end

end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Check nested structure structure                    %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function isFieldResult = myIsField(inStruct, fieldName)
% inStruct is the name of the structure or an array of structures to search
% fieldName is the name of the field for which the function searches

isFieldResult = 0;
f = fieldnames(inStruct(1));

for i=1:length(f)
    if(strcmp(f{i},strtrim(fieldName)))
        isFieldResult = 1;
        return;
    elseif isstruct(inStruct(1).(f{i}))
        isFieldResult = myIsField(inStruct(1).(f{i}), fieldName);
        if isFieldResult
            return;
        end
    end
end
end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Set object indexes to organize 'objects' structure  %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function objects = trim_objects(objects)

if myIsField(objects, 'label1')
    objects = rmfield(objects,'label1');
end
if myIsField(objects, 'label2')
    objects = rmfield(objects,'label2');
end
if myIsField(objects, 'ind')
    objects = rmfield(objects,'ind');
end

n = length(objects);
m = 1;
for k = 1:n
    if length(objects(m).frames_tracked) < 3
        objects(m) = [];
        m = m-1;
    end
    m = m+1;
end
end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Gather information about eccentricity and color of  %
%    the objects in two consecutive frames                               %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [objects,objs_aux0,objs_aux1] = get_association_frame_cam1(objects, objs_match1,...
    objs_aux0_ant,objs_aux1_ant,objs_aux2_ant, objs_aux0,objs_aux1)

free1 = ones(size(objs_match1(1,:))); % Restriction vector

track1 = zeros(length(objs_match1(:,1))-1,1);
% Associate object of previous frame through greedy search
for h = 1:length(objs_match1(:,1))-1
    
    % Run through matrix
    aux = free1.*objs_match1(h,:); % Available matches
    if ~all(aux(:) == 0)
        [~, ind_aux] = max(aux);
    else
        ind_aux = length(objs_match1(1,:));
    end
    % Take match from available ones
    if ind_aux ~= length(objs_match1(1,:))
        track1(h) = ind_aux;
        
        % Restrict that object unless it died
        free1(track1(h)) = 0;
        found1 = 0;
        for ind0 = 1:length(objs_aux0)
            if objs_aux0(ind0).label1 == track1(h)
                for ind01 = 1:length(objs_aux0_ant)
                    if objs_aux0_ant(ind01).label1 == h
                        % Annex new points to final structure
                        for ind02 = 1:length(objects)
                            if objects(ind02).ind == objs_aux0_ant(ind01).ind
                                r = ind02;
                                break
                            end
                        end
                        objects(r).X = cat(1, objects(r).X, objs_aux0(ind0).X);
                        objects(r).Y = cat(1, objects(r).Y, objs_aux0(ind0).Y);
                        objects(r).Z = cat(1, objects(r).Z, objs_aux0(ind0).Z);
                        objects(r).frames_tracked = cat(1, objects(r).frames_tracked, objs_aux0(ind0).frames_tracked);
                        objs_aux0(ind0).ind = objects(r).ind;
                        found1 = 1;
                        break
                    end
                end
                if found1 == 0;
                    for ind01 = 1:length(objs_aux1_ant)
                        if objs_aux1_ant(ind01).label1 == h
                            % Annex new points to final structure
                            for ind02 = 1:length(objects)
                                if objects(ind02).ind == objs_aux1_ant(ind01).ind
                                    r = ind02;
                                    break
                                end
                            end
                            objects(r).X = cat(1, objects(r).X, objs_aux0(ind0).X);
                            objects(r).Y = cat(1, objects(r).Y, objs_aux0(ind0).Y);
                            objects(r).Z = cat(1, objects(r).Z, objs_aux0(ind0).Z);
                            objects(r).frames_tracked = cat(1, objects(r).frames_tracked, objs_aux0(ind0).frames_tracked);
                            objs_aux0(ind0).ind = objects(r).ind;
                            found1 = 1;
                            break
                        end
                    end
                end
                if found1 == 0;
                    for ind01 = 1:length(objs_aux2_ant)
                        if objs_aux2_ant(ind01).label1 == h
                            % Annex new points to final structure
                            for ind02 = 1:length(objects)
                                if objects(ind02).ind == objs_aux2_ant(ind01).ind
                                    r = ind02;
                                    break
                                end
                            end
                            objects(r).X = cat(1, objects(r).X, objs_aux0(ind0).X);
                            objects(r).Y = cat(1, objects(r).Y, objs_aux0(ind0).Y);
                            objects(r).Z = cat(1, objects(r).Z, objs_aux0(ind0).Z);
                            objects(r).frames_tracked = cat(1, objects(r).frames_tracked, objs_aux0(ind0).frames_tracked);
                            objs_aux0(ind0).ind = objects(r).ind;
                            found1 = 1;
                            break
                        end
                    end
                end
                break
            end
        end
        if found1 == 0;
            for ind1 = 1:length(objs_aux1)
                if objs_aux1(ind1).label1 == track1(h)
                    for ind11 = 1:length(objs_aux1_ant)
                        if objs_aux1_ant(ind11).label1 == h
                            % Annex new points to final structure
                            for ind12 = 1:length(objects)
                                if objects(ind12).ind == objs_aux1_ant(ind11).ind
                                    r = ind12;
                                    break
                                end
                            end
                            objects(r).X = cat(1, objects(r).X, objs_aux1(ind1).X);
                            objects(r).Y = cat(1, objects(r).Y, objs_aux1(ind1).Y);
                            objects(r).Z = cat(1, objects(r).Z, objs_aux1(ind1).Z);
                            objects(r).frames_tracked = cat(1, objects(r).frames_tracked, objs_aux1(ind1).frames_tracked);
                            objs_aux1(ind1).ind = objects(r).ind;
                            found1 = 1;
                            break
                        end
                    end
                    if found1 == 0;
                        for ind11 = 1:length(objs_aux0_ant)
                            if objs_aux0_ant(ind11).label1 == h
                                % Annex new points to final structure
                                for ind12 = 1:length(objects)
                                    if objects(ind12).ind == objs_aux0_ant(ind11).ind
                                        r = ind12;
                                        break
                                    end
                                end
                                objects(r).X = cat(1, objects(r).X, objs_aux1(ind1).X);
                                objects(r).Y = cat(1, objects(r).Y, objs_aux1(ind1).Y);
                                objects(r).Z = cat(1, objects(r).Z, objs_aux1(ind1).Z);
                                objects(r).frames_tracked = cat(1, objects(r).frames_tracked, objs_aux1(ind1).frames_tracked);
                                objs_aux1(ind1).ind = objects(r).ind;
                                found1 = 1;
                                break
                            end
                        end
                    end
                    if found1 == 0;
                        for ind11 = 1:length(objs_aux2_ant)
                            if objs_aux2_ant(ind11).label1 == h
                                % Annex new points to final structure
                                for ind12 = 1:length(objects)
                                    if objects(ind12).ind == objs_aux2_ant(ind11).ind
                                        r = ind12;
                                        break
                                    end
                                end
                                objects(r).X = cat(1, objects(r).X, objs_aux1(ind1).X);
                                objects(r).Y = cat(1, objects(r).Y, objs_aux1(ind1).Y);
                                objects(r).Z = cat(1, objects(r).Z, objs_aux1(ind1).Z);
                                objects(r).frames_tracked = cat(1, objects(r).frames_tracked, objs_aux1(ind1).frames_tracked);
                                objs_aux1(ind1).ind = objects(r).ind;
                                break
                            end
                        end
                    end
                    break
                end
            end
        end
    end
end

free1(end) = 0; % Not needed anymore. We will add newborn objects.
c = find(free1(:) ~= 0);

% Annex newborn objects in final structure
for h = 1:length(c)
    last = length(objects) + 1;
    foundit = 0;
    for ind0 = 1:length(objs_aux0)
        if objs_aux0(ind0).label1 == c(h)
            objects(last).X = objs_aux0(ind0).X;
            objects(last).Y = objs_aux0(ind0).Y;
            objects(last).Z = objs_aux0(ind0).Z;
            objects(last).frames_tracked = objs_aux0(ind0).frames_tracked;
            objects(last).label1 = objs_aux0(ind0).label1;
            objects(last).label2 = objs_aux0(ind0).label2;
            objects(last).ind = last;
            objs_aux0(ind0).ind = last;
            foundit = 1;
            break
        end
    end
    if foundit == 0
        for ind1 = 1:length(objs_aux1)
            if objs_aux1(ind1).label1 == c(h)
                objects(last).X = objs_aux1(ind1).X;
                objects(last).Y = objs_aux1(ind1).Y;
                objects(last).Z = objs_aux1(ind1).Z;
                objects(last).frames_tracked = objs_aux1(ind1).frames_tracked;
                objects(last).label1 = objs_aux1(ind1).label1;
                objects(last).label2 = objs_aux1(ind1).label2;
                objects(last).ind = last;
                objs_aux1(ind1).ind = last;
                break
            end
        end
    end
end
end

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%  Function script - Gather information about eccentricity and color of  %
%    the objects in two consecutive frames                               %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [objects,objs_aux0,objs_aux1,objs_aux2] = get_association_frame_cam2(objects, objs_match2,...
    objs_aux0_ant,objs_aux1_ant,objs_aux2_ant, objs_aux0,objs_aux1,objs_aux2)

free2 = ones(size(objs_match2(1,:))); % Restriction vector

% Associate object of previous frame through greedy search
for h = 1:length(objs_match2(:,1))-1
    skip = 0;
    % Run through matrix
    track2 = zeros(length(objs_match2(:,1))-1,1);
    aux = free2.*objs_match2(h,:); % Available matches
    if ~all(aux(:) == 0)
        [~, ind_aux] = max(aux);
    else
        ind_aux = length(objs_match2(1,:));
    end
    % Make objects seen by both cameras unavailable for selection
    for del_ind = 1:length(objs_aux0)
        if ind_aux == objs_aux0(del_ind).label2
            skip = 1; % Object seen by both cameras, no need to repeat it
            free2(ind_aux) = 0;
            break
        end
    end
    if skip == 0
        % Take match from available ones
        if ind_aux ~= length(objs_match2(1,:))
            track2(h) = ind_aux;
            found2 = 0;
            % Restrict that object unless it died
            free2(track2(h)) = 0;
            for ind2 = 1:length(objs_aux2)
                if objs_aux2(ind2).label2 == track2(h)
                    for ind21 = 1:length(objs_aux2_ant)
                        if objs_aux2_ant(ind21).label2 == h
                            % Annex new points to final structure
                            for ind22 = 1:length(objects)
                                if objects(ind22).ind == objs_aux2_ant(ind21).ind
                                    r = ind22;
                                    break
                                end
                            end
                            objects(r).X = cat(1, objects(r).X, objs_aux2(ind2).X);
                            objects(r).Y = cat(1, objects(r).Y, objs_aux2(ind2).Y);
                            objects(r).Z = cat(1, objects(r).Z, objs_aux2(ind2).Z);
                            objects(r).frames_tracked = cat(1, objects(r).frames_tracked, objs_aux2(ind2).frames_tracked);
                            objs_aux2(ind2).ind = objects(r).ind;
                            found2 = 1;
                            break
                        end
                    end
                    if found2 == 0
                        for ind21 = 1:length(objs_aux1_ant)
                            if objs_aux1_ant(ind21).label2 == h
                                % Annex new points to final structure
                                for ind22 = 1:length(objects)
                                    if objects(ind22).ind == objs_aux1_ant(ind21).ind
                                        r = ind22;
                                        break
                                    end
                                end
                                objects(r).X = cat(1, objects(r).X, objs_aux2(ind2).X);
                                objects(r).Y = cat(1, objects(r).Y, objs_aux2(ind2).Y);
                                objects(r).Z = cat(1, objects(r).Z, objs_aux2(ind2).Z);
                                objects(r).frames_tracked = cat(1, objects(r).frames_tracked, objs_aux2(ind2).frames_tracked);
                                objs_aux2(ind2).ind = objects(r).ind;
                                found2 = 1;
                                break
                            end
                        end
                    end
                    if found2 == 0
                        for ind21 = 1:length(objs_aux0_ant)
                            if objs_aux0_ant(ind21).label2 == h
                                % Annex new points to final structure
                                for ind22 = 1:length(objects)
                                    if objects(ind22).ind == objs_aux0_ant(ind21).ind
                                        r = ind22;
                                        break
                                    end
                                end
                                if objects(r).frames_tracked(end) == objs_aux2(ind2).frames_tracked
                                    for ind_rep = 1:length(objs_aux1)
                                        if objs_aux1(ind_rep).ind == r
                                            m1 = min(objs_aux1(ind_rep).X(1), objs_aux2(ind2).X(1));
                                            M1 = max(objs_aux1(ind_rep).X(3), objs_aux2(ind2).X(3));
                                            m2 = min(objs_aux1(ind_rep).Y(1), objs_aux2(ind2).Y(1));
                                            M2 = max(objs_aux1(ind_rep).Y(2), objs_aux2(ind2).Y(2));
                                            m3 = min(objs_aux1(ind_rep).Z(5), objs_aux2(ind2).Z(5));
                                            M3 = max(objs_aux1(ind_rep).Z(1), objs_aux2(ind2).Z(1));
                                            last = length(objs_aux0) + 1;
                                            objs_aux0(last).X = [m1, m1, M1, M1, m1, m1, M1, M1];
                                            objs_aux0(last).Y = [m2, M2, M2, m2, m2, M2, M2, m2];
                                            objs_aux0(last).Z = [M3, M3, M3, M3, m3, m3, m3, m3];
                                            objs_aux0(last).frames_tracked = objs_aux2(ind2).frames_tracked;
                                            objs_aux0(last).label1 = objs_aux1(ind_rep).label1;
                                            objs_aux0(last).label2 = objs_aux2(ind2).label2;
                                            objs_aux0(last).ind = objects(r).ind;
                                            objs_aux1(ind_rep) = [];
                                            objs_aux2(ind2) = [];
                                            break
                                        else
                                            last = length(objects) + 1;
                                            objects(last).X = objs_aux2(ind2).X;
                                            objects(last).Y = objs_aux2(ind2).Y;
                                            objects(last).Z = objs_aux2(ind2).Z;
                                            objects(last).frames_tracked = objs_aux2(ind2).frames_tracked;
                                            objects(last).label1 = objs_aux2(ind2).label1;
                                            objects(last).label2 = objs_aux2(ind2).label2;
                                            objects(last).ind = last;
                                            objs_aux2(ind2).ind = last;
                                            break
                                        end
                                    end
                                    break
                                end
                                objects(r).X = cat(1, objects(r).X, objs_aux2(ind2).X);
                                objects(r).Y = cat(1, objects(r).Y, objs_aux2(ind2).Y);
                                objects(r).Z = cat(1, objects(r).Z, objs_aux2(ind2).Z);
                                objects(r).frames_tracked = cat(1, objects(r).frames_tracked, objs_aux2(ind2).frames_tracked);
                                objs_aux2(ind2).ind = objects(r).ind;
                                break
                            end
                        end
                    end
                    break
                end
            end
        end
    end
end

free2(end) = 0; % Not needed anymore. We will add newborn objects.
c = find(free2(:) ~= 0);

% Annex newborn objects in final structure
for h = 1:length(c)
    last = length(objects) + 1;
    for ind2 = 1:length(objs_aux2)
        if objs_aux2(ind2).label2 == c(h)
            objects(last).X = objs_aux2(ind2).X;
            objects(last).Y = objs_aux2(ind2).Y;
            objects(last).Z = objs_aux2(ind2).Z;
            objects(last).frames_tracked = objs_aux2(ind2).frames_tracked;
            objects(last).label1 = objs_aux2(ind2).label1;
            objects(last).label2 = objs_aux2(ind2).label2;
            objects(last).ind = last;
            objs_aux2(ind2).ind = last;
            break
        end
    end
end
end