實(shí)現(xiàn)功能

實(shí)習(xí)公司的項(xiàng)目，復(fù)現(xiàn)Cognex VisionPro 3D的大部分內(nèi)容，涵蓋眼在手外、眼在手上，包括相機(jī)標(biāo)定、手眼標(biāo)定、3D定位計(jì)算位移偏差。最后的位移偏差與Cognex的結(jié)果在1mm左右。

實(shí)施路線

用python實(shí)現(xiàn)原型驗(yàn)證算法，再移植成C++編譯為dll，供C#調(diào)用。
python的庫主要用到：cv2、numpy
C++的庫主要用到：OpenCV、Eigen

復(fù)現(xiàn)的VisionPro 3D函數(shù)

函數(shù)語句	函數(shù)功能
camCalRes = camCalibrator.Execute(pelRects, extractedFeatures, calHeights, calPoseTypes);	相機(jī)標(biāo)定
heCalibs = heCalibrator.Execute(pelRects, intrinsics, extractedFeatures, robotPositions);	手眼標(biāo)定
modelPoints = triangulator.Execute(cameraCalibs, pointsRaw2D, isPointValid, out is3DPointValid, out resRaw2D, out resPhys3D);	三角測量獲得3D點(diǎn)坐標(biāo)
Cog3DPoseEstimatorUsing2DPointsResult res = pParam.Models[model].Execute(camCalibs, features2D, weights2D);	通過2D點(diǎn)估計(jì)3D姿態(tài)

函數(shù)功能	主體類型	返回值類型
相機(jī)標(biāo)定	Cog3DCameraCalibrator	Cog3DCameraCalibrationResult
手眼標(biāo)定	Cog3DHandEyeCalibrator	List <Cog3DHandEyeCalibrationResult>
三角測量獲得3D點(diǎn)坐標(biāo)	Cog3DTriangulator	Cog3DVect3Collection
通過2D點(diǎn)估計(jì)3D姿態(tài)	List <Cog3DPoseEstimatorUsing2DPoints>	Cog3DPoseEstimatorUsing2DPointsResult

封裝成dll的函數(shù)接口

函數(shù)功能	函數(shù)名	所屬dll
眼在手外的相機(jī)標(biāo)定、手眼標(biāo)定	void getMatsEth()	CalibrateCpp.dll
眼在手上的相機(jī)標(biāo)定、手眼標(biāo)定	void getMatsEih()	CalibrateCpp.dll
3D定位：左右相機(jī)的2D點(diǎn)轉(zhuǎn)換為3D點(diǎn)	void calc_ImageToWorld()	CalcImageToWorldCpp.dll
計(jì)算偏移	void calc_Excursion()	CalcExcursionCpp.dll

函數(shù)接口：

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)

函數(shù)名	輸入	輸出
void getMatsEth()	other_info是長度為3的整型一維數(shù)組，3個(gè)元素分別代表標(biāo)定圖片數(shù)量、標(biāo)定圖片寬、標(biāo)定圖片高 point3d_str是字符串?dāng)?shù)組，為標(biāo)定板角點(diǎn)在CalPlate3D下的坐標(biāo) point2d_str是字符串?dāng)?shù)組，為標(biāo)定板角點(diǎn)在Raw2D下的坐標(biāo) robot_str是字符串?dāng)?shù)組，為機(jī)械手位姿所代表的4*4的矩陣	mtx33_l是浮點(diǎn)型數(shù)組，為左相機(jī)內(nèi)參所代表的3*3的矩陣 mtx33_r是浮點(diǎn)型數(shù)組，為右相機(jī)內(nèi)參所代表的3*3的矩陣 mtx44_l是浮點(diǎn)型數(shù)組，為左相機(jī)的手眼矩陣所代表的4*4的矩陣 mtx44_r是浮點(diǎn)型數(shù)組，為右相機(jī)的手眼矩陣所代表的4*4的矩陣
void getMatsEih()	同上	同上
void calc_ImageToWorld()	_mtx44_cam3dToPhy3d_l是浮點(diǎn)型數(shù)組，為左相機(jī)的手眼矩陣所代表的4*4的矩陣 _mtx33_camIntrin_l是浮點(diǎn)型數(shù)組，為左相機(jī)內(nèi)參所代表的3*3的矩陣 _mtx44_cam3dToPhy3d_r是浮點(diǎn)型數(shù)組，為右相機(jī)的手眼矩陣所代表的4*4的矩陣 _mtx33_camIntrin_r是浮點(diǎn)型數(shù)組，為右相機(jī)內(nèi)參所代表的3*3的矩陣 _Point_Cl是長度為3的齊次形式的浮點(diǎn)型一位數(shù)組，為左相機(jī)的特征點(diǎn)在在Raw2D下的坐標(biāo) _Point_Cr是長度為3的齊次形式的浮點(diǎn)型一位數(shù)組，為右相機(jī)的特征點(diǎn)在在Raw2D下的坐標(biāo)	params是長度為3的非齊次形式的浮點(diǎn)型一位數(shù)組，為特征點(diǎn)在在RobotBase3D下的坐標(biāo)
CalcExcursionCpp.dll	pointNum是整型值，為一組需計(jì)算偏移的點(diǎn)的個(gè)數(shù) _Point_Model_3D是浮點(diǎn)型數(shù)組，為這組點(diǎn)在偏移前坐標(biāo)系下的坐標(biāo)值 _Point_Now_3D是浮點(diǎn)型數(shù)組，為這組點(diǎn)在偏移后坐標(biāo)系下的坐標(biāo)值	_res_excursion是長度為6的浮點(diǎn)型一維數(shù)組，分別是x、y、z、rx、ry、rz

