% add the gmm registration subroutines directory to path 
clear all
close all
mexext
path_matlab = genpath('MATLAB');    
addpath(path_matlab)

% The way particles are loaded depends on how they are stored. The main
% function for pyramid registration needs cell arrays of particles with
% localization in point field and uncertainties in sigma field.

%load segmented particles
load('data/tud_456')
N = 2;                              % number of particles to be fused
subParticles{1,N} = [];             % allocate memory for arrays of particles

% extract a subset of particles from the whole dataset
for i=1:N
    subParticles{i}.points = double(particles{1,i}.points);
    subParticles{i}.sigma  = double(particles{1,i}.sigma);
end

% perform pyramid registration
[FittedParticle, layers] = pyramidAdaFitting2D(subParticles); 

% output
FittedPoints = FittedParticle.points;  
save('test/fused_particle.mat', 'FittedPoints');