双目视觉在机械臂抓取中的应用（3D locate）
点击上方“小白学视觉”，选择加"星标"或“置顶”
重磅干货，第一时间送达实现功能
实习公司的项目，复现Cognex VisionPro 3D的大部分内容，涵盖眼在手外、眼在手上，包括相机标定、手眼标定、3D定位计算位移偏差。最后的位移偏差与Cognex的结果在1mm左右。
实施路线
用python实现原型验证算法，再移植成C++编译为dll，供C#调用。
python的库主要用到：cv2、numpy
C++的库主要用到：OpenCV、Eigen
复现的VisionPro 3D函数
| 函数语句 | 函数功能 |
|---|---|
| camCalRes = camCalibrator.Execute(pelRects, extractedFeatures, calHeights, calPoseTypes); | 相机标定 |
| heCalibs = heCalibrator.Execute(pelRects, intrinsics, extractedFeatures, robotPositions); | 手眼标定 |
| modelPoints = triangulator.Execute(cameraCalibs, pointsRaw2D, isPointValid, out is3DPointValid, out resRaw2D, out resPhys3D); | 三角测量获得3D点坐标 |
| Cog3DPoseEstimatorUsing2DPointsResult res = pParam.Models[model].Execute(camCalibs, features2D, weights2D); | 通过2D点估计3D姿态 |
| 函数功能 | 主体类型 | 返回值类型 |
|---|---|---|
| 相机标定 | Cog3DCameraCalibrator | Cog3DCameraCalibrationResult |
| 手眼标定 | Cog3DHandEyeCalibrator | List |
| 三角测量获得3D点坐标 | Cog3DTriangulator | Cog3DVect3Collection |
| 通过2D点估计3D姿态 | List |
Cog3DPoseEstimatorUsing2DPointsResult |
封装成dll的函数接口
| 函数功能 | 函数名 | 所属dll |
|---|---|---|
| 眼在手外的相机标定、手眼标定 | void getMatsEth() | CalibrateCpp.dll |
| 眼在手上的相机标定、手眼标定 | void getMatsEih() | CalibrateCpp.dll |
| 3D定位：左右相机的2D点转换为3D点 | void calc_ImageToWorld() | CalcImageToWorldCpp.dll |
| 计算偏移 | void calc_Excursion() | CalcExcursionCpp.dll |
函数接口：
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)
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)
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)
void calc_Excursion(int pointNum, double* _Point_Model_3D, double* _Point_Now_3D, double* _res_excursion)
| 函数名 | 输入 | 输出 |
|---|---|---|
| void getMatsEth() | other_info是长度为3的整型一维数组，3个元素分别代表标定图片数量、标定图片宽、标定图片高 point3d_str是字符串数组，为标定板角点在CalPlate3D下的坐标 point2d_str是字符串数组，为标定板角点在Raw2D下的坐标 robot_str是字符串数组，为机械手位姿所代表的4*4的矩阵 |
mtx33_l是浮点型数组，为左相机内参所代表的3*3的矩阵 mtx33_r是浮点型数组，为右相机内参所代表的3*3的矩阵 mtx44_l是浮点型数组，为左相机的手眼矩阵所代表的4*4的矩阵 mtx44_r是浮点型数组，为右相机的手眼矩阵所代表的4*4的矩阵 |
| void getMatsEih() | 同上 | 同上 |
| void calc_ImageToWorld() | _mtx44_cam3dToPhy3d_l是浮点型数组，为左相机的手眼矩阵所代表的4*4的矩阵 _mtx33_camIntrin_l是浮点型数组，为左相机内参所代表的3*3的矩阵 _mtx44_cam3dToPhy3d_r是浮点型数组，为右相机的手眼矩阵所代表的4*4的矩阵 _mtx33_camIntrin_r是浮点型数组，为右相机内参所代表的3*3的矩阵 _Point_Cl是长度为3的齐次形式的浮点型一位数组，为左相机的特征点在在Raw2D下的坐标 _Point_Cr是长度为3的齐次形式的浮点型一位数组，为右相机的特征点在在Raw2D下的坐标 |
params是长度为3的非齐次形式的浮点型一位数组，为特征点在在RobotBase3D下的坐标 |
| CalcExcursionCpp.dll | pointNum是整型值，为一组需计算偏移的点的个数 _Point_Model_3D是浮点型数组，为这组点在偏移前坐标系下的坐标值 _Point_Now_3D是浮点型数组，为这组点在偏移后坐标系下的坐标值 |
_res_excursion是长度为6的浮点型一维数组，分别是x、y、z、rx、ry、rz |
原理概述
6个坐标系
| 坐标系 | 含义 | 备注 |
|---|---|---|
| Raw2D | 图像坐标系（像素单位） | |
| Camera2D | 图像坐标系（物理单位） | 无畸变：去除光学畸变和像素比例的影响 |
| Camera3D | 相机坐标系 | z轴即是相机的光轴，Z=1即为Camera2D |
| CalPlate3D/Phys3D | 标定板坐标系 | |
| Hand3D/Tool | 机械手坐标系/工具坐标系 | |
| RobotBase3D | 基坐标系 |
总体步骤
相机标定
手眼标定
计算偏移
坐标系转换
左右相机通过特征提取，得到Raw2D下的特征点坐标
通过相机内参，将特征点转到Camera2D下
把未知高度作为未知数，将特征点转到Camera3D下
通过手眼矩阵，将特征点转到Hand3D下
通过示教器读取的机械手位姿，将特征点转到RobotBase3D下
方程求解
最终共有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）包含标准的基准标识，能够做到不必在一张图片内拍摄整张棋盘格
相机标定
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
三角测量（Triangulation）
单个标定相机：Raw2D中的2D point ⇒ Phys3D中的3D ray
多个标定相机：Raw2D中的2D point ⇒ Phys3D中的3D point
（从不同位置观察同一物体，可在Phys3D中生成与物体同一特征相关的多个交叉直线，从而算出3D位姿）
注意：2D特征必须准确且可靠（不会被3D影响，如透视收缩）
姿态估计
估计3D物体姿态至少需要3个不共线的点
手眼标定
输入：标定板图像、Hand3D位姿（示教器提供）
眼在手外：Camera3D中的3D point ⇒ RobotBase3D中的3D point
眼在手上：Hand3D中的3D point ⇒ Camera3D中的3D point
最终实现，Raw2D中的2D point ⇒ RobotBase3D中的3D point
3D严格变换（3D Rigid Transforms）
实现两个3D笛卡尔坐标系之间的映射。
只包含旋转和平移，不包含比例、反射、错切。
维基百科讲解：旋转矩阵、欧拉角、四元数
标定实施
输入输出
在不同位姿下，用固定光学和机械参数的相机采集一组标定板的图像
输入：标定板图像、标定板的网格尺寸、标定板位姿
输出：内参、外参
| 内外参 | 变换关系 | 含义 |
|---|---|---|
| 内参（intrinsic） | Raw2DfromCamera2D | 去除光学畸变和像素比例的影响的非线性变换 |
| 外参（extrinsic） | Camera3DfromPhys3D | 两个坐标系之间的6个自由度的线性变换 |
眼在手外/眼在手上
眼在手外：相机固定，相机拍被机械臂带着移动n个姿态的棋盘格
眼在手上：相机移动，机械臂带着移动n个姿态的相机拍固定的棋盘格
推荐单相机标定实施
| 图像数 | 实施 |
|---|---|
| 最少4张 | 以图像平面倾斜20°~30° 以相机光轴旋转90° |
| 推荐9张 | 上述4张+互相平行高度不同 |
| 工业9张 | 1张基准+8张倾斜旋转 |
多相机标定
注意点：
固定两个相机间的相互位姿
固定所有相机的光学参数
多个相机需同时采集图像
本质上：每个相机有独立的Raw2D、Camera2D、Camera3D，有同一个Phys3D
推荐多相机标定实施
轴线
倾斜、旋转：轴线应当大体在相机光心的中心，且大体在工作空间的中心
变高：使用倾斜视角的轴线，并使标定板垂直于轴线
倾斜、旋转
标定板：倾斜20°、旋转90°
变高
标定板：彼此之间平行，改变高度
3D定位
3D视角项目架构
先粗定位，再精定位
寻找特征
关键：2D特征和3D特征的特征对（feature correspondence）
稳定的2D特征（能承受3D的不确定）
1.尖锐、平直的边缘
2.圆
3.具有旋转、缩放、平移不变性的独特特征
英文原文：
• Sharp, straight edges
• Circles
• Unique features that can be found regardless of rotation, scale or translation changes
应当避免的2D特征
1.圆角
2.倒圆边
3.部件其余部分的特出特征
英文原文：
• Round corners
• Round edges
• Features that extrude significantly towards the camera from the rest of the part
检查2D特征的准确性：5个姿态
1.原始姿态
2.视野左上角，z轴旋转20°
3.视野右上角，z轴旋转45°
4.视野左下角，z轴旋转67°
5.视野右下角，z轴旋转90°
相机标定
输入：correspondence pairs，即图像特征与其物理位置
输出：相机内参、相机外参、畸变系数
一般相机标定均采用张正友的标定方法
论文传送：Zhang z．A flexible new technique for canlera calibration[J]．IEEE transactions on pattern analysis and machine intelligence，200．22(11)：1330—1334．
论文解析与实现见我的另一篇博客，[机器视觉]详解相机标定
手眼标定
本质
手眼标定的本质：求解CX=XD，C与相机相关，D与机械臂相关
输入：C、D
输出：X
眼在手外/眼在手上
工业应用中，手（机械臂）和眼（相机）有两种位置关系：
| 分类 | 相机关系 | 所求关系 | 详细描述 |
|---|---|---|---|
| 眼在手外 | 相机固定 | Camera3D与RobotBase3D的关系 | 相机（眼）和机械臂（手）分离，眼的位置相对于手是固定的 |
| 眼在手上 | 相机移动 | Camera3D与Hand3D的关系 | 把相机（眼）固定在机械臂（手）上面，眼随手移动 |
眼在手外
相机固定：Camera3D与RobotBase3D的关系固定
标定板固定在机械臂上：CalPlate3D与Hand3D的关系固定
结合上图，有公式：
标定时控制机械臂从位置1移动到位置2：
坐标系变换关系为：Hand3D → RobotBase3D → Camera3D → CalPlate3D
可得位置1的公式：
合并，可得：
移动到位置2后：
由于cal和tool的相对位置是不变的，可通过此联立方程：
化为CX=XD的形式，可得：
其中，camHbase是待求解的，camHcal可通过相机外参获得，baseHtool可通过示教器读取
眼在手上
相机移动：CalPlate3D与RobotBase3D的关系固定
相机固定在机械臂上：Camera3D与Hand3D的关系固定
结合上图，有公式：
标定时控制机械臂从位置1移动到位置2：
坐标系变换关系为：CalPlate3D → Camera3D → Hand3D → RobotBase3D
可得位置1的公式：
合并，可得：
移动到位置2后：
由于base和obj的相对位置是不变的，可通过此联立方程：
化为CX=XD的形式，可得：
其中，camHtool是待求解的，camHcal可通过相机外参获得，baseHtool可通过示教器读取
计算偏移
本质上是一个NPointToNPoint的问题
输入：一组点在A坐标系下的坐标、这组点在B坐标系下的坐标
输出：这组点从A坐标系变换到B坐标系的转换关系
一般做法是将其看作优化问题，将伪逆作为初始值，只考虑仿射变换中的旋转、平移
具体细节见我的另一篇博客，[机器视觉]欧式空间中的二维点变换关系
参考
| 参考内容 | 参考方面 | 备注 |
|---|---|---|
| 硕士论文《基于OpenCV的机器人双目手眼标定系统的研究与实现》 | 相机标定、手眼标定、3D定位的整体框架 | |
| Cognex VisionPro 3D Developer’s Guide | 主体思路 | 标定流程是重点 P59 手眼标定注意点 P74 P75 |
| 手眼标定的两种方式 | 手眼标定公式推导 | |
| HALCON培训文档：三维定位方法 | 手眼标定思路 |
好消息！
小白学视觉知识星球
开始面向外开放啦
下载1：OpenCV-Contrib扩展模块中文版教程
在「小白学视觉」公众号后台回复：扩展模块中文教程，即可下载全网第一份OpenCV扩展模块教程中文版，涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。
下载2：Python视觉实战项目52讲
在「小白学视觉」公众号后台回复：Python视觉实战项目，即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目，助力快速学校计算机视觉。
下载3：OpenCV实战项目20讲
在「小白学视觉」公众号后台回复：OpenCV实战项目20讲，即可下载含有20个基于OpenCV实现20个实战项目，实现OpenCV学习进阶。
交流群
欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~