原理概述

6個(gè)坐標(biāo)系

坐標(biāo)系	含義	備注
Raw2D	圖像坐標(biāo)系（像素單位）
Camera2D	圖像坐標(biāo)系（物理單位）	無畸變：去除光學(xué)畸變和像素比例的影響
Camera3D	相機(jī)坐標(biāo)系	z軸即是相機(jī)的光軸，Z=1即為Camera2D
CalPlate3D/Phys3D	標(biāo)定板坐標(biāo)系
Hand3D/Tool	機(jī)械手坐標(biāo)系/工具坐標(biāo)系
RobotBase3D	基坐標(biāo)系

總體步驟

1. 相機(jī)標(biāo)定

2. 手眼標(biāo)定

3. 計(jì)算偏移

坐標(biāo)系轉(zhuǎn)換

1. 左右相機(jī)通過特征提取，得到Raw2D下的特征點(diǎn)坐標(biāo)

2. 通過相機(jī)內(nèi)參，將特征點(diǎn)轉(zhuǎn)到Camera2D下

3. 把未知高度作為未知數(shù)，將特征點(diǎn)轉(zhuǎn)到Camera3D下

4. 通過手眼矩陣，將特征點(diǎn)轉(zhuǎn)到Hand3D下

5. 通過示教器讀取的機(jī)械手位姿，將特征點(diǎn)轉(zhuǎn)到RobotBase3D下

   
   

方程求解

最終共有6個(gè)方程，即左右相機(jī)各3個(gè)方程（x、y、z）
但6個(gè)方程中只有5個(gè)未知數(shù)，即RobotBase3D下的X、Y、Z、Camera3D下的左右相機(jī)的z
一般做法，用優(yōu)化方法解超定方程即可

3D視覺基礎(chǔ)知識

位姿

剛體在坐標(biāo)系中用位姿描述
位姿 = 位置（position） + 姿態(tài)（orientation）
一般地，位置和姿態(tài)各用3個(gè)值表示，位置用x、y、z表示偏移，姿態(tài)用rx、ry、rz表示歐拉角旋轉(zhuǎn)

標(biāo)定板

工業(yè)常用標(biāo)定板分兩大類：實(shí)心圓陣列（Halcon）、棋盤格（VisionPro、OpenCV、Matlab）
非精確制造的標(biāo)定板會導(dǎo)致不好的標(biāo)定結(jié)果，比如激光/噴墨打印機(jī)打印的標(biāo)定板

康耐視的棋盤格（Cognex checkerboard）包含標(biāo)準(zhǔn)的基準(zhǔn)標(biāo)識，能夠做到不必在一張圖片內(nèi)拍攝整張棋盤格

相機(jī)標(biāo)定

3D標(biāo)定：在與圖像像素相關(guān)的2D坐標(biāo)系和與物理世界相關(guān)的3D坐標(biāo)系間建立數(shù)學(xué)聯(lián)系
最初定義的物理世界坐標(biāo)系是標(biāo)定板坐標(biāo)系。

3D標(biāo)定后的相機(jī)可以實(shí)現(xiàn)：
①Raw2D中的2D point ? Phys3D中的3D ray
②Raw2D中的2D point + Phys3D中的3D plane ? Phys3D中的3D point
③Phys3D中的3D point ? Raw2D中的2D point

三角測量（Triangulation）

