光平面标定代码
本篇文章主要给出光平面标定代码，鉴于自身水平所限，如有错误，欢迎批评指正。（欢迎进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原始数据：
标定结果：
原文地址：https://blog.csdn.net/weixin_43997893/article/details/142928521
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！