├── Camera_2DLaser └── manualCalib │ ├── #extCalibManual.m# │ ├── #getExtParam.m# │ ├── CalibExtParam.m │ ├── detecttargets.m │ ├── extCalib.m │ ├── extCalibManual.m │ ├── filesInPath.m │ ├── getCamExtParams.m │ ├── getExtParam.m │ ├── getGridPoints.m │ ├── getLinePts.m │ ├── getProjectionErr.m │ ├── getTransformMatrix.m │ ├── glbOptimize.m │ ├── harris.m │ ├── homography2d.m │ ├── mapLaserPts.m │ ├── mapping.m │ ├── normalise2dpts.m │ ├── readLaserLog.m │ ├── readme.txt │ └── zOlderGetLinePts.m ├── GlobalCS_Camera ├── Calib_Results.m ├── finalEstimateQuat.txt ├── finalEstimateRodrigues.txt ├── funkcjaMin.m ├── globalCalibration.m └── xSense.mat ├── IMU_Camera ├── Calib_Results.m ├── arrow3.m ├── calc_cam_vp.m ├── calc_cam_vp_w.m ├── calc_q_imu2cam.m ├── calc_q_imu2cam_w.m ├── clickmove.m ├── fast_normalize_us.m ├── fscatter3.m ├── get_cam_xsens_mti.m ├── great_circ.m ├── imu2cam.mat ├── imucam.m ├── load_imu.m ├── load_imu_w.m ├── normalize_us.m ├── show_rotation_reprojection_error_w.m ├── show_us_vp_cam_imu.m └── vect_ang.m └── README.md /Camera_2DLaser/manualCalib/#extCalibManual.m#: -------------------------------------------------------------------------------- 1 | % estimate the relative position of camera with respect to laser ranger finder 2 | % [Phi,Delta]=extCalibManual(K,kc) 3 | % input: K -- camera intrinic matrix; 4 | % kc -- camera radius and tangent distortion parameters 5 | % output: Phi -- 3x3 orthonormal matrix responding to camera orientation w.r.t laser range finder 6 | % Delta -- 3-vector, camera position w.r.t laser range finder 7 | % the function assumes that all image and laser data is in the current working directory. 8 | % image data is JPEG file and laser data is postfixed as '.log'. 9 | 10 | function [Phi,Delta]=extCalibManual(K,kc) 11 | 12 | global sq_size; 13 | 14 | if nargin<2 15 | kc=zeros(5,1); 16 | end; 17 | 18 | imgFiles=filesInPath('./','jpg'); % load image filenames in the path 19 | logFiles=filesInPath('./','log'); % load laser reading filenames in the path 20 | 21 | num=length(imgFiles); 22 | 23 | Pts=[];Ns=[];Ds=[]; 24 | 25 | for i=1:num 26 | disp(['Processing pose ',num2str(i),' ...']); 27 | img=imread(imgFiles{i}); 28 | 29 | % estimate extrinsic camera paramters w.r.t world coordinate system 30 | [extR,extT]=getExtParam(img,K,kc); 31 | Ns{i}=-extR(:,3); 32 | Ds{i}=-extR(:,3)'*extT; 33 | 34 | log=readlaserlog(logFiles{i}); 35 | [lpts,errs(i)]=getLinePts(log); % extract the laser points hitting on the checkboard 36 | Pts{i}=lpts; 37 | 38 | end 39 | 40 | weights=(1-errs.^10/sum(errs.^10))*num/(num-1); 41 | [Phi,Delta]=getTransformMatrix(Pts, Ns, Ds,weights); 42 | 43 | % n=length(Pts); 44 | % 45 | % thred=1; 46 | % 47 | % while (thred>0.0001) 48 | % valid=[]; 49 | % for i=1:n 50 | % err=estProjErr(Phi,Delta,Pts, Ns, Ds, i); 51 | % if (err<0.05) 52 | % valid=[valid,i]; 53 | % end 54 | % end 55 | % if (length(valid)<=5) 56 | % return; 57 | % end; 58 | % %weights=(1-errs(valid)/sum(errs(valid)))*length(valid)/(length(valid)-1); 59 | % [phi,delta]=getTransformMatrix(Pts(valid), Ns(valid), Ds(valid)); 60 | % thred=norm([rodrigues(phi)-rodrigues(Phi);delta-Delta]); 61 | % Phi=phi; 62 | % Delta=delta; 63 | % end; 64 | 65 | clear global sq_size; 66 | 67 | return 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/#getExtParam.m#: -------------------------------------------------------------------------------- 1 | % function [R,T,imgPts,boardPts]=getExtParameters(im,K,kc) 2 | % input: im-- image that contains the checkerboard; 3 | % K -- 3x3 camera intrinsic matrix 4 | % kc-- 5-vector, radius and tangent distortion parameters 5 | % output: camera rotation R and translation T respective to the checkerboard plane 6 | % imgPts -2xn matrix, the grid points projected on the image 7 | % boardPts -3xn matrix, the grid points on the checkerboard 8 | 9 | function [R,T,imgPts,boardPts]=getExtParameters(im,K,kc) 10 | 11 | global sq_size; 12 | 13 | if nargin<3 14 | kc=zeros(5,1); 15 | end 16 | 17 | %imshow(im); 18 | winz=5; %window size 19 | disp('Please select a rectangle area from the checkerboard pattern.'); 20 | [height, width]=size(im); 21 | figure(1); 22 | [bw,cornerX,cornerY]=roipoly(im); 23 | cornerX=cornerX(1:4); 24 | cornerY=cornerY(1:4); 25 | 26 | [Xc,good,bad,type] = cornerfinder([cornerX';cornerY'],im,winz,winz); % the four corners 27 | 28 | 29 | x = Xc(1,:)'; 30 | y = Xc(2,:)'; 31 | hold on; plot([x;x(1)],[y;y(1)],'ro-');hold off; 32 | 33 | n_sq_x1 = count_squares(im,x(1),y(1),x(2),y(2),winz); 34 | n_sq_x2 = count_squares(im,x(3),y(3),x(4),y(4),winz); 35 | n_sq_y1 = count_squares(im,x(2),y(2),x(3),y(3),winz); 36 | n_sq_y2 = count_squares(im,x(4),y(4),x(1),y(1),winz); 37 | 38 | 39 | if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2) 40 | sq_num_x=input('The number of squares along X axis:'); 41 | sq_num_y=input('The number of squares along Y axis:'); 42 | else 43 | sq_num_x=n_sq_x1; 44 | sq_num_y=n_sq_y1; 45 | end 46 | 47 | 48 | disp(['the size of the section is ',num2str(sq_num_x), 'x', num2str(sq_num_y) ]); 49 | if ~exist('sq_size')|isempty(sq_size) 50 | sq_size=input('The length of the square:[default 76mm]:'); 51 | if isempty(sq_size) 52 | sq_size=76/1000; %76mm 53 | end 54 | end 55 | [imgPts,boardPts]=projectGridPts(Xc,sq_num_x,sq_num_y); 56 | boardPts=boardPts*sq_size; 57 | 58 | [imgPts,good,bad,type] = cornerfinder(imgPts,im,winz,winz); 59 | 60 | [omc,T,R,H] = compute_extrinsic(imgPts,boardPts,[K(1,1);K(2,2)],[K(1,3);K(2,3)],kc,K(1,2)/K(1,1)); 61 | 62 | %N=-R(:,3) 63 | %dist=-R(:,3)'*T 64 | imgPts=imgPts(:,good); 65 | boardPts=boardPts(:,good); 66 | 67 | [projPts] = project_points2(boardPts,omc,T,[K(1,1);K(2,2)],[K(1,3);K(2,3)],kc,K(1,2)/K(1,1)); 68 | 69 | % figure(2); 70 | % imshow(im); 71 | % hold on; 72 | % plot(projPts(1,:),projPts(2,:),'r+'); 73 | % hold off; 74 | 75 | return 76 | 77 | 78 | %------------re-projected grid points of the checherboard---------------------------% 79 | 80 | function [gridPts,ptsCoord]=projectGridPts(cornerPts,sqNumX,sqNumY) 81 | 82 | [Xs,Ys]=meshgrid(1:(sqNumX+1),1:(sqNumY+1)) ; 83 | ptsCoord=[Xs(:)-1,Ys(:)-1,zeros((sqNumX+1)*(sqNumY+1),1)]'; 84 | 85 | Xs=(Xs(:)-1)/sqNumX+1; 86 | Ys=(Ys(:)-1)/sqNumY+1; 87 | 88 | Zx=[cornerPts(1,1) cornerPts(1,2); 89 | cornerPts(1,4) cornerPts(1,3)]; 90 | 91 | Zy=[cornerPts(2,1) cornerPts(2,2); 92 | cornerPts(2,4) cornerPts(2,3)]; 93 | 94 | gridPts(1,:)=interp2(Zx,Xs,Ys)'; 95 | gridPts(2,:)=interp2(Zy,Xs,Ys)'; 96 | 97 | return 98 | 99 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/CalibExtParam.m: -------------------------------------------------------------------------------- 1 | % estimate the relative position of camera with respect to laser ranger finder 2 | % K -- camera intrinic matrix; kc--camera radius and tangent distortion parameters 3 | 4 | function [Phi,Delta]=CalibExtParam(K,kc,imgPts,boardPts) 5 | 6 | logFiles=filesInPath('./','log'); % load laser reading filenames in the path 7 | 8 | num=length(logFiles); 9 | 10 | Pts=[]; 11 | 12 | disp(['loading laser readings ']); 13 | for i=1:num 14 | log=readlaserlog(logFiles{i}); 15 | [lpts,errs(i)]=getLinePts(log); % extract the leaser points hitting on the checkboard 16 | Pts{i}=lpts; 17 | end 18 | 19 | indx=1:num; 20 | [Phi, Delta]=calExtr(K,kc,imgPts(indx),boardPts(indx),Pts(indx),errs(indx)); 21 | 22 | return 23 | 24 | %------------------------------------------------------------------------- 25 | function [Phi, Delta]=calExtr(K,kc,imgPts,boardPts,Pts,errs) 26 | 27 | num=length(imgPts); 28 | Ns=[]; 29 | Ts=[]; 30 | 31 | for i=1:num 32 | 33 | [omc,T,R,H] = compute_extrinsic(imgPts{i},boardPts{i},[K(1,1);K(2,2)],[K(1,3);K(2,3)],kc,0); 34 | Ns{i}=-R(:,3); 35 | Ds{i}=-R(:,3)'*T; 36 | 37 | Rs{i}=R; 38 | Ts{i}=T; 39 | 40 | end 41 | 42 | %--------------estimate camera extrinsic parameters relative to laser range finder -------% 43 | weights=(1-errs.^10/sum(errs.^10))*num/(num-1); 44 | [Phi,Delta]=getTransformMatrix(Pts, Ns, Ds,weights); 45 | 46 | return 47 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/detecttargets.m: -------------------------------------------------------------------------------- 1 | %detecttargets 2 | 3 | function targets=detecttargets(img) 4 | 5 | %expects an input image named img 6 | 7 | if ~exist('max_center_dist') 8 | max_center_dist = 20; 9 | end; 10 | if ~exist('min_size') 11 | min_size = 5; 12 | end; 13 | if ~exist('max_size') 14 | max_size = 10000; 15 | end; 16 | if ~exist('min_size_ratio') 17 | min_size_ratio = 1.0; 18 | end; 19 | if ~exist('max_size_ratio') 20 | max_size_ratio = 10; 21 | end; 22 | 23 | 24 | %inverse threshold the input image 25 | %( local adaptive threshold ) 26 | imsz= size(img); 27 | im_h=imsz(1); 28 | im_w=imsz(2); 29 | thresh = zeros(im_h,im_w); 30 | blocksize = 100; 31 | for i=1:blocksize:im_w 32 | for j=1:blocksize:im_h 33 | i1 = i; 34 | i2 = min([i1+blocksize im_w]); 35 | j1 = j; 36 | j2 = min([j1+blocksize im_h]); 37 | level = 0.9 * mean(reshape(img(j1:j2, i1:i2), (j2-j1+1)*(i2-i1+1), 1)); 38 | thresh(j1:j2,i1:i2) = img(j1:j2, i1:i2) < max([min([level 100]) 50]); 39 | end; 40 | end; 41 | 42 | %label the contiguous blobs 43 | label = bwlabel(thresh, 8); 44 | 45 | %find the regions that are within the right size range 46 | %(discard blobs that are too small or too big) 47 | histogram = hist(label(:),0:max(label(:))); 48 | regions = find( histogram >= min_size & histogram <= max_size ) -1; 49 | 50 | regions = regions(find(regions>0)); 51 | min_x=repmat(im_w, 1, max(label(:))); 52 | max_x=repmat(0, 1, max(label(:))); 53 | min_y=repmat(im_h, 1, max(label(:))); 54 | max_y=repmat(0, 1, max(label(:))); 55 | for j=1:im_h 56 | for i=1:im_w 57 | r=label(j,i); 58 | if (r>0) 59 | if (i < min_x(r)) 60 | min_x(r)=i; 61 | elseif (i > max_x(r)) 62 | max_x(r)=i; 63 | end; 64 | if (j < min_y(r)) 65 | min_y(r)=j; 66 | elseif (j > max_y(r)) 67 | max_y(r)=j; 68 | end; 69 | end; 70 | end; 71 | end; 72 | min_x=min_x(regions); 73 | max_x=max_x(regions); 74 | min_y=min_y(regions); 75 | max_y=max_y(regions); 76 | w = max_x - min_x + 1; 77 | h = max_y - min_y + 1; 78 | c_x = min_x + w / 2; 79 | c_y = min_y + h / 2; 80 | 81 | temp_x = repmat(c_x, size(c_x,2),1); 82 | temp_y = repmat(c_y, size(c_y,2),1); 83 | dist_x = (temp_x - temp_x'); 84 | dist_y = (temp_y - temp_y'); 85 | c_dist = sqrt(dist_x.^2 + dist_y.^2); 86 | 87 | w_temp = repmat(w, size(w,2),1); 88 | w_ratio = w_temp' ./ w_temp; 89 | h_temp = repmat(h, size(h,2),1); 90 | h_ratio = h_temp' ./ h_temp; 91 | 92 | [i j] = find( c_dist < 0.25*w_temp & c_dist < 0.25*h_temp & (w_ratio > min_size_ratio) & (w_ratio < max_size_ratio) & (h_ratio > min_size_ratio) & (h_ratio < max_size_ratio)); 93 | %[i j] = find( (c_dist < w_temp | c_dist < h_temp ) ); 94 | icontainsj = (min_x(j) > min_x(i) & max_x(j) < max_x(i) & (min_y(j) > min_y(i)) & max_y(j) < max_y(i)); 95 | 96 | targets = []; 97 | target_idx = i(icontainsj); 98 | for i=1:length(target_idx) 99 | targets(i).center_x = c_x(target_idx(i)); 100 | targets(i).center_y = c_y(target_idx(i)); 101 | targets(i).w = w(target_idx(i)); 102 | targets(i).h = h(target_idx(i)); 103 | end; 104 | 105 | clear temp_x temp_y dist_x dist_y c_dist w_temp w_ratio h_temp h_ratio 106 | clear min_size max_size min_size_ratio max_size_ratio; 107 | 108 | %imshow(img); hold on; plot([targets.center_x], [targets.center_y], 'r.'); 109 | x=[targets.center_x]; 110 | y=[targets.center_y]; 111 | K=convhull(x,y); 112 | 113 | targets=targets(K(1:end-1)); 114 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/extCalib.m: -------------------------------------------------------------------------------- 1 | % [Phi,Delta]=extCalib(K,kc) 2 | % automatically estimate the relative position of camera with respect to laser ranger finder 3 | % input: K -- camera intrinic matrix; 4 | % kc -- camera radius and tangent distortion parameters 5 | % output: Phi -- 3x3 orthonormal matrix responding to camera orientation w.r.t laser range finder 6 | % Delta -- 3-vector, camera position w.r.t laser range finder 7 | % the function assumes that all image and laser data is in the current working directory. 8 | % image data is JPEG file and laser data is postfixed as '.log'. 9 | 10 | function [Phi,Delta]=extCalib(K,kc) 11 | 12 | 13 | 14 | global sq_size; 15 | 16 | if nargin<2 17 | kc=zeros(5,1); 18 | end; 19 | 20 | imgFiles=filesInPath('./','png'); % load image filenames in the path 21 | logFiles=filesInPath('./','log'); % load laser reading filenames in the path 22 | 23 | num=length(imgFiles); 24 | 25 | 26 | Pts=[];Ns=[];Ds=[]; 27 | 28 | for i=1:num 29 | disp(['Processing pose ',num2str(i),' ...']); 30 | img=imread(imgFiles{i}); 31 | 32 | 33 | 34 | log=readLaserLog(logFiles{i}); 35 | [lpts,errs(i)]=getLinePts(log); % extract the laser points hitting on the checkboard 36 | Pts{i}=lpts; 37 | 38 | % estimate extrinsic camera paramters w.r.t world coordinate system 39 | [extR,extT]=getCamExtParams(img,K,kc); 40 | 41 | Ns{i}=-extR(:,3); % checkerboard plane norm 42 | Ds{i}=-extR(:,3)'*extT; % distance of the plane to camera 43 | 44 | end 45 | %compute weights 46 | weights=(1-errs.^10/sum(errs.^10))*num/(num-1); 47 | 48 | disp('Calculating camera extrinsic parameters '); 49 | [Phi,Delta]=getTransformMatrix(Pts, Ns, Ds,weights); 50 | 51 | for i=1:num 52 | img=imread(imgFiles{i}); 53 | log=readLaserLog(logFiles{i}); 54 | 55 | mapLaserPts(img,log,Phi,Delta,K,kc); 56 | pause; 57 | end; 58 | 59 | % n=length(Pts); 60 | % 61 | % thred=1; 62 | % 63 | % while (thred>0.0001) 64 | % valid=[]; 65 | % for i=1:n 66 | % err=getProjectionErr(Phi,Delta,Pts, Ns, Ds, i) 67 | % if (err<0.05) 68 | % valid=[valid,i]; 69 | % end 70 | % end 71 | % if (length(valid)<=5) 72 | % return; 73 | % end; 74 | % weights=(1-errs(valid)/sum(errs(valid)))*length(valid)/(length(valid)-1); 75 | % [phi,delta]=getTransformMatrix(Pts(valid), Ns(valid), Ds(valid),weights); 76 | % thred=norm([rodrigues(phi)-rodrigues(Phi);delta-Delta]); 77 | % Phi=phi; 78 | % Delta=delta; 79 | % end; 80 | 81 | clear global sq_size; 82 | return 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/extCalibManual.m: -------------------------------------------------------------------------------- 1 | % estimate the relative position of camera with respect to laser ranger finder 2 | % [Phi,Delta]=extCalibManual(K,kc) 3 | % input: K -- camera intrinic matrix; 4 | % kc -- camera radius and tangent distortion parameters 5 | % output: Phi -- 3x3 orthonormal matrix responding to camera orientation w.r.t laser range finder 6 | % Delta -- 3-vector, camera position w.r.t laser range finder 7 | % the function assumes that all image and laser data is in the current working directory. 8 | % image data is JPEG file and laser data is postfixed as '.log'. 9 | 10 | function [Phi,Delta]=extCalibManual(K,kc) 11 | 12 | global sq_size; 13 | 14 | if nargin<2 15 | kc=zeros(5,1); 16 | end; 17 | 18 | imgFiles=filesInPath('./','jpg'); % load image filenames in the path 19 | logFiles=filesInPath('./','log'); % load laser reading filenames in the path 20 | 21 | num=length(imgFiles); 22 | 23 | Pts=[];Ns=[];Ds=[]; 24 | 25 | for i=1:num 26 | disp(['Processing pose ',num2str(i),' ...']); 27 | img=imread(imgFiles{i}); 28 | 29 | % estimate extrinsic camera paramters w.r.t world coordinate system 30 | [extR,extT]=getExtParam(img,K,kc); 31 | Ns{i}=-extR(:,3); 32 | Ds{i}=-extR(:,3)'*extT; 33 | 34 | log=readlaserlog(logFiles{i}); 35 | [lpts,errs(i)]=getLinePts(log); % extract the laser points hitting on the checkboard 36 | Pts{i}=lpts; 37 | 38 | end 39 | 40 | weights=(1-errs.^10/sum(errs.^10))*num/(num-1); 41 | [Phi,Delta]=getTransformMatrix(Pts, Ns, Ds,weights); 42 | 43 | % n=length(Pts); 44 | % 45 | % thred=1; 46 | % 47 | % while (thred>0.0001) 48 | % valid=[]; 49 | % for i=1:n 50 | % err=estProjErr(Phi,Delta,Pts, Ns, Ds, i); 51 | % if (err<0.05) 52 | % valid=[valid,i]; 53 | % end 54 | % end 55 | % if (length(valid)<=5) 56 | % return; 57 | % end; 58 | % %weights=(1-errs(valid)/sum(errs(valid)))*length(valid)/(length(valid)-1); 59 | % [phi,delta]=getTransformMatrix(Pts(valid), Ns(valid), Ds(valid)); 60 | % thred=norm([rodrigues(phi)-rodrigues(Phi);delta-Delta]); 61 | % Phi=phi; 62 | % Delta=delta; 63 | % end; 64 | 65 | clear global sq_size; 66 | 67 | return 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/filesInPath.m: -------------------------------------------------------------------------------- 1 | % function cellFiles = filesInPath(pathString) 2 | % returns a cell array containing all the files in pathString which end 3 | % in '.gz' 4 | % function cellFiles = filesInPath(pathString, extString) 5 | % returns a cell array containing all the files in pathString which end 6 | % in extString 7 | 8 | function cellFiles = filesInPath(pathString, varargin) 9 | if length(varargin) > 0 10 | extString = lower(varargin{1}); 11 | else 12 | extString = '.gz'; 13 | end 14 | dirList = dir(pathString); 15 | cellFiles = {}; 16 | jx = 1; 17 | for ix=1:length(dirList) 18 | fn = lower(dirList(ix).name); 19 | idxes= findstr(fn, extString); 20 | if not(isempty(idxes)) 21 | if idxes(end) == (length(fn) - length(extString) + 1) 22 | cellFiles{jx} = [pathString '/' fn]; 23 | jx = jx+1; 24 | end 25 | end 26 | end 27 | 28 | filenameSize = size(cellFiles{1}); 29 | allOK = 1; 30 | for i = 2:length(cellFiles); 31 | if any(filenameSize ~= size(cellFiles{i})) 32 | allOK = 0; 33 | end 34 | end 35 | if (allOK) 36 | %disp('sorting filenames lexicographically'); 37 | for i = 1:length(cellFiles); 38 | filenames(i,:) = cellFiles{i}; 39 | end 40 | [jnk,idxes] = sortrows(filenames); 41 | for i = 1:length(cellFiles); 42 | cellFiles{i} = filenames(idxes(i),:); 43 | end 44 | else 45 | disp(['filenames of different lengths. Unable to sort' ... 46 | ' lexicographically, Matlab inherent order is by creation date']); 47 | end 48 | 49 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/getCamExtParams.m: -------------------------------------------------------------------------------- 1 | % Estimate the extrinsic parameters of the camera with respect to the checkboard pattern 2 | % function [R,T,imgPts,boardPts]=getCamExtParams(im,K,kc) 3 | % input: im--image that contains the checkerboard; 4 | % K -- camera intrinsic matrix 5 | % kc--radius and tangent distortion parameters 6 | % output: camera rotation R and translation T respective to the checkerboard plane 7 | % imgPts -the grid points projected on the image 8 | % boardPts -the grid points on the checkerboard 9 | 10 | function [R,T,imgPts,boardPts]=getCamExtParams(img,K,kc) 11 | 12 | global sq_size; 13 | if nargin<3 14 | kc=zeros(5,1); 15 | end 16 | 17 | 18 | sq_size=100/1000; %define checkboard square size -- 76mm 19 | if ~exist('sq_size')|isempty(sq_size) 20 | sq_size=input('The length of the square:[default 76mm]:'); 21 | if isempty(sq_size) 22 | sq_size=76/1000; %76mm 23 | end 24 | end 25 | 26 | 27 | 28 | % extract the targets located on the corners of the checkerboard 29 | targets=detecttargets(img); 30 | 31 | if length(targets)~=4 32 | error('failed to estimate the targets'); 33 | end 34 | 35 | % extract projected grid points along with their coords on the checkerboard 36 | [imgPts,boardPts]=getGridPoints(img,targets); 37 | 38 | boardPts=boardPts*sq_size; 39 | 40 | % estimate camera extrinsic paramters -- R and T 41 | [omc,T,R,H] = compute_extrinsic(imgPts,boardPts,[K(1,1);K(2,2)],[K(1,3);K(2,3)],kc,K(1,2)/K(1,1)); 42 | 43 | return 44 | 45 | %-------------test----------------------% 46 | %N=-R(:,3); 47 | %dist=-R(:,3)'*T; 48 | % reproject the grid points onto the image with estimated extrinsic parameters 49 | [projPts] = project_points2(boardPts,omc,T,[K(1,1);K(2,2)],[K(1,3);K(2,3)],kc,K(1,2)/K(1,1)); 50 | 51 | imshow(img); 52 | hold on; 53 | plot(projPts(1,:),projPts(2,:),'r.'); 54 | hold off; 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/getExtParam.m: -------------------------------------------------------------------------------- 1 | % function [R,T,imgPts,boardPts]=getExtParameters(im,K,kc) 2 | % input: im-- image that contains the checkerboard; 3 | % K -- 3x3 camera intrinsic matrix 4 | % kc-- 5-vector, radius and tangent distortion parameters 5 | % output: camera rotation R and translation T respective to the checkerboard plane 6 | % imgPts -2xn matrix, the grid points projected on the image 7 | % boardPts -3xn matrix, the grid points on the checkerboard 8 | 9 | function [R,T,imgPts,boardPts]=getExtParameters(im,K,kc) 10 | 11 | global sq_size; 12 | 13 | if nargin<3 14 | kc=zeros(5,1); 15 | end 16 | 17 | %imshow(im); 18 | winz=5; %window size 19 | disp('Please select a rectangle area from the checkerboard pattern.'); 20 | [height, width]=size(im); 21 | figure(1); 22 | [bw,cornerX,cornerY]=roipoly(im); 23 | cornerX=cornerX(1:4); 24 | cornerY=cornerY(1:4); 25 | 26 | [Xc,good,bad,type] = cornerfinder([cornerX';cornerY'],im,winz,winz); % the four corners 27 | 28 | 29 | x = Xc(1,:)'; 30 | y = Xc(2,:)'; 31 | hold on; plot([x;x(1)],[y;y(1)],'ro-');hold off; 32 | 33 | n_sq_x1 = count_squares(im,x(1),y(1),x(2),y(2),winz); 34 | n_sq_x2 = count_squares(im,x(3),y(3),x(4),y(4),winz); 35 | n_sq_y1 = count_squares(im,x(2),y(2),x(3),y(3),winz); 36 | n_sq_y2 = count_squares(im,x(4),y(4),x(1),y(1),winz); 37 | 38 | 39 | if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2) 40 | sq_num_x=input('The number of squares along X axis:'); 41 | sq_num_y=input('The number of squares along Y axis:'); 42 | else 43 | sq_num_x=n_sq_x1; 44 | sq_num_y=n_sq_y1; 45 | end 46 | 47 | 48 | disp(['the size of the section is ',num2str(sq_num_x), 'x', num2str(sq_num_y) ]); 49 | if ~exist('sq_size')|isempty(sq_size) 50 | sq_size=input('The length of the square:[default 76mm]:'); 51 | if isempty(sq_size) 52 | sq_size=76/1000; %76mm 53 | end 54 | end 55 | [imgPts,boardPts]=projectGridPts(Xc,sq_num_x,sq_num_y); 56 | boardPts=boardPts*sq_size; 57 | 58 | [imgPts,good,bad,type] = cornerfinder(imgPts,im,winz,winz); 59 | 60 | [omc,T,R,H] = compute_extrinsic(imgPts,boardPts,[K(1,1);K(2,2)],[K(1,3);K(2,3)],kc,K(1,2)/K(1,1)); 61 | 62 | %N=-R(:,3) 63 | %dist=-R(:,3)'*T 64 | imgPts=imgPts(:,good); 65 | boardPts=boardPts(:,good); 66 | 67 | [projPts] = project_points2(boardPts,omc,T,[K(1,1);K(2,2)],[K(1,3);K(2,3)],kc,K(1,2)/K(1,1)); 68 | 69 | % figure(2); 70 | % imshow(im); 71 | % hold on; 72 | % plot(projPts(1,:),projPts(2,:),'r+'); 73 | % hold off; 74 | 75 | return 76 | 77 | 78 | %------------re-projected grid points of the checherboard---------------------------% 79 | 80 | function [gridPts,ptsCoord]=projectGridPts(cornerPts,sqNumX,sqNumY) 81 | 82 | [Xs,Ys]=meshgrid(1:(sqNumX+1),1:(sqNumY+1)) ; 83 | ptsCoord=[Xs(:)-1,Ys(:)-1,zeros((sqNumX+1)*(sqNumY+1),1)]'; 84 | 85 | Xs=(Xs(:)-1)/sqNumX+1; 86 | Ys=(Ys(:)-1)/sqNumY+1; 87 | 88 | Zx=[cornerPts(1,1) cornerPts(1,2); 89 | cornerPts(1,4) cornerPts(1,3)]; 90 | 91 | Zy=[cornerPts(2,1) cornerPts(2,2); 92 | cornerPts(2,4) cornerPts(2,3)]; 93 | 94 | gridPts(1,:)=interp2(Zx,Xs,Ys)'; 95 | gridPts(2,:)=interp2(Zy,Xs,Ys)'; 96 | 97 | return 98 | 99 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/getGridPoints.m: -------------------------------------------------------------------------------- 1 | % [gridPts,gridCoords]=getGridPoints(img, targets); 2 | % Detect grid points from the image and return their image coordinates and 3 | % word coordinates 4 | 5 | function [gridPts,gridCoords]=getGridPoints(img, targets); 6 | 7 | regImg=roipoly(img,[targets.center_x],[targets.center_y]); 8 | 9 | origImg=img; 10 | img=rgb2gray(img); 11 | regImg= uint8(double(regImg).*double(img)); 12 | 13 | 14 | [cim, r, c] = harris(regImg, 3, 10000, 3, 0); 15 | 16 | cornerNum=length(r); 17 | rawPts=[c';r';ones(1,cornerNum)]; 18 | 19 | cPts=[[targets.center_x];[targets.center_y]; ones(1,length(targets))]; 20 | cPts=cPts(:,[1:length(targets),1]); 21 | 22 | thrDist=5; 23 | for i=1:length(targets) 24 | line=cross(cPts(:,i),cPts(:,i+1)); line=line/norm(line(1:2)); 25 | dists=abs(line'*rawPts); 26 | rawPts=rawPts(:,dists>thrDist); 27 | end 28 | 29 | [gridPts,good,bad,type] = cornerfinder(rawPts(1:2,:),regImg,3,3); 30 | 31 | 32 | x1=cPts(1:3,1:end-1); 33 | x2=[0 0 1 1; 0 1 1 0; 1 1 1 1]; 34 | 35 | H = homography2d(x1, x2); 36 | 37 | gridPts=[gridPts;ones(1,length(gridPts))]; 38 | hgrids=H*gridPts; 39 | hgrids=hgrids./repmat(hgrids(3,:),[3,1]); 40 | 41 | [ccol,col] = clusterPts(hgrids(1,:)); colNum=length(col); 42 | [crow,row] = clusterPts(hgrids(2,:)); rowNum=length(row); 43 | 44 | gridCoords=zeros(size(hgrids)); 45 | 46 | for i=1:length(hgrids) 47 | hgrids(1,i)=col(ccol(i)); 48 | gridCoords(1,i)=colNum-ccol(i); 49 | hgrids(2,i)=row(crow(i)); 50 | gridCoords(2,i)=rowNum-crow(i); 51 | end 52 | 53 | [results,indx]=sortrows(hgrids(2:-1:1,:)'); 54 | 55 | gridPts=gridPts(1:2, indx(end:-1:1)); 56 | good=good(indx(end:-1:1)); 57 | gridPts=gridPts(:,find(good)); 58 | 59 | 60 | gridCoords=gridCoords(:, indx(end:-1:1)); 61 | gridCoords=gridCoords(:, find(good)); 62 | 63 | % figure 64 | % imshow(origImg); 65 | % hold on; 66 | % plot(gridPts(1,:),gridPts(2,:),'r.'); 67 | % for i=1:length(gridPts) 68 | % text(gridPts(1,i),gridPts(2,i),num2str(i),'Color','r') 69 | % end 70 | 71 | return 72 | 73 | %----------------------------------------------------------------% 74 | 75 | function [clusterIndx,kmc] = clusterPts(x) 76 | 77 | n =length(x); 78 | 79 | K=n; 80 | kmc=x; 81 | 82 | oldK=K; 83 | clusterIndx = zeros(1,n); 84 | D = zeros(K,n); 85 | 86 | while(1), 87 | 88 | if oldK~=K 89 | clusterIndx = zeros(1,n); 90 | D = zeros(K,n); 91 | end 92 | 93 | for j=1:K, % for every cluster 94 | D(j,:) = abs(x-kmc(j)); 95 | end 96 | 97 | % find mini kmcm 98 | [Dmin,index] = min(D); 99 | moved = sum(index~=clusterIndx); 100 | clusterIndx = index; 101 | 102 | for i=1:K 103 | ci=find(clusterIndx==i); 104 | kmc(i)=mean(x(ci)); 105 | end 106 | 107 | kmc=sort(kmc); 108 | nkmc=kmc(1); 109 | for i=1:K 110 | indx=length(nkmc); 111 | if abs(nkmc(end)-kmc(i))<0.05 112 | nkmc(end)=(nkmc(end)+kmc(i))/2; 113 | clusterIndx(find(clusterIndx==i))=indx; 114 | else 115 | nkmc=[nkmc,kmc(i)]; 116 | end 117 | end 118 | 119 | kmc=nkmc; 120 | 121 | oldK =K; 122 | K=length(kmc); 123 | 124 | if (moved==0), return, end 125 | 126 | end % while(1) 127 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/getLinePts.m: -------------------------------------------------------------------------------- 1 | % function [lpts, err]=getLinePts(Pts) 2 | % given that the checkerboard is placed right in front of the planar 3 | % laser rangefinder, extract the laser points hit on the checkerboard 4 | % input: Pts -- [1x 180] distance measurements in meter 5 | % output: lpts -- [2 x n] the laser points hit on the checkerboard plane 6 | % err -- average fitting error of the laser points to the checkerboard plane (actually teh fitting line) 7 | function [lpts, err]=getLinePts(Pts) 8 | 9 | %theta=[0:179]/180*pi; 10 | %theta=[0:0.25:180]/180*pi; 11 | theta=[-45:0.25:225]/180*pi; 12 | Pts = Pts/1000; 13 | numpoints = size(Pts, 2); 14 | 15 | z=Pts.*sin(theta); 16 | x=Pts.*cos(theta); 17 | 18 | % plot(x,z,'b-'); 19 | % for i=1:2 20 | % [xs(i),zs(i)]=ginput(1); 21 | % hold on;plot(xs,zs,'ro-');hold off; 22 | % end 23 | % xs=sort(xs); 24 | % valid=(x>=xs(1))&(x<=xs(2)); 25 | % 26 | % keyboard; 27 | 28 | % the following lines choose the parts of the laser that are on the 29 | % checkerboard. 30 | 31 | f=[-1,1]; 32 | diffL=conv(Pts,f); 33 | diffL=abs(diffL(2: (numpoints + 1))); 34 | 35 | 36 | 37 | for i=round(numpoints/2):-1:1 38 | if diffL(i)>0.25 39 | lowB=i+1; 40 | break; 41 | end 42 | end; 43 | 44 | for i=round(numpoints/2 + 1):1:numpoints 45 | if diffL(i)>0.25 46 | highB=i; 47 | break; 48 | end 49 | end; 50 | 51 | valid=lowB:highB; 52 | 53 | % alternatively, we can do this interactively. 54 | 55 | if 0 56 | figure(1); 57 | P = [x(:) z(:)]; 58 | plot(x(:),z(:),'b.'); hold on; 59 | plot(x(valid),z(valid),'r-'); hold off; 60 | axis([-10 10 0 10]); 61 | 62 | title(['select first (left click) and last (right click) points on' ... 63 | ' board, and click any key to continue.']); 64 | BUTTON = 1; 65 | while BUTTON < 4 66 | [X,Y,BUTTON] = GINPUT(1); 67 | if BUTTON < 4 68 | D = L2_distance([X Y]',P'); 69 | [jnk idx] = min(D); 70 | switch BUTTON 71 | case {1} 72 | highB = idx(1); 73 | otherwise 74 | lowB = idx(1); 75 | end 76 | valid=lowB:highB; 77 | plot(x(:),z(:),'b.'); hold on; 78 | plot(x(valid),z(valid),'r-'); hold off; 79 | axis([-10 10 0 10]); 80 | end 81 | end 82 | valid=lowB:highB; 83 | width=(highB-lowB+1); 84 | 85 | end % if interactive 86 | 87 | x=x(valid); 88 | z=z(valid); 89 | pts=Pts(valid); 90 | 91 | vtheta=theta(valid); 92 | 93 | coef=polyfit(x,z,1); 94 | 95 | x=coef(2)./(tan(vtheta)-coef(1)); 96 | z=tan(vtheta).*x; 97 | 98 | if (nargout>1) 99 | err=sum(abs(pts-sqrt(x.^2+z.^2)))/length(x); 100 | end 101 | 102 | z=Pts(valid).*sin(theta(valid)); 103 | %z=polyval(coef,x); 104 | 105 | lpts=[x;z]; 106 | 107 | figure(2); 108 | plot(Pts.*cos(theta),Pts.*sin(theta),x,z,'r.'); 109 | 110 | 111 | 112 | 113 | 114 | 115 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/getProjectionErr.m: -------------------------------------------------------------------------------- 1 | function [err]=getProjectionErr(Phi,Delta,Pts, Ns, Ds, Indx) 2 | 3 | num=length(Pts); 4 | if nargin<6 5 | Indx=1:num; 6 | end; 7 | 8 | R=Phi'; 9 | err=0; 10 | ptsCount=0; 11 | 12 | for i=Indx 13 | n=length(Pts{i}); 14 | pts=[Pts{i}(1,:);zeros(1,n);Pts{i}(2,:)]; 15 | T=-R*Delta; 16 | T=repmat(T,[1,n]); 17 | newPts=R*pts+T; %apply rotation and translation 18 | err=err+sum(abs(Ns{i}'*newPts-Ds{i})); 19 | ptsCount=ptsCount+n; 20 | 21 | end 22 | 23 | err=err/ptsCount; -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/getTransformMatrix.m: -------------------------------------------------------------------------------- 1 | %Function [Phi,Delta]=getTransformMatrix(Pts,Ns,Ds) 2 | %return transformation matrix H from laser coordinates to camera coordinates 3 | % Pts -- the laser points hitting on the checkboard. 4 | % n cells, each of which is 2xm matrix,recorresponding to m points (x,0,z) in laser coordinate system 5 | % Ns -- The normal of the planar suface of the checkerboard 6 | % 3 x n matrix --corresponding to the planes where Pts from 7 | % Ds -- The distance from camera center to the checkboard plane 8 | 9 | function [Phi,Delta]=getTransformMatrix(Pts, Ns, Ds, weights) 10 | 11 | if nargin<4 12 | weights=ones(1,size(Pts,2))'; 13 | end 14 | 15 | preH=getInitialTransformMatrix(Pts,Ns,Ds); 16 | 17 | preH(:,2)=preH(:,2)/norm(preH(:,2)); 18 | preH(:,1)=preH(:,1)/norm(preH(:,1)); 19 | 20 | estRot=[preH(:,1),-cross(preH(:,1),preH(:,2)),preH(:,2)]; 21 | [U,S,V] = svd(estRot); 22 | estRot = U*V'; 23 | 24 | initV(1:3)=rodrigues(estRot); 25 | initV(4:6)=preH(:,3); 26 | initV=initV'; 27 | 28 | vars.Pts=Pts; 29 | vars.Ns=Ns; 30 | vars.Ds=Ds; 31 | vars.weights=weights; 32 | 33 | ptsNum=0; 34 | for i=1:length(Pts) 35 | ptsNum=ptsNum+size(Pts{i},2); 36 | end 37 | %options = optimset('LevenbergMarquardt','on','LargeScale','off'); 38 | options = optimset('Algorithm','Levenberg-Marquardt','LargeScale','off'); 39 | V=fminunc(@estimateErr,initV,options,vars); 40 | 41 | Phi=rodrigues(V(1:3))'; 42 | Delta=-Phi*V(4:end); 43 | 44 | 45 | fval=estProjErr(Phi,Delta,Pts, Ns, Ds); 46 | disp(['Average distance error : ',num2str(fval), 'm.']); 47 | 48 | return 49 | 50 | %------------------------get initial H -------------------------------------% 51 | function H=getInitialTransformMatrix(Pts, Ns, Ds) 52 | 53 | num=length(Pts); 54 | exPts=[]; 55 | exNs=[]; 56 | exDs=[]; 57 | 58 | for i=1:num 59 | n=size(Pts{i},2); 60 | exPts=[exPts,Pts{i}]; 61 | exNs=[exNs,repmat(Ns{i},[1,n])]; 62 | exDs=[exDs,repmat(Ds{i},[1,n])]; 63 | disp([i size(exPts,2) size(exNs,2) size(exDs,2)]); 64 | end 65 | 66 | exPts=[exPts;ones(1,length(exPts))]; 67 | exPts=exPts'; 68 | 69 | exPts=repmat(exPts,[1,3]); 70 | Nx=repmat(exNs(1,:)',[1,3]); 71 | Ny=repmat(exNs(2,:)',[1,3]); 72 | Nz=repmat(exNs(3,:)',[1,3]); 73 | exNs=[Nx,Ny,Nz]; 74 | 75 | M=exPts.*exNs; 76 | h=M\exDs'; 77 | H=reshape(h,3,3)'; 78 | return 79 | %------------------------residual function-------------------------------------% 80 | 81 | function [err]=estimateErr(x,vars) 82 | 83 | Pts=vars.Pts; 84 | Ns=vars.Ns; 85 | Ds=vars.Ds; 86 | 87 | num=length(Pts); 88 | R=rodrigues(x(1:3)); %rotation matrix 89 | 90 | err=0; 91 | for i=1:num 92 | 93 | n=size(Pts{i},2); 94 | pts=[Pts{i}(1,:);zeros(1,n);Pts{i}(2,:)]; 95 | T=x(4:end); 96 | T=repmat(T,[1,n]); 97 | newPts=R*pts+T; %apply rotation and translation 98 | 99 | err=err+sum((Ns{i}'*newPts-Ds{i}).^2)*vars.weights(i); 100 | 101 | end 102 | 103 | %-----------------------projection error with estimated extrinsic parameters ------------------% 104 | function [err]=estProjErr(Phi,Delta,Pts, Ns, Ds, Indx) 105 | 106 | num=size(Pts,2); 107 | if nargin<6 108 | Indx=1:num; 109 | end; 110 | 111 | R=Phi'; 112 | err=0; 113 | ptsCount=0; 114 | 115 | for i=Indx 116 | n=size(Pts{i},2); 117 | pts=[Pts{i}(1,:);zeros(1,n);Pts{i}(2,:)]; 118 | T=-R*Delta; 119 | T=repmat(T,[1,n]); 120 | newPts=R*pts+T; %apply rotation and translation 121 | err=err+sum(abs(Ns{i}'*newPts-Ds{i})); 122 | ptsCount=ptsCount+n; 123 | 124 | end 125 | 126 | err=err/ptsCount; 127 | 128 | return 129 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/glbOptimize.m: -------------------------------------------------------------------------------- 1 | % Estimate camera position w.r.t laser range finder and do 2 | % global optimization on intrinsic and extrinsic camera parameters 3 | 4 | % K -- camera intrinic matrix; kc--camera radius and tangent distortion parameters 5 | 6 | function [Phi,Delta, newK, newkc]=glbOptimize(K,kc) 7 | 8 | if nargin<2 9 | kc=zeros(5,1); 10 | end; 11 | 12 | imgFiles=filesInPath('./','jpg'); % load image filenames in the path 13 | logFiles=filesInPath('./','log'); % load laser reading filenames in the path 14 | 15 | num=length(imgFiles); 16 | 17 | Pts=[];Ns=[];Ds=[]; 18 | 19 | for i=1:num 20 | disp(['Processing image ',num2str(i),' ...']); 21 | img=imread(imgFiles{i}); 22 | 23 | [extR,extT,gridPts,ptsCoord]=getCamExtParams(img,K,kc); 24 | Ns{i}=-extR(:,3); 25 | Ds{i}=-extR(:,3)'*extT; 26 | 27 | Rs{i}=extR; 28 | Ts{i}=extT; 29 | 30 | log=readlaserlog(logFiles{i}); 31 | [lpts,errs(i)]=getLinePts(log); % extract the leaser points hitting on the checkboard 32 | 33 | Pts{i}=lpts; 34 | imgPts{i}=gridPts; 35 | boardPts{i}=ptsCoord; 36 | end 37 | 38 | %--------------estimate camera extrinsic parameters relative to laser range finder -------% 39 | weights=(1-errs.^20/sum(errs.^20))*num/(num-1); 40 | [Phi,Delta]=getTransformMatrix(Pts, Ns, Ds,weights); 41 | 42 | %---------------do global optimization ---------------------------------% 43 | inx=getXfromParameters(Phi,Delta,K,kc,Rs,Ts); 44 | vars.imgPts=imgPts; 45 | vars.boardPts=boardPts; 46 | vars.Pts=Pts; 47 | 48 | options = optimset('LevenbergMarquardt','on','LargeScale','off','Display','iter'); 49 | x=fminunc(@estimateErr,inx,options,vars); 50 | 51 | [invPhi,invDelta,newK,newkc,newRs,newTs]=getParamfromX(x); 52 | 53 | Phi=invPhi'; 54 | Delta=-invPhi'*invDelta; 55 | 56 | 57 | return 58 | 59 | 60 | %-----------------------------------end of glbOptimize ----------------------------% 61 | 62 | function [err]=estimateErr(x,vars) 63 | 64 | imgPts=vars.imgPts; 65 | boardPts=vars.boardPts; 66 | Pts=vars.Pts; 67 | 68 | [invPhi,invDelta,K,kc,Rs,Ts]=getParamfromX(x); 69 | 70 | num=length(Pts); 71 | err=0; 72 | 73 | for i=1:num 74 | 75 | n=length(Pts{i}); 76 | pts=[Pts{i}(1,:);zeros(1,n);Pts{i}(2,:)]; 77 | 78 | extR=Rs{i}; 79 | extT=Ts{i}; 80 | 81 | N=-extR(:,3); 82 | D=-extR(:,3)'*extT; 83 | 84 | projPts=[extR,extT]*[boardPts{i};ones(1,length(boardPts{i}))]; 85 | projPts=projPts./repmat(projPts(3,:),[3,1]); 86 | 87 | %call r^2 = a^2 + b^2. 88 | %The distorted point coordinates are: xd = [xx;yy] where: 89 | % 90 | %xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); 91 | %yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; 92 | % 93 | %The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion 94 | % 95 | %Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: 96 | % 97 | %xxp = f(1)*(xx + alpha*yy) + c(1) 98 | %yyp = f(2)*yy + c(2) 99 | 100 | 101 | rdists=sum(projPts(1:2,:).^2); 102 | projPts(1,:)=projPts(1,:).*(1+kc(1)*rdists+kc(2)*rdists.^2); 103 | projPts(2,:)=projPts(2,:).*(1+kc(1)*rdists+kc(2)*rdists.^2); 104 | projPts=K*projPts; 105 | 106 | projErr=sum(sum((projPts(1:2,:)-imgPts{i}).^2))/length(imgPts{i}); 107 | 108 | delta=repmat(invDelta,[1,n]); 109 | newPts=invPhi*pts+delta; %apply rotation and translation 110 | 111 | err=err+10*sum((N'*newPts-D).^2)+projErr; 112 | 113 | end 114 | return 115 | 116 | 117 | %-----------------------------------------------------% 118 | function x=getXfromParameters(Phi,Delta,K,kc,Rs,Ts) 119 | 120 | x=[]; 121 | x(1:3)=rodrigues(Phi'); 122 | x(4:6)=-Phi'*Delta; 123 | 124 | x(7)=K(1,1);x(8)=K(2,2);x(9)=K(1,3);x(10)=K(2,3); 125 | x(7:10)=x(7:10)+0.25*randn(1,4); 126 | 127 | x(11:14)=kc(1:4)+0.001*randn(1,4); 128 | for i=1:length(Rs) 129 | x((15+6*(i-1)):(14+6*i))=[rodrigues(Rs{i});Ts{i}]; 130 | end 131 | 132 | x=x'; 133 | 134 | return 135 | 136 | %-------------------------------------------------------% 137 | function [invPhi,invDelta,K,kc,Rs,Ts]=getParamfromX(x) 138 | 139 | invPhi=rodrigues(x(1:3)); %rotation matrix 140 | invDelta=x(4:6); 141 | 142 | K=[x(7),0, x(9); 0, x(8), x(10); 0, 0, 1]; 143 | 144 | kc=zeros(5,1); 145 | 146 | kc(1:4)=x(11:14); 147 | for i=1:(length(x(15:end))/6) 148 | Rs{i}=rodrigues(x((15+6*(i-1)):(15+6*i-4))); 149 | Ts{i}=x((15+6*i-3):(15+6*i-1)); 150 | end 151 | 152 | return 153 | 154 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/harris.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | % HARRIS - Harris corner detector 4 | % 5 | % Usage: [cim, r, c] = harris(im, sigma, thresh, radius, disp) 6 | % 7 | % Arguments: 8 | % im - image to be processed. 9 | % sigma - standard deviation of smoothing Gaussian. Typical 10 | % values to use might be 1-3. 11 | % thresh - threshold (optional). Try a value ~1000. 12 | % radius - radius of region considered in non-maximal 13 | % suppression (optional). Typical values to use might 14 | % be 1-3. 15 | % disp - optional flag (0 or 1) indicating whether you want 16 | % to display corners overlayed on the original 17 | % image. This can be useful for parameter tuning. 18 | % 19 | % Returns: 20 | % cim - binary image marking corners. 21 | % r - row coordinates of corner points. 22 | % c - column coordinates of corner points. 23 | % 24 | % If thresh and radius are omitted from the argument list 'cim' is returned 25 | % as a raw corner strength image and r and c are returned empty. 26 | 27 | % Reference: 28 | % C.G. Harris and M.J. Stephens. "A combined corner and edge detector", 29 | % Proceedings Fourth Alvey Vision Conference, Manchester. 30 | % pp 147-151, 1988. 31 | % 32 | % Author: 33 | % Peter Kovesi 34 | % Department of Computer Science & Software Engineering 35 | % The University of Western Australia 36 | % pk@cs.uwa.edu.au www.cs.uwa.edu.au/~pk 37 | % 38 | % March 2002 39 | 40 | function [cim, r, c] = harris(im, sigma, thresh, radius, disp) 41 | 42 | error(nargchk(2,5,nargin)); 43 | 44 | dx = [-1 0 1; -1 0 1; -1 0 1]; % Derivative masks 45 | dy = dx'; 46 | 47 | Ix = conv2(im, dx, 'same'); % Image derivatives 48 | Iy = conv2(im, dy, 'same'); 49 | 50 | % Generate Gaussian filter of size 6*sigma (+/- 3sigma) and of 51 | % minimum size 1x1. 52 | g = fspecial('gaussian',max(1,fix(6*sigma)), sigma); 53 | 54 | Ix2 = conv2(Ix.^2, g, 'same'); % Smoothed squared image derivatives 55 | Iy2 = conv2(Iy.^2, g, 'same'); 56 | Ixy = conv2(Ix.*Iy, g, 'same'); 57 | 58 | cim = (Ix2.*Iy2 - Ixy.^2)./(Ix2 + Iy2 + eps); % Harris corner measure 59 | 60 | % Alternate Harris corner measure used by some. Suggested that 61 | % k=0.04 %- I find this a bit arbitrary and unsatisfactory. 62 | % cim = (Ix2.*Iy2 - Ixy.^2) - k*(Ix2 + Iy2).^2; 63 | 64 | if nargin > 2 % We should perform nonmaximal suppression and threshold 65 | 66 | % Extract local maxima by performing a grey scale morphological 67 | % dilation and then finding points in the corner strength image that 68 | % match the dilated image and are also greater than the threshold. 69 | sze = 2*radius+1; % Size of mask. 70 | mx = ordfilt2(cim,sze^2,ones(sze)); % Grey-scale dilate. 71 | cim = (cim==mx)&(cim>thresh); % Find maxima. 72 | 73 | [r,c] = find(cim); % Find row,col coords. 74 | 75 | if nargin==5 & disp % overlay corners on original image 76 | figure, imagesc(im), axis image, colormap(gray), hold on 77 | % axis off 78 | plot(c,r,'r+'), title('corners detected'); 79 | end 80 | 81 | else % leave cim as a corner strength image and make r and c empty. 82 | r = []; c = []; 83 | end 84 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/homography2d.m: -------------------------------------------------------------------------------- 1 | % HOMOGRAPHY2D - computes 2D homography 2 | % 3 | % Usage: H = homography2d(x1, x2) 4 | % 5 | % Arguments: 6 | % x1 - 3xN set of homogeneous points 7 | % x2 - 3xN set of homogeneous points such that x1<->x2 8 | % Returns: 9 | % H - the 3x3 homography such that x2 = H*x1 10 | % 11 | % This code follows the normalised direct linear transformation 12 | % algorithm given by Hartley and Zisserman p92. 13 | % 14 | 15 | % Peter Kovesi 16 | % School of Computer Science & Software Engineering 17 | % The University of Western Australia 18 | % pk@cs.uwa.edu.au www.cs.uwa.edu.au/~pk 19 | % May 2003 20 | 21 | function H = homography2d(x1, x2) 22 | 23 | % check matrix sizes 24 | if ~all(size(x1) == size(x2)) 25 | error('x1 and x2 must have same dimensions'); 26 | end 27 | 28 | % Attempt to normalise each set of points so that the origin 29 | % is at centroid and mean distance from origin is sqrt(2). 30 | [x1, T1] = normalise2dpts(x1); 31 | [x2, T2] = normalise2dpts(x2); 32 | 33 | % Note that it may have not been possible to normalise 34 | % the points if one was at infinity so the following does not 35 | % assume that scale parameter w = 1. 36 | 37 | Npts = length(x1); 38 | A = zeros(3*Npts,9); 39 | 40 | O = [0 0 0]; 41 | for n = 1:Npts 42 | X = x1(:,n)'; 43 | x = x2(1,n); y = x2(2,n); w = x2(3,n); 44 | A(3*n-2,:) = [ O -w*X y*X]; 45 | A(3*n-1,:) = [ w*X O -x*X]; 46 | A(3*n ,:) = [-y*X x*X O ]; 47 | end 48 | 49 | [U,D,V] = svd(A); 50 | 51 | % Extract homography 52 | H = reshape(V(:,9),3,3)'; 53 | 54 | % Denormalise 55 | H = T2\H*T1; 56 | 57 | 58 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/mapLaserPts.m: -------------------------------------------------------------------------------- 1 | % function mapLaserPts(img,log,phi,delta,K,kc) 2 | % project the laser reading onto the camera image with given camera pose and intrinsic paramaters 3 | % img - camera image 4 | % log - laser reading 5 | % phi, delta - camera orientation and position 6 | % K, kc - camera internal matrix and distortion paramaters 7 | 8 | function mapLaserPts(img,log,phi,delta,K,kc) 9 | 10 | if nargin<5 11 | kc=zeros(5,1); 12 | end; 13 | 14 | [height,width]=size(img); 15 | 16 | %theta=0:179; 17 | %theta=theta/180*pi; 18 | %theta=[0:0.25:180]/180*pi; 19 | theta=[-45:0.25:225]/180*pi; 20 | 21 | log = log./1000; 22 | 23 | x=log.*cos(theta); 24 | z=log.*sin(theta); 25 | 26 | phi=phi'; 27 | delta=-phi*delta; 28 | 29 | Pts=[x;zeros(1,length(theta));z]; 30 | t=repmat(delta,[1,length(log)]); 31 | p=phi*Pts+t; 32 | 33 | %p=p(:,300:400); 34 | 35 | % p=p./repmat(p(3,:),[3,1]); 36 | % 37 | % r=sum(p(1:2,:).^2); 38 | % p(1,:)=p(1,:).*( 1+kc(1)*r+kc(2)*r.^2+kc(5)*r.^3 )+ 2*kc(3)*p(1,:).*p(2,:) + kc(4)*(r + 2*p(1,:).^2);; 39 | % p(2,:)=p(2,:).*( 1+kc(1)*r+kc(2)*r.^2+kc(5)*r.^3 )+ kc(3)*(r + 2*p(2,:).^2) + 2*kc(4)*p(1,:).*p(2,:);; 40 | % pts=K*p; 41 | 42 | 43 | [pts] = project_points2(p,zeros(3,1),zeros(3,1),[K(1,1);K(2,2)],[K(1,3);K(2,3)],kc,K(1,2)/K(1,1)); 44 | 45 | valid=(pts(1,:)0)&(pts(2,:)0); 46 | 47 | figure,imshow(img); 48 | hold on, plot(pts(1,valid),pts(2,valid),'r.','MarkerSize',20); 49 | hold off; 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/mapping.m: -------------------------------------------------------------------------------- 1 | % function mapping(idx,Phi,Delta,K,kc); 2 | % project the laser reading onto the camera image with given camera pose and intrinsic paramaters 3 | % idx - index of the image and laser rangding in the data sequence. 4 | 5 | function mapping(idx,Phi,Delta,K,kc); 6 | 7 | if nargin<5; 8 | kc=zeros(5,1); 9 | end 10 | 11 | imgFiles=filesInPath('./','jpg'); % load image filenames in the path 12 | logFiles=filesInPath('./','log'); % load laser reading filenames in the path 13 | 14 | img=imread(imgFiles{idx}); 15 | log=readLaserLog(logFiles{idx}); 16 | mapLaserPts(img,log,Phi,Delta,K,kc); 17 | 18 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/normalise2dpts.m: -------------------------------------------------------------------------------- 1 | % NORMALISE2DPTS - normalises 2D homogeneous points 2 | % 3 | % Function translates and normalises a set of 2D homogeneous points 4 | % so that their centroid is at the origin and their mean distance from 5 | % the origin is sqrt(2). This process typically improves the 6 | % conditioning of any equations used to solve homographies, fundamental 7 | % matrices etc. 8 | % 9 | % Usage: [newpts, T] = normalise2dpts(pts) 10 | % 11 | % Argument: 12 | % pts - 3xN array of 2D homogeneous coordinates 13 | % 14 | % Returns: 15 | % newpts - 3xN array of transformed 2D homogeneous coordinates 16 | % T - The 3x3 transformation matrix, newpts = T*pts 17 | % 18 | % Note that if one of the points is at infinity no normalisation 19 | % is possible. In this case a warning is printed and pts is 20 | % returned as newpts and T is the identity matrix. 21 | 22 | % Peter Kovesi 23 | % School of Computer Science & Software Engineering 24 | % The University of Western Australia 25 | % pk@cs.uwa.edu.au www.cs.uwa.edu.au/~pk 26 | % May 2003 27 | 28 | 29 | function [newpts, T] = normalise2dpts(pts) 30 | 31 | if ~all(pts(3,:)) 32 | 33 | warning('Attempt to normalise a point at infinity') 34 | newpts = pts; 35 | T = eye(3); 36 | return; 37 | end 38 | 39 | % Ensure homogeneous coords have scale of 1 40 | pts(1,:) = pts(1,:)./pts(3,:); 41 | pts(2,:) = pts(2,:)./pts(3,:); 42 | 43 | c = mean(pts(1:2,:)')'; % Centroid. 44 | newp(1,:) = pts(1,:)-c(1); % Shift origin to centroid. 45 | newp(2,:) = pts(2,:)-c(2); 46 | 47 | meandist = mean(sqrt(newp(1,:).^2 + newp(2,:).^2)); 48 | 49 | scale = sqrt(2)/meandist; 50 | 51 | T = [scale 0 -scale*c(1) 52 | 0 scale -scale*c(2) 53 | 0 0 1 ]; 54 | 55 | newpts = T*pts; 56 | 57 | 58 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/readLaserLog.m: -------------------------------------------------------------------------------- 1 | function Data=readLaserLog(filename) 2 | 3 | fid = fopen(filename); 4 | Data = fscanf(fid,'%f',[1 inf]); 5 | fclose(fid); 6 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/readme.txt: -------------------------------------------------------------------------------- 1 | Extrinsix calibration toolbox for a camera and laser range finder 2 | 3 | Place a big checkerboard in front of the system of a camera and laser 4 | range finder. Make sure: 5 | 6 | 1) the entire checkerboard is visible for the camera and 7 | 2) the center (!) laser range finder point hits the checkerboard. 8 | - this is because we compute the intersection of the laser plane and the checkerboard by starting at the center point and going out until we find a discontinuity. 9 | 10 | 3) capture the data simultaneously from both the camera and the laser range finder in the different checkerboard poses. 11 | 4) Put all image and laser data in the current working directory. image 12 | data is JPEG file and laser data is postfixed as '.log'. 13 | 14 | 5) this toolbox works with (and requires) camera calibration toolbox from 15 | http://www.vision.caltech.edu/bouguetj/calib_doc/ 16 | 6) that toolbox (usually called "TOOLBOX_calib", must be in your matlab path. 17 | 7) The main call is to "extCalib", you must call it with the K,kc parameters (the camera intrinsic calibration and curvature correction), from the calibration toolbox above. 18 | 19 | 20 | %%%%%%%%%%%%%%%%%%%% Example execution: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | This readme file is in a directory includes a subdirectory "Sample". 23 | 24 | change your matlab working directory to that ("cd sample" if this is your current directory). 25 | 26 | copy the definition of K,kc from the txt file "camera_intrinsics.txt" into the matlab command window. 27 | 28 | type: 29 | 30 | [Phi,Delta] = extCalib(K,kc); 31 | 32 | ... it will ask you for the dimensions of the squares in the calibration grid, the sample data comes from a grid with squares 76mm (the default), so just press return. 33 | 34 | 35 | Run time on a mid-range laptop is much less than a minute. 36 | 37 | 38 | Good luck! 39 | Robert Pless (working with Qilong Zhang). 40 | 41 | 42 | -------------------------------------------------------------------------------- /Camera_2DLaser/manualCalib/zOlderGetLinePts.m: -------------------------------------------------------------------------------- 1 | % function [lpts, err]=getLinePts(Pts) 2 | % given that the checkerboard is placed right in front of the planar 3 | % laser rangefinder, extract the laser points hit on the checkerboard 4 | % input: Pts -- [1x 180] distance measurements in meter 5 | % output: lpts -- [2 x n] the laser points hit on the checkerboard plane 6 | % err -- average fitting error of the laser points to the checkerboard plane (actually teh fitting line) 7 | function [lpts, err]=getLinePts(Pts) 8 | 9 | theta=[0:179]/180*pi; 10 | z=Pts.*sin(theta); 11 | x=Pts.*cos(theta); 12 | 13 | % plot(x,z,'b-'); 14 | % for i=1:2 15 | % [xs(i),zs(i)]=ginput(1); 16 | % hold on;plot(xs,zs,'ro-');hold off; 17 | % end 18 | % xs=sort(xs); 19 | % valid=(x>=xs(1))&(x<=xs(2)); 20 | % 21 | % keyboard; 22 | 23 | % the following lines choose the parts of the laser that are on the 24 | % checkerboard. 25 | 26 | f=[-1,1]; 27 | diffL=conv(Pts,f); 28 | diffL=abs(diffL(2:181)); 29 | 30 | for i=95:-1:1 31 | if diffL(i)>0.25 32 | lowB=i+1; 33 | break; 34 | end 35 | end; 36 | 37 | for i=96:1:180 38 | if diffL(i)>0.25 39 | highB=i; 40 | break; 41 | end 42 | end; 43 | 44 | valid=lowB:highB; 45 | 46 | % alternatively, we can do this interactively. 47 | 48 | figure(1); 49 | P = [x(:) z(:)]; 50 | plot(x(:),z(:),'b.'); hold on; 51 | plot(x(valid),z(valid),'r-'); hold off; 52 | axis([-10 10 0 10]); 53 | 54 | title(['select first (left click) and last (right click) points on' ... 55 | ' board, and click any key to continue.']); 56 | BUTTON = 1; 57 | while BUTTON < 4 58 | [X,Y,BUTTON] = GINPUT(1); 59 | if BUTTON < 4 60 | D = L2_distance([X Y]',P'); 61 | [jnk idx] = min(D); 62 | switch BUTTON 63 | case {1} 64 | highB = idx(1); 65 | otherwise 66 | lowB = idx(1); 67 | end 68 | valid=lowB:highB; 69 | plot(x(:),z(:),'b.'); hold on; 70 | plot(x(valid),z(valid),'r-'); hold off; 71 | axis([-10 10 0 10]); 72 | end 73 | end 74 | 75 | valid=lowB:highB; 76 | width=(highB-lowB+1); 77 | 78 | x=x(valid); 79 | z=z(valid); 80 | pts=Pts(valid); 81 | 82 | vtheta=theta(valid); 83 | 84 | coef=polyfit(x,z,1); 85 | 86 | x=coef(2)./(tan(vtheta)-coef(1)); 87 | z=tan(vtheta).*x; 88 | 89 | if (nargout>1) 90 | err=sum(abs(pts-sqrt(x.^2+z.^2)))/length(x); 91 | end 92 | 93 | z=Pts(valid).*sin(theta(valid)); 94 | %z=polyval(coef,x); 95 | 96 | lpts=[x;z]; 97 | 98 | figure(2); 99 | plot(Pts.*cos(theta),Pts.*sin(theta),x,z,'r.'); 100 | 101 | 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /GlobalCS_Camera/Calib_Results.m: -------------------------------------------------------------------------------- 1 | % Intrinsic and Extrinsic Camera Parameters 2 | % 3 | % This script file can be directly excecuted under Matlab to recover the camera intrinsic and extrinsic parameters. 4 | % IMPORTANT: This file contains neither the structure of the calibration objects nor the image coordinates of the calibration points. 5 | % All those complementary variables are saved in the complete matlab data file Calib_Results.mat. 6 | % For more information regarding the calibration model visit http://www.vision.caltech.edu/bouguetj/calib_doc/ 7 | 8 | 9 | %-- Focal length: 10 | fc = [ 808.143359087797650 ; 803.722815217184120 ]; 11 | 12 | %-- Principal point: 13 | cc = [ 806.922767448549730 ; 595.368936470950760 ]; 14 | 15 | %-- Skew coefficient: 16 | alpha_c = 0.000000000000000; 17 | 18 | %-- Distortion coefficients: 19 | kc = [ 0.015194690568994 ; -0.002315564936138 ; 0.000047447414640 ; 0.001332436234638 ; 0.000000000000000 ]; 20 | 21 | %-- Focal length uncertainty: 22 | fc_error = [ 20.812502964581171 ; 20.534491948864972 ]; 23 | 24 | %-- Principal point uncertainty: 25 | cc_error = [ 19.018560711382317 ; 18.660601497873184 ]; 26 | 27 | %-- Skew coefficient uncertainty: 28 | alpha_c_error = 0.000000000000000; 29 | 30 | %-- Distortion coefficients uncertainty: 31 | kc_error = [ 0.015540234387041 ; 0.005005191930111 ; 0.005458592022633 ; 0.005811261135820 ; 0.000000000000000 ]; 32 | 33 | %-- Image size: 34 | nx = 1626; 35 | ny = 1236; 36 | 37 | 38 | %-- Various other variables (may be ignored if you do not use the Matlab Calibration Toolbox): 39 | %-- Those variables are used to control which intrinsic parameters should be optimized 40 | 41 | n_ima = 42; % Number of calibration images 42 | est_fc = [ 1 ; 1 ]; % Estimation indicator of the two focal variables 43 | est_aspect_ratio = 1; % Estimation indicator of the aspect ratio fc(2)/fc(1) 44 | center_optim = 1; % Estimation indicator of the principal point 45 | est_alpha = 0; % Estimation indicator of the skew coefficient 46 | est_dist = [ 1 ; 1 ; 1 ; 1 ; 0 ]; % Estimation indicator of the distortion coefficients 47 | 48 | 49 | %-- Extrinsic parameters: 50 | %-- The rotation (omc_kk) and the translation (Tc_kk) vectors for every calibration image and their uncertainties 51 | 52 | %-- Image #1: 53 | omc_1 = [ 2.050099e+00 ; 1.620767e+00 ; 8.984588e-01 ]; 54 | Tc_1 = [ -1.055528e+03 ; -6.558372e+02 ; 1.700018e+03 ]; 55 | omc_error_1 = [ 2.118085e-02 ; 2.379435e-02 ; 4.128709e-02 ]; 56 | Tc_error_1 = [ 4.758255e+01 ; 4.471117e+01 ; 5.749922e+01 ]; 57 | 58 | %-- Image #2: 59 | omc_2 = [ 2.197496e+00 ; 2.090952e+00 ; 2.025620e-01 ]; 60 | Tc_2 = [ -5.701800e+02 ; -9.435103e+02 ; 1.561346e+03 ]; 61 | omc_error_2 = [ 1.143642e-02 ; 2.327769e-02 ; 3.860535e-02 ]; 62 | Tc_error_2 = [ 4.054791e+01 ; 3.912022e+01 ; 5.140301e+01 ]; 63 | 64 | %-- Image #3: 65 | omc_3 = [ -2.343356e+00 ; -1.786749e+00 ; -2.436156e-01 ]; 66 | Tc_3 = [ 2.738887e+02 ; -8.978982e+02 ; 1.532202e+03 ]; 67 | omc_error_3 = [ 2.185359e-02 ; 2.036159e-02 ; 3.615196e-02 ]; 68 | Tc_error_3 = [ 3.893594e+01 ; 3.815546e+01 ; 4.770423e+01 ]; 69 | 70 | %-- Image #4: 71 | omc_4 = [ -1.552243e-01 ; -3.037986e+00 ; -4.712282e-01 ]; 72 | Tc_4 = [ 9.759739e+02 ; -7.930151e+02 ; 1.779064e+03 ]; 73 | omc_error_4 = [ 1.070218e-02 ; 2.767835e-02 ; 3.653257e-02 ]; 74 | Tc_error_4 = [ 4.481104e+01 ; 4.594204e+01 ; 5.909559e+01 ]; 75 | 76 | %-- Image #5: 77 | omc_5 = [ 4.362600e-03 ; 2.898180e+00 ; 1.097475e+00 ]; 78 | Tc_5 = [ 6.563179e+02 ; -5.638455e+02 ; 1.796399e+03 ]; 79 | omc_error_5 = [ 1.132456e-02 ; 2.539100e-02 ; 3.712160e-02 ]; 80 | Tc_error_5 = [ 4.407820e+01 ; 4.406037e+01 ; 5.248838e+01 ]; 81 | 82 | %-- Image #6: 83 | omc_6 = [ 1.892132e+00 ; -2.187205e+00 ; 2.185497e-01 ]; 84 | Tc_6 = [ 6.385433e+02 ; -3.125614e+02 ; 1.838261e+03 ]; 85 | omc_error_6 = [ 1.080145e-02 ; 2.328471e-02 ; 3.650084e-02 ]; 86 | Tc_error_6 = [ 4.446618e+01 ; 4.392863e+01 ; 5.127716e+01 ]; 87 | 88 | %-- Image #7: 89 | omc_7 = [ 2.156103e+00 ; -2.303778e+00 ; -2.205177e-01 ]; 90 | Tc_7 = [ 1.460529e+03 ; -5.644999e+02 ; 1.860511e+03 ]; 91 | omc_error_7 = [ 1.192499e-02 ; 2.468026e-02 ; 4.459769e-02 ]; 92 | Tc_error_7 = [ 4.882245e+01 ; 4.955440e+01 ; 6.533731e+01 ]; 93 | 94 | %-- Image #8: 95 | omc_8 = [ -2.184727e+00 ; 2.159517e+00 ; 2.369678e-01 ]; 96 | Tc_8 = [ 1.336415e+03 ; -6.478523e+02 ; 1.858282e+03 ]; 97 | omc_error_8 = [ 2.372868e-02 ; 8.867071e-03 ; 4.452032e-02 ]; 98 | Tc_error_8 = [ 4.820160e+01 ; 4.896434e+01 ; 6.358249e+01 ]; 99 | 100 | %-- Image #9: 101 | omc_9 = [ -3.169693e+00 ; 6.404049e-02 ; 1.531393e-02 ]; 102 | Tc_9 = [ 1.888326e+03 ; 3.538879e+02 ; 2.558784e+03 ]; 103 | omc_error_9 = [ 2.174184e-02 ; 1.346647e-02 ; 4.672716e-02 ]; 104 | Tc_error_9 = [ 6.737901e+01 ; 6.598934e+01 ; 8.457727e+01 ]; 105 | 106 | %-- Image #10: 107 | omc_10 = [ -1.461277e+00 ; -2.066289e+00 ; 7.086774e-01 ]; 108 | Tc_10 = [ 1.499351e+03 ; 7.817901e+02 ; 2.021338e+03 ]; 109 | omc_error_10 = [ 1.600172e-02 ; 4.183999e-02 ; 4.857715e-02 ]; 110 | Tc_error_10 = [ 5.472375e+01 ; 5.303126e+01 ; 7.306598e+01 ]; 111 | 112 | %-- Image #11: 113 | omc_11 = [ -2.758811e-01 ; -2.801913e+00 ; 9.986627e-01 ]; 114 | Tc_11 = [ 1.378317e+03 ; 8.792693e+02 ; 2.032736e+03 ]; 115 | omc_error_11 = [ 1.556222e-02 ; 3.229746e-02 ; 4.480095e-02 ]; 116 | Tc_error_11 = [ 5.413454e+01 ; 5.335328e+01 ; 6.997050e+01 ]; 117 | 118 | %-- Image #12: 119 | omc_12 = [ 1.574040e+00 ; -2.161334e+00 ; 3.807451e-01 ]; 120 | Tc_12 = [ 1.465260e+03 ; 9.957477e+02 ; 2.010265e+03 ]; 121 | omc_error_12 = [ 1.822142e-02 ; 3.292969e-02 ; 3.738414e-02 ]; 122 | Tc_error_12 = [ 5.431769e+01 ; 5.370006e+01 ; 7.379437e+01 ]; 123 | 124 | %-- Image #13: 125 | omc_13 = [ 2.328456e+00 ; -1.961080e+00 ; -3.457400e-02 ]; 126 | Tc_13 = [ 7.613431e+02 ; 1.468675e+03 ; 2.145107e+03 ]; 127 | omc_error_13 = [ 2.588715e-02 ; 1.771594e-02 ; 3.385850e-02 ]; 128 | Tc_error_13 = [ 5.659458e+01 ; 5.585994e+01 ; 7.129415e+01 ]; 129 | 130 | %-- Image #14: 131 | omc_14 = [ -1.010437e+00 ; 2.735438e+00 ; -1.124500e+00 ]; 132 | Tc_14 = [ 9.080959e+01 ; 1.155660e+03 ; 2.082843e+03 ]; 133 | omc_error_14 = [ 1.177122e-02 ; 3.100107e-02 ; 4.408423e-02 ]; 134 | Tc_error_14 = [ 5.231173e+01 ; 5.197012e+01 ; 6.004838e+01 ]; 135 | 136 | %-- Image #15: 137 | omc_15 = [ 2.567319e+00 ; -1.008105e+00 ; 7.768687e-01 ]; 138 | Tc_15 = [ -3.136454e+02 ; 1.004068e+03 ; 2.062221e+03 ]; 139 | omc_error_15 = [ 2.916086e-02 ; 1.487529e-02 ; 4.475280e-02 ]; 140 | Tc_error_15 = [ 5.137902e+01 ; 5.048626e+01 ; 6.006509e+01 ]; 141 | 142 | %-- Image #16: 143 | omc_16 = [ 1.899294e+00 ; 1.413959e+00 ; 5.810917e-01 ]; 144 | Tc_16 = [ -1.249944e+03 ; 1.042025e+03 ; 1.948965e+03 ]; 145 | omc_error_16 = [ 2.728346e-02 ; 1.790123e-02 ; 3.559751e-02 ]; 146 | Tc_error_16 = [ 5.362255e+01 ; 5.230570e+01 ; 7.070333e+01 ]; 147 | 148 | %-- Image #17: 149 | omc_17 = [ 1.953538e+00 ; 1.647528e+00 ; 5.612612e-01 ]; 150 | Tc_17 = [ -1.485765e+03 ; 8.250420e+02 ; 2.018558e+03 ]; 151 | omc_error_17 = [ 3.026111e-02 ; 2.644191e-02 ; 4.954306e-02 ]; 152 | Tc_error_17 = [ 5.636877e+01 ; 5.444095e+01 ; 7.585581e+01 ]; 153 | 154 | %-- Image #18: 155 | omc_18 = [ 2.051121e+00 ; 2.355331e+00 ; -6.528109e-02 ]; 156 | Tc_18 = [ -1.474334e+03 ; 2.780656e+02 ; 1.873818e+03 ]; 157 | omc_error_18 = [ 1.643107e-02 ; 2.426802e-02 ; 3.672761e-02 ]; 158 | Tc_error_18 = [ 5.077990e+01 ; 4.951211e+01 ; 6.235323e+01 ]; 159 | 160 | %-- Image #19: 161 | omc_19 = [ -1.339581e+00 ; 2.694858e+00 ; -4.118229e-01 ]; 162 | Tc_19 = [ -1.138217e+03 ; -2.422353e+02 ; 1.938492e+03 ]; 163 | omc_error_19 = [ 1.898653e-02 ; 2.344220e-02 ; 3.742657e-02 ]; 164 | Tc_error_19 = [ 5.059505e+01 ; 4.823268e+01 ; 5.863266e+01 ]; 165 | 166 | %-- Image #20: 167 | omc_20 = [ -2.658509e-01 ; 3.136552e+00 ; 8.439147e-02 ]; 168 | Tc_20 = [ -7.479187e+02 ; -2.769684e+02 ; 1.839202e+03 ]; 169 | omc_error_20 = [ 9.344944e-03 ; 2.825663e-02 ; 3.495837e-02 ]; 170 | Tc_error_20 = [ 4.641823e+01 ; 4.432763e+01 ; 5.142711e+01 ]; 171 | 172 | %-- Image #21: 173 | omc_21 = [ 1.071180e+00 ; -2.924838e+00 ; -5.082420e-03 ]; 174 | Tc_21 = [ 7.376275e+02 ; -8.288943e+01 ; 1.791630e+03 ]; 175 | omc_error_21 = [ 9.328994e-03 ; 2.635793e-02 ; 3.610851e-02 ]; 176 | Tc_error_21 = [ 4.283941e+01 ; 4.293026e+01 ; 5.044063e+01 ]; 177 | 178 | %-- Image #22: 179 | omc_22 = [ -1.879829e+00 ; 2.478960e+00 ; -1.692703e-03 ]; 180 | Tc_22 = [ 7.381542e+02 ; 2.908873e+01 ; 1.797388e+03 ]; 181 | omc_error_22 = [ 1.813574e-02 ; 1.842022e-02 ; 3.655386e-02 ]; 182 | Tc_error_22 = [ 4.290928e+01 ; 4.306955e+01 ; 5.037777e+01 ]; 183 | 184 | %-- Image #23: 185 | omc_23 = [ -5.636452e-02 ; -3.061401e+00 ; 3.604008e-01 ]; 186 | Tc_23 = [ 7.972410e+02 ; -4.549687e+02 ; 1.862606e+03 ]; 187 | omc_error_23 = [ 1.022143e-02 ; 2.591891e-02 ; 3.581324e-02 ]; 188 | Tc_error_23 = [ 4.597741e+01 ; 4.488243e+01 ; 5.335051e+01 ]; 189 | 190 | %-- Image #24: 191 | omc_24 = [ 8.396558e-01 ; 2.837495e+00 ; 9.034256e-01 ]; 192 | Tc_24 = [ 5.603400e+02 ; -6.694648e+02 ; 1.605784e+03 ]; 193 | omc_error_24 = [ 1.416757e-02 ; 2.331207e-02 ; 3.321507e-02 ]; 194 | Tc_error_24 = [ 3.953388e+01 ; 4.051553e+01 ; 4.880957e+01 ]; 195 | 196 | %-- Image #25: 197 | omc_25 = [ 7.111139e-02 ; -3.105927e+00 ; 5.107115e-02 ]; 198 | Tc_25 = [ 5.080465e+02 ; 4.638250e+02 ; 1.814312e+03 ]; 199 | omc_error_25 = [ 7.562019e-03 ; 2.505767e-02 ; 3.921716e-02 ]; 200 | Tc_error_25 = [ 4.374199e+01 ; 4.375056e+01 ; 5.001151e+01 ]; 201 | 202 | %-- Image #26: 203 | omc_26 = [ -1.314163e+00 ; -2.784504e+00 ; 1.468504e-01 ]; 204 | Tc_26 = [ 3.777240e+02 ; 4.461936e+02 ; 1.829222e+03 ]; 205 | omc_error_26 = [ 6.114226e-03 ; 2.557576e-02 ; 4.268335e-02 ]; 206 | Tc_error_26 = [ 4.403659e+01 ; 4.380123e+01 ; 4.905696e+01 ]; 207 | 208 | %-- Image #27: 209 | omc_27 = [ NaN ; NaN ; NaN ]; 210 | Tc_27 = [ NaN ; NaN ; NaN ]; 211 | omc_error_27 = [ NaN ; NaN ; NaN ]; 212 | Tc_error_27 = [ NaN ; NaN ; NaN ]; 213 | 214 | %-- Image #28: 215 | omc_28 = [ NaN ; NaN ; NaN ]; 216 | Tc_28 = [ NaN ; NaN ; NaN ]; 217 | omc_error_28 = [ NaN ; NaN ; NaN ]; 218 | Tc_error_28 = [ NaN ; NaN ; NaN ]; 219 | 220 | %-- Image #29: 221 | omc_29 = [ NaN ; NaN ; NaN ]; 222 | Tc_29 = [ NaN ; NaN ; NaN ]; 223 | omc_error_29 = [ NaN ; NaN ; NaN ]; 224 | Tc_error_29 = [ NaN ; NaN ; NaN ]; 225 | 226 | %-- Image #30: 227 | omc_30 = [ -3.095552e+00 ; 3.010500e-01 ; 2.475460e-01 ]; 228 | Tc_30 = [ 3.100097e+02 ; 7.407856e+02 ; 1.847740e+03 ]; 229 | omc_error_30 = [ 2.554800e-02 ; 7.468855e-03 ; 3.775143e-02 ]; 230 | Tc_error_30 = [ 4.534016e+01 ; 4.422083e+01 ; 5.111211e+01 ]; 231 | 232 | %-- Image #31: 233 | omc_31 = [ -2.688491e+00 ; -4.816281e-01 ; 2.050825e-01 ]; 234 | Tc_31 = [ 1.923025e+02 ; 6.069615e+02 ; 1.940110e+03 ]; 235 | omc_error_31 = [ 2.240459e-02 ; 1.165849e-02 ; 3.243171e-02 ]; 236 | Tc_error_31 = [ 4.693810e+01 ; 4.591532e+01 ; 5.044564e+01 ]; 237 | 238 | %-- Image #32: 239 | omc_32 = [ 2.207366e+00 ; -2.337351e-01 ; -2.245971e-01 ]; 240 | Tc_32 = [ 1.155270e+02 ; 1.019692e+03 ; 1.757484e+03 ]; 241 | omc_error_32 = [ 2.608838e-02 ; 6.821797e-03 ; 3.210088e-02 ]; 242 | Tc_error_32 = [ 4.444956e+01 ; 4.421094e+01 ; 5.425099e+01 ]; 243 | 244 | %-- Image #33: 245 | omc_33 = [ 2.667678e+00 ; -4.675307e-01 ; -1.057148e-01 ]; 246 | Tc_33 = [ 1.510590e+02 ; 9.124810e+02 ; 1.863580e+03 ]; 247 | omc_error_33 = [ 4.697853e-02 ; 1.806759e-02 ; 8.225828e-02 ]; 248 | Tc_error_33 = [ 4.615869e+01 ; 4.596574e+01 ; 5.436113e+01 ]; 249 | 250 | %-- Image #34: 251 | omc_34 = [ NaN ; NaN ; NaN ]; 252 | Tc_34 = [ NaN ; NaN ; NaN ]; 253 | omc_error_34 = [ NaN ; NaN ; NaN ]; 254 | Tc_error_34 = [ NaN ; NaN ; NaN ]; 255 | 256 | %-- Image #35: 257 | omc_35 = [ -1.449940e+00 ; 1.822325e+00 ; -6.583723e-01 ]; 258 | Tc_35 = [ -9.411699e+02 ; -5.789024e+02 ; 1.859779e+03 ]; 259 | omc_error_35 = [ 2.015580e-02 ; 1.845254e-02 ; 3.458091e-02 ]; 260 | Tc_error_35 = [ 5.026059e+01 ; 4.745121e+01 ; 5.536657e+01 ]; 261 | 262 | %-- Image #36: 263 | omc_36 = [ 2.854519e+00 ; 1.332583e+00 ; 2.578614e-02 ]; 264 | Tc_36 = [ -1.308371e+03 ; 1.283290e+03 ; 2.723823e+03 ]; 265 | omc_error_36 = [ 2.320683e-02 ; 1.763132e-02 ; 3.961824e-02 ]; 266 | Tc_error_36 = [ 7.049159e+01 ; 6.993369e+01 ; 8.534622e+01 ]; 267 | 268 | %-- Image #37: 269 | omc_37 = [ NaN ; NaN ; NaN ]; 270 | Tc_37 = [ NaN ; NaN ; NaN ]; 271 | omc_error_37 = [ NaN ; NaN ; NaN ]; 272 | Tc_error_37 = [ NaN ; NaN ; NaN ]; 273 | 274 | %-- Image #38: 275 | omc_38 = [ 1.865654e+00 ; 9.962327e-01 ; 5.826874e-01 ]; 276 | Tc_38 = [ -2.120456e+03 ; 1.287432e+03 ; 2.419886e+03 ]; 277 | omc_error_38 = [ 2.839238e-02 ; 1.852487e-02 ; 3.729584e-02 ]; 278 | Tc_error_38 = [ 7.164077e+01 ; 6.939522e+01 ; 9.251563e+01 ]; 279 | 280 | %-- Image #39: 281 | omc_39 = [ 2.259133e+00 ; 1.569681e+00 ; 3.100819e-01 ]; 282 | Tc_39 = [ -2.131917e+03 ; 4.417286e+02 ; 2.568345e+03 ]; 283 | omc_error_39 = [ 3.166500e-02 ; 3.176082e-02 ; 5.010557e-02 ]; 284 | Tc_error_39 = [ 7.081412e+01 ; 6.868075e+01 ; 8.996423e+01 ]; 285 | 286 | %-- Image #40: 287 | omc_40 = [ 2.117311e+00 ; 1.828874e+00 ; 3.810718e-01 ]; 288 | Tc_40 = [ -2.143544e+03 ; -1.267015e+01 ; 2.576211e+03 ]; 289 | omc_error_40 = [ 2.811985e-02 ; 3.538183e-02 ; 5.219681e-02 ]; 290 | Tc_error_40 = [ 7.102577e+01 ; 6.840100e+01 ; 8.945058e+01 ]; 291 | 292 | %-- Image #41: 293 | omc_41 = [ 1.948124e+00 ; 2.118736e+00 ; -2.360182e-01 ]; 294 | Tc_41 = [ -1.575157e+03 ; -4.626057e+02 ; 1.924476e+03 ]; 295 | omc_error_41 = [ 7.016203e-03 ; 2.576667e-02 ; 3.632739e-02 ]; 296 | Tc_error_41 = [ 5.221226e+01 ; 5.110523e+01 ; 6.751973e+01 ]; 297 | 298 | %-- Image #42: 299 | omc_42 = [ 1.948164e+00 ; 2.118328e+00 ; -2.364870e-01 ]; 300 | Tc_42 = [ -1.574883e+03 ; -4.624930e+02 ; 1.924003e+03 ]; 301 | omc_error_42 = [ 7.011544e-03 ; 2.576510e-02 ; 3.632351e-02 ]; 302 | Tc_error_42 = [ 5.219934e+01 ; 5.109360e+01 ; 6.750641e+01 ]; 303 | 304 | -------------------------------------------------------------------------------- /GlobalCS_Camera/finalEstimateQuat.txt: -------------------------------------------------------------------------------- 1 | 8.1290317e-01 1.0050934e-01 -1.1883932e-02 -5.7353734e-01 2 | -------------------------------------------------------------------------------- /GlobalCS_Camera/finalEstimateRodrigues.txt: -------------------------------------------------------------------------------- 1 | -2.1457562e-01 2 | 2.5370798e-02 3 | 1.2244348e+00 4 | -------------------------------------------------------------------------------- /GlobalCS_Camera/funkcjaMin.m: -------------------------------------------------------------------------------- 1 | function F = funkcjaMin( X ) 2 | % Global equations 3 | global Rhs; 4 | global Lhs; 5 | % Recalculation to rotation matrix 6 | W = rodrigues(X); 7 | % Equation error 8 | F = abs(Rhs - W * Lhs); 9 | end 10 | 11 | -------------------------------------------------------------------------------- /GlobalCS_Camera/globalCalibration.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | clc; 3 | close all; 4 | 5 | global Rhs; 6 | global Lhs; 7 | 8 | % IMU 2 chess = chess 2 IMU 9 | chess2imu = [ -1 0 0 ; 0 1 0; 0 0 -1]; 10 | 11 | % Loading data from IMU 12 | xsense = load('xSense.mat'); 13 | counter = xsense.XsenseWork(:,1); 14 | accData = xsense.XsenseWork(:,3:5); 15 | magData = xsense.XsenseWork(:,13:15); 16 | 17 | % Loading positions of the images 18 | Calib_Results 19 | 20 | % Names of the rodrigues angles of chessboard positions 21 | omc = {omc_1, omc_2, omc_3, omc_4, omc_5, omc_6, omc_7, omc_8, omc_9, ... 22 | omc_10, omc_11, omc_12, omc_13, omc_14, omc_15, omc_16, omc_17, omc_18, ... 23 | omc_19, omc_20, omc_21, omc_22, omc_23, omc_24, omc_25, omc_26, omc_27, ... 24 | omc_28, omc_29, omc_30, omc_31, omc_32, omc_33, omc_34, omc_35, omc_36, ... 25 | omc_37, omc_38, omc_39, omc_40, omc_41, omc_42}; 26 | 27 | % For some of them, the chessboard was not properly detected 28 | forbidden = [27, 28, 29, 34, 37]; 29 | 30 | Rhs = []; 31 | Lhs = []; 32 | for i = 1:42 33 | 34 | if ( sum(forbidden == i) == 0 ) % if we have a rotation estimate 35 | % chess board in global cs 36 | Rc = rodrigues(omc{i}); 37 | 38 | % The indices of the imu data to be avg 39 | zakres = 1:15 + (i-1)*15; 40 | 41 | % Let's take avg -> '-' because Android's Z is pointed up in the air 42 | Z = - mean(accData(zakres,:)); 43 | Z = Z / norm(Z); % we want to know the direction 44 | % Similar -> avg of magnetometer treated as Android's Y 45 | Y = mean(magData(zakres,:)); 46 | % To have a proper coordinate system, the magnetometer vector has 47 | % to defined in a plane perpendicular to the gravity vector -> 48 | % projection onto the plane 49 | Proj = [Z;Z;Z]; % making magnetic vector perpendicular to gravity 50 | Y = Y' - Proj*Y'; % removing part in gravity direction 51 | Y = Y' / norm(Y); 52 | % The X axis comes from cross product 53 | X = cross(Y,Z); 54 | X = X / norm(X); 55 | 56 | % This the rotation of GLOBAL coordinate system in the IMU cs 57 | Rimu = [X' Y' Z']; 58 | 59 | % Global in camera system -> chess in camera system * imu in chess 60 | % system * global in imu system 61 | globalInCamera = Rc * chess2imu * Rimu; 62 | 63 | % Solve system of equations: X * (CS of camera) = Global in camera 64 | Rhs = [Rhs globalInCamera]; 65 | Lhs = [Lhs [1 0 0; 0 1 0; 0 0 1]]; 66 | 67 | % Lets minimize using Levenberg-Marquardt with initial estimate 68 | % equal to identity matrix 69 | % The optimization is done in rodrigues angles to avoid defining 70 | % additional constaints as it would have to using quaternions or 71 | % rotation matrices 72 | X = lsqnonlin(@funkcjaMin, rodrigues(eye(3)) ); 73 | % resulting rotation matrix 74 | fprintf('Estimate after %d pairs\n', i); 75 | rodrigues(X) 76 | end; 77 | end; 78 | 79 | fprintf('\n\n\n\t\tFinal estimation:\n\n\n'); 80 | % Final reoptimization with parameters allowing to optimize it better 81 | X = lsqnonlin(@funkcjaMin, rodrigues(eye(3)) , [], [], optimset('Algorithm', 'levenberg-marquardt', 'TolFun', 1e-40,'Tolx', 1e-40, 'MaxFunEvals', 2000) ); 82 | % resulting rotation matrix 83 | rodrigues(X) 84 | 85 | % Saving the estimate in rodrigues 86 | save('finalEstimateRodrigues.txt','X', '-ascii') 87 | % Saving the estimate in quat 88 | X = dcm2quat(rodrigues(X)); 89 | save('finalEstimateQuat.txt','X', '-ascii') 90 | -------------------------------------------------------------------------------- /GlobalCS_Camera/xSense.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MichalNowicki/CalibrationCodes/37657f0edc3b7832c6d60e9b7cbf8b182d054cab/GlobalCS_Camera/xSense.mat -------------------------------------------------------------------------------- /IMU_Camera/Calib_Results.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MichalNowicki/CalibrationCodes/37657f0edc3b7832c6d60e9b7cbf8b182d054cab/IMU_Camera/Calib_Results.m -------------------------------------------------------------------------------- /IMU_Camera/arrow3.m: -------------------------------------------------------------------------------- 1 | function H=arrow3(p1,p2,s,w,h,ip,alpha,beta) 2 | % ARROW3 3 | % ARROW3(P1,P2) will draw vector lines (2D/3D) from P1 to P2 with 4 | % arrowheads, where P1 and P2 can be either nx2 matrices (for 2D), 5 | % or nx3 matrices (for 3D). 6 | % 7 | % ARROW3(P1,P2,S,W,H,IP,ALPHA,BETA) can be used to specify properties 8 | % of the line and arrowhead. S is a character string made with one 9 | % element from any or all of the following 3 columns: 10 | % 11 | % Color Switches LineStyle LineWidth 12 | % ------------------ ------------------- -------------------- 13 | % k black (default) - solid (default) 0.5 points (default) 14 | % y yellow : dotted 0 no lines 15 | % m magenta -. dashdot / LineWidthOrder 16 | % c cyan -- dashed 17 | % r red * LineStyleOrder 18 | % g green 19 | % b blue 20 | % w white __________ __ 21 | % a apple green ^ | 22 | % d dark gray / \ | 23 | % e evergreen / \ | 24 | % f fuchsia / \ | 25 | % h honey / \ Height 26 | % i indigo / \ | 27 | % j jade / \ | 28 | % l lilac / \ | 29 | % n nutbrown /______ ______\ __|__ 30 | % p pink | | | | 31 | % q kumquat |---- Width ----| 32 | % s sky blue | | | | 33 | % t tan | | 34 | % u umber | | 35 | % v violet | | 36 | % z zinc | | 37 | % x random named color | | 38 | % o ColorOrder 39 | % 40 | % The components of S may be specified in any order. Invalid 41 | % characters in S will be ignored and replaced by default settings. 42 | % 43 | % Prefixing the color code with '_' produces a darker shade, e.g. 44 | % '_t' is dark tan; prefixing the color code with '^' produces a 45 | % lighter shade, e.g. '^q' is light kumquat. The relative 46 | % brightness of light and dark color shades is controled by the 47 | % scalar parameter BETA. Color code prefixes do not affect the 48 | % basic colors (kymcrgbw) or the special color switches (xo). 49 | % 50 | % ColorOrder may be achieved in two fashions: The user may either 51 | % set the ColorOrder property (using RGB triples) or define the 52 | % global variable ColorOrder (using a string of valid color codes). 53 | % If the color switch is specified with 'o', and the global variable 54 | % ColorOrder is a string of color codes (color switches less 'x' and 55 | % 'o', optionally prefixed with '_' or '^'), then the ColorOrder 56 | % property will be set to the sequence of colors indicated by the 57 | % ColorOrder variable. If the color switch is specified with 'o', 58 | % and the global variable ColorOrder is empty or invalid, then the 59 | % current ColorOrder property will be used. Note that the ColorOrder 60 | % variable takes precedence over the ColorOrder property. 61 | % 62 | % The current LineStyleOrder property will be used if LineStyle is 63 | % specified with '*'. MATLAB cycles through the line styles defined 64 | % by the LineStyleOrder property only after using all colors defined 65 | % by the ColorOrder property. If however, the global variable 66 | % LineWidthOrder is defined, and LineWidth is specified with '/', 67 | % then each line will be drawn with sequential color, linestyle, and 68 | % linewidth. 69 | % 70 | % W [default = 1/72 of the PlotBox (PositionRectangle) diagonal for 71 | % linear (log) plots] is a vector of arrowhead widths. For linear 72 | % plots with equal axes, the units of W are the same as those of the 73 | % coordinate data (P1,P2). For linear plots with unequal axes, the 74 | % units of W are scaled to fit as if the axes were equal. For log 75 | % plots, the units of W are 1/72 of the PositionRectangle diagonal. 76 | % H (default = 3W) is a vector of arrowhead heights. If vector IP is 77 | % neither empty nor negative, initial point markers will be plotted 78 | % with diameter IP; for default diameter W, use IP = 0. 79 | % 80 | % ALPHA (default = 1) is a vector of FaceAlpha values ranging between 81 | % 0 (clear) and 1 (opaque). FaceAlpha is a surface (arrowhead and 82 | % initial point marker) property and does not affect lines. BETA 83 | % (default = 0.4) is a scalar that controls the relative brightness 84 | % of light and dark color shades, ranging between 0 (no contrast) and 85 | % 1 (maximum contrast). 86 | % 87 | % Plotting lines with a single color, linestyle, and linewidth is 88 | % faster than plotting lines with multiple colors and/or linestyles. 89 | % Plotting lines with multiple linewidths is slower still. 90 | % 91 | % H = ARROW3(P1,P2,...) returns a vector of handles to line and 92 | % surface objects created by ARROW3. 93 | % 94 | % ARROW3 COLORS will plot a table of named colors with default 95 | % brightness. ARROW3('colors',BETA) will plot a table of named 96 | % colors with brightness BETA. 97 | % 98 | % If a particular aspect ratio is required, use DASPECT or PBASPECT 99 | % before calling ARROW3. Changing DASPECT or PBASPECT after calling 100 | % ARROW3 may alter the appearance of arrowheads and initial point 101 | % markers. ARROW3 sets DataAspectRatioMode to manual for linear 102 | % plots and sets PlotBoxAspectRatioMode to manual for log plots. 103 | % 104 | % ARROW3 UPDATE will restore the the appearance of arrowheads and 105 | % initial point markers that have become corrupted by changes to 106 | % limits or aspect ratios. 107 | % 108 | % Usage Examples: 109 | % 110 | % % 2D vectors 111 | % Arrow3([0 0],[1 3]) 112 | % Arrow3([0 0],[1 2],'-.e') 113 | % Arrow3([0 0],[10 10],'--x2',1) 114 | % Arrow3(zeros(10,2),50*rand(10,2),'x',1,3) 115 | % Arrow3(zeros(10,2),[10*rand(10,1),500*rand(10,1)],'u') 116 | % Arrow3(10*rand(10,2),50*rand(10,2),'x',1,[],1) 117 | % 118 | % % 3D vectors 119 | % Arrow3([0 0 0],[1 1 1]) 120 | % Arrow3(zeros(20,3),50*rand(20,3),'--x1.5',2) 121 | % Arrow3(zeros(100,3),50*rand(100,3),'x',1,3) 122 | % Arrow3(zeros(10,3),[10*rand(10,1),500*rand(10,1),50*rand(10,1)],'a') 123 | % Arrow3(10*rand(10,3),50*rand(10,3),'x',[],[],0) 124 | % 125 | % % Just for fun 126 | % Arrow3(zeros(100,3),50*rand(100,3),'x',10,3,[],0.95) 127 | % light('Position',[-10 -10 -10],'Style','local') 128 | % light('Position',[60,60,60]), lighting gouraud 129 | % 130 | % % ColorOrder variable, color code prefixes, and Beta 131 | % global ColorOrder, ColorOrder='^ui^e_hq^v'; 132 | % theta=[0:pi/22:pi/2]'; 133 | % Arrow3(zeros(12,2),[cos(theta),sin(theta)],'1.5o',0.1,[],[],[],0.5) 134 | % 135 | % % ColorOrder property, LineStyleOrder, LineWidthOrder, and Alpha 136 | % global ColorOrder, ColorOrder=[]; 137 | % set(gca,'ColorOrder',[1,0,0;0,0,1;0.25,0.75,0.25;0,0,0]) 138 | % set(gca,'LineStyleOrder',{'-','--','-.',':'}) 139 | % global LineWidthOrder, LineWidthOrder=[1,2,4,8]; 140 | % w=[5,10,15,20]; h=[20,40,30,40]; alpha=[1,1,0.2,0.2]; 141 | % Arrow3(zeros(4,2),[10*rand(4,1),500*rand(4,1)],'o*/',w,h,10,alpha) 142 | % 143 | % % Log plot 144 | % loglog([1e2,1e8],[1e-2,1e-1],'wo','MarkerSize',eps), hold on 145 | % p1=repmat([1e3,2e-2],15,1); 146 | % q1=[1e7,1e6,1e5,1e4,1e3,1e7,1e7,1e7,1e7,1e7,1e7,1e6,1e5,1e4,1e3]; 147 | % q2=1e-2*[9,9,9,9,9,7,5,4,3,2,1,1,1,1,1]; p2=[q1',q2']; 148 | % global ColorOrder, ColorOrder=[]; 149 | % set(gca,'ColorOrder',rand(15,3)) 150 | % Arrow3(p1,p2,'o'), grid on, hold off 151 | % 152 | % % Color tables 153 | % Arrow3('colors') % default color table 154 | % Arrow3('colors',0.3) % low contrast color table 155 | % Arrow3('colors',0.5) % high contrast color table 156 | 157 | % Copyright(c)2002, Version 4.51 158 | % Jeff Chang 159 | % Tom Davis 160 | 161 | % Revision History: 162 | % 163 | % 12/26/02 - Added UserData and UPDATE option (TD) 164 | % 11/16/02 - Added more named colors, color code prefix, 165 | % global ColorOrder, ALPHA , and BETA (TD) 166 | % 10/12/02 - Added global LineWidthOrder, 167 | % vectorized W, H and IP (TD) 168 | % 10/05/02 - Changed CLF to CLA for subplot support, 169 | % added ColorOrder and LineStyleOrder support (TD) 170 | % 04/27/02 - Minor log plot revisions (TD) 171 | % 03/26/02 - Added log plot support (TD) 172 | % 03/24/02 - Adaptive grid spacing control to trade off 173 | % appearance vs. speed based on size of matrix (JC) 174 | % 03/16/02 - Added "axis tight" for improved appearance (JC) 175 | % 03/12/02 - Added initial point marker (TD) 176 | % 03/03/02 - Added aspect ratio support (TD) 177 | % 03/02/02 - Enchance program's user friendliness (JC) 178 | % (lump Color, LineStyle, and LineWidth together) 179 | % 03/01/02 - Replaced call to ROTATE (TD) 180 | % 02/28/02 - Modified line plotting, 181 | % added linewidth and linestyle (TD) 182 | % 02/27/02 - Minor enhancements on 3D appearance (JC) 183 | % 02/26/02 - Minor enhancements for speed (TD&JC) 184 | % 02/26/02 - Optimise PLOT3 and SURF for speed (TD) 185 | % 02/25/02 - Return handler, error handling, color effect, 186 | % generalize for 2D/3D vectors (JC) 187 | % 02/24/02 - Optimise PLOT3 and SURF for speed (TD) 188 | % 02/23/02 - First release (JC&TD) 189 | 190 | %========================================================================== 191 | % Error Checking 192 | if nargin<8 | isempty(beta), beta=0.4; end, beta=abs(beta(1)); 193 | if strcmpi(p1,'colors') % plot color table 194 | if nargin>1, beta=abs(p2(1)); end 195 | LocalColorTable(1,beta); return 196 | end 197 | fig=gcf; ax=gca; global Update; 198 | if strcmpi(p1,'update'), ud=get(ax,'UserData'); % update surfaces 199 | if size(ud,2)<13, error('Invalid UserData'), end 200 | Update=1; LocalUpdate(fig,ax,ud); return 201 | end 202 | InputError=['Invalid input, type HELP ',upper(mfilename),... 203 | ' for usage examples']; 204 | if nargin<2, error(InputError), end 205 | [r1,c1]=size(p1); [r2,c2]=size(p2); n=r1; 206 | if c1<2 | c1>3, error(InputError), end 207 | if r1~=r2, error('P1 and P2 must have same number of rows'), end 208 | if c1~=c2, error('P1 and P2 must have same number of columns'), end 209 | if c1==2, p1=[p1,zeros(n,1)]; p2=[p2,zeros(n,1)]; end 210 | L=get(ax,'LineStyleOrder'); C=get(ax,'ColorOrder'); 211 | ST=get(ax,'DefaultSurfaceTag'); LT=get(ax,'DefaultLineTag'); 212 | EC=get(ax,'DefaultSurfaceEdgeColor'); 213 | if strcmp(get(ax,'nextplot'),'add') & strcmp(get(fig,'nextplot'),'add') 214 | Xr=get(ax,'xlim'); Yr=get(ax,'ylim'); Zr=get(ax,'zlim'); 215 | set(ax,'XLimMode','auto','YLimMode','auto','ZLimMode','auto'); 216 | xs=strcmp(get(ax,'xscale'),'log'); 217 | ys=strcmp(get(ax,'yscale'),'log'); 218 | zs=strcmp(get(ax,'zscale'),'log'); 219 | if zs, error('Z log scale not supported'), end 220 | xys=xs+ys; restore=1; 221 | if xys & c1==3 & any([p1(:,c1);p2(:,c1)]~=0) 222 | error('3D log plot not supported') 223 | end 224 | else, restore=0; cla; view(c1); xys=0; 225 | set(ax,'nextplot','add'); set(fig,'nextplot','add'); 226 | end 227 | 228 | %========================================================================== 229 | % Style Control 230 | [vc,cn]=LocalColorTable(0); prefix=''; OneColor=0; 231 | if nargin<3, [c,ls,lw]=LocalValidateCLSW; % default Color, LineStyle/Width 232 | else, 233 | [c,ls,lw]=LocalValidateCLSW(s); 234 | if length(c)>1, if sum('_^'==c(1)), prefix=c(1); end, c=c(2); end 235 | if c=='x', c=cn(randperm(23),:); % random named color (less white) 236 | elseif c=='o', global ColorOrder % ColorOrder 237 | if length(ColorOrder) 238 | [c,failed]=LocalColorMap(lower(ColorOrder),vc,cn,beta); 239 | if failed, warning(['Invalid ColorOrder variable, ',... 240 | 'current ColorOrder property will be used']) 241 | else, C=c; end 242 | end, c=C; 243 | elseif ~sum(vc==c), c='k'; warning(['Invalid color switch, ',... 244 | 'default color (black) will be used']) 245 | end 246 | end 247 | if length(c)==1 % single color 248 | c=LocalColorMap([prefix,c],vc,cn,beta); OneColor=1; 249 | end 250 | set(ax,'ColorOrder',c); c=repmat(c,ceil(n/size(c,1)),1); 251 | if ls~='*', set(ax,'LineStyleOrder',ls); end % LineStyleOrder 252 | if lw=='/', global LineWidthOrder % LineWidthOrder 253 | if length(LineWidthOrder) 254 | lw=repmat(LineWidthOrder(:),ceil(n/length(LineWidthOrder)),1); 255 | else, lw=0.5; warning(['Undefined LineWidthOrder, ',... 256 | 'default width (0.5) will be used']) 257 | end 258 | end 259 | if nargin<7 | isempty(alpha), alpha=1; end 260 | a=repmat(alpha(:),ceil(n/length(alpha)),1); % FaceAlpha 261 | 262 | %========================================================================== 263 | % Log Plot 264 | if xys 265 | units=get(ax,'units'); set(ax,'units','points'); 266 | pos=get(ax,'position'); set(ax,'units',units); 267 | if strcmp(get(ax,'PlotBoxAspectRatioMode'),'auto') 268 | set(ax,'PlotBoxAspectRatio',[pos(3),pos(4),1]); 269 | end 270 | par=get(ax,'PlotBoxAspectRatio'); 271 | set(ax,'DataAspectRatio',[par(2),par(1),par(3)]); 272 | % map coordinates onto unit square 273 | q=[p1;p2]; xr=Xr; yr=Yr; 274 | if xs, xr=log10(xr); q(:,1)=log10(q(:,1)); end 275 | if ys, yr=log10(yr); q(:,2)=log10(q(:,2)); end 276 | q=q-repmat([xr(1),yr(1),0],2*n,1); 277 | dx=xr(2)-xr(1); dy=yr(2)-yr(1); 278 | q=q*diag([1/dx,1/dy,1]); 279 | q1=q(1:n,:); q2=q(n+1:end,:); 280 | else, xs=0; ys=0; dx=0; dy=0; xr=0; yr=0; end 281 | 282 | %========================================================================== 283 | % Line 284 | set(ax,'DefaultLineTag','arrow3'); 285 | if length(lw)==1 286 | if lw>0 287 | if OneColor & ls~='*' % single color, linestyle, and linewidth 288 | P=zeros(3*n,3); i=1:n; 289 | P(3*i-2,:)=p1(i,:); P(3*i-1,:)=p2(i,:); P(3*i,1)=NaN; 290 | H1=plot3(P(:,1),P(:,2),P(:,3),'LineWidth',lw); 291 | else % single linewidth 292 | H1=plot3([p1(:,1),p2(:,1)]',[p1(:,2),p2(:,2)]',... 293 | [p1(:,3),p2(:,3)]','LineWidth',lw); 294 | end 295 | else, H1=[]; end 296 | else % use LineWidthOrder 297 | ls=repmat(cellstr(L),ceil(n/size(L,1)),1); 298 | H1=zeros(n,1); 299 | for i=1:n 300 | H1(i)=plot3([p1(i,1),p2(i,1)],[p1(i,2),p2(i,2)],[p1(i,3),p2(i,3)],... 301 | ls{i},'Color',c(i,:),'LineWidth',lw(i)); 302 | end 303 | end 304 | 305 | %========================================================================== 306 | % Scale 307 | ar=get(ax,'DataAspectRatio'); ar=sqrt(3)*ar/norm(ar); 308 | set(ax,'DataAspectRatioMode','manual'); 309 | if nargin<4 | isempty(w) % width 310 | if xys, w=1; 311 | else, xr=get(ax,'xlim'); yr=get(ax,'ylim'); zr=get(ax,'zlim'); 312 | w=norm([xr(2)-xr(1),yr(2)-yr(1),zr(2)-zr(1)])/72; 313 | end 314 | end 315 | w=repmat(w(:),ceil(n/length(w)),1); 316 | if nargin<5 | isempty(h), h=3*w; end % height 317 | h=repmat(h(:),ceil(n/length(h)),1); 318 | if nargin>5 & ~isempty(ip) % ip 319 | i=find(ip==0); ip(i)=w(i); 320 | ip=repmat(ip(:),ceil(n/length(ip)),1); 321 | else, ip=-ones(n,1); end 322 | 323 | %========================================================================== 324 | % UserData 325 | if isempty(Update) 326 | set(ax,'UserData',[get(ax,'UserData'); 327 | p1,p2,c(1:n,:),w(1:n),h(1:n),ip(1:n),a]); 328 | end, Update=[]; 329 | 330 | %========================================================================== 331 | % Arrowhead 332 | if xys, whip=[w,h,ip]*sqrt(2)/72; 333 | w=whip(:,1); h=whip(:,2); ip=whip(:,3); p1=q1; p2=q2; 334 | end 335 | W=(p1-p2)./repmat(ar,n,1); 336 | W=W./repmat(sqrt(sum(W.*W,2)),1,3); % new z direction 337 | U=[-W(:,2),W(:,1),zeros(n,1)]; 338 | N=sqrt(sum(U.*U,2)); i=find(N num 346 | 347 | set(ax,'DefaultSurfaceTag','arrow3','DefaultSurfaceEdgeColor','none'); 348 | [x,y,z]=cylinder([0,1],m); 349 | G=surface(x/2,y/2,z); dar=diag(ar); 350 | X=get(G,'XData'); Y=get(G,'YData'); Z=get(G,'ZData'); 351 | H2=zeros(n,1); [j,k]=size(X); 352 | for i=1:n % translate, rotate, and scale 353 | H2(i)=copyobj(G,ax); 354 | xyz=[w(i)*X(:),w(i)*Y(:),h(i)*Z(:)]*[U(i,:);V(i,:);W(i,:)]*dar; 355 | x=reshape(xyz(:,1),j,k)+p2(i,1); 356 | y=reshape(xyz(:,2),j,k)+p2(i,2); 357 | z=reshape(xyz(:,3),j,k)+p2(i,3); 358 | LocalSetSurface(xys,xs,ys,dx,dy,xr,yr,x,y,z,a(i),c(i,:),H2(i)); 359 | end, delete(G); 360 | 361 | %========================================================================== 362 | % Initial Point Marker 363 | if any(ip>0) 364 | [x,y,z]=sphere(m); 365 | G=surface(x*ar(1)/2,y*ar(2)/2,z*ar(3)/2); 366 | X=get(G,'XData'); Y=get(G,'YData'); Z=get(G,'ZData'); 367 | H3=zeros(n,1); 368 | for i=1:n % translate 369 | if ip(i)>0 370 | H3(i)=copyobj(G,ax); 371 | x=p1(i,1)+X*ip(i); y=p1(i,2)+Y*ip(i); z=p1(i,3)+Z*ip(i); 372 | LocalSetSurface(xys,xs,ys,dx,dy,xr,yr,x,y,z,a(i),c(i,:),H3(i)); 373 | end 374 | end, delete(G); 375 | else, H3=[]; end 376 | 377 | %========================================================================== 378 | % Finish 379 | if xys, set(ax,'DataAspectRatioMode','auto'); 380 | xr=Xr; yr=Yr; zr=Zr; 381 | else, set(fig,'Renderer','OpenGL'); 382 | w=max(w)*ar/2; ip=max([ip;0])*ar/2; 383 | xr=[min([p1(:,1)-ip(1);p2(:,1)-w(1)]),max([p1(:,1)+ip(1);p2(:,1)+w(1)])]; 384 | yr=[min([p1(:,2)-ip(2);p2(:,2)-w(2)]),max([p1(:,2)+ip(2);p2(:,2)+w(2)])]; 385 | zr=[min([p1(:,3)-ip(3);p2(:,3)-w(3)]),max([p1(:,3)+ip(3);p2(:,3)+w(3)])]; 386 | if restore 387 | xr=[min(xr(1),Xr(1)),max(xr(2),Xr(2))]; 388 | yr=[min(yr(1),Yr(1)),max(yr(2),Yr(2))]; 389 | zr=[min(zr(1),Zr(1)),max(zr(2),Zr(2))]; 390 | else, set(ax,'nextplot','replace'); end 391 | end 392 | set(ax,'LineStyleOrder',L,'ColorOrder',C,'DefaultLineTag',LT,... 393 | 'DefaultSurfaceTag',ST,'DefaultSurfaceEdgeColor',EC,... 394 | 'xlim',xr,'ylim',yr,'zlim',zr); 395 | if c1==3, set(ax,'CameraViewAngle',get(ax,'CameraViewAngle'),... 396 | 'PlotBoxAspectRatio',get(ax,'PlotBoxAspectRatio')); 397 | end 398 | if nargout, H=[H1(:);H2(:);H3(:)]; end 399 | 400 | %========================================================================== 401 | % Update 402 | function LocalUpdate(fig,ax,ud) 403 | p1=ud(:,1:3); p2=ud(:,4:6); c=ud(:,7:9); 404 | w=ud(:,10); h=ud(:,11); ip=ud(:,12); a=ud(:,13); 405 | H=get(ax,'children'); tag=get(H,'tag'); type=get(H,'type'); 406 | delete(H(strcmp(tag,'arrow3') & strcmp(type,'surface'))); 407 | set(ax,'ColorOrder',c,'nextplot','add'); set(fig,'nextplot','add'); 408 | arrow3(p1,p2,'o0',w,h,ip,a); 409 | 410 | %========================================================================== 411 | % SetSurface 412 | function LocalSetSurface(xys,xs,ys,dx,dy,xr,yr,x,y,z,a,c,H) 413 | if xys 414 | x=x*dx+xr(1); y=y*dy+yr(1); 415 | if xs, x=10.^x; end 416 | if ys, y=10.^y; end 417 | end 418 | set(H,'XData',x,'YData',y,'ZData',z,'FaceAlpha',a,'FaceColor',c); 419 | 420 | %========================================================================== 421 | % ColorTable 422 | function [vc,cn]=LocalColorTable(n,beta) 423 | vc='kymcrgbadefhijlnpqstuvzw'; % valid color codes 424 | % k y m c 425 | cn=[0.00,0.00,0.00; 1.00,1.00,0.00; 1.00,0.00,1.00; 0.00,1.00,1.00; 426 | % r g b a 427 | 1.00,0.00,0.00; 0.00,1.00,0.00; 0.00,0.00,1.00; 0.00,0.70,0.00; 428 | % d e f h 429 | 0.40,0.40,0.40; 0.00,0.40,0.00; 0.90,0.00,0.40; 1.00,0.80,0.00; 430 | % i j l n 431 | 0.00,0.00,0.70; 0.20,0.80,0.50; 0.80,0.40,0.80; 0.50,0.20,0.00; 432 | % p q s t 433 | 1.00,0.40,0.60; 1.00,0.40,0.00; 0.00,0.80,1.00; 0.80,0.40,0.00; 434 | % u v z w 435 | 0.70,0.00,0.00; 0.60,0.00,1.00; 0.60,0.60,0.60; 1.00,1.00,1.00;]; 436 | if n % plot color table 437 | name={'black','yellow','magenta','cyan',... 438 | 'red','green','blue','apple green',... 439 | 'dark gray','evergreen','fuchsia','honey',... 440 | 'indigo','jade','lilac','nutbrown',... 441 | 'pink','kumquat','sky blue','tan',... 442 | 'umber','violet','zinc','white'}; 443 | c=['yhtn';'gjae';'csbi';'plmv';'frqu';'wzdk']; 444 | clf, set(gcf,'DefaultAxesXTick',[],'DefaultAxesYTick',[],... 445 | 'DefaultAxesXTickLabel',[],'DefaultAxesYTickLabel',[],... 446 | 'DefaultAxesXLim',[0,0.75],'DefaultAxesYLim',[0,0.75],... 447 | 'DefaultRectangleEdgeColor','none'); 448 | for i=1:24 449 | subplot(4,6,i); j=find(vc==c(i)); title(name{j}); 450 | dark=LocalBrighten(cn(j,:),-beta); 451 | light=LocalBrighten(cn(j,:),beta); 452 | rectangle('Position',[0,0.00,0.75,0.25],'FaceColor',dark); 453 | rectangle('Position',[0,0.25,0.75,0.25],'FaceColor',cn(j,:)); 454 | rectangle('Position',[0,0.50,0.75,0.25],'FaceColor',light); 455 | rectangle('Position',[0,0.00,0.75,0.75],'EdgeColor','k'); 456 | if rem(i,6)==1 457 | set(gca,'YTickLabel',{'dark','normal','light'},... 458 | 'YTick',[0.125,0.375,0.625]); 459 | if i==19 460 | text(0,-0.25,['{\bf\itARROW3} Named Color Table ',... 461 | '( \beta = ',num2str(beta),' )']); 462 | end 463 | end 464 | end 465 | end 466 | 467 | %========================================================================== 468 | % ColorMap 469 | function [C,failed]=LocalColorMap(c,vc,cn,beta) 470 | n=length(c); failed=0; C=zeros(n,3); i=1; j=1; 471 | while 1 472 | if isempty(find([vc,'_^']==c(i))), failed=1; break, end 473 | if sum('_^'==c(i)) 474 | if i+1>n, failed=1; break, end 475 | if ~sum(vc==c(i+1)), failed=1; break, end 476 | cc=cn(find(vc==c(i+1)),:); gamma=beta; 477 | if c(i)=='_', gamma=-beta; end 478 | C(j,:)=LocalBrighten(cc,gamma); i=i+2; 479 | else, C(j,:)=cn(find(vc==c(i)),:); i=i+1; end 480 | if i>n, break, end, j=j+1; 481 | end, if n>j, C(j+1:n,:)=[]; end 482 | 483 | %========================================================================== 484 | % Brighten 485 | function C=LocalBrighten(c,beta) 486 | C=c.^((1-min(1-sqrt(eps),abs(beta)))^sign(beta)); 487 | 488 | %========================================================================== 489 | % Generate valid value for color, linestyle and linewidth 490 | function [c,ls,lw]=LocalValidateCLSW(s) 491 | if nargin<1, c='k'; ls='-'; lw=0.5; 492 | else 493 | % identify linestyle 494 | if findstr(s,'--'), ls='--'; s=strrep(s,'--',''); 495 | elseif findstr(s,'-.'), ls='-.'; s=strrep(s,'-.',''); 496 | elseif findstr(s,'-'), ls='-'; s=strrep(s,'-',''); 497 | elseif findstr(s,':'), ls=':'; s=strrep(s,':',''); 498 | elseif findstr(s,'*'), ls='*'; s=strrep(s,'*',''); 499 | else, ls='-'; end 500 | 501 | % identify linewidth 502 | tmp=double(s); 503 | tmp=find(tmp>45 & tmp<57); 504 | if length(tmp) 505 | if any(s(tmp)=='/'), lw='/'; else, lw=str2num(s(tmp)); end 506 | s(tmp)=''; 507 | else, lw=0.5; end 508 | 509 | % identify color 510 | if length(s), s=lower(s); 511 | if length(s)>1, c=s(1:2); 512 | else, c=s(1); end 513 | else, c='k'; end 514 | end -------------------------------------------------------------------------------- /IMU_Camera/calc_cam_vp.m: -------------------------------------------------------------------------------- 1 | % calc_cam_vp() 2 | % 3 | % calculate image vanishing points 4 | % 5 | % jlobo mar 2004 6 | 7 | % get vanishing point for each frame 8 | cam=[]; 9 | for n=1:frames 10 | % vp direct from camera extrinsic 11 | eval(['vp=Rc_' num2str(n) '(:,2);']); % Y chessboard axis is vertical 12 | cam=[cam; vp']; 13 | end 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /IMU_Camera/calc_cam_vp_w.m: -------------------------------------------------------------------------------- 1 | % calc_cam_vp_w() 2 | % 3 | % calculate image vanishing points and weights (fixed maxang=2.5deg) 4 | % 5 | % jlobo mar 2004 6 | % jlobo Set 2006: update with weights wi based on omc_error_i 7 | 8 | maxang=2.5; %maxang err of 2.5 deg, reject if above 9 | 10 | % get vanishing point for each frame 11 | cam=[]; 12 | cam_ae=[]; 13 | for n=1:frames 14 | % vp direct from camera extrinsic 15 | eval(['vp=Rc_' num2str(n) '(:,2);']); % Y chessboard axis is vertical 16 | cam=[cam; vp']; 17 | eval(['ae=norm(omc_error_' num2str(n) ');']); % magnitude of rotation error, aprox 3x stddev 18 | cam_ae=[cam_ae; ae]; 19 | end 20 | 21 | % weight=1-ang_err_std/ang_max if >=0 22 | maxang=pi*maxang/180; 23 | cam_w=1-cam_ae./maxang; 24 | cam_w(cam_w<0)=0; 25 | 26 | %% un-comment to have optional plot 27 | deg_ae=cam_ae.*180/pi; 28 | plot(deg_ae,'o-'); 29 | hold on; 30 | plot(cam_w,'*-r'); 31 | xlabel('Frame number'); 32 | ylabel('o- Error (deg), *- Weight'); 33 | hold off; 34 | -------------------------------------------------------------------------------- /IMU_Camera/calc_q_imu2cam.m: -------------------------------------------------------------------------------- 1 | % calc_q_imu2cam() 2 | % 3 | % calculate rotation given IMU and CAM observations of vertical direction 4 | % 5 | % based on Horn's method 6 | % 7 | % requires robot toolbox by Peter Corke 8 | % 9 | % jlobo set 2003 10 | 11 | function q = calc_q_imu2cam(imu,cam); 12 | 13 | [lines cols]=size(imu); 14 | 15 | % we now want to estimate the quaternion q that rotates imu to cam 16 | 17 | % compute the sums of all 9 possible products between pairs of coordinates 18 | Sxx=0; 19 | Sxy=0; 20 | Sxz=0; 21 | Syx=0; 22 | Syy=0; 23 | Syz=0; 24 | Szx=0; 25 | Szy=0; 26 | Szz=0; 27 | for i = 1:lines 28 | Sxx=Sxx+imu(i,1)*cam(i,1); 29 | Sxy=Sxy+imu(i,1)*cam(i,2); 30 | Sxz=Sxz+imu(i,1)*cam(i,3); 31 | Syx=Syx+imu(i,2)*cam(i,1); 32 | Syy=Syy+imu(i,2)*cam(i,2); 33 | Syz=Syz+imu(i,2)*cam(i,3); 34 | Szx=Szx+imu(i,3)*cam(i,1); 35 | Szy=Szy+imu(i,3)*cam(i,2); 36 | Szz=Szz+imu(i,3)*cam(i,3); 37 | end 38 | 39 | A=[ (Sxx+Syy+Szz) Syz-Szy Szx-Sxz Sxy-Syx 40 | Syz-Szy (Sxx-Syy-Szz) Sxy+Syx Szx+Sxz 41 | Szx-Sxz Sxy+Syx (-Sxx+Syy-Szz) Syz+Szy 42 | Sxy-Syx Szx+Sxz Syz+Szy (-Sxx-Syy+Szz) ]; 43 | 44 | % compute eigen values and vectors 45 | [V,D] = eig(A); 46 | 47 | % sort eigenvalues 48 | lD=diag(D); 49 | [B,index] = sort(abs(lD)); 50 | 51 | % choose the eigenvector corresponding to 52 | % the largets eigen value 53 | r = V(:,index(4)); 54 | q=Quaternion(r'); 55 | -------------------------------------------------------------------------------- /IMU_Camera/calc_q_imu2cam_w.m: -------------------------------------------------------------------------------- 1 | % calc_q_imu2cam_w() 2 | % 3 | % calculate rotation given IMU and CAM observations of vertical direction 4 | % and corresponding input error weights 5 | % 6 | % based on Horn's method 7 | % 8 | % requires robot toolbox by Peter Corke 9 | % 10 | % jlobo set 2003 11 | % jlobo dec 2006 update with weights 12 | 13 | function qw = calc_q_imu2cam_w(imu,imu_w,cam,cam_w); 14 | 15 | [lines cols]=size(imu); 16 | 17 | % (joint) observation weight 18 | w=cam_w.*imu_w; 19 | 20 | % we now want to estimate the quaternion q that rotates imu to cam 21 | 22 | % compute the sums of all 9 possible products between pairs of coordinates 23 | Sxx=0; 24 | Sxy=0; 25 | Sxz=0; 26 | Syx=0; 27 | Syy=0; 28 | Syz=0; 29 | Szx=0; 30 | Szy=0; 31 | Szz=0; 32 | for i = 1:lines 33 | Sxx=Sxx+imu(i,1)*cam(i,1)*w(i); 34 | Sxy=Sxy+imu(i,1)*cam(i,2)*w(i); 35 | Sxz=Sxz+imu(i,1)*cam(i,3)*w(i); 36 | Syx=Syx+imu(i,2)*cam(i,1)*w(i); 37 | Syy=Syy+imu(i,2)*cam(i,2)*w(i); 38 | Syz=Syz+imu(i,2)*cam(i,3)*w(i); 39 | Szx=Szx+imu(i,3)*cam(i,1)*w(i); 40 | Szy=Szy+imu(i,3)*cam(i,2)*w(i); 41 | Szz=Szz+imu(i,3)*cam(i,3)*w(i); 42 | end 43 | 44 | A=[ (Sxx+Syy+Szz) Syz-Szy Szx-Sxz Sxy-Syx 45 | Syz-Szy (Sxx-Syy-Szz) Sxy+Syx Szx+Sxz 46 | Szx-Sxz Sxy+Syx (-Sxx+Syy-Szz) Syz+Szy 47 | Sxy-Syx Szx+Sxz Syz+Szy (-Sxx-Syy+Szz) ]; 48 | 49 | % compute eigen values and vectors 50 | [V,D] = eig(A); 51 | 52 | % sort eigenvalues 53 | lD=diag(D); 54 | [B,index] = sort(abs(lD)); 55 | 56 | % choose the eigenvector corresponding to 57 | % the largets eigen value 58 | r = V(:,index(4)); 59 | qw=Quaternion(r'); 60 | -------------------------------------------------------------------------------- /IMU_Camera/clickmove.m: -------------------------------------------------------------------------------- 1 | function clickmove(handle,opt,arg) 2 | % function clickmove(handle,opt) 3 | % 4 | % This function allows the user to manipulate the 3-d view of a graph 5 | % by point-click-drag. The advantage this function has over similar 6 | % ones is its highly intuitive interface, and the fact that the graph is 7 | % displayed as it is pivoted, whereas in Matlab's built-in function it is 8 | % simplified into a box. In point of fact, I was unaware of the existence 9 | % of Matlab's built-in 3d rotate function when I first wrote this; but I still 10 | % like this one better. 11 | % 12 | % If the user does not provide a handle, the function will 13 | % take the output of the gcf command (the current figure). If 14 | % there are no current figures, the function will create one. 15 | % 16 | % If the opt argument is empty or zero, the function will assign this 17 | % function to all the current axis' children as well; if it is non-zero 18 | % the function will be only assigned to the current axis (In other words, 19 | % clicking on the AXES will invoke the function but clicking on the GRAPHIC 20 | % objects will not) 21 | % 22 | % To turn the function off, enter the syntax 23 | % clickmove(h,'off') 24 | % where h is the figure handle to the axes or figure, or even graphic 25 | % object for which you want to turn off clickmove. 26 | % 27 | % Made by Brandon Kuczenski 28 | % Updated / finalized 10-10-2001 29 | % brandon_kuczenski@kensingtonlabs.com 30 | 31 | 32 | if nargin==0 33 | handle=gcf; 34 | handletemp=gcf; 35 | opt=0; 36 | else 37 | handletemp=handle; 38 | if ~ishandle(handle) 39 | warning('Input Parameter was not a handle ... selecting Current Figure') 40 | handle=gcf; 41 | end 42 | while handle~=floor(handle) 43 | % i.e the handle is not a figure but a child (figures are always integers) 44 | handle=get(handle,'parent'); 45 | if strcmp(get(handle,'type'),'axes') 46 | handletemp=handle; % move up to axes - the lowest handle we can meaningfully 47 | % deal with 48 | end 49 | end 50 | if nargin==1 51 | %i.e. no opt was provided 52 | opt=0; 53 | end 54 | 55 | end 56 | 57 | if ischar(opt)&strcmp(opt,'off') 58 | if ishandle(handletemp) 59 | % then because of line 39 we know it is an axes or a figure 60 | set(findobj(handletemp,'tag','clickmove'),'ButtonDownFcn','','tag','') 61 | else 62 | error('Can''t turn Clickmove off of a non-object') 63 | end 64 | else 65 | if nargin<3; arg=0;end 66 | h=handle; 67 | switch arg 68 | case 1 69 | % disp('case 1') 70 | % THis is when the button is first pushed 71 | figure(h); 72 | a=gca; 73 | set(h,'windowbuttonmotionfcn',['clickmove(' ... 74 | num2str(h) ',[],2)']); 75 | set(h,'windowbuttonupfcn',['clickmove(' num2str(h) ',[],3)']); 76 | G.orig=get(a,'userdata'); 77 | G.unit=get(gcf,'units'); 78 | set(h,'units','normalized'); 79 | G.pos=get(h,'currentpoint'); % [ X Y ] 80 | G.view=get(a,'View'); 81 | set(a,'Userdata',G); 82 | case 2 83 | % disp('case 2') 84 | % this is buttonmotionfcn while button is down 85 | % h is assumed to be the current figure, since it was clicked on 86 | figure(h); 87 | a=gca; 88 | G=get(a,'Userdata'); 89 | H=get(h,'currentpoint'); % [ X Y ] 90 | th(1)=atan(5*(H(1)-0.5)) - atan(5*(G.pos(1)-0.5)); 91 | th(2)=atan(5*(H(2)-0.5)) - atan(5*(G.pos(2)-0.5)); 92 | th=th*180/pi; 93 | newview=mod(G.view-th,360); 94 | % disp(newview); 95 | 96 | set(a,'view',newview); 97 | 98 | case 3 99 | % disp('case 3') 100 | % this is buttonupfcn while button is down 101 | % Now we just have to clean up after ourselves 102 | figure(h); 103 | a=gca; % must be the CA because it was clicked on 104 | G=get(a,'userdata'); 105 | set(h,'units',G.unit); 106 | set(h,'windowbuttonmotionfcn',''); 107 | set(h,'windowbuttonupfcn',''); 108 | set(a,'userdata',G.orig); 109 | otherwise 110 | 111 | % disp('case 0') 112 | figure(h); 113 | set(h,'doublebuffer','on') 114 | a=gca; 115 | if ishandle(handletemp)&strcmp(get(handletemp,'type'),'axes') 116 | a=handletemp; 117 | end 118 | set(a,'Buttondownfcn',['clickmove(' num2str(h) ',[],1)'],'tag','clickmove') 119 | if opt==0 120 | set(get(a,'children'),'Buttondownfcn', ... 121 | ['clickmove(' num2str(h) ',[],1)'],'tag','clickmove'); 122 | end 123 | 124 | end 125 | 126 | end % if 'off' ... else 127 | -------------------------------------------------------------------------------- /IMU_Camera/fast_normalize_us.m: -------------------------------------------------------------------------------- 1 | % fast simplified version of normalize_us 2 | % 3 | % unit sphere normalized pixel vector 4 | % 5 | % adapted from normalize (http://www.vision.caltech.edu/bouguetj/calib_doc/index.html) 6 | % 7 | % jlobo April 2004 8 | 9 | function [Xn] = fast_normalize_us(x_kk,fc,cc,kc,alpha_c), 10 | 11 | % fast simplified version of normalize_us 12 | % 13 | % unit sphere normalized pixel vector 14 | % 15 | % adapted from normalize (http://www.vision.caltech.edu/bouguetj/calib_doc/index.html) 16 | % 17 | % jlobo April 2004 18 | % 19 | %[Xn] = normalize_us(x_kk,fc,cc,kc,alpha_c) 20 | % 21 | %Computes the unit sphere normalized coordinates xn given the pixel coordinates x_kk 22 | %and the intrinsic camera parameters fc, cc and kc. 23 | % 24 | %INPUT: x_kk: Feature locations on the images 25 | % fc: Camera focal length 26 | % cc: Principal point coordinates 27 | % kc: Distortion coefficients 28 | % alpha_c: Skew coefficient 29 | % 30 | %OUTPUT: Xn: Normalized feature locations on the unit sphere (a 3XN matrix) 31 | % 32 | %Important functions called within that program: 33 | % 34 | %comp_distortion_oulu: undistort pixel coordinates. 35 | 36 | % if nargin < 5, 37 | % alpha_c = 0; 38 | % if nargin < 4; 39 | % kc = [0;0;0;0;0]; 40 | % if nargin < 3; 41 | % cc = [0;0]; 42 | % if nargin < 2, 43 | % fc = [1;1]; 44 | % end; 45 | % end; 46 | % end; 47 | % end; 48 | 49 | 50 | %% First: Subtract principal point, and divide by the focal length: 51 | %x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 52 | 53 | % % Second: undo skew 54 | % x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); 55 | % 56 | % if norm(kc) ~= 0, 57 | % % Third: Compensate for lens distortion: 58 | % x_n = comp_distortion_oulu(x_distort,kc); 59 | % else 60 | % x_n = x_distort; 61 | % end; 62 | 63 | % xn are 2D image points on the plane at z=1 64 | % project to unit sphere 65 | Xn=[(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2); ones(size(x_kk(1,:)))]; 66 | Xn(3,:)=( Xn(1,:).^2 + Xn(2,:).^2 + Xn(3,:) ).^(-.5); 67 | Xn(1,:)=Xn(1,:).*Xn(3,:); 68 | Xn(2,:)=Xn(2,:).*Xn(3,:); -------------------------------------------------------------------------------- /IMU_Camera/fscatter3.m: -------------------------------------------------------------------------------- 1 | function [h] = fscatter3(X,Y,Z,C,cmap); 2 | % [h] = fscatter3(X,Y,Z,C,cmap); 3 | % Plots point cloud data in cmap color classes and 3 Dimensions, 4 | % much faster and very little memory usage compared to scatter3 ! 5 | % X,Y,Z,C are vectors of the same length 6 | % with C being used as index into colormap (can be any values though) 7 | % cmap is optional colourmap to be used 8 | % h are handles to the line objects 9 | 10 | % Felix Morsdorf, Jan 2003, Remote Sensing Laboratory Zuerich 11 | 12 | if nargin == 4 13 | numclass = 256; % Number of color classes 14 | cmap = hsv(256); 15 | elseif nargin == 5 16 | numclass = max(size(cmap)); 17 | if numclass == 1 18 | cmap = hsv(256); 19 | numclass = 256; 20 | end 21 | end 22 | 23 | % avoid too many calculations 24 | 25 | mins = min(C); 26 | maxs = max(C); 27 | minz = min(Z); 28 | maxz = max(Z); 29 | minx = min(X); 30 | maxx = max(X); 31 | miny = min(Y); 32 | maxy = max(Y); 33 | 34 | % construct colormap : 35 | 36 | col = cmap; 37 | 38 | % determine index into colormap 39 | 40 | ii = round(interp1([floor(mins) ceil(maxs)],[1 numclass],C)); 41 | hold on 42 | colormap(cmap); 43 | 44 | % plot each color class in a loop 45 | 46 | k = 0; 47 | for j = 1:numclass 48 | jj = find(ii == j); 49 | if ~isempty(jj) 50 | k = k + 1; 51 | h(k) = plot3(X(jj),Y(jj),Z(jj),'.','color',col(j,:), ... 52 | 'markersize',2); 53 | end 54 | end 55 | axis([minx maxx miny maxy minz maxz]) 56 | axis image 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /IMU_Camera/get_cam_xsens_mti.m: -------------------------------------------------------------------------------- 1 | % get_cam_xsens_mti 2 | % 3 | % Capture images from USB or firewire camera, store as bmp 4 | % Capture corresponding IMU data from Xsens MTi, 5 | % store as text file with ax ay az .... on each line, minimum 10 lines 6 | % load_imu will read 1st 3 values on from the 1st 10 lines as the x, y,z acceleration in m.s^2 7 | % 8 | % jlobo Jun 2008 update for xsens mti sensor and R14 matlab with imagging toolbox grab 9 | % jlobo Mar 2004 10 | 11 | imaqreset; 12 | %imaqhwinfo 13 | %imaqhwinfo('winvideo',1) 14 | vid=videoinput('winvideo', 1); 15 | preview(vid); 16 | 17 | base_name=input('File base name: ','s'); 18 | 19 | % xsens 20 | 21 | % set up instance of MTObj in MATLAB 22 | h=actxserver('MotionTracker.FilterComponent'); 23 | 24 | %St=input(['What COM-port is your MTi or MTx connected to? <1>'],'s'); 25 | COMport = 5 ; %str2num(St); 26 | % assume default value for baud rate, but this can also be set 27 | baudrate = 115200; 28 | 29 | % call MT_SetCOMPort is required, unless using COM 1 30 | h.MT_SetCOMPort(COMport,baudrate); 31 | 32 | % request device information from MTi or MTx 33 | [DeviceID] = h.MT_QueryMotionTrackerB 34 | 35 | % request calibrated inertial and magnetic data along with orientation data 36 | h.MT_SetCalibratedOutput(1); 37 | % request orientation data in Euler-angles 38 | h.MT_SetOutputMode(1); 39 | 40 | % That's it! 41 | 42 | 43 | i=1; 44 | escolha=menu('Capture Images',sprintf('Capture #%03d',i),'Quit'); 45 | while(escolha~=2) 46 | imagem=getsnapshot(vid); 47 | 48 | 49 | 50 | %get IMU 51 | imu=[]; 52 | for j=1:10 53 | 54 | % MTObj is ready to start processing the data stream from the MTi or MTx 55 | h.MT_StartProcess; % start processing data 56 | 57 | % wait short moment for object to read data from COM-port 58 | pause(0.05); 59 | 60 | % retrieve the data 61 | [arg1,inertialData] = MT_GetCalibratedData(h,1); % get latest calibrated data from buffer 62 | [arg1,eulerAngle] = MT_GetOrientationData(h,1); % get latest orientation data from buffer 63 | 64 | % if data retrieved succesfully (arg1=1) 65 | if arg1==1, 66 | status= double(arg1) % MTObj status (can be converted to double for easy use in Matlab) 67 | inertialData = double(inertialData) % data values (can be converted to double for easy use in Matlab) 68 | eulerAngle = double(eulerAngle) % data values (can be converted to double for easy use in Matlab) 69 | else 70 | display('Error reading xsens'); 71 | end 72 | 73 | 74 | % stop processing before removing object 75 | h.MT_StopProcess; 76 | 77 | gx = inertialData(4); 78 | gy = inertialData(5); 79 | gz = inertialData(6); 80 | ax = inertialData(1); 81 | ay = inertialData(2); 82 | az = inertialData(3); 83 | imu=[ imu [ax ay az gx gy gz]']; 84 | end 85 | 86 | name=sprintf('%s_imu_%03d.txt',base_name,i); 87 | eval([ 'save ' name ' imu -ASCII; ']); 88 | 89 | imshow(imagem); 90 | img_name=sprintf('%s_%03d',base_name,i); 91 | imwrite(imagem,strcat(img_name,'.bmp'),'bmp') 92 | i=i+1; 93 | escolha=menu('Capture Images',sprintf('Capture #%03d',i),'Quit'); 94 | end 95 | 96 | 97 | % stop processing before removing object 98 | h.MT_StopProcess; 99 | 100 | % when finished with MTObj, release it from the MATLAB workspace 101 | delete(h); % release MTObj COM-object 102 | clear h; 103 | 104 | closepreview(vid); 105 | delete(vid); 106 | imaqreset; 107 | 108 | -------------------------------------------------------------------------------- /IMU_Camera/great_circ.m: -------------------------------------------------------------------------------- 1 | function circ = great_circ(v1,v2); 2 | % 3 | % generate 40 3D-points of great circle along plane v1 v2 4 | % ... normal to vector v1 X v2 5 | % 6 | % 7 | % (c) jlobo Set2003 8 | 9 | 10 | switch nargin % 1 arg => nromal vector; 2 args => 2 points 11 | 12 | case 2 %2 args => 2 points 13 | 14 | n=40; 15 | circ=[]; 16 | % Gram-Schmidt 17 | v1=unit(v1); 18 | v2=v2-dot(v1,v2)*v1; 19 | v2=unit(v2); 20 | for i = 0:n 21 | theta=i/n*2*pi; 22 | a=cos(theta); 23 | b=sin(theta); 24 | m=a*v1+b*v2; 25 | m=unit(m); 26 | circ=[circ; m]; 27 | end 28 | 29 | 30 | case 1 31 | 32 | n=40; 33 | circ=[]; 34 | % vectors normal to given v1 35 | nv=v1; 36 | v1(1)=-nv(3)/nv(1); 37 | v1(2)=0; 38 | v1(3)=1; 39 | v2(1)=0; 40 | v2(2)=1; 41 | v2(3)=-nv(2)/nv(3); 42 | v2=unit(v2); 43 | % Gram-Schmidt 44 | v1=unit(v1); 45 | v2=v2-dot(v1,v2)*v1; 46 | v2=unit(v2); 47 | for i = 0:n 48 | theta=i/n*2*pi; 49 | a=cos(theta); 50 | b=sin(theta); 51 | m=a*v1+b*v2; 52 | m=unit(m); 53 | circ=[circ; m]; 54 | end 55 | 56 | 57 | otherwise 58 | end 59 | 60 | % 61 | -------------------------------------------------------------------------------- /IMU_Camera/imu2cam.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MichalNowicki/CalibrationCodes/37657f0edc3b7832c6d60e9b7cbf8b182d054cab/IMU_Camera/imu2cam.mat -------------------------------------------------------------------------------- /IMU_Camera/imucam.m: -------------------------------------------------------------------------------- 1 | % 2 | % Inertial Measurement Unit and Camera Calibration Toolbox (IMU to CAM rotation and translation calibration): 3 | % 4 | % Rotation calibration: 5 | % It is assumed that IMU and CAM rigid mount is taken through several static poses and sampled at the same time, 6 | % with the chessboard target vertical in all images, and that the corresponing IMU data was sampled for each frame 7 | % 8 | % INPUT: Calib_Results.mat -> Generated by the standard calibration toolbox; 9 | % images -> 3 digit numbered .bmp with base_name given by Calib_Results 10 | % imu data -> 3 digit numbered .txt with base_name+'imu' with imu data (mininum 10 lines, ax ay az ... (m.s^2)) 11 | % 12 | % OUTPUT: imu2cam.mat -> computed rotation quaternion 13 | % 14 | % Features: 15 | % -> estimated IMU to CAM rotation quaternion 16 | % -> input error weighing is used 17 | % -> rendered unit sphere projection showing orientation vectors 18 | % and chessboard vanishing points 19 | % (use 'opengl software if you experience rendering problems) 20 | % -> plot reprojection angular error (angle between imu verticals 21 | % rotated to cam and vertical vanishing point) 22 | % 23 | % requires: 24 | % - robot toolbox by Peter Corke (used here for quaternion and rotation matrix manipulation) 25 | % (http://www.cat.csiro.au/cmst/staff/pic/robot/) 26 | % - TOOLBOX_calib ((c) Jean-Yves Bouguet - Intel Corporation) to generate camera Calib_Results.mat 27 | % (http://www.vision.caltech.edu/bouguetj/calib_doc/index.html) 28 | % - fscatter3 29 | % - clickmove 30 | % - arrow3 31 | % 32 | % jlobo Jun 2008 updated to use weights and imu text file input 33 | % jlobo April 2004 34 | 35 | 36 | m=menu('IMU and CAM Calibration Toolbox',... 37 | 'Load camera calibration files',... 38 | 'Quit IMU and CAM Calibration Toolbox'); 39 | switch m 40 | case 1 41 | load Calib_Results; 42 | fprintf(1,'\nLoaded calib data for %s image set.\n',calib_name); 43 | case 2 44 | break 45 | end 46 | [i frames]=size(image_numbers); 47 | m=menu(sprintf('IMU and CAM Calibration Toolbox\n\nbase name: %s\n\nframes 1 to %d',calib_name,frames),... 48 | sprintf('Load IMU data (%simu_###.txt)',calib_name),... 49 | 'Quit IMU and CAM Calibration Toolbox'); 50 | switch m 51 | case 1 52 | [imu, imu_w]=load_imu_w(calib_name,frames); 53 | fprintf(1,'\nLoaded IMU data from %s_###.mat files.\n',calib_name); 54 | case 2 55 | break 56 | end 57 | m=menu(sprintf('IMU and CAM Calibration Toolbox\n\nbase name: %s\n\nframes 1 to %d',calib_name,frames),... 58 | 'Run rotation calibration',... 59 | 'Quit IMU and CAM Calibration Toolbox'); 60 | switch m 61 | case 1 62 | %cam = calc_cam_vp(frames,fc,cc,kc,alpha_c); 63 | calc_cam_vp_w; % sets cam, not implemneted as a function to have variables available 64 | %q=calc_q_imu2cam(imu,cam); % force the use of weights 65 | qw=calc_q_imu2cam_w(imu, imu_w,cam, cam_w); 66 | fprintf(1,'\nIMU to CAM Rotation quaternion.\n'); 67 | q=qw 68 | fprintf(1,'\n'); 69 | case 2 70 | break 71 | end 72 | n=1; 73 | while 1 74 | m=menu(sprintf('IMU and CAM Calibration Toolbox\n\nbase name: %s\n\nframes 1 to %d',calib_name,frames),... 75 | 'Show unit sphere and rotation for selected frame',... 76 | sprintf('next frame (current frame = %d)',n),... 77 | 'Show rotation reprojection error',... 78 | sprintf('Save rotation quaternion to %simu2cam.mat',calib_name),... 79 | 'Quit IMU and CAM Calibration Toolbox'); 80 | switch m 81 | case 1 82 | show_us_vp_cam_imu; 83 | case 2 84 | n=n+1; 85 | if n>frames 86 | n=1; 87 | end; 88 | case 3 89 | %show_rotation_reprojection_error; 90 | figure('Name','Rotation Reprojection Error'); 91 | angerr=[]; 92 | for i=1:frames 93 | angerr=[angerr vect_ang(q*imu(i,:)',cam(i,:))]; 94 | if angerr(i)>pi/2 95 | angerr(i)=angerr(i)-pi; 96 | end 97 | end 98 | %To obtain the root-mean-square (RMS) value, use norm(A)/sqrt(n) 99 | rms_angerr=norm(angerr)/sqrt(frames); 100 | plot(abs(angerr*180/pi),'r*'); 101 | title(sprintf('Root Mean Square Angle Error %.3f (deg)',180*rms_angerr/pi)); 102 | set(gca,'xtick',image_numbers); 103 | set(gca,'YGrid','on'); 104 | xlabel('Frames'); 105 | ylabel('Error (deg)'); 106 | case 4 107 | save imu2cam.mat q; 108 | fprintf(1,'\nRotation quaternion saved as %simu2cam.mat.\n',calib_name); 109 | q 110 | Euler_angles=tr2eul(q.T()) 111 | Roll_Pitch_Yaw_angles=tr2rpy(q.T()) 112 | %SUBSREF Reference methods on a QUATERNION object 113 | % QUATERNION.d return a 4-vector of quaternion elements 114 | % QUATERNION.s return the scalar component 115 | % QUATERNION.v return the vector component 116 | % QUATERNION.t return a 4x4 homogeneous transform 117 | % QUATERNION.r return a 3x3 orthonormal rotation matrix 118 | case 5 119 | break 120 | end 121 | end -------------------------------------------------------------------------------- /IMU_Camera/load_imu.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MichalNowicki/CalibrationCodes/37657f0edc3b7832c6d60e9b7cbf8b182d054cab/IMU_Camera/load_imu.m -------------------------------------------------------------------------------- /IMU_Camera/load_imu_w.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MichalNowicki/CalibrationCodes/37657f0edc3b7832c6d60e9b7cbf8b182d054cab/IMU_Camera/load_imu_w.m -------------------------------------------------------------------------------- /IMU_Camera/normalize_us.m: -------------------------------------------------------------------------------- 1 | % normalize_us 2 | % 3 | % unit sphere normalized pixel vector 4 | % 5 | % [Xn] = normalize_us(x_kk,fc,cc,kc,alpha_c) 6 | % 7 | % jlobo April 2004 8 | 9 | function [Xn] = normalize_us(x_kk,fc,cc,kc,alpha_c), 10 | 11 | %normalize_us 12 | % 13 | % unit sphere normalized pixel vector 14 | % 15 | % adapted from normalize (http://www.vision.caltech.edu/bouguetj/calib_doc/index.html) 16 | % 17 | % jlobo April 2004 18 | % 19 | %[Xn] = normalize_us(x_kk,fc,cc,kc,alpha_c) 20 | % 21 | %Computes the unit sphere normalized coordinates xn given the pixel coordinates x_kk 22 | %and the intrinsic camera parameters fc, cc and kc. 23 | % 24 | %INPUT: x_kk: Feature locations on the images 25 | % fc: Camera focal length 26 | % cc: Principal point coordinates 27 | % kc: Distortion coefficients 28 | % alpha_c: Skew coefficient 29 | % 30 | %OUTPUT: Xn: Normalized feature locations on the unit sphere (a 3XN matrix) 31 | % 32 | %Important functions called within that program: 33 | % 34 | %comp_distortion_oulu: undistort pixel coordinates. 35 | 36 | if nargin < 5, 37 | alpha_c = 0; 38 | if nargin < 4; 39 | kc = [0;0;0;0;0]; 40 | if nargin < 3; 41 | cc = [0;0]; 42 | if nargin < 2, 43 | fc = [1;1]; 44 | end; 45 | end; 46 | end; 47 | end; 48 | 49 | 50 | % First: Subtract principal point, and divide by the focal length: 51 | x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 52 | 53 | % Second: undo skew 54 | x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); 55 | 56 | if norm(kc) ~= 0, 57 | % Third: Compensate for lens distortion: 58 | x_n = comp_distortion_oulu(x_distort,kc); 59 | else 60 | x_n = x_distort; 61 | end; 62 | 63 | % xn are 2D image points on the plane at z=1 64 | % project to unit sphere 65 | Xn=[ x_n(1,:); x_n(2,:); ones(size(x_n(1,:)))]; 66 | Xn(3,:)=( Xn(1,:).^2 + Xn(2,:).^2 + Xn(3,:) ).^(-.5); 67 | Xn(1,:)=Xn(1,:).*Xn(3,:); 68 | Xn(2,:)=Xn(2,:).*Xn(3,:); -------------------------------------------------------------------------------- /IMU_Camera/show_rotation_reprojection_error_w.m: -------------------------------------------------------------------------------- 1 | % show_rotation_reprojection_error 2 | % 3 | % show missmatch between observated and rotated vertical 4 | % 5 | % assumes the following variables exist: 6 | % Calib_Results have been loaded 7 | % cam -vertical vanishing points (normalized to unit sphere) 8 | % imu -inertial unit verticals 9 | % qw - imu to cam rotation quaternion 10 | % frames - frame from set 11 | % 12 | % uses: 13 | % vect_ang 14 | % 15 | % jlobo April 2004 16 | 17 | %show_rotation_reprojection_error; 18 | figure('Name','Rotation W Reprojection Error'); 19 | angerr=[]; 20 | for i=1:frames 21 | angerr=[angerr vect_ang(qw*imu(i,:)',cam(i,:))]; 22 | if angerr(i)>pi/2 23 | angerr(i)=angerr(i)-pi; 24 | end 25 | end 26 | %To obtain the root-mean-square (RMS) value, use norm(A)/sqrt(n) 27 | %matlab function ref: http://www-ccs.ucsd.edu/matlab/techdoc/ref/norm.html 28 | % norm(vector)sum(abs(A).^2)^(1/2), i.e. str(mean(squares)) 29 | rms_angerr=norm(angerr)/sqrt(frames); 30 | plot(abs(angerr*180/pi),'r*'); 31 | title(sprintf('Root Mean Square Angle Error %.3f (deg)',180*rms_angerr/pi)); 32 | set(gca,'xtick',image_numbers); 33 | set(gca,'YGrid','on'); 34 | xlabel('Frames'); 35 | ylabel('Error (deg)'); 36 | -------------------------------------------------------------------------------- /IMU_Camera/show_us_vp_cam_imu.m: -------------------------------------------------------------------------------- 1 | % show_us_vp_cam_imu 2 | % 3 | % show unit sphere image and vertical vanishing point, 4 | % inertial vertical, and its rotation to camera frame 5 | % 6 | % assumes: 7 | % Calib_Results have been loaded 8 | % cam -vertical vanishing points (normalized to unit sphere) 9 | % ins -inertial unit verticals (imu ref) 10 | % q -imu to cam rotation quaternion 11 | % n -current frame from set 12 | % 13 | % uses: 14 | % [xn] = normalize(x_kk,fc,cc,kc,alpha_c) from Camera Calibration toolbox 15 | % fscatter3 16 | % clickmove 17 | % arrow3 18 | % 19 | % jlobo June 2008: use 'opengl software' if rendering is faulty 20 | % jlobo April 2004 21 | 22 | % sphere overlay levels 23 | a=1.02; 24 | b=1.01; 25 | 26 | 27 | % set image name and figure title for current frame n 28 | filename=sprintf('%s%03d.bmp',calib_name,n); 29 | figure('Name',sprintf('Frame %d : %s',n,filename)); 30 | 31 | % simple names, not indexed by n 32 | eval(['x=x_' num2str(n) ';']); 33 | 34 | % setup figure with nice rendered shiny sphere 35 | subplot('Position',[0.31 0 .69 1]) 36 | [X,Y,Z]=sphere(40); 37 | H=surf(X,Y,Z,Z); 38 | caxis([-1 1]); 39 | set(H,'FaceAlpha',0.6) 40 | set(H,'FaceLighting','phong','FaceColor','interp','AmbientStrength',0.5) 41 | light('Position',[1 -1 0.5],'Style','infinite'); 42 | axis equal; 43 | view(150,75); 44 | shading interp; 45 | 46 | % load frame image and show it at top left corner 47 | I = imread(filename); 48 | subplot('Position',[0.01 .69 .3 .3]) 49 | imshow(I), 50 | 51 | % plot chessboard grid on image plane 52 | hold on; 53 | plot(x(1,:),x(2,:),'b+','markersize',5); % all grid points 54 | plot(x(1,1),x(2,1),'ro','markersize',5); % corner 55 | plot(x(1,43),x(2,43),'yo','markersize',5); % corner: grid origin, marked yellow 56 | plot(x(1,6),x(2,6),'ro','markersize',5); % corner 57 | plot(x(1,48),x(2,48),'ro','markersize',5); % corner 58 | hold off; 59 | 60 | % project chessboard grid to unit sphere 61 | X_us=normalize_us(x,fc,cc,kc,alpha_c); 62 | 63 | %find vp from 4 outer grid points 64 | point_a=X_us(:,1); 65 | point_b=X_us(:,43); 66 | point_c=X_us(:,6); 67 | point_d=X_us(:,48); % grid origin, marked yellow 68 | line_m1=cross(point_a,point_b); 69 | line_m1=unit(line_m1); 70 | line_m2=cross(point_c,point_d); 71 | line_m2=unit(line_m2); 72 | vp=cross(line_m1,line_m2); 73 | % vp direct from camera extrinsic 74 | eval(['vp=Rc_' num2str(n) '(:,2)']); % Y chessboard axis is vertical 75 | 76 | %plot chessboard grid on unit sphere 77 | subplot('Position',[0.31 0 .69 1]) 78 | hold on; 79 | plot3(a*X_us(1,:),a*X_us(2,:),a*X_us(3,:),'b.','markersize',4); 80 | 81 | %plot vanishing point, and its construction on unit sphere 82 | plot3(vp(1),vp(2),vp(3),'r.','markersize',20); 83 | plot3(a*point_a(1),a*point_a(2),a*point_a(3),'r.','markersize',10); 84 | plot3(a*point_b(1),a*point_b(2),a*point_b(3),'y.','markersize',10); % grid origin, marked yellow 85 | plot3(a*point_c(1),a*point_c(2),a*point_c(3),'r.','markersize',10); 86 | plot3(a*point_d(1),a*point_d(2),a*point_d(3),'r.','markersize',10); 87 | circ=great_circ(point_a',point_b'); 88 | plot3(a*circ(:,1),a*circ(:,2),a*circ(:,3),'b-','linewidth',1.5); 89 | circ=great_circ(point_c',point_d'); 90 | plot3(a*circ(:,1),a*circ(:,2),a*circ(:,3),'b-','linewidth',1.5); 91 | circ=great_circ(vp'); 92 | plot3(a*circ(:,1),a*circ(:,2),a*circ(:,3),'r-','linewidth',2); 93 | arrow3([0 0 0],1.3*vp','r',.1,.2); 94 | 95 | %plot imu vertical on unit sphere 96 | plot3(1.1*imu(n,1),1.1*imu(n,2),1.1*imu(n,3),'k.','markersize',10); 97 | arrow3([0 0 0],1.3*imu(n,:),'k',.1,.1) 98 | circ=great_circ(imu(n,:)); 99 | plot3(a*circ(:,1),a*circ(:,2),a*circ(:,3),'k:','linewidth',1); 100 | 101 | %plot imu vertical rotated to camera frame on unit sphere 102 | cimu=imu(n,:); 103 | cimu(:)=(q*imu(n,:)'); 104 | plot3(1.2*cimu(1),1.2*cimu(2),1.2*cimu(3),'m.','markersize',10); 105 | arrow3([0 0 0],1.4*cimu,'m',.1,.1) 106 | circ=great_circ(cimu); 107 | plot3(a*circ(:,1),a*circ(:,2),a*circ(:,3),'m:','linewidth',1); 108 | hold off; 109 | 110 | % subsample image and project to unit sphere 111 | [umax,vmax,color_dim]=size(I); 112 | res=5; %step controls image res 113 | IPOINTS=ones(4,(umax/res)*(vmax/res)); %pre-allocate for faster matlab 114 | mi=[0 0 0 0]'; 115 | i=1; 116 | for u=1:res:umax 117 | for v=1:res:vmax 118 | mi(1:3)=fast_normalize_us([v u]',fc,cc,kc,alpha_c); % fast model normalize to unit sphere 119 | mi(4)=I(u,v,1); 120 | IPOINTS(:,i)=mi; 121 | i=i+1; 122 | end 123 | end 124 | fscatter3(b*IPOINTS(1,:),b*IPOINTS(2,:),b*IPOINTS(3,:),IPOINTS(4,:),gray(256)); 125 | 126 | % adjust view and set mouse click move 127 | %set(gca,'CameraUpVector',cam(n,:)); 128 | set(gca,'CameraViewAngle',7); 129 | axis tight; 130 | axis off; 131 | clickmove; 132 | hold off; 133 | 134 | 135 | 136 | 137 | 138 | -------------------------------------------------------------------------------- /IMU_Camera/vect_ang.m: -------------------------------------------------------------------------------- 1 | % vect_ang 2 | % 3 | % angle between two vectors 4 | % 5 | % angle=vect_ang(v1,v2) 6 | % 7 | % jlobo Set 2003 8 | 9 | function angle=vect_ang(v1,v2) 10 | 11 | angle=acos(dot(v1,v2)/(norm(v1)*norm(v2))); 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | CalibrationCodes 2 | ================ 3 | 4 | Codes of different calibration routines: IMU vs CAMERA, CAMERA vs LASER, Ground truth camera vs earth's cs 5 | --------------------------------------------------------------------------------