% Authors: Robert Kim and Steven Su
% Date: May 17, 2011
% Interventional Photoacoustic Registration
% 600.446: CIS II, Spring 2011
% The Johns Hopkins University

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Performs registration between ultrasound and stereocamera images. Type
% register in command window to run and then select appropriate directory.
% The directory name indicates which point is used as a verification/test
% point.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Allows user to select directory.
program_dir = cd;
data_dir = uigetdir('Select');
cd(data_dir);
data_folders = dir('*us*');
verification_folders = dir('*Verification*');
fnames = dir('*.bmp');
cd(program_dir);

% Allows user to select 3 seperate CWL centroid points on the stereocamera
% images
camera_points = zeros(3,3);
for i=1:length(fnames)
    Image_to_read = [data_dir '\' fnames(i).name];
    [cx,cy] = getCameraPoint(Image_to_read);
    camera_points(:,i) = [cx cy 0]';
end
close all

% Processes corresponding ultrasound images for the centroid of the
% photoacoustic signal. Returns 3 photoacoustic ultrasound image
% coordinates.
disp('Getting the ultrasound point...');
us_points = zeros(3,3);
for i=1:length(data_folders)
    us_image = [data_dir '\' data_folders(i).name '\'];
    [us_cx us_cy dummy] = getUltrasoundPoint(us_image);
    us_points(:,i) = [us_cx us_cy 0]';
end

% Point cloud transformation (registration) from the camera domain to the
% ultrasound domain
[R p] = PointCloudTransform(camera_points,us_points);

% Performs verification of the transformation obtained above. The user 
% selects the centroid CWL point on the designated verification/test point.
% Using the obtained transformation, the predicted ultrasound coordinates
% are predicted based ont eh centroid CWL point. A plot of the actual 
% centroid of the photoacoustic signal and the predicted photoacoustic signal
% is then created. The error between the actual and predicted points is
% then calculated. 
for i=1:length(verification_folders)
    veri_dir = [data_dir '\' verification_folders(i).name];
    cd(veri_dir);
    veri_folders = dir('*us*');
    image_name = dir('*.bmp');
    cd(program_dir);
    [vcx vcy] = getCameraPoint([veri_dir '\' image_name.name]);
    close all;
    veri_camera_point = [vcx vcy 0];
    theoretical_us_point = R*(veri_camera_point)' + p;
    theoretical_us_point(1,:) = theoretical_us_point(1,:)*60/128;
    theoretical_us_point(2,:) = theoretical_us_point(2,:)/40*1540*10^(-6)*10^3;
    theoretical_us_point
    [vus_cx vus_cy Bpost] = getUltrasoundPoint([veri_dir '\' veri_folders.name '\']);
    
    imagesc( [1,size(Bpost,2)*60/128],[1,size(Bpost,1)]/40*1540*10^(-6)*10^3,Bpost);
    colormap(gray);
    title('Unprocessed Beamformed');
    axis square;
    xlabel('mm');
    ylabel('mm');
    hold on;    
    actual_us_point = [vus_cx*60/128 vus_cy/40*1540*10^(-6)*10^3 0]
    plot(actual_us_point(1),actual_us_point(2),'gh');
    plot(theoretical_us_point(1),theoretical_us_point(2),'rh');
    legend('Actual','Computed');
   
    off_distance = norm(theoretical_us_point' - actual_us_point)
end
