标定入门及其程序

标定

模型：眼在手外

目的：确定相机在机器人基座的坐标位置，便于对后续观测点位置进行确定。搭建整体的视觉抓取平台。

ROS标定方法见苏大头师兄记录

#Easy Handeye配置_u012894550的博客-CSDN博客​blog.csdn.net

ROS下通过如下图得到图片，rviz获取位姿数据

棋盘格_matlab方法

1. 得到棋盘格并通过机械臂末端固定棋盘格

I = checkerboard(200,10,7);
I = I(1:size(I,1)/2,1:size(I,2)/2);
imsave(imshow(I));

2. 移动机械臂确保整个棋盘格都可以被看到

每个动作在变化中一定要有移动的旋转，否则会出现无法计算的问题

3. 保存相机获取的棋盘格图像，通过示教器获取机械臂位姿信息，重复>=10次

图片

位姿

4. 运行matlab代码

%-------------------------------------------------------
clear
clc
% Define images to process
imageFileNames = {'E:\抓取\10\image1.png',...
    'E:\抓取\10\image2.png',...
    'E:\抓取\10\image3.png',...
    'E:\抓取\10\image4.png',...
    'E:\抓取\10\image5.png',...
    'E:\抓取\10\image6.png',...
    'E:\抓取\10\image7.png',...
    'E:\抓取\10\image8.png',...
    'E:\抓取\10\image9.png',...
    'E:\抓取\10\image10.png',...
    };
% Detect checkerboards in images
[imagePoints, boardSize, imagesUsed] = detectCheckerboardPoints(imageFileNames);
imageFileNames = imageFileNames(imagesUsed);
​
% Generate world coordinates of the corners of the squares
squareSize = 17.5;  % in units of 'mm'
worldPoints = generateCheckerboardPoints(boardSize, squareSize);
​
% Calibrate the camera
[cameraParams, imagesUsed, estimationErrors] = estimateCameraParameters(imagePoints, worldPoints, ...
    'EstimateSkew', false, 'EstimateTangentialDistortion', false, ...
    'NumRadialDistortionCoefficients', 2, 'WorldUnits', 'mm', ...
    'InitialIntrinsicMatrix', [], 'InitialRadialDistortion', []);
​
% View reprojection errors
h1=figure; showReprojectionErrors(cameraParams, 'BarGraph');
​
% Visualize pattern locations
h2=figure; showExtrinsics(cameraParams, 'CameraCentric');
​
% Display parameter estimation errors
displayErrors(estimationErrors, cameraParams);
​
Trans = cameraParams.TranslationVectors;
Rot = cameraParams.RotationMatrices;
for i=1:size(Trans,1)
    tempc =  [Rot(:,:,i),Trans(i,:)';0 0 0 1];
    TRMarker2Camera(:,:,i) = tempc; %存储每一个棋盘格在相机坐标系下的旋转矩阵
end
​
%读取机械臂末端到base的姿态变换即为Tre2rb 这个地方可能有问题需要
load endpose
for i=1:size(Trans,1)
rpy = endpose(:,4:6,i);
r=[rpy(1),rpy(2),rpy(3)];
theta = sqrt(rpy(1) * rpy(1) + rpy(2) * rpy(2) + rpy(3) * rpy(3));
kx = rpy(1)/theta;
ky = rpy(2)/theta;
kz = rpy(3)/theta;
r1 = kx * kx * (1-cos(theta))+ cos(theta); 
r2 = kx * ky * (1-cos(theta))- kz * sin(theta);
r3 = kx * kz * (1-cos(theta))+ ky * sin(theta);
r4 = kx * ky * (1-cos(theta))+ kz * sin(theta);
r5 = ky * ky * (1-cos(theta))+ cos(theta);
r6 = ky * kz * (1-cos(theta))- kx * sin(theta);
r7 = kx * kz * (1-cos(theta))- ky * sin(theta);
r8 = ky * kz * (1-cos(theta))+ kx * sin(theta);
r9 = kz * kz * (1-cos(theta))+ cos(theta);
dcm = [r1 r2 r3 ; r4 r5 r6 ; r7 r8 r9];
Tre2rb(:,:,i) = [dcm,endpose(:,1:3,i)';0 0 0 1]; 
TRrb2re(:,:,i)=inv(Tre2rb(:,:,i));
​
end
​
n=size(TRMarker2Camera,3);
A=[];
B=[];
C=[];
d=[];
M=zeros(3,3);
l=0;
​
for i=[1:size(Trans,1)-1]
   l=l+1;
    B(:,:,l)=(TRMarker2Camera(:,:,i+1))/TRMarker2Camera(:,:,i); 
    
    Tb(:,:,l)=B(1:3,4,l);  % translation matrix of B
    
    A(:,:,l)= TRrb2re(:,:,i+1)\(TRrb2re(:,:,i)); % multiplication A = inv(Deri+1) * Deri
   
    Ra(:,:,l)=A(1:3,1:3,l);  % tranlation and rotation matrix of A
    Ta(:,:,l)=A(1:3,4,l);
    
    alpha=logMatrix(A(:,:,l));   %matrix logarithms
    beta=logMatrix(B(:,:,l));
​
    M=M+beta*alpha';
  
end
Rx=(M'*M)^(-1/2)*M';  
​
for i=1:l
    C=[C; eye(3)-Ra(:,:,i)];
    d=[d; Ta(:,:,i)-Rx*Tb(:,:,i)];
end
​
Tx=(C'*C)\(C'*d);
​
TRCamera2base=[Rx Tx; 0 0 0 1];
​
Rerr=0;
Terr=0;
for i=1:l
    ZUOCE(:,:,i)=A(:,:,i)*TRCamera2base;
    zuocezuobiao(1:3,i)=ZUOCE(1:3,4,i);
    YOUCE(:,:,i)=TRCamera2base*B(:,:,i);
    yuocezuobiao(1:3,i)=YOUCE(1:3,4,i);
AXsubtractXB(:,:,i)=A(:,:,i)*TRCamera2base-TRCamera2base*B(:,:,i);
Rerr=Rerr+sqrt((sum(AXsubtractXB(1:3,1,i).^2)+sum(AXsubtractXB(1:3,2,i).^2)+sum(AXsubtractXB(1:3,3,i).^2))/9);
RERR(1,i)=sqrt((sum(AXsubtractXB(1:3,1,i).^2)+sum(AXsubtractXB(1:3,2,i).^2)+sum(AXsubtractXB(1:3,3,i).^2))/9);
Terr=Terr+sqrt(sum(AXsubtractXB(1:3,4,i).^2)/3);
TERR(1,i)=sqrt(sum(AXsubtractXB(1:3,4,i).^2)/3);
end
subplot(2,1,1)
plot(RERR(1,:));
set(gca,'xtick',0:1:l)  
subplot(2,1,2)
plot(TERR(1,:));
set(gca,'xtick',0:1:l)  
% figure
% plot3(zuocezuobiao(1,:),zuocezuobiao(2,:),zuocezuobiao(3,:),'r',yuocezuobiao(1,:),yuocezuobiao(2,:),yuocezuobiao(3,:),'b')
ERR0=[Rerr/l,Terr/l]
TRCamera2base(1:3,4)=TRCamera2base(1:3,4)/1000
TRMarkertoHololens=inv(TRCamera2base);