1.背景介绍

  2D视觉机器人指的是机器人通过2D相机提供的视觉信息，完成某些实际的功能。
  下面以眼在手上标定为例：

  上图中，相机③被固定在机械臂末端②，标定板④即之后的工作平面，为了做手眼标定在工作平面上放一个标定板。
  标定过程中标定板④放在固定位置，由上图可知，标定板④和基底①的相对位置不变，然后通过示教器控制机械臂带着相机在不同的位姿下对标定板④进行拍照，拍照的过程中保存当前拍到的标定板图片以及记录图片对应示教器上的世界坐标。
  眼在手上标定的目的是求出相机③坐标系到机械臂末端②坐标系的变换矩阵。

  ①为什么需要手眼标定？

  假设以物体识别目标进行抓取为例，在工作平面④上面有一个方形物体⑤，装在机械臂末端上面的2D光学相机会对物体进行拍照捕捉，识别后的结果是给出当前物体⑤在相机成像平面内的像素坐标，然后要用机械臂去抓取该物体⑤。但得出的物体⑤的坐标信息是以相机坐标系为基准的，而输入给机械臂的信息是以机械臂基底①坐标系为基准的，这时候就需要将物体⑤的坐标信息转换到以机械臂基底①为基准的参数。

1.1 问题

  PS：关于上面例子中的问题，不是本文的重点，是通过三个4*4的矩阵相乘得到物体⑤在基底坐标系下的坐标，后面的博文会记录（https://blog.csdn.net/qq_45445740/article/details/123567627?spm=1001.2014.3001.5501），这里暂时不提。之前我一直困惑为什么2D相机没有深度信息，是怎么得到z轴方向的参数的？
  ②为什么需要相机标定？

  因为物体⑤在相机③中的坐标是在相机成像平面内的一个2维坐标信息，而上面说的三个4*4的矩阵相乘是3维的，产生了新的问题：根据相机识别到物体的2维信息，如何转换为3维的目标信息？

1.2 思路

  相机标定是得到相机的内参矩阵和畸变系数。
  相机的内参矩阵：

  其中，f表示焦距，c_x表示使用像素来描述x轴方向焦距的长度，c_y表示使用像素来描述y轴方向焦距的长度。一般来说，c_x=w/2，c_y=h/2，w表示相机拍摄照片的像素宽度，h表示像素高度。

  根据视频中up主的思路，有了上面的相机内参矩阵，就可以得到角点在相机坐标系中的三维坐标。前提：需要假设物体⑤在工作平面上，且物体有固定的厚度，也就保证了物体的上表面中所有的点在z轴方向上到相机的距离是相同的。

  通过上面的公式求出X和Y。式中内参矩阵是标定得到已知的，Z是固定值，在代码中假设标定板放在世界坐标系中Z=0的平面上，x和y是角点的像素坐标也是已知值。

代码：https://blog.csdn.net/qq_45445740/article/details/122339711

2.操作流程

• ①将相机固定在机械臂末端，调整好焦距和曝光，固定好棋盘格标定板，控制机器臂在不同姿态下对标定板拍照，拍摄的每张照片都要保证整个标定板都在相机的视野内，至少13张标定图片。
• ②在程序中加载每张标定图片
• ③首先提取每张照片的角点，使用cv::findChessboardCorners()函数提取角点，这里的角点专指的是标定板上的内角点，并且这些角点与标定板的边缘不接触。

// 使用该API对每张图片是否提取到角点进行判断
CV_EXPORTS_W bool findChessboardCorners( InputArray image, 
										 Size patternSize, 
										 OutputArray corners,
                                         int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE );

// Image：传入拍摄的棋盘图Mat图像，必须是8位的灰度或者彩色图像
// patternSize：每个棋盘图上内角点的行列数，一般情况下，行列数不要相同，便于后续标定程序识别标定板的方向
// corners：用于存储检测到的内角点图像坐标位置，一般用元素是Point2f的向量来表示：vector image_points_buf;
// flage：用于定义棋盘图上内角点查找的不同处理方式，有默认值

• ④为了提高标定精度，需要在初步提取的角点信息上进一步提取亚像素信息，降低相机标定偏差，使用cv::find4QuadCornerSubpix()函数，这个方法是专门用来获取棋盘图上内角点的精确位置的。

// 查找棋盘角的亚像素精确位置  
CV_EXPORTS_W bool find4QuadCornerSubpix( InputArray img, 
										 InputOutputArray corners, 
										 Size region_size );

