torr_tool.m

matlab三维重建程序

handles.n_inliers  = n_inliers;

%first estimate F
[f_nl,f_sq_errors] = torr_estimateF(handles.inlier_matches, handles.m3, [], 'non_linear',1,f);
handles.f = f_nl;
disp('goodness of fit after non linear:')
norm(f_sq_errors)

%finally display inliers
clear_button_Callback(h, eventdata, handles, varargin);
display_inliers(h, eventdata, handles, varargin);

%we can now save the result
set(handles.save_F_button, 'Enable', 'on');
set(handles.detect_corner_button, 'Enable', 'off');
set(handles.match_button, 'Enable', 'off');
set(handles.display_epipolar_button, 'Enable', 'on');
set(handles.display_epipolar_button2, 'Enable', 'on');
%set(handles.mapsac_plane_button, 'Enable', 'on');

set(handles.ImproveF_button, 'Enable', 'on');
set(handles.sfm_button, 'Enable', 'on');


% saving the GUI data
guidata(handles.figure1, handles);

% --------------------------------------------------------------------
function varargout = mapsac_parameters_button_Callback(h, eventdata, handles, varargin)

pos_size = get(handles.figure1,'Position');
dlg_pos = [pos_size(1)+pos_size(3)/5 pos_size(2)+pos_size(4)/5];
f_param = torr_f_menu(handles, dlg_pos);

handles.no_samp = f_param(1);
handles.f_threshold = f_param(2);
% saving the GUI data
guidata(handles.figure1, handles);
%--------------------------------------------------------------


%here we save all the stuff in corner file, match file and add some extras....
% --------------------------------------------------------------------
function varargout = save_F_button_Callback(h, eventdata, handles, varargin)
[fname,pname] = uiputfile_name('*.Fmatches','Put F & inlying matches');


f = handles.f;
inlier_matches = handles.inlier_matches;
n_inliers = handles.n_inliers;

n_matches = handles.n_matches;
matches12 = handles.matches12;
f = handles.f;
f

FID = fopen([pname fname],'w');