單個(gè)標(biāo)定相機(jī)：Raw2D中的2D point ? Phys3D中的3D ray
多個(gè)標(biāo)定相機(jī)：Raw2D中的2D point ? Phys3D中的3D point
（從不同位置觀察同一物體，可在Phys3D中生成與物體同一特征相關(guān)的多個(gè)交叉直線，從而算出3D位姿）

注意：2D特征必須準(zhǔn)確且可靠（不會被3D影響，如透視收縮）

姿態(tài)估計(jì)

估計(jì)3D物體姿態(tài)至少需要3個(gè)不共線的點(diǎn)

手眼標(biāo)定

輸入：標(biāo)定板圖像、Hand3D位姿（示教器提供）

眼在手外：Camera3D中的3D point ? RobotBase3D中的3D point
眼在手上：Hand3D中的3D point ? Camera3D中的3D point

最終實(shí)現(xiàn)，Raw2D中的2D point ? RobotBase3D中的3D point

3D嚴(yán)格變換（3D Rigid Transforms）

實(shí)現(xiàn)兩個(gè)3D笛卡爾坐標(biāo)系之間的映射。
只包含旋轉(zhuǎn)和平移，不包含比例、反射、錯切。

維基百科講解：旋轉(zhuǎn)矩陣、歐拉角、四元數(shù)

標(biāo)定實(shí)施

輸入輸出

在不同位姿下，用固定光學(xué)和機(jī)械參數(shù)的相機(jī)采集一組標(biāo)定板的圖像
輸入：標(biāo)定板圖像、標(biāo)定板的網(wǎng)格尺寸、標(biāo)定板位姿
輸出：內(nèi)參、外參

內(nèi)外參	變換關(guān)系	含義
內(nèi)參（intrinsic）	Raw2DfromCamera2D	去除光學(xué)畸變和像素比例的影響的非線性變換
外參（extrinsic）	Camera3DfromPhys3D	兩個(gè)坐標(biāo)系之間的6個(gè)自由度的線性變換

眼在手外/眼在手上

眼在手外：相機(jī)固定，相機(jī)拍被機(jī)械臂帶著移動n個(gè)姿態(tài)的棋盤格
眼在手上：相機(jī)移動，機(jī)械臂帶著移動n個(gè)姿態(tài)的相機(jī)拍固定的棋盤格

推薦單相機(jī)標(biāo)定實(shí)施

圖像數(shù)	實(shí)施
最少4張	以圖像平面傾斜20°~30° 以相機(jī)光軸旋轉(zhuǎn)90°
推薦9張	上述4張+互相平行高度不同
工業(yè)9張	1張基準(zhǔn)+8張傾斜旋轉(zhuǎn)

多相機(jī)標(biāo)定

注意點(diǎn)：

1. 固定兩個(gè)相機(jī)間的相互位姿

2. 固定所有相機(jī)的光學(xué)參數(shù)

3. 多個(gè)相機(jī)需同時(shí)采集圖像

本質(zhì)上：每個(gè)相機(jī)有獨(dú)立的Raw2D、Camera2D、Camera3D，有同一個(gè)Phys3D

推薦多相機(jī)標(biāo)定實(shí)施

軸線

傾斜、旋轉(zhuǎn)：軸線應(yīng)當(dāng)大體在相機(jī)光心的中心，且大體在工作空間的中心
變高：使用傾斜視角的軸線，并使標(biāo)定板垂直于軸線

傾斜、旋轉(zhuǎn)

標(biāo)定板：傾斜20°、旋轉(zhuǎn)90°

變高

標(biāo)定板：彼此之間平行，改變高度

3D定位

3D視角項(xiàng)目架構(gòu)

先粗定位，再精定位

尋找特征

關(guān)鍵：2D特征和3D特征的特征對（feature correspondence）

穩(wěn)定的2D特征（能承受3D的不確定）

1.尖銳、平直的邊緣
2.圓
3.具有旋轉(zhuǎn)、縮放、平移不變性的獨(dú)特特征

英文原文：
? Sharp, straight edges
? Circles
? Unique features that can be found regardless of rotation, scale or translation changes

應(yīng)當(dāng)避免的2D特征

1.圓角
2.倒圓邊
3.部件其余部分的特出特征

英文原文：
? Round corners
? Round edges
? Features that extrude significantly towards the camera from the rest of the part

檢查2D特征的準(zhǔn)確性：5個(gè)姿態(tài)

1.原始姿態(tài)
2.視野左上角，z軸旋轉(zhuǎn)20°
3.視野右上角，z軸旋轉(zhuǎn)45°
4.視野左下角，z軸旋轉(zhuǎn)67°
5.視野右下角，z軸旋轉(zhuǎn)90°

