基于双目视觉的非标机械臂的空间定位流程（未完待续）

系统

接上一次的非标机构完成双目视觉的空间定位：
https://blog.csdn.net/woshigaowei5146/article/details/123636453?spm=1001.2014.3001.5501

目标：利用末端执行器附近的双目相机获取目标的空间位置，再利用非标机械臂完成零件的安装。

大致流程如下：

坐标系变换原理

双目标定

原理

双目标定的目的是获得两个相机的内参、外参、和两个相机之间的位置关系。

相机标定方法可分为两种，第一种是需要参照物的传统标定方法；另一种则是不需参照物的相机自标定法。

第一种：张正友标定法和 Tasi 两步标定法。第二种：基于 Kruppa 方程的标定法

张正友标定法的基本步骤是：在不同角度下，对标定参考物(棋盘格)进行拍摄，然后提取出棋盘格的顶点，接着解析出相机的畸变系数和内外参数，最后再根据极大似然估计，对参数进行 优化。

准备

• 左右两个相机的焦距应该保持一致：调整两个摄像头的焦距相同的方法：离两个相机相同远处放置标定板，分别调节两个相机的焦距，使得两个画面的清晰度相似；
• 双目相机标定时的照片必须是左右相机同时拍摄的；
• Matlab的标定只能用棋盘格，不能用圆点，需根据视场大小、焦距等参数提前准备；
• 两个相机的连线交点最好和焦点不要差太多（自己的体会）；

步骤

LabVIEW的双目标定很难用，很难找到标定点。可以用标定工具NI Calibration Trianing Interface。

可参考：https://mp.weixin.qq.com/s/kcecV6PNE92FB8ugSoV4tw

所以，选择用matlab中的双目标定模块或输入stereoCameraCalibrator。

其他：
1.是否使用先前标定好的相机参数，如果使用，存入箭头5指向的地方
2.畸变参数的不同表达形式（3个参数表达往往更精准）
3.是否认为图像坐标系x和y轴是垂直的，如果勾选，那内参矩阵会有细微变化，可以自己观察一下
4.是否计算切向畸变
5.优化预设参数（内参矩阵）
6.开始标定

直接选择2 Coefficirents，因为在我们标定板精度不高时，采用较高的畸变参数可能会出现问题。

导入左，右相机拍摄的图片文件夹，同时设置棋盘格宽度。数量建议20张以上。从不同的角度拍摄图片：

Size of checkerboard square为棋盘中每一个方格的长度，单位为毫米。

自己写了个双相机同时拍摄的程序用于抓图：（等相机稳定下来再拍）

https://download.csdn.net/download/woshigaowei5146/85290969?spm=1001.2014.3001.5503

输入单目标定时得到的内参矩阵（也可不输）。校准过程中可以根据Reprojection Errors曲线，手动去除，降低误差。

【若误差非常大，首先去除几个误差较大的组，如果不行可能调焦没有清晰，重新拍照，如果还不行重新打开MATLAB就好了，不知道什么原因】

计算完成之后，参考单目标定过程，导出计算结果。CameraParameters1为左摄像头的单独标定参数，CameraParameters2为右摄像头的单独标定参数。
stereoParams.TranslationOfCamera2：可以直接使用。
stereoParams.RotationOfCamera2：如需要在其他地方使用该矩阵，需转置以后使用

CameraParameters1内：

RadialDistortion：径向畸变，来源于光学透镜的特性，由K1，K2，K3确定。
TangentialDistortion：切向畸变，相机装配误差，传感器与光学镜头非完全平行，由两个参数P1，P2确定。
【参数的排放顺序，即K1，K2，P1，P2，K3】
IntrinsicMatrix：存放的是摄像头的内参，只与摄像机的内部结构有关，需要先转置再使用。
FundamentalMatrix：基础矩阵。
FundamentalMatrix ：本质矩阵。

标定结果保存为mat，方便以后应用
save(‘my1stereoParams.mat’ , ‘stereoParams’);

或者直接保存Session（两种都可以）

或保存为excel:（参数需要选择为3 Coefficients）