// img：输入的Mat矩阵，最好是8位灰度图像，检测效率更高
// corners：初始的角点坐标向量，同时作为亚像素坐标位置的输出，所以需要是浮点型数据，一般用元素是Pointf2f/Point2d的向量来表示vector iamgePointsBuf；
// region_size：角点搜索窗口的尺寸；

• ⑤检查识别到的角点方向是否一致。由于在建立棋盘格上的三维坐标系的时候，我们默认是从棋盘格左上角到右下角建立的，如果识别反了，则会有个别图片棋盘格识别的角点和输入的棋盘格三维坐标对应不上。
  
• ⑥在棋盘标定图上绘制找到的内角点（非必须，仅为了显示），使用cv::drawChessboardCorners()函数。

// 该函数绘制单个棋盘角，如果棋盘没有被发现，则绘制为红色的圆圈，如果棋盘被发现，则绘制为与线连接的彩色角。  
CV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, 
										 Size patternSize,
                                         InputArray corners, 
                                         bool patternWasFound );

// image：8位灰度或者彩色图像
// patternSize：每张标定棋盘上内角点的行列数
// corners：初始的角点坐标向量，同时作为亚像素坐标位置的输出，所以需要是浮点型数据，一般用元素是Pointf2f/Point2d的向量来表示：vector iamgePointsBuf；
// patternWasFound：标志位，用来指示定义的棋盘内角点是否被完整的探测到，true表示别完整的探测到，函数会用直线依次连接所有的内角点，作为一个整体，false表示有未被探测到的内角点，这时候函数会以（红色）圆圈标记处检测到的内角点；
// 					patternWasFound=ture时，依次连接各个内角点
// 					patternWasFound=false时，以（红色）圆圈标记处角点位置

• ⑦开始相机标定，准备好cv::calibrateCamera()函数所需要的参数，因为第一个参数需要的是所有标定图片上所有检测到的角点在世界坐标系下的三维坐标，而这是2D相机，没有深度信息，所以我们假设标定板放在世界坐标系中 z=0 的平面上，即Z轴方向为0。

CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
                                     InputArrayOfArrays imagePoints, 
                                     Size imageSize,
                                     InputOutputArray cameraMatrix, 
                                     InputOutputArray distCoeffs,
                                     OutputArrayOfArrays rvecs, 
                                     OutputArrayOfArrays tvecs,
                                     int flags = 0, TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );

// objectPoints：为世界坐标系中的三维点。在使用时，应该输入一个三维坐标点的向量的向量，即vector> object_points。需要依据棋盘上单个黑白矩阵的大小，计算出（初始化）每一个内角点的世界坐标。
// imagePoints：为每一个内角点对应的图像坐标点。和objectPoints一样，应该输入vector> image_points_seq形式的变量；
// imageSize：为图像的像素尺寸大小，在计算相机的内参和畸变矩阵时需要使用到该参数；
// cameraMatrix：为相机的内参矩阵，输入一个Mat cameraMatrix即可，如Mat cameraMatrix=Mat(3,3,CV_32FC1,Scalar::all(0));
// distCoeffs：为畸变矩阵，输入一个Mat distCoeffs=Mat(1,5,CV_32FC1,Scalar::all(0))即可；
// rvecs：为旋转向量，应该输入一个Mat类型的vector，即vectorrvecs;
// tvecs：为位移向量，和rvecs一样，应该为vector tvecs；
// flags：为标定时所采用的算法。有如下几个参数：
// 				CV_CALIB_USE_INTRINSIC_GUESS：使用该参数时，在cameraMatrix矩阵中应该有fx,fy,u0,v0的估计值。否则的话，将初始化(u0,v0）图像的中心点，使用最小二乘估算出fx，fy。
// 				CV_CALIB_FIX_PRINCIPAL_POINT:在进行优化时会固定光轴点。当CV_CALIB_USE_INTRINSIC_GUESS参数被设置，光轴点将保持在中心或者某个输入的值。
// 				CV_CALIB_FIX_ASPECT_RATIO：固定fx/fy的比值，只将fy作为可变量，进行优化计算。当CV_CALIB_USE_INTRINSIC_GUESS没有被设置，fx和fy将会被忽略。只有fx/fy的比值在计算中会被用到。
// 				CV_CALIB_ZERO_TANGENT_DIST：设定切向畸变参数（p1,p2）为零。
// 				CV_CALIB_FIX_K1,…,CV_CALIB_FIX_K6：对应的径向畸变在优化中保持不变。
// 				CV_CALIB_RATIONAL_MODEL：计算k4，k5，k6三个畸变参数。如果没有设置，则只计算其它5个畸变参数。
// 				第九个参数criteria是最优迭代终止条件设定。

