默默的点滴

原理

下图表示了小孔成像模型（图片及公式参考 OpenCV官方资料）

这个图里涉及4个坐标系：

1. 世界坐标系：其坐标原点可视情况而定，可以表示空间的物体，单位为长度单位，比如MM（毫米），用矩阵$\begin{bmatrix} X_W \\ Y_W \\Z_W \end{bmatrix}$表示；
2. 相机坐标系：以摄像机光心为原点（在针孔模型中也就是针孔为中心），z轴与光轴重合，也就是z轴指向相机的前方（与成像平面垂直），x轴与y轴的正方向与世界坐标系平行，单位为长度单位，比如MM（毫米），用矩阵$\begin{bmatrix}X_c \\ Y_c \\ Z_c\end{bmatrix}$表示；
3. 图像物理坐标系（也叫成像平面坐标系）：用物理长度单位表示像素的位置，坐标原点为摄像机光轴与图像物理坐标系的交点位置，单位为长度单位，比如MM（毫米），用矩阵$\begin{bmatrix}x \\ y \end{bmatrix}$表示。
4. 像素坐标系：坐标原点在左上角，以像素为单位，有明显的范围限制，即用于表示全画面的像素长和像素长宽，矩阵$\begin{bmatrix}u \\ v \end{bmatrix}$表示。

以下公式描述了$\begin{bmatrix}u & v \end{bmatrix}^T$、$\begin{bmatrix}x & y \end{bmatrix}^T$、$\begin{bmatrix}X_c & Y_c & Z_c\end{bmatrix}^T$和$\begin{bmatrix}X_W & Y_W & Z_W \end{bmatrix}^T$之间的转换关系。

$z\begin{bmatrix}u \\ v\\ 1 \end{bmatrix}= \begin{bmatrix}1/d_x&0&c_x\\0&1/d_y&c_y\\0&0&1 \end{bmatrix} \begin{bmatrix}f&0&0\\ 0&f&0\\ 0&0&1 \end{bmatrix} \begin{bmatrix}r11&r12&r13&t1\\ r21&r22&r23&t2\\ r31&r32&r33&t3 \end{bmatrix} \begin{bmatrix}X_W \\ Y_W \\Z_W \\ 1\end{bmatrix}$

以上公式中，$d_x$和$d_y$表示1个像素有多少长度，即用传感器的尺寸除以像素数量，比如2928.384um * 2205.216um的传感的分辨率为2592 * 1944，每个像素的大小即约1.12um。

由于相机与物体的视角来看，都是三维坐标，因此两者之间的变换只需要进行矩阵的旋转、平移即可达到坐标系转换的目的（不同坐标系中，物体的绝对大小并不会随着坐标系的变化而变化，因此不涉及缩放处理）。对于变换矩阵  $\begin{bmatrix}r11&r12&r13&t1\\ r21&r22&r23&t2\\ r31&r32&r33&t3 \end{bmatrix}$ 需要理解，矩阵是由 3*3 的旋转矩阵 r (rotation) 和 3*1的平移向量 t (translation)组成。

$f$表示焦距，在上图中，根据相似三角形，P点和p点具有以下关系：

$\frac{X_c}{x} = \frac{Y_c}{y} = \frac{Z_c}{f}$ 即$x=X_c/(\frac{Z_c}{f})$ $y=Y_c/(\frac{Z_c}{f})$，可见：$f$越大，$x$和$y$越大，$Z_c$越大，$x$和$y$越小。

$c_x$和$c_y$表示中心点在像素坐标系中的位置。

要求像素坐标系中某像素点对应在世界坐标系中的位置，需要知道相机的内参、外参，相机的内参可以通过标定获得，外参可以人为设定。

第一步，将像素坐标变换到相机坐标系：

$z\begin{bmatrix}u \\ v\\ 1 \end{bmatrix} = \begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1 \end{bmatrix} \begin{bmatrix}x \\ y\\ 1 \end{bmatrix} = K\begin{bmatrix}x \\ y\\ 1 \end{bmatrix}$