相機(jī)標(biāo)定

輸入：correspondence pairs，即圖像特征與其物理位置
輸出：相機(jī)內(nèi)參、相機(jī)外參、畸變系數(shù)

一般相機(jī)標(biāo)定均采用張正友的標(biāo)定方法
論文傳送：Zhang z．A flexible new technique for canlera calibration[J]．IEEE transactions on pattern analysis and machine intelligence，200．22(11)：1330—1334．

論文解析與實(shí)現(xiàn)見我的另一篇博客，[機(jī)器視覺]詳解相機(jī)標(biāo)定

手眼標(biāo)定

本質(zhì)

手眼標(biāo)定的本質(zhì)：求解CX=XD，C與相機(jī)相關(guān)，D與機(jī)械臂相關(guān)
輸入：C、D
輸出：X

眼在手外/眼在手上

工業(yè)應(yīng)用中，手（機(jī)械臂）和眼（相機(jī)）有兩種位置關(guān)系：

分類	相機(jī)關(guān)系	所求關(guān)系	詳細(xì)描述
眼在手外	相機(jī)固定	Camera3D與RobotBase3D的關(guān)系	相機(jī)（眼）和機(jī)械臂（手）分離，眼的位置相對于手是固定的
眼在手上	相機(jī)移動	Camera3D與Hand3D的關(guān)系	把相機(jī)（眼）固定在機(jī)械臂（手）上面，眼隨手移動

眼在手外

相機(jī)固定：Camera3D與RobotBase3D的關(guān)系固定
標(biāo)定板固定在機(jī)械臂上：CalPlate3D與Hand3D的關(guān)系固定

結(jié)合上圖，有公式：

標(biāo)定時(shí)控制機(jī)械臂從位置1移動到位置2：
坐標(biāo)系變換關(guān)系為：Hand3D → RobotBase3D → Camera3D → CalPlate3D
可得位置1的公式：

合并，可得：

移動到位置2后：

由于cal和tool的相對位置是不變的，可通過此聯(lián)立方程：

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

其中，camHbase是待求解的，camHcal可通過相機(jī)外參獲得，baseHtool可通過示教器讀取

眼在手上

相機(jī)移動：CalPlate3D與RobotBase3D的關(guān)系固定
相機(jī)固定在機(jī)械臂上：Camera3D與Hand3D的關(guān)系固定

結(jié)合上圖，有公式：

標(biāo)定時(shí)控制機(jī)械臂從位置1移動到位置2：
坐標(biāo)系變換關(guān)系為：CalPlate3D → Camera3D → Hand3D → RobotBase3D
可得位置1的公式：

合并，可得：

移動到位置2后：

由于base和obj的相對位置是不變的，可通過此聯(lián)立方程：

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

其中，camHtool是待求解的，camHcal可通過相機(jī)外參獲得，baseHtool可通過示教器讀取

計(jì)算偏移

本質(zhì)上是一個(gè)NPointToNPoint的問題
輸入：一組點(diǎn)在A坐標(biāo)系下的坐標(biāo)、這組點(diǎn)在B坐標(biāo)系下的坐標(biāo)
輸出：這組點(diǎn)從A坐標(biāo)系變換到B坐標(biāo)系的轉(zhuǎn)換關(guān)系

一般做法是將其看作優(yōu)化問題，將偽逆作為初始值，只考慮仿射變換中的旋轉(zhuǎn)、平移

具體細(xì)節(jié)見我的另一篇博客，[機(jī)器視覺]歐式空間中的二維點(diǎn)變換關(guān)系

參考

參考內(nèi)容	參考方面	備注
碩士論文《基于OpenCV的機(jī)器人雙目手眼標(biāo)定系統(tǒng)的研究與實(shí)現(xiàn)》	相機(jī)標(biāo)定、手眼標(biāo)定、3D定位的整體框架
Cognex VisionPro 3D Developer’s Guide	主體思路	標(biāo)定流程是重點(diǎn) P59 手眼標(biāo)定注意點(diǎn) P74 P75
手眼標(biāo)定的兩種方式	手眼標(biāo)定公式推導(dǎo)
HALCON培訓(xùn)文檔：三維定位方法	手眼標(biāo)定思路

 End 

聲明：部分內(nèi)容來源于網(wǎng)絡(luò)，僅供讀者學(xué)習(xí)、交流之目的。文章版權(quán)歸原作者所有。如有不妥，請聯(lián)系刪除。