基于Matlab实现三维数组重建中涑调整（源码+数据）.rar

function [S0_new, P_new, avg_distance, F_new, C_new, KC_new] = bundle_adjustment(... S0,P,I_left,I_right,ref_global,miu,type,change_intrinsic,filename,... F,C,KC,RT) % [S0_new, P_new,avg_distance, F_new, C_new, KC_new] % = bundle_adjustment( % S0, ,P, I_left, I_right, ref_global, % miu,type, change_intrinsic, filename,F,C,KC) % % Using bundle adjustment to minimize the reprojection error of % image points. M image, N feature points. % %Input: S0: The estimated 3d points. % Type: matrix, size: 3*N % P: The transformation matrix from global 3d points to the % local. % Type: cell, size: 3*4(matrix,[R T]) of 1*M(cell) % I_left: Local 2d points of left image, including visible and % invisible points. % Type: cell, size: 2*N(matrix) of 1*M(cell) % I_right: Local 2d points of right image, including visible and % invisible points. If no need of I_right, make it to % zero. (The left and right can not be zero at same time) % Type: cell, size: 2*N(matrix) of 1*M(cell) % ref_global: The information of the local points'(or image) % index in the global points(image). % Type: matrix, size: N*M % miu: The parameter to adjust LM: (JTJ +miu*I) = JTeps % Default: 1 % Type: There are three type:‘both’,'point','parameter' % 'both': points and parameters are both adjusted % 'point': only points will be adjusted % 'parameter': only other parameters except points will % be adjusted. % Default: 'both' % change_intrinsic: Whether change camera intrinsic parameter % such as F, C, KC. nonzero means change, and zero means not. % Default: 0 % filename: the name of the camera calibration file. It has the % intrinsic and extrinsic parameters. % Default: 'CameraCalibration.xml' % F: The focal length of the camera(s). % Type: matrix % Size: ①2*1 It means left and right camera have the same % focal length for every image. % ②4*1 The first two rows are the focal length of the % left camera, and the last two rows are the right. % Every image will share the same focal length. % ③2*M Left and the right camera have the same focal % length, which is different in differnt images. % ④4*M Left and the right camera have different focal % length, and it is different in differnt images. % C: The principle point's coordinates of the camera(s) % Type: matrix % Size: ①2*1 ②4*1 ③2*M ④4*M % KC: The distortion parameters of the camera(s). % Type: matrix % Size: ①5*1 ②10*1 ③5*M ④10*M % RT: The exrtinsic parameter between the two camera. % Type: matrix, size:3*4 % Note: If you want to input camera intrinsic parameter % mannually(rather than using camera calibration file, no parameter % of F, C, KC or RT can be omitted. Their size should have same type. % % %Output: S0_new: The updated value of S0, same type and size as S0. % P_new: The updated value of P, same type and size as P. % F_new: The updated value of focal length. % Type: matrix, size: 4*M. The first 2 rows are about % left camera, the following two 2 are about the % right. % C_new: The updated value of principle length. % Type: matrix, size: 4*M. The first 2 rows are about % left camera, the following 2 rows are about the % right. % KC_new: The updated value of distortion parameter. % Type: matrix, size: 10*M. The first 5 rows are about % left camera, the following 5 rows are about the % right. % % Note: If change_intrinsic's value is 0, then F_new, C_new, KC_new % will return the value of the calibration result with the size % of 4*1([fx_left;fy_left;fx_right;fy_right]) % 4*1([cx_left;cy_left;cx_right;cy_right]) % 10*1([kc1_left;..;kc5_left];kc1_right;..;kc5_right]) % seperately. % If change_intrinsic's value is 1, then F_new, C_new, KC_new % will return the updated value of the calibration result with % the size of 4*M, 4*M, 10*M seperately. The parameter M means % different image. % %Eg: ①No willing to change intrinsic parameter, and the calibration result % are in the specific xml file: % [S0_new, P_new, avg_distance] = bundle_adjustment(... % S0,P,I_left,I_right,ref_global) % ②Willing to change the intrinsic parameter, and input calibration % result mannually: % [S0_new, P_new,avg_distance, F_new, C_new, KC_new] % = bundle_adjustment(... % S0,P, I_left, I_right, ref_global,... % miu,type,1,0,F,C,KC) if nargin < 9 filename = 'CameraCalibration.xml'; if nargin < 8 change_intrinsic = 0; if nargin < 7 type = 'both'; if nargin < 6 miu = 1; end end end end %Judge the information input is valid. if size(I_left,2)==0||size(I_right,2)==0 disp('The image input is empty, please check it'); return end if size(I_left,2)~=size(I_right) disp('The number of left image does not equal to the right'); return; end N = size(S0,2); %N is the number of the feature point M = max(size(I_left,2),size(I_right,2)); %M is the number of the image gathered. for j = 1:M if size(I_left{j},2)~=size(I_right{j},2) disp('The point''s number of image: '); disp(j) disp('is not equal between the left and right'); return; end end if nargin > 9 if size(F,2)~=size(C,2)||size(C,2)~=size(KC,2) disp('The size of F C kC is not in same type'); return end if size(F,1)~=2&&size(F,1)~=4 disp('The number of rows of F must be 2 or 4'); return end if size(C,1)~=2&&size(C,1)~=4 disp('The number of rows of C must be 2 or 4'); return end if size(KC,1)~=5&&size(KC,1)~=10 disp('The number of rows of KC must be 5 or 10'); return end end change_intrinsic = logical(change_intrinsic); %Load calibration result %F_left: Focal length of the left camera. % Type: matrix, size: 2*1([fx;fy]) or 2*M % If its size is 2*M, it means the Focal length of each image % does not need to be adjusted. %C_left: Principle point of the left camera. % Type: matrix, size: 2*1([cx;cy]) or 2*M % If its size is 2*M, it means the priciple point of each % image does not need to be adjusted. %KC_left: Distortion parameters of the left camera. % Type: matrix, size: 5*1([kc1;kc2;kc3;kc4;kc5]) or 5*M % If its size is 5*M, it means the distortion parameters of % each image does not need to be adjusted. %F_right: Focal length of the right camera. %C_right: Principle point of the right camera. %KC_right: Distortion parameters of the right camera. %RT: The transformation matrix from left camera reference % coordinates to the right camera. % Type: matrix, size: 3*4([R T]) if nargin < 9 [F_left,C_left,KC_left,F_right,C_right,KC_right,RT] = read_calibration_xml(filename); F_left = repmat(F_left,1,M); C_left = repmat(C_left,1,M); KC_left = repmat(KC_left,1,M);