两边乘以K的逆后推导出：

$\begin{bmatrix}x \\ y\\ z \end{bmatrix}=K^{-1} \begin{bmatrix}u \\ v\\ 1 \end{bmatrix}$

第二步，从相机坐标系变换到世界坐标系：

$\begin{bmatrix}X_c \\ Y_c\\ Z_c \end{bmatrix} = R \begin{bmatrix}X \\ Y\\ Z \end{bmatrix} + t$

将方程乘以$R^{-1}$，可以推导出：

$\begin{bmatrix}X \\ Y\\ Z \end{bmatrix} = \begin{bmatrix}X_c \\ Y_c \\ Z_c \end{bmatrix}R^{-1} - t R^{-1}= z\begin{bmatrix}x\\ y\\ 1 \end{bmatrix}R^{-1} - t R^{-1}$

代码

通过输入相机的内参，旋转向量，平移向量和像素坐标，可以通过以下函数求出对应的世界坐标点。
以下代码中需求注意要对平移向量取转置，将1x3矩阵变为3x1矩阵后，才能实现3x3矩阵和3x1矩阵的乘法运算。

验证

先使用projectPoints生成像素点：

使用以下欧拉角：

对应的平移向量，表示空间坐标原点相对在相平面原点偏移x=134mm，y=132mm，z=200mm。

生成空间坐标点：

经projectPoints计算后对应的像素空间点是：

经函数求出的空间坐标点是：

可以对比按11*8格和30mm/格所生成空间坐标点结果，基本一致。

参考链接

---

• 像素坐标转世界坐标的计算
• Camera Calibration and 3D Reconstruction
•  计算机视觉基础3——内部参数描述

一、背景

Calibration是机器人开发者永远的痛。虽然说方法说起来几十年前就有，但每一个要用摄像头的人都还是要经过一番痛苦的踩坑，没有轻轻松松拿来就效果好的包。其实人类不就是个手眼协调的先进“机器人”吗，O(∩_∩)O哈哈~

机器人视觉应用中，手眼标定是一个非常基础且关键的问题。简单来说手眼标定的目的就是获取机器人坐标系和相机坐标系的关系，最后将视觉识别的结果转移到机器人坐标系下。

手眼标定行业内分为两种形式，根据相机固定的地方不同，如果相机和机器人末端固定在一起，就称之为“眼在手”（eye in hand），如果相机固定在机器人外面的底座上，则称之为“眼在外”（eye to hand）。

二、手眼关系的数学描述

1. eye in hand，这种关系下，两次运动，机器人底座和标定板的关系始终不变。求解的量为相机和机器人末端坐标系的位姿关系。

2. eye to hand，这种关系下，两次运动，机器人末端和标定板的位姿关系始终不变。求解的量为相机和机器人底座坐标系之间的位姿关系。

手眼标定eye in hand 和eye to hand 的区别主要是机器人那边，一个是end相对于base，另一个是base相对于end。千万注意。

三、AX = XB问题的求解

旋转和平移分步法求解：

• Y. Shiu, S. Ahmad Calibration of Wrist-Mounted Robotic Sensors by Solving Homogeneous Transform Equations of the Form AX = XB. In IEEE Transactions on Robotics and Automation, 5(1):16-29, 1989.
• R. Tsai, R. Lenz A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration. In IEEE Transactions on Robotics and Automation, 5(3):345-358, 1989.

迭代求解及相关资料可以看看相关网上的英文教程 Calibration and Registration Techniques for Robotics 其中也有一些AX= XB的matlab代码可以使用。

ROS 下也有相关的一些package可以利用

• https://github.com/IFL-CAMP/easy_handeye
• handeye - ROS Wiki
• Visual Servoing Platform: Tutorial: Camera extrinsic calibration
• HandEyeCalibration TUM这里也有很多手眼标定求解的参考链接

四、其他参考资料

3D 视觉之手眼标定 邱强Flyqq 微信文章

