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

系统

接上一次的非标机构完成双目视觉的空间定位：
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：如需要在其他地方使用该矩阵，需转置以后使用

FundamentalMatrix：基础矩阵。
EssentialMatrix：本质矩阵。

CameraParameters1内：

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

标定结果保存为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’);

I1 = imread('D:\XXX\88.png');%读取左右图片
I2 = imread('D:\XXX\88.png');

I1 = cat(3,I1,I1,I1);
I2 = cat(3,I2,I2,I2);

figure
imshow(stereoAnaglyph(I1, I2));
title('原始图');

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

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

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

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

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

计算视差

• 计算视差并显示

%% 计算视差
disparityRange = [24 32];   %%% 参数可调,关键！！！ %%%
% 通过块匹配计算视差图。视差图校正立体对图像，返回为二维灰度图像
disparityMap = disparitySGM(rgb2gray(J1), rgb2gray(J2), 'DisparityRange',disparityRange,'UniquenessThreshold',20);%%% 参数可调,关键！！！ %%%
figure;
imshow(disparityMap, disparityRange);
title('视差图');
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得到空间三维坐标包括深度

%% 计算深度
points3D  = reconstructScene(disparityMap, reprojectionMatrix);%从视差图重建三维场景，世界点的坐标，作为m × n × 3数组返回。
% points3D  = double(points3D );
points3D  = points3D ./1000;%根据单位调整

Z = points3D (:,:,3);
Z(isnan(Z)) = 0;%剔除
% Z(Z>50) = 0;
points3D (:,:,3)=Z;
% histogram(Z)
figure;
imshow(points3D);
title('深度图');
colorbar

目标点空间定位

%% 三维点云
ptCloud = pointCloud(points3D ,'Color',J1);
ptCloud = removeInvalidPoints(ptCloud);%从点云中移除无效点
ptCloud = pcdenoise(ptCloud);%从三维点云中去除噪声

% Create a streaming point cloud viewer
player3D = pcplayer([-3, 3], [-3, 3], [0, 8], 'VerticalAxis', 'y', 'VerticalAxisDir', 'down');

% Visualize the point cloud
view(player3D, ptCloud);

%% 寻找目标位置
object = imread('D:\XXX\11.png');

object=rgb2gray(object);
I1=rgb2gray(I1);
c = normxcorr2(object,I1);
[max_c, imax] = max(abs(c(:)));
[ypeak, xpeak] = ind2sub(size(c),imax(1));
corr_offset = [(xpeak-size(object,2)) (ypeak-size(object,1))];
figure
imshow(I1); 
title('目标匹配');
hold on;
rectangle('position',[corr_offset(1) corr_offset(2) 60 60],'curvature',[1,1],'edgecolor','g','linewidth',2);
centroids(:,1)=corr_offset(1)+30;
centroids(:,2)=corr_offset(2)+30;

