实现功能

实习公司的项目，复现Cognex VisionPro 3D的大部分内容，涵盖眼在手外、眼在手上，包括相机标定、手眼标定、3D定位计算位移偏差。最后的位移偏差与Cognex的结果在1mm左右。

实施路线

用python实现原型验证算法，再移植成C++编译为dll，供C#调用。python的库主要用到：cv2、numpyC++的库主要用到：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姿态

函数功能主体类型返回值类型相机标定Cog3DCameraCalibratorCog3DCameraCalibrationResult手眼标定Cog3DHandEyeCalibratorList
<Cog3DHandEyeCalibrationResult>三角测量获得3D点坐标Cog3DTriangulatorCog3DVect3Collection通过2D点估计3D姿态List
<Cog3DPoseEstimatorUsing2DPoints>Cog3DPoseEstimatorUsing2DPointsResult

封装成dll的函数接口

函数功能函数名所属dll眼在手外的相机标定、手眼标定void getMatsEth()CalibrateCpp.dll眼在手上的相机标定、手眼标定void getMatsEih()CalibrateCpp.dll3D定位：左右相机的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.dllpointNum是整型值，为一组需计算偏移的点的个数
_Point_Model_3D是浮点型数组，为这组点在偏移前坐标系下的坐标值
_Point_Now_3D是浮点型数组，为这组点在偏移后坐标系下的坐标值_res_excursion是长度为6的浮点型一维数组，分别是x、y、z、rx、ry、rz

原理概述

6个坐标系

坐标系含义备注Raw2D图像坐标系（像素单位）

Camera2D图像坐标系（物理单位）无畸变：去除光学畸变和像素比例的影响Camera3D相机坐标系z轴即是相机的光轴，Z=1即为Camera2DCalPlate3D/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培训文档：三维定位方法手眼标定思路

来源：大专栏

End

声明：部分内容来源于网络，仅供读者学习、交流之目的。文章版权归原作者所有。如有不妥，请联系删除。