/*在使用该函数进行标定运算之前，需要对棋盘上每一个内角点的空间坐标系的位置坐标进行初始化，
标定的结果是生成相机的内参矩阵cameraMatrix、相机的5个畸变系数distCoeffs，另外每张图像都会生成属于自己的平移向量和旋转向量。*/

• ⑧对相机标定的结果进行评价，方法是通过相机标定得到的摄像机内外参数，然后对空间的三维点进行重新投影计算，得到空间三维点在图像上新的投影点的坐标，通过cv::projectPoints()函数计算得到新的二维投影点，然后通过cv::norm()计算新的投影点和旧的投影点之间的偏差，偏差越小，标定结果越好。

CV_EXPORTS_W void projectPoints( InputArray objectPoints,
                                 InputArray rvec,
                                 InputArray tvec,
                                 InputArray cameraMatrix, 
                                 InputArray distCoeffs,
                                 OutputArray imagePoints,
                                 OutputArray jacobian = noArray(),
                                 double aspectRatio = 0 );

// objectPoints：物体点的坐标，为3xN或者Nx3的矩阵，这儿N是视图中的所有所有点的数目。
// rvec：旋转向量，1x3或者3x1。
// InputArray tvec：平移向量，1x3或者3x1。
// cameraMatrix：摄像机内参数矩阵。
// distCoeffs：形变参数向量，4x1或者1x4，为[k1,k2,p1,p2]。如果是NULL，所有形变系数都设为0。
// imagePoints：输出数组，存储图像点坐标。大小为2xN或者Nx2，这儿N是视图中的所有点的数目。
// jacobian = noArray()：默认参数，Nx10矩阵，从上到下分为4个部分。
// aspectRatio = 0 ：默认参数，关于形变系数的图像上点的导数，Nx4矩阵。

CV_EXPORTS_W double norm(InputArray src1, 
						 InputArray src2,
                         int normType = NORM_L2, 
                         InputArray mask = noArray());
                      
// src1：输入矩阵1
// src2：输入矩阵2
// normType：为范数的类型，默认的是L2范数

• ⑨开始眼在手上的标定，通过cv::calibrateHandEye()函数，四个输入，两个输出，其中R_gripper2base和t_gripper2base由示教器获得，即机械臂末端到机械人基底的旋转矩阵和平移向量；R_target2cam和t_target2cam可通过相机标定中的输出rvecs和tvecs得到。rvecs表示旋转向量，可通过cv::Rodrigues()将旋转向量转为旋转矩阵R_target2cam，tvecs就等于t_target2cam平移向量。

#include 

cv::calibrateHandEye(InputArrayOfArrays R_gripper2base,
					 InputArrayOfArrays t_gripper2base,
					 InputArrayOfArrays R_target2cam,
					 InputArrayOfArrays t_target2cam,
					 OutputArray R_cam2gripper,
					 OutputArray t_cam2gripper,
					 HandEyeCalibrationMethod method = CALIB_HAND_EYE_TSAI 
)	

// R_gripper2base：描述为将在机械臂末端坐标系（夹爪坐标系，gripper）中的点转换到机器人基坐标系（base）中。表示机械臂末端坐标系（夹爪坐标系，gripper）到机器人基坐标系（base）的变换矩阵中的旋转矩阵部分，包含了3x3的旋转矩阵和3x1的旋转向量。
// t_gripper2base：和R_gripper2base同理，表示的是平移向量的部分。
// R_target2cam：描述为将在目标标定板坐标系（target）中的点转换到相机坐标系（cam）中。表示目标标定板坐标系（target）到相机坐标系（cam）的变换矩阵中的旋转矩阵部分，包含了3x3的旋转矩阵和3x1的旋转向量。
// t_target2cam：和R_target2cam同理，表示的是平移向量的部分。
// R_cam2gripper描述为将在相机坐标系（cam）中的点转换到机械臂末端坐标系（夹爪坐标系，gripper）中。表示相机坐标系（cam）到机械臂末端坐标系（夹爪坐标系，gripper）的变换矩阵中的旋转矩阵部分，包含了3x3的旋转矩阵和3x1的旋转向量。
// t_cam2gripper：和R_cam2gripper同理，表示的是平移向量的部分。
// method = CALIB_HAND_EYE_TSAI：有五种计算方法，一般选择TSAI。

• ⑩最终得到R_cam2gripper和t_cam2gripper，拼接成相机到机械臂末端的变换矩阵。

关于手眼标定结果的应用：
https://blog.csdn.net/qq_45445740/article/details/123567627?spm=1001.2014.3001.5501
感谢视频讲解：https://www.bilibili.com/video/BV17K4y1u7iJ?spm_id_from=333.1007.top_right_bar_window_history.content.click