rowName = cell(1,10);
rowName{1,1} = '平移矩阵';
rowName{1,2} = '旋转矩阵';
rowName{1,3} = '相机1内参矩阵';
rowName{1,4} = '相机1径向畸变';
rowName{1,5} = '相机1切向畸变';
rowName{1,6} = '相机2内参矩阵';
rowName{1,7} = '相机2径向畸变';
rowName{1,8} = '相机2切向畸变';
rowName{1,9} = '相机1畸变向量';
rowName{1,10} = '相机2畸变向量';
xlswrite('out.xlsx',rowName(1,1),1,'A1');
xlswrite('out.xlsx',rowName(1,2),1,'A2');
xlswrite('out.xlsx',rowName(1,3),1,'A5');
xlswrite('out.xlsx',rowName(1,4),1,'A8');
xlswrite('out.xlsx',rowName(1,5),1,'A9');
xlswrite('out.xlsx',rowName(1,6),1,'A10');
xlswrite('out.xlsx',rowName(1,7),1,'A13');
xlswrite('out.xlsx',rowName(1,8),1,'A14');
xlswrite('out.xlsx',rowName(1,9),1,'A15');
xlswrite('out.xlsx',rowName(1,10),1,'A16');
xlswrite('out.xlsx',stereoParams.TranslationOfCamera2,1,'B1');  % 平移矩阵
xlswrite('out.xlsx',stereoParams.RotationOfCamera2.',1,'B2');  % 旋转矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters1.IntrinsicMatrix.',1,'B5');  % 相机1内参矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters1.RadialDistortion,1,'B8');  % 相机1径向畸变(1,2,5)
xlswrite('out.xlsx',stereoParams.CameraParameters1.TangentialDistortion,1,'B9');  % 相机1切向畸变(3,4)
xlswrite('out.xlsx',stereoParams.CameraParameters2.IntrinsicMatrix.',1,'B10');  % 相机2内参矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters2.RadialDistortion,1,'B13');  % 相机2径向畸变(1,2,5)
xlswrite('out.xlsx',stereoParams.CameraParameters2.TangentialDistortion,1,'B14');  % 相机2切向畸变(3,4)
xlswrite('out.xlsx',[stereoParams.CameraParameters1.RadialDistortion(1:2), stereoParams.CameraParameters1.TangentialDistortion,...
    stereoParams.CameraParameters1.RadialDistortion(3)],1,'B15');  % 相机1畸变向量
xlswrite('out.xlsx',[stereoParams.CameraParameters2.RadialDistortion(1:2), stereoParams.CameraParameters2.TangentialDistortion,...
    stereoParams.CameraParameters2.RadialDistortion(3)],1,'B16');  % 相机2畸变向量

• Matlab标定函数：

生成和检测校准模式
detectCheckerboardPoints 检测图像中的棋盘格图案
generateCheckerboardPoints 生成棋盘角位置
detectCircleGridPoints 检测图像中的圆形网格图案
generateCircleGridPoints 生成圆网格点位置
vision.calibration.PatternDetector 用于定义自定义平面图案检测器的界面

估计相机参数
针孔相机
estimateCameraParameters 校准单个或立体相机
estimateCameraMatrix 从世界到图像点的对应关系估计相机投影矩阵

鱼眼相机
estimateFisheyeParameters 校准鱼眼相机

立体相机
estimateStereoBaseline 估计立体相机的基线

存储结果
单相机
cameraParameters 用于存储相机参数的对象
cameraIntrinsics 用于存储相机内在参数的对象
cameraMatrix 相机投影矩阵

鱼眼相机
fisheyeIntrinsics 用于存储鱼眼相机参数的对象
fisheyeParameters 存储鱼眼相机参数的对象

立体相机
stereoParameters 用于存储立体相机系统参数的对象

错误度量
cameraCalibrationErrors 用于存储估计相机参数的标准误差的对象
stereoCalibrationErrors 用于存储估计立体参数的标准误差的对象
extrinsicsEstimationErrors 用于存储估计的相机内在函数和失真系数的标准误差的对象
intrinsicsEstimationErrors 用于存储估计的相机内在函数和失真系数的标准误差的对象
fisheyeCalibrationErrors 用于存储估计鱼眼相机参数的标准误差的对象
fisheyeIntrinsicsEstimationErrors 用于存储估计鱼眼相机内在函数的标准误差的对象

消除失真
针孔相机
undistortImage 校正镜头畸变的图像
undistortPoints 镜头畸变的正确点坐标

鱼眼相机
undistortFisheyeImage 校正镜头畸变的鱼眼图像
undistortFisheyePoints 鱼眼镜头畸变的正确点坐标

可视化结果
pcshow 绘制 3-D 点云
plotCamera 在 3-D 坐标中绘制相机
showExtrinsics 可视化外部相机参数
showReprojectionErrors 可视化校准错误
stereoAnaglyph 从立体图像对创建红青色浮雕

估计相机姿势
extrinsics 计算校准相机的位置
extrinsicsToCameraPose 将外部变量转换为相机姿势
cameraPoseToExtrinsics 将相机位姿转换为外部位姿
relativeCameraPose 计算相机位姿之间的相对旋转和平移

转换
rotationMatrixToVector 将 3-D 旋转矩阵转换为旋转向量
rotationVectorToMatrix 将 3-D 旋转向量转换为旋转矩阵
cameraIntrinsicsToOpenCV 将相机内在参数从MATLAB转换为 OpenCV
cameraIntrinsicsFromOpenCV 将相机内在参数从 OpenCV 转换为MATLAB
stereoParametersToOpenCV 将立体相机参数从MATLAB转换为 OpenCV
stereoParametersFromOpenCV 将立体相机参数从 OpenCV 转换为MATLAB

图像极线校正、对应点匹配、空间定位

图像校正

黑白相机首先将moon转化为内RGB。I1 = cat(3,I1,I1,I1);

https://stackoverflow.com/questions/54652372/color-must-correspond-to-the-number-of-input-points-when-using-pointcloud-in-m

• 图像校正：确认已经将标定参数保存：save(‘my1stereoParams.mat’ , ‘stereoParams’);

clear all
clc

I1 = imread('D:\Work\项目\2022\DIM-机械臂\实验\双目实验\Cam1\66.png');%读取左右图片
I2 = imread('D:\Work\项目\2022\DIM-机械臂\实验\双目实验\Cam2\66.png');
figure
imshowpair(I1, I2, 'montage');
title('Original Images');

%加载stereoParameters对象。
% load('my1stereoParams.mat');%加载你保存的相机标定的mat
load('calibrationSession3.mat');%加载你保存的相机标定的mat

%图像校正
[J1, J2] = rectifyStereoImages(I1, I2, calibrationSession.CameraParameters,'OutputView','full');
figure
imshow(stereoAnaglyph(J1,J2));
title('Undistorted Images');

calibrationSession.CameraParameters——之前保存的session。

rectifyStereoImages——校正一对立体图像。将图像投影到共同的图像平面上，使得对应的点具有相同的行坐标。此处，利用了双目标定的结果矩阵stereoParams。

stereoAnaglyph——将图像I1和I2组合成红青色浮雕。当输入的是校正后的立体图像时，可以用红蓝立体眼镜查看输出图像的立体效果。

计算视差

• 计算视差并显示

%彩色相机
%disparityMap = disparity(rgb2gray(J1), rgb2gray(J2), 'BlockSize',15,'DisparityRange',[0,400],'ContrastThreshold',0.5,'UniquenessThreshold',15);
%黑白相机
%disparityMap = disparity(J1, J2, 'BlockSize',15,'DisparityRange',[0,400],'ContrastThreshold',0.5,'UniquenessThreshold',15);

%用disparityBM方法显示**视差图**
disparityRange = [0 48];
disparityMap = disparityBM(J1, J2, 'DisparityRange',disparityRange,'UniquenessThreshold',8);
figure;
imshow(disparityMap, disparityRange);
title('Disparity Map');
colormap jet
colorbar

实际操作发现：有时候校正的视差图效果反而没有不校正的视差图效果好，但是后面还是要用到校正。

未校正的视差图：

校正后的视差图：

disparity——计算视差图；
disparityBM——通过块匹配计算视差图；
disparitySGM——通过半全局匹配计算视差图；
三个函数的效果不一样。

disparityRange ——需要根据实际的视差，设置范围，可先设置为一个较大的值，再缩小。

• disparity的参数：

’BlockMatching’或’SemiGlobal’：视差估计算法的一种（该种算法为默认算法），通过比较图像中每个像素块的绝对差值之和（SAD）来计算视差。

’DisparityRange’,[0,400]：视差范围，范围可以自己设定，不能超过图像的尺寸，当双目距离较远或者物体距离较近时，应适当增大该参数的值。

’BlockSize’, 15：设置匹配时方块大小。

’ContrastThreshold’,0.5：对比度的阈值，阈值越大，错误匹配点越少，能匹配到的点也越少。

’UniquenessThreshold’,15：唯一性阈值，设置值越大，越破坏了像素的唯一性，设置为0，禁用该参数。

’DistanceThreshold’,400：从图像左侧到右侧检测的最大跨度，跨度越小越准确，但很容易造成无法匹配。禁用该参数[]。

’TextureThreshold’,0.0002（默认）：最小纹理阈值，定义最小的可靠纹理，越大越造成匹配点少，越少越容易匹配到小纹理，引起误差。

————————————————————————————————————————————

计算深度

%根据双目标定结果stereoParams和视差图disparityMap得到**空间三维坐标**包括深度：

%计算深度
pointCloud3D = reconstructScene(disparityMap, calibrationSession.CameraParameters);
pointCloud3D = double(pointCloud3D);
pointCloud3D = pointCloud3D/1000;%根据单位调整

Z = pointCloud3D(:,:,3);
Z(isnan(Z)) = 0;
pointCloud3D(:,:,3)=Z;

figure;
imshow(pointCloud3D);
title('深度图');

%显示距离5-6之间的图像，彩色相机：
%Z = double(pointCloud3D(:,:,3));
%mask = repmat(Z> 5&Z <6,[1,1,3]);
%J1(~mask)= 0;
%figure;
%imshow(J1);

%显示距离5-6之间的图像，黑白相机：
Z = double(pointCloud3D(:,:,3));
mask = repmat(Z> 0&Z <50,[1,1]);
J1(~mask)= 0;
J2(~mask)= 0;
figure;
imshowpair(J1, J2, 'montage');

%% 彩色相机另外一种方法：
%% 存储 3D 图的点云数据
%ptCloud = pointCloud(pointCloud3D , 'Color', J1);

%% 使用 pcplayer 观察点云图
%player3D = pcplayer([-1, 1], [-1, 1], [0, 2], 'VerticalAxis', 'Y', 'VerticalAxisDir', 'Up');
%view(player3D, ptCloud);

深度图：

手眼标定（eye-in-hand ）

手眼标定的目的：获取机器人执行器末端（抓取臂）坐标系和相机坐标系之间的相互关系。

常见的方法是9点标定法：让机械手的末端去走这就9个点得到在机器人坐标系中的坐标，同时还要用相机识别9个点得到像素坐标。

设想的方法是把相机固定在机械臂末端执行器的某一位置，变换多个位姿，利用相机拍若干张标定板的图像获取标定板相对于相机坐标系的位姿，并同时记录当时机械臂的位姿矩阵。

为了实现唯一解，至少需要3个位置的摄像机标定结果。

tsai函数：求解AX=XB
【求解的多种方法：https://blog.csdn.net/Kang14789/article/details/119719633】

需要已知标定板相对于相机坐标系的位姿A，机械臂末端执行器相对于基座的位姿矩阵B。

function X = tsai(A,B)
% Calculates the least squares solution of
% AX = XB
% 
% A New Technique for Fully Autonomous 
% and Efficient 3D Robotics Hand/Eye Calibration
% Lenz Tsai
%
% Mili Shah
% July 2014

[m,n] = size(A); n = n/4;
S = zeros(3*n,3);
v = zeros(3*n,1);
%Calculate best rotation R
for i = 1:n
    A1 = logm(A(1:3,4*i-3:4*i-1)); 
    B1 = logm(B(1:3,4*i-3:4*i-1));
    a = [A1(3,2) A1(1,3) A1(2,1)]'; a = a/norm(a);
    b = [B1(3,2) B1(1,3) B1(2,1)]'; b = b/norm(b);
    S(3*i-2:3*i,:) = skew(a+b);
    v(3*i-2:3*i,:) = a-b;
end
x = S\v;
theta = 2*atan(norm(x));
x = x/norm(x);
R = (eye(3)*cos(theta) + sin(theta)*skew(x) + (1-cos(theta))*x*x')';
%Calculate best translation t
C = zeros(3*n,3);
d = zeros(3*n,1);
I = eye(3);
for i = 1:n
    C(3*i-2:3*i,:) = I - A(1:3,4*i-3:4*i-1);
    d(3*i-2:3*i,:) = A(1:3,4*i)-R*B(1:3,4*i);
end
t = C\d;
%Put everything together to form X
X = [R t;0 0 0 1];

skew函数：求向量反对称矩阵

function Sk = skew( x )
%SKEW 此处显示有关此函数的摘要
%   此处显示详细说明
   Sk = [0,-x(3),x(2);x(3),0,-x(1);-x(2),x(1),0];
end

计算手眼矩阵X

clc
clear
close all

%%%%%%%%%%%%%%%%%%%%%%% T6矩阵参数%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%位姿1的时候机器人末端相对于机器人基坐标系下变换矩阵
Pose1=[1141.243,-15.261,-97.721,178.91,0.47,92.37];
Px = Pose1(1);
Py = Pose1(2);
Pz = Pose1(3);
rota = Pose1(4)*pi/180;
rotb = Pose1(5)*pi/180;
rotc = Pose1(6)*pi/180;
Rx = [1 0 0; 0 cos(rota) -sin(rota); 0 sin(rota) cos(rota)];
Ry = [cos(rotb) 0 sin(rotb); 0 1 0; -sin(rotb) 0 cos(rotb)];
Rz = [cos(rotc) -sin(rotc) 0; sin(rotc) cos(rotc) 0; 0 0 1];
R1 = Rz*Ry*Rx;
T1= [Px Py Pz]';
%%%%%%%%%%位姿2的时候机器人末端相对于机器人基坐标系下变换矩阵
Pose2=[1103.946,-163.910,-107.673,-160.90,-0.14,-91.62];
Px = Pose2(1);
Py = Pose2(2);
Pz = Pose2(3);
rota = Pose2(4)*pi/180;
rotb = Pose2(5)*pi/180;
rotc = Pose2(6)*pi/180;
Rx = [1 0 0; 0 cos(rota) -sin(rota); 0 sin(rota) cos(rota)];
Ry = [cos(rotb) 0 sin(rotb); 0 1 0; -sin(rotb) 0 cos(rotb)];
Rz = [cos(rotc) -sin(rotc) 0; sin(rotc) cos(rotc) 0; 0 0 1];
R2 = Rz*Ry*Rx;
T2= [Px Py Pz]';
%%%%%%%%%%位姿3的时候机器人末端相对于机器人基坐标系下变换矩阵
Pose3=[1073.714,2.669,-142.448,-142.86,0.84,-178.55];
Px = Pose3(1);
Py = Pose3(2);
Pz = Pose3(3);
rota = Pose3(4)*pi/180;
rotb = Pose3(5)*pi/180;
rotc = Pose3(6)*pi/180;
Rx = [1 0 0; 0 cos(rota) -sin(rota); 0 sin(rota) cos(rota)];
Ry = [cos(rotb) 0 sin(rotb); 0 1 0; -sin(rotb) 0 cos(rotb)];
Rz = [cos(rotc) -sin(rotc) 0; sin(rotc) cos(rotc) 0; 0 0 1];
R3 = Rz*Ry*Rx;
T3= [Px Py Pz]';
%%%%%%%%%位姿1,2,3时候机器人末端相对于机器人基坐标系下变换矩阵
T61=[R1 T1;0 0 0 1]   ;      
T62=[R2 T2;0 0 0 1];
T63=[R3 T3;0 0 0 1];

%%%%%%摄像机外参数矩阵（平面靶标在摄像机坐标系下表示）%%%%%%%%
Extrinsic1=[0.051678,-0.998634,0.007660,21.747985;
         -0.998617,-0.051600,0.010060,27.391246;
        -0.009651,-0.008169,-0.999920,319.071378];%%%3行4列矩阵
Extrinsic2=[0.014949,0.999738,0.017361,-35.869608 
        0.949779,-0.019626,0.312304,-20.701811
        0.312563,0.011821,-0.949823,306.463155];
Extrinsic3=[0.999176,0.039246,0.010343,-26.361812
        0.025037,-0.796606,0.603980,20.533884
        0.031943,-0.603223,-0.796933,318.110756];

%%%%%%%
TC1=[Extrinsic1; 0 0 0 1];     
TC2=[Extrinsic2; 0 0 0 1];
TC3=[Extrinsic3; 0 0 0 1];

TL1=inv(T61)*T62;
TL2=inv(T62)*T63;

TR1=TC1*inv(TC2);
TR2=TC2*inv(TC3);

A=[TL1,TL2]
B=[TR1,TR2]
X= tsai(A,B)

问题故障解决

校正图变成原形了，发现选择了3 coefficient，改成2 coefficient

下一步计划

为更加准确的安装零件，需获取目标的空间位姿。

参考

https://blog.csdn.net/leonardohaig/article/details/81254179
https://blog.csdn.net/a6333230/article/details/88245102
https://blog.csdn.net/weixin_46133643/article/details/123897977
https://xgyopen.github.io/2018/08/16/2018-08-16-imv-calibration-eye-hand/
https://cloud.tencent.com/developer/article/1746508
https://blog.csdn.net/weixin_37834269/article/details/106578468