本篇文章主要给出光平面标定代码，鉴于自身水平所限，如有错误，欢迎批评指正。（欢迎进Q群交流：874653199）
数据分为棋盘格数据和激光条数据，激光条数据为在第22个位姿至第26个位姿下打在棋盘格标定板上采集的图像。
clc;
clear;%% 相机标定
image_filename='.\Calibration';%图像路径physicalLength=10;%物理长度image_format='jpeg';%图像格式namelist = dir([image_filename '\*.' image_format]);num = length(namelist);for i = 1:numfilename=namelist(i).name;filename_new=[image_filename '\' filename];imageFileNames{i}=filename_new;
end[imagePoints, boardSize, imagesUsed] = detectCheckerboardPoints(imageFileNames);
imageFileNames = imageFileNames(imagesUsed);originalImage = imread(imageFileNames{1});
[mrows, ncols, ~] = size(originalImage);squareSize = physicalLength; % in units of 'millimeters'
worldPoints = generateCheckerboardPoints(boardSize, squareSize);[cameraParams, imagesUsed, estimationErrors] = estimateCameraParameters(imagePoints, worldPoints, ...'EstimateSkew', false, 'EstimateTangentialDistortion', true, ...'NumRadialDistortionCoefficients', 2, 'WorldUnits', 'millimeters', ...'InitialIntrinsicMatrix', [], 'InitialRadialDistortion', [], ...'ImageSize', [mrows, ncols]);% h1=figure; showReprojectionErrors(cameraParams);
% h2=figure; showExtrinsics(cameraParams, 'CameraCentric');
% displayErrors(estimationErrors, cameraParams);
% undistortedImage = undistortImage(originalImage, cameraParams);IntrinsicMatrix=cameraParams.IntrinsicMatrix;
IntrinsicMatrix=IntrinsicMatrix';RadialDistortion=cameraParams.RadialDistortion;TangentialDistortion=cameraParams.TangentialDistortion;MeanReprojectionError =cameraParams.MeanReprojectionError;%% 光平面标定
boardSize=boardSize-1;path_laser='.\Laser';[a, b, c, d] = calibrateLightPlane(cameraParams, 22:26, path_laser,[8,12],0.3, 1);%% 写入标定结果
tempFile=[image_filename '\标定结果.txt'];
fid=fopen(tempFile,'w');fprintf(fid,'相机内参：\n');
for m=1:3
for n=1:3fprintf(fid,'%.6f ',IntrinsicMatrix(m,n));
endfprintf(fid,'\n');
endfprintf(fid,'畸变系数 k1,k2,p1,p2：\n');
fprintf(fid,'%.6f \n',RadialDistortion(1,1));
fprintf(fid,'%.6f \n',RadialDistortion(1,2));
fprintf(fid,'%.6f \n',TangentialDistortion(1,1));
fprintf(fid,'%.6f \n',TangentialDistortion(1,2)); fprintf(fid,'反投影误差：\n');
fprintf(fid,'%.6f \n',MeanReprojectionError(1,1)); fprintf(fid,'光平面系数：\n');
fprintf(fid,'%.6f \n',a);
fprintf(fid,'%.6f \n',b);
fprintf(fid,'%.6f \n',c);
fprintf(fid,'%.6f \n',d); fclose(fid);%% 光平面标定
function [a, b, c, d] = calibrateLightPlane(cameraParams, laserImagesIndex, laserImagesPath,boardSize, thresh, plotFlag)intriMatrix = cameraParams.IntrinsicMatrix';
radialDistortion = cameraParams.RadialDistortion;
tangentialDistortion=cameraParams.TangentialDistortion;
M1 = [intriMatrix(1,:), 0; intriMatrix(2,:), 0; intriMatrix(3,:), 0];allLaserPoint = [];
len = length(laserImagesIndex);
for i = 1 : lenindex = laserImagesIndex(i);R = cameraParams.RotationMatrices(:, :, index)';T = cameraParams.TranslationVectors(index, :)'; M2 = [R, T; 0, 0, 0, 1];M = M1 * M2;M = [M(:, 1:2), M(:, 4)];frame = imread([laserImagesPath, '\', num2str(index), '_laser.jpeg']);if(numel(size(frame))>2)frame = rgb2gray(frame);endpoints = cameraParams.ReprojectedPoints(:, :, index);col = [points(1,1), points(boardSize(1), 1), points(boardSize(1) * boardSize(2), 1), points(boardSize(1) * (boardSize(2) - 1)+1, 1)];row = [points(1,2), points(boardSize(1), 2), points(boardSize(1) * boardSize(2), 2), points(boardSize(1) * (boardSize(2) - 1)+1, 2)];BW = roipoly(frame, col, row);frame = uint8(BW) .* frame;laserPixel = getLaserCenter(frame, thresh, 50, plotFlag);laserPixel=undistortCoor(intriMatrix,radialDistortion,tangentialDistortion,laserPixel);laserpixel = [laserPixel(:, 1)'; laserPixel(:, 2)'; ones(1, length(laserPixel(:, 1)))];laser_wcs = pinv(M)*laserpixel;laser_wcs(1, :) = laser_wcs(1, :) ./laser_wcs(3,:);laser_wcs(2, :) = laser_wcs(2, :) ./laser_wcs(3,:);laser_wcs = [laser_wcs(1, :)', laser_wcs(2, :)', zeros(length(laserpixel(1, :)), 1), ones(length(laserpixel(1, :)), 1)];laser_ccs = (M2 * laser_wcs')';allLaserPoint = [allLaserPoint; laser_ccs];
endplaneData = allLaserPoint(:, 1:3);
xyz0=mean(planeData, 1);
centeredPlane=bsxfun(@minus, planeData, xyz0);
[U, S, V]=svd(centeredPlane);a=V(1,3);
b=V(2,3);
c=V(3,3);
d=-dot([a b c], xyz0);fprintf(' 光平面方程为: %f * X + %f * Y + %f * Z + %f = 0 \n', a, b, c, d);% 绘制拟合结果
if plotFlag ~= 0figure;rangeX = max(allLaserPoint(:, 1)) - min(allLaserPoint(:, 1));rangeY = max(allLaserPoint(:, 2)) - min(allLaserPoint(:, 2));y_buff = linspace(min(allLaserPoint(:, 2)) - min(rangeX, rangeY), ...max(allLaserPoint(:, 2)) + min(rangeX, rangeY), 100);x_buff = linspace(min(allLaserPoint(:, 1)) - min(rangeX, rangeY), ...max(allLaserPoint(:, 1)) + min(rangeX, rangeY), 100);[X_buff,Y_buff] = meshgrid(x_buff, y_buff);Z_buff = -d/c -a/c*X_buff -b/c *Y_buff; % 依旧是平面方程mesh(X_buff,Y_buff,Z_buff);xlabel('X'); ylabel('Y'); zlabel('Z');title('激光平面');hold on;plot3(allLaserPoint(:, 1), allLaserPoint(:, 2), allLaserPoint(:, 3), 'rx'); clear x_buff y_buff X_buff Y_buff Z_buff
endendfunction pixels = getLaserCenter(frame_grey, thresh, grey_thresh,plotFlag)
if nargin < 4plotFlag = 0;
endif max(frame_grey) < grey_threshpixels = [];return;
end[row, col] = size(frame_grey);% steger算法求中心
pixels= steger(frame_grey, thresh);if plotFlag ~= 0figure;imshow(frame_grey);hold onplot(pixels(:, 1),pixels(:, 2),'r.')hold off
endend%% steger光条中心提取
function pixel=steger(srcImage,thresh)if(length(size(srcImage))>2)srcImage=rgb2gray(srcImage);
endsigma=3;
% thresh= 0.1;%graythresh(srcImage);%otsu srcImage=double(srcImage);[m,n]=size(srcImage);ky=[-1,1];
kx=[-1;1];
kyy=[1,-2,1];
kxx=[1;-2;1];
kxy=[1,-1;-1,1];gausFilter = fspecial('gaussian',9,sigma); %高斯滤波
dstImage=imfilter(srcImage,gausFilter,'replicate'); dx=imfilter(dstImage,kx);
dy=imfilter(dstImage,ky);
dxx=imfilter(dstImage,kxx);
dyy=imfilter(dstImage,kyy);
dxy=imfilter(dstImage,kxy);hessian=zeros(2,2);
points=zeros(m*n,2);for i=1:mfor j=1:nif(srcImage(i,j)/255>thresh)hessian(1,1)=dxx(i,j);hessian(1,2)=dxy(i,j);hessian(2,1)=dxy(i,j);hessian(2,2)=dyy(i,j);[eigenvectors,eigenvalues]=eig(hessian);if(abs(eigenvalues(1,1))>= abs(eigenvalues(2,2)))nx=eigenvectors(1,1);ny=eigenvectors(2,1);fmax_dist=eigenvalues(1,1);elsenx=eigenvectors(1,2);ny=eigenvectors(2,2);fmax_dist=eigenvalues(2,2);endt=-(nx*dx(i,j)+ny*dy(i,j))/(nx*nx*dxx(i,j)+2 * nx*ny*dxy(i,j)+ny*ny*dyy(i,j));if(abs(t*nx) <= 0.5 && abs(t*ny) <= 0.5)points((i-1)*m+j,:)=[ j+t*ny,i+t*nx];endendend
endindex = find(points(:,1)==0);
points(index,:) = [];
index = find(points(:,2)==0);
points(index,:) = [];pixel=points;figure(1);
imshow(uint8(srcImage));
hold on
plot(points(:,1),points(:,2),'r.','MarkerSize',1)
hold offend%% 光条中心坐标迭代去畸变
function undistortPoints=undistortCoor(intrinsic,distortCoff1,distortCoff2,points)
fx=intrinsic(1,1);
fy=intrinsic(2,2);
cx=intrinsic(1,3);
cy=intrinsic(2,3);
k1=distortCoff1(1);
k2=distortCoff1(2);
p1=distortCoff2(1);
p2=distortCoff2(2);
k3=0;n=size(points,1);
undistortPoints=zeros(n,2);for i=1:npx=(points(i,1)-cx)/fx;py=(points(i,2)-cy)/fy;px0=px;py0=py;for k=1:6pxfang = px*px;pyfang = py*py;prfang = pxfang + pyfang; pdoublexy = 2*px*py;dk = 1 + (k3*prfang^2+k2*prfang+k1)*prfang;%径向畸变校正模型dpx= p1*(pdoublexy + p2*(prfang+2*pxfang));dpy=p1*((prfang+2*pyfang) + p1*pdoublexy);%cm,cn为进行畸变校正后的在相机像平面上的坐标px=(px0-dpx)/dk;py=(py0-dpy)/dk; endx = fx*px + cx ; y = fy*py + cy ; undistortPoints(i,:)=[x,y];
endend原始数据：
标定结果：