%save F stuff
fprintf(FID,'%1.0f \n',n_inliers);
fprintf(FID,'%12.8f \n', f);
fprintf(FID,'%1.1f %1.1f %1.1f %1.1f  \n',inlier_matches');


%save correlation matches
fprintf(FID,'%1.0f \n',n_matches);
fprintf(FID,'%1.1f %1.1f %1.1f %1.1f  \n',matches12');

%now store original corners for good measure:
n_corners = handles.n_corners;
ccr1 = handles.ccr1;
ccr2 = handles.ccr2;
n_corners1 = handles.n_corners1;
n_corners2 = handles.n_corners2;
save_corners_to_file(n_corners1, n_corners2, ccr1, ccr2, FID);
fclose(FID);


% --------------------------------------------------------------------
function varargout = load_F_button_Callback(h, eventdata, handles, varargin)
% Stub for Callback of the uicontrol handles.pushbutton28.


[fname,pname] = uigetfile_name('*.fmatches','Load Match File');
if isempty(fname)
    return
end


%set(handles.mapsac_plane_button, 'Enable', 'on');
set(handles.mapsac_button, 'Enable', 'on');
set(handles.mapsac_parameters_button, 'Enable', 'on');
set(handles.save_F_button, 'Enable', 'on');

set(handles.ImproveF_button, 'Enable', 'on');

set(handles.sfm_button, 'Enable', 'on');


FID = fopen([pname fname]);


%save F stuff
n_inliers = fscanf(FID,'%f',1);
f = fscanf(FID,'%f',[1 9]);
inlier_matches = fscanf(FID,'%f %f %f %f',[4 n_inliers]);

disp('fundamental matrix')
handles.f = f
handles.inlier_matches = inlier_matches';
handles.n_inliers  = n_inliers;



n_matches = fscanf(FID,'%f',1);
handles.n_matches = n_matches;
matches12  = fscanf(FID,'%f %f %f %f',[4 n_matches]);

[n_corners1, n_corners2, ccr1, ccr2] = load_corners_from_file(FID);
handles.n_corners1 = n_corners1;
handles.n_corners2 = n_corners2;
handles.ccr1 = ccr1';
handles.ccr2 = ccr2';



fclose(FID);


handles.matches12 = matches12';

set(handles.save_corner_button, 'Enable', 'on');
set(handles.save_match_button, 'Enable', 'on');
set(handles.match_button, 'Enable', 'on');
set(handles.match_param_button, 'Enable', 'on');
set(handles.detect_corner_button, 'Enable', 'off');
set(handles.match_button, 'Enable', 'off');
set(handles.display_epipolar_button, 'Enable', 'on');
set(handles.display_epipolar_button2, 'Enable', 'on');


% Stub for Callback of the uicontrol handles.pushbutton28.
set(handles.mapsac_button, 'Enable', 'on');
set(handles.mapsac_parameters_button, 'Enable', 'on');
%we can now save the result
set(handles.save_F_button, 'Enable', 'on');



% saving the GUI data
guidata(handles.figure1, handles);
clear_button_Callback(h, eventdata, handles, varargin)
%display_matches(h, eventdata, handles, varargin)

%finally display inliers
display_inliers(h, eventdata, handles, varargin);
%%%----------------------------------------------------------------------------------
%%%%this is not 100% checked....
function varargout = display_epipolar_button_Callback(h, eventdata, handles, varargin)

torr_display_epipolar(handles.f,handles.axes2,handles.axes3,handles.m3);


%%%----------------------------------------------------------------------------------

function varargout = display_epipolar_button2_Callback(h, eventdata, handles, varargin)
f = handles.f;

%need to use the transpose of F for image 2--1 
f = [f(1) f(4) f(7) f(2) f(5) f(8) f(3) f(6) f(9)];
torr_display_epipolar(f,handles.axes3,handles.axes2,handles.m3);


%%%----------------------------------------------------------------------------------



% --------------------------------------------------------------------
function varargout = mapsac_plane_button_Callback(h, eventdata, handles, varargin)
% Stub for Callback of the uicontrol handles.mapsac_plane_button.
% Stub for Callback of the uicontrol handles.mapsac_button.


%run mapsac to get f
[f,f_sq_errors, n_inliers,inlier_index] = torr_mapsac_H(handles.matches12(:,1),handles.matches12(:,2),handles.matches12(:,3),handles.matches12(:,4), ...
    handles.n_matches, handles.m3, handles.no_samp, handles.f_threshold)

handles.f = f;
handles.inlier_matches = handles.matches12(inlier_index,:);
handles.n_inliers  = n_inliers;

%finally display inliers
clear_button_Callback(h, eventdata, handles, varargin);
display_inliers(h, eventdata, handles, varargin);

%we can now save the result
set(handles.save_F_button, 'Enable', 'on');
set(handles.detect_corner_button, 'Enable', 'off');
set(handles.match_button, 'Enable', 'off');
set(handles.display_epipolar_button, 'Enable', 'on');
set(handles.display_epipolar_button2, 'Enable', 'on');


% saving the GUI data
guidata(handles.figure1, handles);




% --------------------------------------------------------------------
function varargout = ImproveF_button_Callback(h, eventdata, handles, varargin)
% Stub for Callback of the uicontrol handles.ImproveF_button.

%this function simply operates on the set of matches with no robustness
%to be used with manual input etc...
handles.m3 = 256;

%first estimate F
f = torr_estimateF(handles.matches12, handles.m3, [], 'lin+non_lin');
handles.f = f;


%we can now save the result
set(handles.save_F_button, 'Enable', 'on');
set(handles.detect_corner_button, 'Enable', 'off');
set(handles.match_button, 'Enable', 'off');
set(handles.display_epipolar_button, 'Enable', 'on');
set(handles.display_epipolar_button2, 'Enable', 'on');
%set(handles.mapsac_plane_button, 'Enable', 'on');

set(handles.ImproveF_button, 'Enable', 'on');
set(handles.sfm_button, 'Enable', 'on');

% saving the GUI data
guidata(handles.figure1, handles);


% --------------------------------------------------------------------


% --------------------------------------------------------------------
function varargout = sfm_button_Callback(h, eventdata, handles, varargin)
%this function estalishes the frame, self calibrates and estimates X


f = handles.f;
nF = [[f(1) f(2) f(3)]; [f(4) f(5) f(6)];[f(7) f(8) f(9)]];

nx1 = handles.inlier_matches(:,1);
ny1 = handles.inlier_matches(:,2);
nx2 = handles.inlier_matches(:,3);
ny2 = handles.inlier_matches(:,4);
no_matches = length(nx1);
m3 = handles.m3;

%next self calibrate for focal length
[focal_length, nE,C] = torr_self_calib_f(nF,handles.C);

%
disp('initial estimate of focal length')
focal_length

%now we have an Essential matrix we can establish the camera frame...
[P1,P2,R,t,srot_axis,rot_angle,g]  = torr_linear_EtoPX(nE,handles.inlier_matches,C,handles.m3);

% %next convert the 6 parameters of g to a fundamental matrix
% f2 = torr_g2F(g,C);

[g,f] = torr_nonlinG(g ,nx1,ny1,nx2,ny2, length(nx1), handles.m3, C)

rot_axis = torr_sphere2unit([g(2) g(3)]);
t = torr_sphere2unit([g(5) g(6)]);
rot_angle = g(4);

disp('non_linear estimate of focal length')
focal_length
disp('rotation')
R
rot_axis
rot_angle
disp('translation')
t

%next correct the points so that they lie on the fundamental matrix
[corrected_matches error2] = torr_correctx4F(f, nx1,ny1,nx2,ny2, no_matches, m3);

%corrected matches should have zero error:
e2 = torr_errf2(f, corrected_matches(:,1), corrected_matches(:,2), corrected_matches(:,3),corrected_matches(:,4), length(nx1), m3);
disp('corrected match error is')
norm(e2)

%next we need to obtain P1 & P2
[P1, P2] = torr_g2FP(g,C);

%now use P matrices and corrected matches to get structure:
X = torr_triangulate(corrected_matches, m3, P1, P2);

%note structure is upside down!

%test reprojection error
% rx1 = (P1 * X)';
% 
% rx1(:,1) = m3 * rx1(:,1) ./ rx1(:,3);
% rx1(:,2) = m3 * rx1(:,2) ./ rx1(:,3);
% 
% rx2 = P2 * X;

inlier_index = torr_robust_chieral(X,P1,P2);

disp('number of outliers from chierality:')
no_matches - length(inlier_index)

X = X(:,inlier_index);
%flash up a new window and display the structure plus cameras:
%invert = 1;

% note becuase of row/column coordinate system of the image the X's are upside down so invert them prior
% to display.
X(2,:) = -X(2,:);
torr_display_structure(X, P1, P2);
X(2,:) = -X(2,:);


handles.P1 = P1;
handles.P2 = P2;
handles.X = X;
handles.inlier_matches = handles.inlier_matches(inlier_index,:);
handles.focal_length = focal_length;
handles.E = nE;
handles.C = C;
handles.f = f;
handles.n_matches = no_matches;

% saving the GUI data
guidata(handles.figure1, handles);



% --------------------------------------------------------------------
function varargout = sfm_parambutton_Callback(h, eventdata, handles, varargin)

pos_size = get(handles.figure1,'Position');
dlg_pos = [pos_size(1)+pos_size(3)/5 pos_size(2)+pos_size(4)/5];

cal_param = torr_cal_menu(handles, dlg_pos);

handles.focal_length = cal_param(1);
handles.aspect_ratio = cal_param(2);
handles.ppx =  cal_param(3);
handles.ppy =  cal_param(4);

%work out calibration matrix
C = [ handles.aspect_ratio 0 handles.ppx; 0 1 handles.ppy; 0 0 1/handles.focal_length];
C
handles.C = C;
% saving the GUI data
guidata(handles.figure1, handles);



% --------------------------------------------------------------------
function varargout = initialize(handles)

disp('this function isnt ready yet');
%phils atuff added, initialization
%corner parameters
handles.n_corners = 500; %max no of corners permitted/requested

handles.n_corners1 = 0; %the actual number detected
handles.n_corners2 = 0; 

handles.corner_sigma = 1.0;
handles.corner_width = 3;


%matchin parameters
handles.max_disparity = 100;
handles.match_half_size = 3;


%F parameters:
handles.no_samp = 500; % no of samples in the mapsac algorithm
handles.f_threshold = 6.0; % f_threshold in the mapsac algorithm

%informational variables
handles.n_matches = 0; %how many matches have we dectected.


%general parameters
%debugt mode
handles.debug = 1;
handles.m3 = 256; %third homogeous pixel coordinate, chosen as 256 to help conditioning, see my thesis
handles.pathname = ['C:\matlabR12\bin\'];


%calibration matrix 
handles.focal_length = 3; %first guess, which is in units of m3 i.e
                            %focal length in pixels is 3 * m3
handles.aspect_ratio = 1;
handles.ppx =  0;
handles.ppy =  0;
%work out calibration matrix
C = [a 0 handles.ppx; 0 1 handles.ppy; 0 0 1/handles.focal_length];
handles.C = C;

if ~handles.debug
    helpdlg('Phil asks: "how ya diddling", first load some images')
end


set(handles.save_image1_button, 'Enable', 'off');
set(handles.save_image2_button, 'Enable', 'off');
set(handles.Save_Images_menu, 'Enable', 'off');


%for corners
set(handles.detect_corner_button, 'Enable', 'off');
set(handles.save_corner_button, 'Enable', 'off');
set(handles.load_corner_button, 'Enable', 'off');
set(handles.corn_param_button, 'Enable', 'off');

%for correlation matches
set(handles.match_button, 'Enable', 'off');
set(handles.match_param_button, 'Enable', 'off');
set(handles.manual_match_button, 'Enable', 'off');
set(handles.save_match_button, 'Enable', 'off');    
set(handles.load_match_button, 'Enable', 'off');

%for F and correlation matches
set(handles.mapsac_button, 'Enable', 'off');
set(handles.mapsac_parameters_button, 'Enable', 'off');
set(handles.save_F_button, 'Enable', 'off');
set(handles.display_epipolar_button, 'Enable', 'off');
set(handles.display_epipolar_button2, 'Enable', 'off');

%self calibrate
set(handles.calibrate_button, 'Enable', 'on');




guidata(handles.figure1, handles);





% --------------------------------------------------------------------
function varargout = epi_button_Callback(h, eventdata, handles, varargin)

 torr_disp_epip_geom(handles.f,handles.matches12,handles.axes2,handles.axes3,handles.m3);


 f = handles.f;
f = [f(1) f(4) f(7) f(2) f(5) f(8) f(3) f(6) f(9)];

torr_disp_epip_geom(handles.f,handles.matches12,handles.axes3,handles.axes2,handles.m3);



% --------------------------------------------------------------------