项目简述

实现功能

实习公司的项目，复现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

函数接口：

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是浮点型数组，为左相机内参所代表的33的矩阵 mtx33_r是浮点型数组，为右相机内参所代表的33的矩阵 mtx44_l是浮点型数组，为左相机的手眼矩阵所代表的44的矩阵 mtx44_r是浮点型数组，为右相机的手眼矩阵所代表的44的矩阵
void getMatsEih()	同上	同上
void calc_ImageToWorld()	_mtx44_cam3dToPhy3d_l是浮点型数组，为左相机的手眼矩阵所代表的44的矩阵 _mtx33_camIntrin_l是浮点型数组，为左相机内参所代表的33的矩阵 _mtx44_cam3dToPhy3d_r是浮点型数组，为右相机的手眼矩阵所代表的44的矩阵 _mtx33_camIntrin_r是浮点型数组，为右相机内参所代表的33的矩阵 _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

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的关系固定

结合上图，有公式：

$^{cam}H_{cal} \ = \ ^{cam}H_{base} \ ^{base}H_{tool} \ ^{tool}H_{cal}$

标定时控制机械臂从位置1移动到位置2：
坐标系变换关系为：Hand3D → RobotBase3D → Camera3D → CalPlate3D
可得位置1的公式：

$base = baseHtool(1) * tool(1) \\ cam = camHbase *base \\ obj(1) = inv(camHcal(1)) * cam$

合并，可得：

$obj(1) = inv(camHcal(1)) * camHbase * baseHtool(1) * tool(1)$

移动到位置2后：

$obj(2) = inv(camHcal(2)) * camHbase * baseHtool(2) * tool(2)$

由于cal和tool的相对位置是不变的，可通过此联立方程：

$inv(camHcal(1)) * camHbase * baseHtool(1) = inv(camHcal(2)) * camHbase * baseHtool(2)$

化为CX=XD的形式，可得：

$camHcal(2) * inv(camHcal(1)) * camHbase = camHbase * baseHtool(2) * inv(baseHtool(1))$

其中，camHbase是待求解的，camHcal可通过相机外参获得，baseHtool可通过示教器读取

眼在手上

相机移动：CalPlate3D与RobotBase3D的关系固定
相机固定在机械臂上：Camera3D与Hand3D的关系固定

结合上图，有公式：

$^{cam}H_{cal} \ = \ ^{cam}H_{tool} \ ({^{base}H_{tool}})^{-1} \ ^{base}H_{cal}$

标定时控制机械臂从位置1移动到位置2：
坐标系变换关系为：CalPlate3D → Camera3D → Hand3D → RobotBase3D
可得位置1的公式：

$cam(1) = camHcal(1) * obj \\ tool(1) = inv(camHtool) * cam(1) \\ base = baseHtool(1) * tool(1)$

合并，可得：

$base = baseHtool(1) * inv(camHtool) * camHcal(1) * obj$

移动到位置2后：

$base = baseHtool(2) * inv(camHtool) * camHcal(2) * obj$

由于base和obj的相对位置是不变的，可通过此联立方程：

$baseHtool(1) * inv(camHtool) * camHcal(1) = baseHtool(2) * inv(camHtool) * camHcal(2)$

化为CX=XD的形式，可得：

$camHcal(2) * inv(camHcal(1)) * camHtool = camHtool * inv(baseHtool(2)) * baseHtool(1)$

其中，camHtool是待求解的，camHcal可通过相机外参获得，baseHtool可通过示教器读取

计算偏移

本质上是一个NPointToNPoint的问题
输入：一组点在A坐标系下的坐标、这组点在B坐标系下的坐标
输出：这组点从A坐标系变换到B坐标系的转换关系

一般做法是将其看作优化问题，将伪逆作为初始值，只考虑仿射变换中的旋转、平移

具体细节见我的另一篇博客，[机器视觉]欧式空间中的二维点变换关系

参考

参考内容	参考方面	备注
硕士论文《基于OpenCV的机器人双目手眼标定系统的研究与实现》	相机标定、手眼标定、3D定位的整体框架
Cognex VisionPro 3D Developer’s Guide	主体思路	标定流程是重点 P59 手眼标定注意点 P74 P75
手眼标定的两种方式	手眼标定公式推导
HALCON培训文档：三维定位方法	手眼标定思路

项目简述

实现功能

实施路线

复现的VisionPro 3D函数

封装成dll的函数接口

原理概述

6个坐标系

总体步骤

坐标系转换

方程求解

3D视觉基础知识

位姿

标定板

相机标定

三角测量（Triangulation）

姿态估计

手眼标定

3D严格变换（3D Rigid Transforms）

标定实施

输入输出

眼在手外/眼在手上

推荐单相机标定实施

多相机标定

推荐多相机标定实施

轴线

倾斜、旋转

变高

3D定位

3D视角项目架构

寻找特征

稳定的2D特征（能承受3D的不确定）

应当避免的2D特征

检查2D特征的准确性：5个姿态

相机标定

手眼标定

本质

眼在手外/眼在手上

眼在手外

眼在手上

计算偏移

参考