双目视觉在机械臂抓取中的应用（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 <Cog3DHandEyeCalibrationResult>
三角测量获得3D点坐标	Cog3DTriangulator	Cog3DVect3Collection
通过2D点估计3D姿态	List <Cog3DPoseEstimatorUsing2DPoints>	Cog3DPoseEstimatorUsing2DPointsResult

封装成dll的函数接口

函数功能	函数名	所属dll
眼在手外的相机标定、手眼标定	void getMatsEth()	CalibrateCpp.dll
眼在手上的相机标定、手眼标定	void getMatsEih()	CalibrateCpp.dll
3D定位：左右相机的2D点转换为3D点	void calc_ImageToWorld()	CalcImageToWorldCpp.dll
计算偏移	void calc_Excursion()	CalcExcursionCpp.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)

函数名	输入	输出
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	基坐标系

总体步骤

1. 相机标定

2. 手眼标定

3. 计算偏移

坐标系转换

1. 左右相机通过特征提取，得到Raw2D下的特征点坐标

2. 通过相机内参，将特征点转到Camera2D下

3. 把未知高度作为未知数，将特征点转到Camera3D下

4. 通过手眼矩阵，将特征点转到Hand3D下

5. 通过示教器读取的机械手位姿，将特征点转到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张倾斜旋转

多相机标定

注意点：

1. 固定两个相机间的相互位姿

2. 固定所有相机的光学参数

3. 多个相机需同时采集图像

本质上：每个相机有独立的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“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~