%% 目标点空间定位
% Find the 3-D world coordinates of the centroids.
centroidsIdx = sub2ind(size(disparityMap), centroids(:,2), centroids(:,1));
X = points3D(:, :, 1);
Y = points3D(:, :, 2);
Z = points3D(:, :, 3);
centroids3D = [X(centroidsIdx)'; Y(centroidsIdx)'; Z(centroidsIdx)'];

% Find the distances from the camera in meters.
dists = sqrt(sum(centroids3D .^ 2));
    
% Display the detected people and their distances.
labels = cell(1, numel(dists));
for i = 1:numel(dists)
    labels{i} = sprintf('%0.2f meters', dists(i));
end
figure;
imshow(insertObjectAnnotation(I1, 'rectangle', [corr_offset(1),corr_offset(2),60,60], labels));
title('目标空间定位');

三维重建

另一种三维点云重建。

close all
clc
clear

I1 = imread('D:\Work\项目\2022\DIM-机械臂\双目\Cam1\88.png');
I2 = imread('D:\Work\项目\2022\DIM-机械臂\双目\Cam2\88.png');

% 灰度图转为3通道
I1 = cat(3,I1,I1,I1);
I2 = cat(3,I2,I2,I2);

figure
imshow(stereoAnaglyph(I1, I2));
title('原始图');
%% 加载stereoParameters对象。
% load('calibrationSession5.mat');%加载你保存的相机标定的mat
load('my1stereoParams.mat');%加载你保存的相机标定的mat

%% 图像校正，消除畸变
cameraParams1 = cameraParameters('IntrinsicMatrix',stereoParams.CameraParameters1.IntrinsicMatrix,'RadialDistortion',stereoParams.CameraParameters1.RadialDistortion);
cameraParams2 = cameraParameters('IntrinsicMatrix',stereoParams.CameraParameters2.IntrinsicMatrix,'RadialDistortion',stereoParams.CameraParameters2.RadialDistortion);

%纠正图像的镜头失真
I1 = undistortImage(I1, cameraParams1);
I2 = undistortImage(I2, cameraParams2);
figure
imshow(stereoAnaglyph(I1, I2));
% imshowpair(I1, I2, 'montage');
title('校正图');

%% 图像对应点匹配
% 利用最小特征值算法检测角点，并返回角点对象，参数可调
imagePoints1 = detectMinEigenFeatures(rgb2gray(I1), 'MinQuality', 0.01);%%%% 参数可调，越小匹配的点越多 %%%%

figure
imshow(I1, 'InitialMagnification', 50);
title('特征点检测');
hold on
plot(selectStrongest(imagePoints1, 150));%选择指标最强的点

% 创建点跟踪器，%使用Kanade-Lucas-Tomasi (KLT)算法跟踪视频中的点 
tracker = vision.PointTracker('MaxBidirectionalError', 1, 'NumPyramidLevels', 5);

% 初始化点跟踪器
imagePoints1 = imagePoints1.Location;
initialize(tracker, imagePoints1, I1);

% 跟踪点
[imagePoints2, validIdx] = step(tracker, I2);
matchedPoints1 = imagePoints1(validIdx, :);
matchedPoints2 = imagePoints2(validIdx, :);

% 显示对应的特征点
figure
showMatchedFeatures(I1, I2, matchedPoints1, matchedPoints2);
title('特征点匹配');

%% 计算本质矩阵
% 从一对图像的对应点估计本质矩，E是本质矩阵；
% epipolarInliers：以m乘1逻辑索引向量的形式返回。true表示使用matchedPoints1和matchedPoints2中相应的索引匹配点来计算基本矩阵。false表示索引点未用于计算。 
[E, epipolarInliers] = estimateEssentialMatrix(matchedPoints1, matchedPoints2, cameraParams1, cameraParams2,'Confidence', 99.99);

% 找到索引对应的点
inlierPoints1 = matchedPoints1(epipolarInliers, :);
inlierPoints2 = matchedPoints2(epipolarInliers, :);

% 显示匹配结果
figure
showMatchedFeatures(I1, I2, inlierPoints1, inlierPoints2);%显示对应的特征点
title('极限约束-特征点匹配');

%% 计算相机姿态之间的相对旋转和平移
% orient:相机的方向，返回一个3 × 3矩阵
% loc:摄像机的位置，以1乘3的单位向量返回
[orient, loc] = relativeCameraPose(E, cameraParams1, cameraParams2, inlierPoints1, inlierPoints2);

%% 三维匹配点重建，重新匹配特征点
% 检测密集特征点,使用ROI来排除靠近图像边缘的点。 
roi = [30, 30, size(I1, 2) - 30, size(I1, 1) - 30];
% 利用最小特征值算法检测角点，并返回角点对象,参数阈值可调
imagePoints1 = detectMinEigenFeatures(rgb2gray(I1),'ROI', roi, 'MinQuality', 0.001);%%%% 参数可调，越小匹配的点越多 %%%%

% 创建点跟踪器，%使用Kanade-Lucas-Tomasi (KLT)算法跟踪视频中的点 
tracker = vision.PointTracker('MaxBidirectionalError', 1, 'NumPyramidLevels', 5);

% 初始化点跟踪器
imagePoints1 = imagePoints1.Location;
initialize(tracker, imagePoints1, I1);

% 跟踪点
[imagePoints2, validIdx] = step(tracker, I2);
matchedPoints1 = imagePoints1(validIdx, :);
matchedPoints2 = imagePoints2(validIdx, :);

% 计算相机每个位置的相机矩阵。 
% 第一个摄像机在原点沿z轴看去。 因此，它的旋转矩阵是单位矩阵，它的平移向量是0。 
tform1 = rigid3d;
camMatrix1 = cameraMatrix(cameraParams1, tform1);%摄像机投影矩阵，返回为4 × 3矩阵。
%camMatrix1 = cameraMatrix(cameraParams1, eye(3), [0 0 0]);

% 计算第二个相机的外部特性
cameraPose = rigid3d(orient, loc);
tform2 = cameraPoseToExtrinsics(cameraPose);
camMatrix2 = cameraMatrix(cameraParams2, tform2);%摄像机投影矩阵，返回为4 × 3矩阵。
% [R, t] = cameraPoseToExtrinsics(orient, loc);
% camMatrix2 = cameraMatrix(cameraParams2, R, t);

%% 计算点的3-D位置
points3D = triangulate(matchedPoints1, matchedPoints2, camMatrix1, camMatrix2);%立体图像中未变形匹配点的三维定位，Mx3矩阵。

% Get the color of each reconstructed point
numPixels = size(I1, 1) * size(I1, 2);
allColors = reshape(I1, [numPixels, 3]);
colorIdx = sub2ind([size(I1, 1), size(I1, 2)], round(matchedPoints1(:,2)), round(matchedPoints1(:, 1)));%获取匹配特征点的索引
color = allColors(colorIdx, :);
color = single(color);

% 创建点云
ptCloud = pointCloud(points3D,'Color', color); %pointCloud对象从3D坐标系中的一组点创建点云数据

ptCloud = removeInvalidPoints(ptCloud);%从点云中移除无效点
ptCloud = pcdenoise(ptCloud);%从三维点云中去除噪声

%% 显示
% 可视化相机的位置和方向
cameraSize = 0.3;
figure
plotCamera('Size', cameraSize, 'Color', 'r', 'Label', '1', 'Opacity', 0);%在三维坐标中绘制第一个相机
hold on
grid on
plotCamera('Location', loc, 'Orientation', orient, 'Size', cameraSize, 'Color', 'b', 'Label', '2', 'Opacity', 0);%在三维坐标中绘制第二个相机

% 可视化点云
pcshow(ptCloud, 'VerticalAxis', 'y', 'VerticalAxisDir', 'down', 'MarkerSize', 45);%绘制三维点云

% 旋转和缩放绘图
camorbit(0, -30);%围绕照相机目标将照相机位置旋转 dtheta 和 dphi（均以度为单位）。
camzoom(1);%放大和缩小场景

% 轴标签
xlabel('x-axis');
ylabel('y-axis');
zlabel('z-axis')
title('三维重建');

%% 寻找目标位置
object = imread('D:\Work\项目\2022\DIM-机械臂\双目\22.png');

object=rgb2gray(object);
I1=rgb2gray(I1);
c = normxcorr2(object,I1);
[max_c, imax] = max(abs(c(:)));
[ypeak, xpeak] = ind2sub(size(c),imax(1));
corr_offset = [(xpeak-size(object,2)) (ypeak-size(object,1))];
figure
imshow(I1); 
title('目标匹配');
hold on;
rectangle('position',[corr_offset(1) corr_offset(2) 60 60],'curvature',[1,1],'edgecolor','g','linewidth',2);
centroids(:,1)=corr_offset(1)+30;
centroids(:,2)=corr_offset(2)+30;

%% 目标点空间定位
% 计算离目标像素最近的点
FindPoint=abs(centroids(:,1)-matchedPoints1(:,1))+abs(centroids(:,2)-matchedPoints1(:,2));
[t1,index]=min(FindPoint);
% 找到点云中对应的点
centroids3D = [points3D(index,1)'; points3D(index,2)'; points3D(index,3)'];

dists = points3D(index,3);%sqrt(sum(centroids3D .^ 2));
    
% Display the detected people and their distances.
labels = cell(1, numel(dists));
for i = 1:numel(dists)
    labels{i} = sprintf('%0.2f meters', dists(i));
end
figure;
imshow(insertObjectAnnotation(I1, 'rectangle', [corr_offset(1),corr_offset(2),60,60], labels));
title('目标空间定位');

视差的选择非常重要，涉及到最终的效果。

两者的结果相差0.07。

手眼标定（eye-in-hand ）

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

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

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

求解AX=XB方法：

http://math.loyola.edu/~mili/Calibration/index.html

手眼标定的四种经典算法：
通过几何关系求解---------Tsai-Lenz算法
通过欧几里得群求解-------Navy算法
通过单位四元数求解-------Horaud算法
通过对偶四元数求解-------Dual quaternions算法
其中最常用的是Tsai-Lenz算法。

为了实现唯一解，至少需要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

参考：

http://math.loyola.edu/~mili/Calibration/index.html
http://math.loyola.edu/~mili/Calibration/AXXB/tsai.m

计算手眼矩阵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)

T61、T62、T63（或者说A或B）可通过正运动学的fkine求得末端相对于基坐标系的位姿（齐次变换矩阵）。只要知道机器人在当前状态下每个关节的角度，我们就可以计算得到 A 变换。

https://mp.weixin.qq.com/s/nJx1dlpBXaL2_iT_J4W5Kg

也可以通过按照上文中，从机器人控制器或示教盒中读取末端的“平移向量+欧拉角”，从而计算出末端的齐次变换矩阵。

https://blog.csdn.net/xiongchao99/article/details/52850990

B是相机在标定板坐标系下的位姿。其实就是相机的外参（从世界坐标系转换到相机坐标系），注意不是相机2相对于相机1的位姿。

摄像机标定使用MATLAB标定工具箱的话，所得到的外参旋转矩阵和平移向量先要转置，即R=r’，T=t’。

图中所示即为外参（外参平移向量、外参旋转矩阵）。

求出相机坐标系相对于工具坐标系的值，再结合双目视觉的结果：目标相对于相机坐标系的值，再加上工具相对于基坐标系的值，就可以得到目标相对于基坐标系的值。

问题故障解决

校正图变成原形了，发现选择了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://blog.csdn.net/weixin_37834269/article/details/106578468
https://ww2.mathworks.cn/help/vision/ug/depth-estimation-from-stereo-video.html
https://ww2.mathworks.cn/help/vision/ug/structure-from-motion-from-two-views.html

手眼标定：

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/qq_27865227/article/details/114266140
https://guyuehome.com/33989

综合：

https://blog.csdn.net/xiongchao99/article/details/52850990