├── .gitignore ├── README.md ├── estimateCameraPosture2.m ├── estimatePostureBoardFunc.m ├── estimatePosturePointCloudFunc.m ├── getCameraParameters.m ├── getExtrinsicParameters.m ├── getGroundPlaneFromPointCloud.m ├── main.m ├── niPcFromImage.m ├── noiseAnalysis.m ├── result ├── gaussian.fig ├── paper.pdf ├── result.gif ├── result.jpg ├── salt.fig └── untitled.fig ├── showCameraModel.m ├── showColorSensorModel.m ├── showDepthSensorModel.m ├── showError.m └── translate2RigidMatrix.m /.gitignore: -------------------------------------------------------------------------------- 1 | /*.asv 2 | /calibImage/ 3 | /color.avi 4 | /depth.mj2 -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/README.md -------------------------------------------------------------------------------- /estimateCameraPosture2.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/estimateCameraPosture2.m -------------------------------------------------------------------------------- /estimatePostureBoardFunc.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/estimatePostureBoardFunc.m -------------------------------------------------------------------------------- /estimatePosturePointCloudFunc.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/estimatePosturePointCloudFunc.m -------------------------------------------------------------------------------- /getCameraParameters.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/getCameraParameters.m -------------------------------------------------------------------------------- /getExtrinsicParameters.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/getExtrinsicParameters.m -------------------------------------------------------------------------------- /getGroundPlaneFromPointCloud.m: -------------------------------------------------------------------------------- 1 | function [az, ax, H, orientation, location, isUsed, model, planePc, pc_new] = getGroundPlaneFromPointCloud( pc ) 2 | %getGroundPlaneFromPointCloud ������ά���ƹ��Ƴ���ƽ������������� 3 | % pc: ��ά���� 4 | 5 | maxDistance = 1; 6 | referenceVector = [0,0,1]; 7 | maxAngularDistance = 90; 8 | delta = 0.01 9 | 10 | isContinue = true; 11 | isUsed = true; 12 | 13 | remainPtCloud = pc; 14 | invalidTform = affine3d; 15 | while isContinue 16 | % ����ƽ��ģ�� 17 | [model,inlierIndices,outlierIndices] = pcfitplane(remainPtCloud, maxDistance,referenceVector,maxAngularDistance); 18 | model 19 | 20 | planePc = select(pc,inlierIndices); 21 | remainPtCloud = select(pc,outlierIndices); 22 | if remainPtCloud.Count < pc.Count*delta 23 | isUsed = false; 24 | break; 25 | end 26 | tform = getExternalParameterFromPointCloud(model); % ����ƽ�淨������ȡ�������� 27 | if isequal(tform.T, invalidTform.T) 28 | H = 0; az = 0; ax = 0; orientation=0; location=0; pc_new=0; 29 | isUsed = false; 30 | break; 31 | end 32 | pc_new = pctransform(pc, tform); % ������ת��Ϊ��������ϵ�µĵ��� 33 | 34 | orientation = tform.T(1:3, 1:3); % ��ȡ��ת���� 35 | location = tform.T(4, 1:3); % ��ȡƽ�ƾ��� 36 | 37 | [az, ax] = getCameraAngle(orientation); 38 | H = location(3); 39 | 40 | %������������ 41 | tformC2W = invert(tform); 42 | pc_remain_world = pctransform(remainPtCloud, tformC2W); 43 | pc_plane_world = pctransform(planePc, tformC2W); 44 | isContinue = ~judgeTheCondition(az, pc_remain_world, pc_plane_world); 45 | end 46 | 47 | end 48 | 49 | function [isFinish] = judgeTheCondition(az, pc_world, pc_plane) 50 | % judgeTheCondition: �жϵ�ǰƽ���Ƿ�����Ҫ�󣬹�����������Z����ƽ��н�С��45�� && 51 | % ƽ���ϵĵ�Zֵ����λ����ʣ�������ķ�λ���� 52 | % pc_world: ��ȥƽ���ϵĵ��ʣ��� 53 | % pc_plane: ƽ���ϵĵ� 54 | % ����ֵ�����������������������isFinish = true�� ���� isFinish=false 55 | 56 | if pc_world.Count == 0 || pc_plane.Count == 0 57 | isFinish = true; 58 | return ; 59 | end 60 | 61 | if abs(az) > pi / 4 62 | isFinish = false; 63 | return ; 64 | end 65 | 66 | %ͳ��pc_plane ��Zֵ����λ�� 67 | word_point_z = prctile(pc_world.Location(:, 3), 50); 68 | plane_point_z = median(pc_plane.Location(:, 3)); 69 | if(plane_point_z < word_point_z) 70 | isFinish = false; 71 | return ; 72 | end 73 | 74 | isFinish = true; 75 | end 76 | 77 | function [isUsed] = isCorrectPc(pc, model) 78 | %isCorrectPc judge whether current model is correct ground 79 | %pc point cloud 80 | %model plane model 81 | 82 | % get the external parameter from model 83 | % translate pc to world coordinate 84 | % judge current model 85 | 86 | end 87 | 88 | function [tform] = getExternalParameterFromPointCloud(model) 89 | %getExternalParameterFromPointCloud get the external parameter from 90 | %plane model 91 | %model plane model 92 | 93 | param = model.Parameters; 94 | H = param(4); 95 | 96 | % get world axis in camera coordinate 97 | oc = [0 0 0]'; 98 | ow = getProjectedPoint(oc, model); % ����������ϵԭ����ƽ���ϵ�ͶӰ 99 | zc = [0 0 1]'; 100 | yw = getProjectedPoint(zc, model) - ow; %��ȡ������Z����ƽ���ϵ�ͶӰ 101 | 102 | if norm(yw) == 0 103 | tform = affine3d; 104 | return ; 105 | end 106 | 107 | yw = yw/norm(yw); 108 | 109 | zw = ow - oc; 110 | zw = zw / norm(zw); 111 | 112 | xw= cross(yw,zw); % ��������������ȡ 113 | 114 | % world coordinate points to camera coordinate points 115 | R = [xw yw zw]; 116 | T = -ow'*R; 117 | R = translate2RigidMatrix(R); 118 | %camera coordinate points to world coordinate points 119 | if sum(sum(isnan(R))) > 0 120 | tform = affine3d; 121 | return ; 122 | end 123 | tform = affine3d([R zeros(3, 1);T 1]); 124 | end 125 | 126 | function [pp] = getProjectedPoint(p, model) 127 | %getProjectedPoint get p's projected point pp 128 | %p: 3-D points 129 | %model: plane model 130 | 131 | param = model.Parameters; 132 | A = [param(1) param(2) param(3) 0; 1 0 0 param(1); 0 1 0 param(2); 0 0 1 param(3)]; 133 | b = [-param(4) p(1) p(2) p(3)]'; 134 | pp = A\b; 135 | pp = pp(1:3); 136 | end 137 | 138 | function [R t] = rigidTransform3D(A, B) 139 | % rigidTransform3D get transformation between 140 | % cameraPts and worldPts by the corresponding points 141 | % A: points in camera coordinate 142 | % B: points in world coordinate 143 | % see: http://nghiaho.com/?page_id=671 144 | % see: http://nghiaho.com/uploads/code/rigid_transform_3D.m 145 | 146 | if nargin ~= 2 147 | error('Missing parameters'); 148 | end 149 | 150 | %assert(size(A) == size(B)) 151 | 152 | centroid_A = mean(A); 153 | centroid_B = mean(B); 154 | 155 | N = size(A,1); 156 | H = (A - repmat(centroid_A, N, 1))' * (B - repmat(centroid_B, N, 1)); 157 | [U,S,V] = svd(H); 158 | R = V*U'; 159 | if det(R) < 0 160 | %printf("Reflection detected\n"); 161 | V(:,3) = V(:,3)*(-1); 162 | R = V*U'; 163 | end 164 | 165 | t = -R*centroid_A' + centroid_B' 166 | 167 | R = R'; 168 | t = t'; 169 | end 170 | 171 | function [pc_new] = transformPointCloud(pc, tform) 172 | % transformPointCloud: transform point cloud pc to pc_new by tform (didn't 173 | % need rigid matrix) 174 | % note: pctransform can do the same work. but it needs rigid transform 175 | % tform: transformation p*tform.T 176 | % pc: original point cloud 177 | 178 | pts = pc.Location; 179 | M = size(pts, 1); 180 | N = size(pts, 2); 181 | A = M*N; 182 | pts = reshape(pts, [A, 3]); 183 | pts = [pts ones(A, 1)]; 184 | pts = pts*tform.T; 185 | pts = pts(:, 1:3); 186 | pts = reshape(pts, [M, N, 3]); 187 | pc_new = pointCloud(pts); 188 | end 189 | 190 | function [az, ax] = getCameraAngle(orientation) 191 | %getCameraAngle Get the angle of camera. 192 | % orientation: Camera's orientation (translate points in camera coordinate 193 | % to points in world coordinate. 194 | nw = [0 0 1]; % the ground norm vector 195 | zw = [0 0 1] * orientation; 196 | az = atan2(norm(cross(nw,zw)),dot(nw,zw)); 197 | 198 | xw = [1 0 0] * orientation; 199 | ax = atan2(norm(cross(nw,xw)),dot(nw,xw)) - pi/2; 200 | end -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/main.m -------------------------------------------------------------------------------- /niPcFromImage.m: -------------------------------------------------------------------------------- 1 | function [ pc ] = niPcFromImage( imgD ) 2 | %niPcFromImage get the point cloud from depth image 3 | % imgD: The depth image 4 | 5 | % the common parameters 6 | s = size(imgD); 7 | H = s(1); 8 | W = s(2); 9 | xyzPts = zeros(H, W, 3); 10 | s = 1; 11 | 12 | % primesense parameter 13 | hFov = deg2rad(45); 14 | vFov = deg2rad(57); 15 | cx = W/2; 16 | cy = H/2; 17 | fx = W/(2*tan(hFov/2)); 18 | fy = H/(2*tan(vFov/2)); 19 | 20 | for r = 1 : H 21 | for c = 1 : W 22 | d = double(imgD(r, c))*s; 23 | if imgD(r, c)==0 24 | xyzPts(r, c, :) = NaN; 25 | continue; 26 | end 27 | x = (c-cx)/fx*d; 28 | y = (r-cy)/fy*d; 29 | z = d; 30 | xyzPts(r, c, :) = [x y z]; 31 | end 32 | end 33 | pc = pointCloud(xyzPts); 34 | end 35 | 36 | -------------------------------------------------------------------------------- /noiseAnalysis.m: -------------------------------------------------------------------------------- 1 | clc;clear all; 2 | fileNameC = 'color.avi'; 3 | fileNameD = 'depth.mj2'; 4 | 5 | % using color sensor to get camera's parameter 6 | startTime = 3; %��ʼץȡ��Ƶ֡��ʱ��(s) 7 | totalImageNum = 30; %ץȡ��Ƶ֡������ 8 | gapImageNum = 30; %ץȡ��Ƶ֡�ļ�� 9 | size = [7 9]; %���̴�СΪ7xm 10 | squareSizeInMM = 40; %���������εij�Ϊ40mm 11 | 12 | % ����Ƶ�д���Ƶ�ĵ�3s��ʼ��ÿ��30֡ѡȡ30֡��Ƶ֡������ѡȡ����Ƶ֡��������ڲα궨 13 | [ cameraParams, estimationErrors ] = getCameraParameters('color.avi', startTime, 20, 30, [7 9], 40); 14 | worldPoints = cameraParams.WorldPoints; 15 | 16 | % ����Ƶ�������õ�ǰʱ�� 17 | vc = VideoReader('color.avi'); 18 | vc.CurrentTime = 929/ vc.FrameRate; 19 | vd = VideoReader('depth.mj2'); 20 | vd.CurrentTime = 929/ vd.FrameRate; 21 | 22 | i = 1; 23 | % figure; 24 | B = []; C = []; X=[]; 25 | 26 | if hasFrame(vd) && hasFrame(vc) 27 | %% ���ݲ�ɫͼ������������ 28 | imgC = readFrame(vc); 29 | 30 | [az, ax, H, orientation, location, isUsed, imagePoints] = estimatePostureBoardFunc( imgC, cameraParams ); 31 | B = [B; [rad2deg(az) rad2deg(ax) -H/10]]; 32 | 33 | 34 | %% �������ͼ������������ 35 | imgD = readFrame(vd); 36 | figure 37 | for i = 1:0.5:40 38 | i 39 | imageProcess = imgD; 40 | % imageProcess = addNoise(imageProcess, 'salt & pepper', i/100.0); 41 | imageProcess = addNoise(imageProcess, 'gaussian', i/100.0); 42 | % imshow(imageProcess, [0, 5000]) 43 | [ az, ax, H, orientation, location, isUsed, pc, model, planePc, pc_new ] = estimatePosturePointCloudFunc( imageProcess ); 44 | if ~isUsed 45 | continue 46 | end 47 | 48 | C = [C; [rad2deg(az) rad2deg(ax) -H/10]]; 49 | X = [X; i/100.0]; 50 | % subplot(1,2,1); 51 | % imshow(imageProcess, [0, 9000]) 52 | % subplot(1,2,2); 53 | showError(B, C, X); 54 | pause(0.01) 55 | end 56 | 57 | end 58 | 59 | 60 | function showError(B, C, X) 61 | subplot(2, 3, 6); 62 | D = abs(B - C); 63 | % x = 1:size(D, 1); 64 | y1 = D(:, 1); 65 | y2 = D(:, 2); 66 | y3 = D(:, 3); 67 | plot(X, y1, 'r-',X, y2, 'g-', X, y3, 'b-'); 68 | legend('\Delta\theta', '\Delta\phi', '\Delta H'); 69 | end 70 | 71 | function img = addNoise(img, type, r) 72 | minP = double(min(img(:))); 73 | maxP = double(max(img(:))); 74 | img = (double(img) - minP) ./ (maxP-minP); 75 | % img = imnoise(img, 'gaussian', r); 76 | img = imnoise(img, type, r); 77 | img = uint16(img.*(maxP-minP)+minP); 78 | end -------------------------------------------------------------------------------- /result/gaussian.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/result/gaussian.fig -------------------------------------------------------------------------------- /result/paper.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/result/paper.pdf -------------------------------------------------------------------------------- /result/result.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/result/result.gif -------------------------------------------------------------------------------- /result/result.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/result/result.jpg -------------------------------------------------------------------------------- /result/salt.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/result/salt.fig -------------------------------------------------------------------------------- /result/untitled.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shijieS/RGBDCameraExtrinsicCalibration/2c5b4bbd341b52ac030e503322fa384bcf1e2e4f/result/untitled.fig -------------------------------------------------------------------------------- /showCameraModel.m: -------------------------------------------------------------------------------- 1 | function [ ] = showCameraModel( location, orientation, squareSizeInMM, worldPoints ) 2 | %showCameraModel show the camera model in 3D space 3 | % h: the figure handle 4 | % location: the camera location 5 | % orientation: the camera orientation 6 | % squareSizeInMM: the board size 7 | % worldPoints: board point 8 | 9 | plotCamera('AxesVisible', true, 'Location', location,'Orientation', orientation,'Size',squareSizeInMM*2); 10 | %cameratoolbar('SetMode','orbit'); 11 | hold on 12 | plot3(worldPoints(:, 1), worldPoints(:, 2), zeros(length(worldPoints), 1), '.'); 13 | plot3(0,0,0,'g.'); 14 | %set(gca,'CameraUpVector',[0 0 -1]); % make the z axis point down 15 | offset = squareSizeInMM*5; 16 | plot3(offset*[1 0 0 0 0],offset*[0 0 1 0 0],... 17 | offset*[0 0 0 0 1],'b-','linewidth',2); 18 | 19 | grid on 20 | axis equal 21 | axis manual 22 | xlabel('X (mm)'); 23 | ylabel('Y (mm)'); 24 | zlabel('Z (mm)'); 25 | xlim([-1000,1000]); 26 | ylim([-500,1500]); 27 | zlim([-1700, 200]); 28 | 29 | hold off 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /showColorSensorModel.m: -------------------------------------------------------------------------------- 1 | function showColorSensorModel(orientation, location, squareSizeInMM, worldPoints, imagePoints, isUsed) 2 | if ~isUsed 3 | return ; 4 | end 5 | subplot(2, 3, 1);hold on; 6 | plot(imagePoints(:, 1), imagePoints(:, 2), 'ro'); 7 | hold off 8 | 9 | subplot(2, 3, 3) 10 | showCameraModel( location, orientation, squareSizeInMM, worldPoints ); 11 | end 12 | 13 | -------------------------------------------------------------------------------- /showDepthSensorModel.m: -------------------------------------------------------------------------------- 1 | 2 | function showDepthSensorModel(location, orientation, pc, model, pc_new, isUsed) 3 | if ~isUsed 4 | return ; 5 | end 6 | 7 | subplot(2, 3, 4); 8 | pcshow(pc);hold on 9 | plot(model); 10 | plotCamera('Location',zeros(1,3), 'Orientation',eye(3),'Size',60); 11 | xlabel('X_C(mm)');ylabel('Y_C(mm)'); zlabel('Z_C(mm)'); 12 | hold off; 13 | 14 | subplot(2, 3, 5); 15 | pcshow(pc_new);hold on; 16 | plotCamera('AxesVisible', true, 'Location',location,'Orientation',orientation,'Size',60); 17 | xlabel('X_W(mm)');ylabel('Y_W(mm)'); zlabel('Z_W(mm)'); 18 | hold off; 19 | end -------------------------------------------------------------------------------- /showError.m: -------------------------------------------------------------------------------- 1 | function showError(B, C) 2 | subplot(2, 3, 6); 3 | D = abs(B - C); 4 | x = 1:size(D, 1); 5 | y1 = D(:, 1); 6 | y2 = D(:, 2); 7 | y3 = D(:, 3); 8 | plot(x, y1, 'r-', x, y2, 'g-', x, y3, 'b-'); 9 | legend('\Delta\theta', '\Delta\phi', '\Delta H'); 10 | end -------------------------------------------------------------------------------- /translate2RigidMatrix.m: -------------------------------------------------------------------------------- 1 | function [ D ] = translate2RigidMatrix( M ) 2 | %translate2RigidMatrix translate matrix to rigid matrix 3 | %note: it is an experience to translate the matrix to rigid matrix. firstly 4 | %we translate the matrix to eul angle, then we translate euler angle to 5 | %matrix again. 6 | % M: the translating matrix 7 | D = eul2rotm(rotm2eul(M)); 8 | end 9 | 10 | --------------------------------------------------------------------------------