光平面标定代码

        本篇文章主要给出光平面标定代码，鉴于自身水平所限，如有错误，欢迎批评指正。（欢迎进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:num
    filename=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:3
    fprintf(fid,'%.6f     ',IntrinsicMatrix(m,n)); 
end
    fprintf(fid,'\n'); 
end

fprintf(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 : len
    index = 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);
    end
    points = 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];
end


planeData = 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 ~= 0
    figure;
    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
end

end


function pixels = getLaserCenter(frame_grey, thresh, grey_thresh,plotFlag)
if nargin < 4
    plotFlag = 0; 
end

if max(frame_grey) < grey_thresh
    pixels = [];
    return;
end

[row, col] = size(frame_grey);

% steger算法求中心
pixels= steger(frame_grey, thresh);

if plotFlag ~= 0
    figure;
    imshow(frame_grey);
    hold on
    plot(pixels(:, 1),pixels(:, 2),'r.')
    hold off
end

end

%% steger光条中心提取
function pixel=steger(srcImage,thresh)

if(length(size(srcImage))>2)
  srcImage=rgb2gray(srcImage);  
end

sigma=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:m
    for j=1:n
        if(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);
            else
            nx=eigenvectors(1,2);
            ny=eigenvectors(2,2);
            fmax_dist=eigenvalues(2,2);
                
            end
            
            t=-(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];
            end
            
            
        end
        
    end
end

index = 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 off


end


%% 光条中心坐标迭代去畸变
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:n
    px=(points(i,1)-cx)/fx;
    py=(points(i,2)-cy)/fy;
    px0=px;py0=py;
           for k=1:6
            pxfang = 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; 
           end
        x = fx*px + cx ;  
        y = fy*py + cy ; 
       undistortPoints(i,:)=[x,y]; 
end

end

原始数据：

标定结果：