3D视觉 | 机器人3D手眼标定实验
复现Cognex VisionPro 3D的大部分内容，涵盖眼在手外、眼在手上，包括相机标定、手眼标定、3D定位计算位移偏差。最后的位移偏差与Cognex的结果在1mm左右。
— Edited By Hugo
# 1
用python实现原型验证算法，再移植成C++编译为dll，供C#调用。
python的库主要用到：cv2、numpy。
C++的库主要用到：OpenCV、Eigen。
python的库主要用到：cv2、numpy。
C++的库主要用到：OpenCV、Eigen。
复现的VisionPro 3D函数：
封装成dll的函数接口：
函数接口：
1.void getMatsEth(int* other_info, char* point3d_str, char* point2d_str, char* robot_str, double* mtx33_l, double* mtx33_r, double* mtx44_l, double* mtx44_r)
2.void getMatsEih(int* other_info, char* point3d_str, char* point2d_str, char* robot_str, double* mtx33_l, double* mtx33_r, double* mtx44_l, double* mtx44_r)
3.void calc_ImageToWorld(double* _mtx44_cam3dToPhy3d_l, double* _mtx33_camIntrin_l, double* _mtx44_cam3dToPhy3d_r, double* _mtx33_camIntrin_r, double* _Point_Cl, double* _Point_Cr, double* params)
4.void calc_Excursion(int pointNum, double* _Point_Model_3D, double* _Point_Now_3D, double* _res_excursion)
# 2
总体步骤：
1.相机标定
2.手眼标定
3.计算偏移
坐标系转换：
1.左右相机通过特征提取，得到Raw2D下的特征点坐，通过相机内参，将特征点转到Camera2D下。
2.把未知高度作为未知数，将特征点转到Camera3D下，通过手眼矩阵，将特征点转到Hand3D下
3.通过示教器读取的机械手位姿，将特征点转到RobotBase3D下
# 3
方程求解，代入数据：
最终共有6个方程，即左右相机各3个方程（x、y、z）,但6个方程中只有5个未知数，即RobotBase3D下的X、Y、Z、Camera3D下的左右相机的z,一般做法，用优化方法解超定方程即可。
3D视觉基础知识：
位姿，刚体在坐标系中用位姿描述，位姿 = 位置（position） + 姿态（orientation）。
一般地，位置和姿态各用3个值表示，位置用x、y、z表示偏移，姿态用rx、ry、rz表示欧拉角旋转。
标定板：
工业常用标定板分两大类：实心圆阵列（Halcon）、棋盘格（VisionPro、OpenCV、Matlab），非精确制造的标定板会导致不好的标定结果，比如激光/喷墨打印机打印的标定板。
康耐视的棋盘格（Cognex checkerboard）包含标准的基准标识，能够做到不必在一张图片内拍摄整张棋盘格。
篇幅有限，下一篇继续分享：
1.相机标定
2.姿态评估
3.手眼标定
# 4
相机标定：
3D标定：在与图像像素相关的2D坐标系和与物理世界相关的3D坐标系间建立数学联系最初定义的物理世界坐标系是标定板坐标系。
3D标定后的相机可以实现：
①Raw2D中的2D point ⇒ Phys3D中的3D ray
②Raw2D中的2D point + Phys3D中的3D plane ⇒ Phys3D中的3D point
③Phys3D中的3D point ⇒ Raw2D中的2D point
①Raw2D中的2D point ⇒ Phys3D中的3D ray
②Raw2D中的2D point + Phys3D中的3D plane ⇒ Phys3D中的3D point
③Phys3D中的3D point ⇒ Raw2D中的2D point
三角测量（Triangulation）
单个标定相机：Raw2D中的2D point ⇒ Phys3D中的3D ray
多个标定相机：Raw2D中的2D point ⇒ Phys3D中的3D point（从不同位置观察同一物体，可在Phys3D中生成与物体同一特征相关的多个交叉直线，从而算出3D位姿）
多个标定相机：Raw2D中的2D point ⇒ Phys3D中的3D point（从不同位置观察同一物体，可在Phys3D中生成与物体同一特征相关的多个交叉直线，从而算出3D位姿）
注意：2D特征必须准确且可靠（不会被3D影响，如透视收缩）
# 5
姿态估计：
估计3D物体姿态至少需要3个不共线的点。
手眼标定：
输入：标定板图像、Hand3D位姿（示教器提供）
眼在手外：Camera3D中的3D point ⇒ RobotBase3D中的3D point
眼在手上：Hand3D中的3D point ⇒ Camera3D中的3D point
眼在手上：Hand3D中的3D point ⇒ Camera3D中的3D point
最终实现，Raw2D中的2D point ⇒ RobotBase3D中的3D point
3D严格变换（3D Rigid Transforms）实现两个3D笛卡尔坐标系之间的映射。只包含旋转和平移，不包含比例、反射、错切。
维基百科讲解：旋转矩阵、欧拉角、四元数
# 6
标定实施：
在不同位姿下，用固定光学和机械参数的相机采集
工业应用中，手（机械臂）和眼（相机）有两种位置关系：
由于cal和tool的相对位置是不变的，可通过此联立方程：
化为CX=XD的形式，可得：
其中，camHbase是待求解的，camHcal可通过相机外参获得，baseHtool可通过示教器读取。
由于base和obj的相对位置是不变的，可通过此联立方程：
化为CX=XD的形式，可得：
其中，camHtool是待求解的，camHcal可通过相机外参获得，baseHtool可通过示教器读取。
/3 
