OpenCV手眼标定

OpenCV 手眼标定

使用 OpenCV 进行手眼标定，版本需要 4.1.0 及以上。

为了确定手眼变换，至少需要两个具有非平行旋转轴的运动。因此，至少需要 3 个不同的姿势，但强烈建议使用更多的姿势。—— OpenCV 官方文档提示

cv::calibrateHandEye()

OpenCV有关手眼标定官方文档

void 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
)	

• 矩阵输入参数类型都为 std::vector<cv::Mat>，即 $n$ 张标定图片，需要有 $n$ 组相关的矩阵依序放入 std::vector 以传入参数
• method：求解 $n-1$ 个 $A\ast X=X\ast B$ 方程组的方法
  • method类型官方文档
  • CALIB_HAND_EYE_TSAI：一种完全自主高效的3D机器人手眼标定新技术
  • CALIB_HAND_EYE_PARK：机器人传感器标定，求解欧几里德群上的 $AX=XB$
  • CALIB_HAND_EYE_HORAUD：手眼标定
  • CALIB_HAND_EYE_ANDREFF：在线手眼标定
  • CALIB_HAND_EYE_DANIILIDIS：使用双四元数的手眼标定

坐标系：

• base：机械臂基地坐标系
• gripper：机械臂末端坐标系（夹爪坐标系）
• cam：相机坐标系
• target：标定板坐标系（世界坐标系）

方法变量名跟眼在手上的情况关联，代表：

