手眼标定（一）：Opencv4实现手眼标定及手眼系统测试

前言

由于项目需要，要在win10环境下实现“眼在手上”的手眼系统，为此查阅了不少资料。但大多是理论资料，或者不可用的代码。虽然本人基于Halcon 12.0实现了手眼标定，但代码太冗余，效率低。因此本人拟通过Opencv4实现手眼标定。
（第一次写博客，不足之处敬请批评指正！）

1 程序环境

1. 编译器：Visual Studio 2015；
2. Opencv版本：Opencv3.4.6、或Opencv4以上版本；

2 原理

（1）主要使用Opencv的calibrateHandEye()函数，其中包括五种方法，相关文献见链接link。
（2）本代码主要根据Tsai两步法进行，利用公式AX=XB，先计算旋转矩阵R，再计算平移矩阵T。
（3）Tsai两步法的原理可参考博主一“手眼标定”： link；博主二“机械臂的手眼标定 opencv实现”：link.

3 程序源码

主要参考github代码1： link；github代码2：link.
本人整理优化后的源码如下：

/***********************************************************************
说明：Opencv4实现手眼标定及手眼测试
作者：jian xu @CUG
日期：2020年01月08日
***********************************************************************/

#include <iostream>
#include <string>

#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>

//导入静态链接库
#if (defined _WIN32 || defined WINCE || defined __CYGWIN__)
#define OPENCV_VERSION "412"
#pragma comment(lib, "opencv_world" OPENCV_VERSION ".lib")

#endif

using namespace cv;
using namespace std;

//相机中13组标定板的位姿，x,y,z，rx,ry,rz,
Mat_<double> CalPose = (cv::Mat_<double>(13, 6) <<
	-0.072944147641399, -0.06687830562048944, 0.4340418493881254, -0.2207496117519063, 0.0256862005614321, 0.1926014162476009,
	-0.01969337858360518, -0.05095294728651902, 0.3671266719105768, 0.1552099329677287, -0.5763323472739464, 0.09956130526058841,
	0.1358164536530692, -0.1110802522656379, 0.4001396735998251, -0.04486168331242635, -0.004268942058870162, 0.05290073845562016,
	0.1360676260120161, -0.002373036366121294, 0.3951670952829301, -0.4359637938379769, 0.00807193982932386, 0.06162504121755787,
	-0.1047666520352697, -0.01377729010376614, 0.4570029374109721, -0.612072103513551, -0.04939465180949879, -0.1075464055169537,
	0.02866460103085085, -0.1043911269729344, 0.3879127305077527, 0.3137563103168434, -0.02113958397023016, 0.1311397970432597,
	0.1122741829392126, 0.001044006395747612, 0.3686697279333643, 0.1607160803445018, 0.2468677059920437, 0.1035103912091547,
	-0.06079521129779342, -0.02815190820828123, 0.4451740202390909, 0.1280935541917056, -0.2674407142401368, 0.1633865613363686,
	-0.02475533256363622, -0.06950841248698086, 0.2939836207787282, 0.1260629671933584, -0.2637748974005461, 0.1634102148863728,
	0.1128618887222624, 0.00117877722121125, 0.3362496409334229, 0.1049541359309871, -0.2754352318773509, 0.4251492928748009,
	0.1510545750008333, -0.0725019944548204, 0.3369908269102371, 0.2615745097093249, -0.1295598776133405, 0.6974394284203849,
	0.04885313290076512, -0.06488755216394324, 0.2441532410787161, 0.1998243391807502, -0.04919417529483511, -0.05133193756053007,
	0.08816140480523708, -0.05549965109057759, 0.3164905645998022, 0.164693654482863, 0.1153894876338608, 0.01455551646362294);

//机械臂末端13组位姿,x,y,z,rx,ry,rz
Mat_<double> ToolPose = (cv::Mat_<double>(13, 6) <<
	-0.3969707, -0.460018, 0.3899877, 90.2261, -168.2015, 89.7748,
	-0.1870185, -0.6207147, 0.2851157, 57.2636, -190.2034, 80.7958,
	-0.1569776, -0.510021, 0.3899923, 90.225, -178.2038, 81.7772,
	-0.1569787, -0.5100215, 0.3299975, 90.2252, -156.205, 81.7762,
	-0.3369613, -0.4100348, 0.3299969, 90.2264, -146.2071, 71.778,
	-0.2869552, -0.6100449, 0.4299998, 90.2271, -199.2048, 86.7806,
	-0.2869478, -0.6600489, 0.4299948, 105.2274, -189.2053, 86.7814,
	-0.286938, -0.6300559, 0.4299997, 75.2279, -189.2056, 86.783,
	-0.2869343, -0.5700635, 0.2800084, 75.2291, -189.2055, 86.7835,
	-0.1669241, -0.5700796, 0.280015, 75.2292