3D 视觉之手眼标定
又快一个月没有推送了。主要是因为这段时间在北京，每天忙着学习新知识，所以就没时间写了。
但是，最近很多人问关于「机器人手眼标定」的问题，而之前说要开源的标定包也跳票很久，（目测还会继续跳票一会儿）。所以决定先讲讲机器人手眼标定的原理，方便大家自己编写程序。
我其实不太喜欢写满是公式的东西，不能用日常语言讲清楚，那就说明对这个问题的理解不够深入。
但是，对于手眼标定这种事情，如果不用公式：
好吧，言归正传。
对于一个带有视觉的机器人系统，相机所获得的所有信息都是在相机坐标系下描述的。要想让机器人利用视觉系统得到的信息，首先就是要确定相机坐标系与机器人之间的相对关系，这便是机器人手眼标定的研究内容。
如果是一个没有相关研究积累的实验室，想开始从事机器人 3D 视觉等方面的研究，往往第一步就卡在了手眼标定上。
两种相机安装方式：（左）眼在手外，eye-to-hand；（右）眼在手上，eye-in-hand
如上图所示，在机器人系统中，相机的安装方式可以分为两大类：
眼在手外，eye-to-hand：也即摄像头安装在手臂之外的部分，与机器人的基座（世界坐标系）相对固定，不随着机械臂的运动而运动；
眼在手上，eye-in-hand：也即摄像头安装在机械臂上，会随着机械臂的运动而发生运动。
这两个方式的求解略有不同，但基本原理相似。
一、首先看看眼在手外的情况：
如上图所示，kinect 放在机械臂旁边的桌上，在没人碰它的情况下，相机与机械臂的基座是相对固定的。
强势打水印
我们便会得到如图所示这些坐标系：
{B}：机器人基坐标系
{E}：机器人末端连杆（与标定板固连的连杆）坐标系
{K}：标定板坐标系，（字母 B 和 C 都被占用了，所以随便选个 K）
{C}：相机坐标系
以及它们之间的坐标变换：
A：机器人末端在机械臂基座坐标系下的位姿，这其实就是机器人运动学正解的问题（已知，后面会补充）；
B：标定板在机器人末端坐标系下的位姿，由于是随便装的标定板，所以未知；
C：相机在标定板坐标系下的位姿，这个其实就是求解相机的外参（已知，后面会补充）；
D：相机在机器人基坐标系下的位姿，这就是我们想求的东西，而 D=A·B·C
所以，只要我们能计算得到 B 变换，那么相机在机器人坐标系下的位姿 D 也就自然得到了。
如上图所示，我们让机器人走两个位置，保证这两个位置都能使得摄像头看到标定板，于是有：
因为标定板与机械臂末端连杆是固定的，所以 B 变换也是固定不变的。
稍微变换一下：
这是一个典型的 AX = XB 问题，而且根据定义，其中的 X 是一个 4x4 齐次变换矩阵：
在谷歌学术用关键词「AX=XB」搜索便可以拿到各种求解方法：
这几篇论文都很详细地给出了推导过程，按照它们写程序基本没问题。这篇文章最后，如果我今天还有时间，我就具体介绍一种解法（划掉）。
这样，我们有了 A，B 和 C，就能直接求解得到我们想要的手眼标定结果 D 了。
二、第二种就是眼在手上的情况
如上图所示，Realsense 固定在机械臂末端，会随着机械臂的运动而运动。
我们会得到一堆跟前面情况类似的坐标系（完全相同，就不赘述了）和坐标变换关系（略有不同）：
A：机器人末端在机械臂基座坐标系下的位姿，这其实就是机器人运动学正解的问题（已知，后面会补充）；
B：相机在机器人末端坐标系下的位姿，这个变换是固定的，只要知道这个变换，我们就可以随时计算相机的实际位置，所以这就是我们想求的东西；
C：相机在标定板坐标系下的位姿，这个其实就是求解相机的外参（已知，后面会补充）；
D：相机在机器人基坐标系下的位姿，实际使用过程中标定板并不存在，所以这个变换关系我们不关心。
这个跟前面类似，直接让机械臂运动两个位置，保证这两个位置下都可以看到标定板。然后构建空间变换回路：
很好，又成了一个 AX=XB 问题。
你看，机器人手眼标定就是这么简单！
=============================
前面为了理清思路，我把每个部分的细节给省略掉了。这里开始部分填坑。
我们一个一个来解决：
1）变换 A：它是机械臂末端在机器人基座坐标系下的位姿，其实就是机器人学里面最基础的运动学正解。
这个没什么好多说的，就是去翻机器人学的教程，然后用 DH 矩阵等方法来计算机器人运动学正解。如果是在 ROS 里，可以直接通过 KDL 或者 TF 等来直接得到。
简而言之：只要知道机器人在当前状态下每个关节的角度，我们就可以计算得到 A 变换。
2）变换 C：它是相机在标定板坐标系下的位姿。其实就是相机的外参。
先解释一下，对于一般相机而言，我们会用小孔成像模型来对其进行建模。
所以，在相机坐标系下，空间中 3D 点 (X, Y, Z, 1)_c 与图像上对应 2D 点 (x y)_c 正好是满足如下关系：
其中的变换矩阵，就是我们说的内参矩阵，包含相机焦距等只与相机内部结构有关的参数。
然后，我们一般描述物体，都是在世界坐标系下的进行的，也即 (X, Y, Z, 1)_w ，而不是相机坐标系（因为我们无法知道相机坐标系的位置）。所以，我们需要先把物体坐标从世界坐标系变换到相机坐标系。
于是，我们就得到了完整的相机模型：
相机模型有了，我们要做的就是先确定内参矩阵，然后根据不同时刻拍摄的标定板图片来求取外参矩阵：也即相机在世界坐标系下的变换关系 C。
目前来说，要解决这个问题基本上都是采用张正友老师的方法：
Zhang, Zhengyou. "A flexible new technique for camera calibration." IEEE Transactions on pattern analysis and machine intelligence 22.11 (2000): 1330-1334.
当然，这部分要让初学者自己实现张老师论文里的算法，可能还是有点强人所难了。
不过，幸好 OpenCV 或者 Matlab 的相机标定工具箱都有这个算法的实现。直接去找这两者的官方文档就可以了。基本操作就是：
拍摄 20 张左右带有标定板的图片
然后输入标定板的几何参数，用来确定标定板角点在标定板上的坐标 [X, Y, Z]
然后利用人工标记或者角点提取算法找到图像中每个角点的像素坐标 [u, v]
之后，OpenCV 或者 Matlab 就会根据张老师的方法，帮我们计算得到相机的内参矩阵。
而在实际机器人标定过程中，就可以载入之前标定好的内参矩阵，来计算外参 C （OpenCV 里也有相应函数实现）。
3）求解 AX=XB：
实在是懒得编辑公式了，我直接推荐几篇论文吧，对应着四种不同解法：
Tsai, Roger Y., and Reimar K. Lenz. "A new technique for fully autonomous and efficient 3D robotics hand/eye calibration." IEEE Transactions on robotics and automation 5.3 (1989): 345-358.
Horaud, Radu, and Fadi Dornaika. "Hand-eye calibration." The international journal of robotics research 14.3 (1995): 195-210.
Park, Frank C., and Bryan J. Martin. "Robot sensor calibration: solving AX= XB on the Euclidean group." IEEE Transactions on Robotics and Automation10.5 (1994): 717-721.
Daniilidis, Konstantinos. "Hand-eye calibration using dual quaternions." The International Journal of Robotics Research 18.3 (1999): 286-298.
四种方法我这边都试过，大多数情况下，四种方法精度接近，最后一种 Dual Quaternions 的方法会稍微比其他算法好一点。
虽然这几篇文献都有点旧了，但大家可以简单看看它们发表的期刊，就能大概知道机器人手眼标定对机器人领域的意义了。
=============================
好吧，最后，其实我前面说的「手眼标定很简单」是开玩笑的。机器人领域基本上所有用到的模型都只是「近似模型」。也就是说，上面说的每一步，都有改进空间。例如：
1）直接使用的机器人运动学模型是认为机器人加工、装配、控制没误差的情况。
实际上这种假设是不成立的。也就是说，我们只拿到每个关节角度，计算的末端位姿是存在误差的。可以从机器人运动学参数辨识等方面入手提高最后的标定精度；
2）相机模型其实也不是标准的小孔模型，像素坐标与空间坐标之间其实是非线性的对应关系。从相机模型角度就又有一大堆工作可做。
但是，我们如果一开始就陷入各种细节问题，很容易陷进去就出不来了，这就是机器人领域最坑的地方，有时候一定要先抓住 big picture。
对了，最后照例应该放几个我们自己标定的动图：
Motoman SDA：眼在手外
UR5：眼在手外
好吧，毕竟人在北京，电脑里找不到「眼在手上」的标定视频。
最最后，机器人的每个标定位姿，最好是人工指定，如果让它自己生成的话，一定要防止下面的情况发生：