$$\{\begin{array}{l}{A}_i={(}_{gripper}^{base}{T}^{(i+1)}{)}^{(-1)}{}_{gripper}^{base}{T}^{(i)}\\ X={}_{cam}^{gripper}T\\ B_i={}_{target}^{cam}{T}^{(i+1)}{(}_{target}^{cam}{T}^{(i)}{)}^{(-1)}\end{array},i\in [1,n-1]$$

对于眼在手上

眼在手上，标定求解的是 相机坐标系 相对于 机械臂末端坐标系 的变换矩阵 ${}_{cam}^{gripper}T$。

输入：

• R_gripper2base：机械臂末端坐标系 相对于 机械臂基底坐标系的 旋转矩阵 ${}_{gripper}^{base}R$
• t_gripper2base：机械臂末端坐标系 相对于 机械臂基底坐标系的 平移矩阵 ${}_{gripper}^{base}t$
• R_target2cam：世界坐标系 相对于 相机坐标系的 旋转矩阵 ${}_{target}^{cam}R$
• t_target2cam：世界坐标系 相对于 相机坐标系的 平移矩阵 ${}_{target}^{cam}t$

输出：

• R_cam2gripper：相机坐标系 相对于 机械臂末端坐标系的 旋转矩阵 ${}_{cam}^{gripper}R$
• t_cam2gripper：相机坐标系 相对于 机械臂末端坐标系的 平移矩阵 ${}_{cam}^{gripper}t$

对于眼在手外

眼在手外，标定求解的是 相机坐标系 相对于 机械臂基底坐标系 的转换矩阵 ${}_{cam}^{base}T$。

输入：

• R_gripper2base为 机械臂基底坐标系 到 机械臂末端坐标系的 旋转矩阵 ${}_{base}^{gripper}R$
• t_gripper2base为 机械臂基底坐标系 到 机械臂末端坐标系的 平移矩阵 ${}_{base}^{gripper}t$
• R_target2cam为 标定板坐标系 到 相机坐标系的 旋转矩阵 ${}_{target}^{cam}R$
• t_target2cam为 标定板坐标系 到 相机坐标系的 平移矩阵 ${}_{target}^{cam}t$

输出：

• R_cam2gripper相机坐标系 到 机械臂基底坐标系的 旋转矩阵 ${}_{cam}^{base}R$
• t_cam2gripper相机坐标系 到 机械臂基底坐标系的 平移矩阵 ${}_{cam}^{base}t$

由机械臂正运动学解出 机械臂基底坐标系 与 机械臂末端坐标系的 之间的变换矩阵

${}_{end}^{base}R$和${}_{end}^{base}T$，可通过解算机械臂的位姿求得
对于每个位姿，通常会返回六个参数：${\theta }_x{\theta }_y{\theta }_z{t}_x{t}_y{t}_z$
这六个参数是机械臂末端在基底坐标系下的位姿的表示
即：${}_{end}^{base}R=R_z{R}_y{R}_x{}_{end}^{base}T={\left(\begin{array}{ccc}{t}_x& t_y& t_z\end{array}\right)}^T$
${\theta }_x{\theta }_y{\theta }_z$为欧拉角，欧拉角(旋转角)到旋转矩阵的转换，见旋转矩阵.md

solvePnP

使用OpenCV中的solvePnP，可以求解相机到标定板坐标系的旋转矩阵和平移矩阵，即${}_{camera}^{board}R$和${}_{camera}^{board}T$

solvePnP是用来求解2D-3D的位姿对应关系，在这里，图片是(2D)，而标定板坐标系是(3D)
利用solvePnP函数，就可以得到图片（相机坐标系）与标定板坐标系的变换关系

bool cv::solvePnP(
    InputArray objectPoints,
    InputArray imagePoints,
    InputArray cameraMatrix,
    InputArray distCoeffs,
    OutputArray rvec,
    OutputArray tvec,
    bool useExtrinsicGuess = false,
    int flags = SOLVEPNP_ITERATIVE
)	

输入：
objectPoints为棋盘格坐标系，由棋盘格真实坐标生成，一般以棋盘格左上角顶点建立，$Z$轴坐标都为0
斗
imagePoints为图片识别到的角点坐标，与objectPoints中的值一一对应
cameraMatrix为相机内参矩阵
distCoeffs为相机畸变矩阵

输出：
rvec为标定板坐标系到相机坐标系的旋转向量，可用cv::Rodrigues()变为旋转矩阵
tvec为标定板坐标系到相机坐标系的平移向量

即得：
${}_{board}^{camera}R{}_{board}^{camera}T$

注：
输入时，可以使用vector of vector，即vector<vector<>>，输出是vector，意味着可以一次性输入所有位姿图片，然后计算得到每张图的变换矩阵，从而进行手眼标定

注意事项

1. 直接利用cv::calibrateCamera得到的$R,t$，来作为标定板坐标系到相机坐标系的输入
   利用cv::calibrateCamera得到的$R,t$，它的内参是一个新相机内参，而之后进行手眼标定得到的转换矩阵也是对于新相机内参而言的
   而在识别待抓取物体时，进行的PnP坐标结算，输入的相机内参是老内参，不是新相机内参，如果再使用对于新相机内参而言的手眼标定得到的转换矩阵，进行到机械臂基底的转换，这样就会有错误了，导致每次抓出总会有偏差
   因此，无论是手眼标定，还是PnP结算，都要使用同一个相机内参
2. 识别标定板的角点标反了
   由于在建立棋盘格上的三维坐标系的时候，我们默认是从棋盘格左上角到右下角建立的，如果识别反了，则会有个别图片棋盘格识别的角点和输入的棋盘格三维坐标对应不上，导致标定错误
   要注意拍摄标定图片时的规范操作，不要让原本标定板最右下角的角点在原本标定板最左上角的角点的左边
3. 手眼标定时，标定板面积过小且只在图片中心处移动
   这样会使得标定的结果更偏向于中心处准确，而图片边缘处会略微的不准
4. 手眼标定时，机械臂末端位姿只上下前后移动，旋转角几乎不变
   这样时没法得到准确的旋转矩阵的，6个分量都需要移动，才能计算准确
5. 手眼标定位姿太少(图片太少)
   图片太少计算结果的稳定性比较差，建议十张以上
6. 抓取时点云坐标系不是手眼标定的相机坐标系，常见于使用RGB-D相机
   使用厂商提供的RGB相机到点云坐标系的变换矩阵，变换相机坐标系到实际点云坐标系用于抓取