手眼标定的两种方式_wzj5530的专栏-CSDN博客_手眼标定 图不错

深入浅出地理解机器人手眼标定_二毛的博客-CSDN博客_机器人手眼标定 部分halcon代码

eye-in-hand手眼标定系统_二毛的博客-CSDN博客 halcon代码

手眼标定之9点法_GoRunningSnail的博客-CSDN博客 部分原理

UR5、Kinect2手眼标定总结_zhang970187013的博客-CSDN博客 UR5 与easy hand eye

一般用“两步法”求解基本方程，即先从基本方程上式求解出旋转部分，再代入求解出平移部分。

经典手眼标定算法之Tsai-Lenz的OpenCV实现_YunlinWang的博客-CSDN博客

============== Halcon 官方示例-手眼标定 ==================

五、Matlab下手眼标定解算

相机与机器人是eye-to-hand模式，机器人为加拿大Kinova 6轴机械臂，机器人pose为基座相对于末端的x，y，z，rx，ry，rz，rw, 单位为米。姿态使用单位四元数表示。

2017.08.29Kinova_pose_all_8_1.txt

pattern pose为标定板相对于相机的x，y，z，rx，ry，rz，rw, 单位为米。姿态使用单位四元数表示。

2017.08.29Pattern_pose_all_8_1.txt

此Matlab文件调用数据进行离线解算。Calibration and Registration Techniques for Robotics 的这部分 Registering Two Sets of 6DoF Data with 1 Unknown，有code下载，下载好命名为shiu.m和tsai.m供下面程序调用就行。我这里贴出

Jaco_handeye_test_10.m 测试程序中用到了Peter Corke老师的机器人工具箱。我的Matlab版本R2013a，利用机器人工具箱的一些转换函数（四元数的构建，欧拉角转换等），它安装和基本使用参考这里：Matlab机器人工具箱_Learning by doing-CSDN博客_matlab机器人工具箱

稍微解释一下，程序做的就是读入机器人和相机的两两姿态信息，转换为4x4 的齐次变换矩阵，送入tsai.m程序求解。

手眼标定eye in hand 和eye to hand 的区别主要是机器人那边，一个是end相对于base，另一个是base相对于end。千万注意。

====================平面九点标定法====================

当利用RGB相机或者是机器人只进行平面抓取（也即固定姿态抓取，机器人六自由度位置和姿态简化为只考虑平移，姿态人为给定并且固定，这时机器人可以移动到目标点上方），问题可以简化为平面RGB图像的目标像素点集A（x1，y1）与机器人在平面（X1，Y1）的点对关系。具体做法是相机识别像素点给到A，然后利用示教器查看机器人在基座标系下的坐标，当做B。

相机坐标和机器人坐标写成齐次的形式，投影矩阵X是一个3x3的矩阵我们需要6组对应点来求解最小配置解。利用奇异值分解SVD来求取。

D:\opencv_work\cubeSolver\cv_solver\ConsoleApplication1\CV_SVD.cpp

D:\Matlab_work\handeye\NinePoints_Calibration.m

OpenCV: Operations on arrays

结果对比：左halcon C#（第三列为0,0,1，没做显示），右opencv c++，底下为Matlab结果，三者一致，算法检测通过。

=============

四轴平面机器人的手眼标定_Stones1025的博客-CSDN博客

这种方法利用点对，求仿摄变换矩阵

================= Eye in hand 数据及Ground truth =========================

Marker in Camera 八组数据，单位：米及弧度，姿态用的是RotVector表示

Robot end-effector in Base 八组数据，单位：米及弧度，姿态用的是RotVector表示

Ground truth：Camera in end-effector

参考链接

---

• 摄像头和机械臂的手眼标定
• 机器人手眼标定Ax=xB（eye to hand和eye in hand）及平面九点法标定
• Open source visual servoing platform library  
• 机械臂 手眼标定 手眼矩阵 eye-in-hand 原理、实践及代码
• 像素坐标转世界坐标的计算