//?HandEyeCalibrationTest.cpp?:?此文件包含?“main“?函數。程序執行將在此處開始并結束。
//
#include?
#include?
#include?

//#pragma?comment（?lib?“opencv_world430.lib“?）

using?namespace?std;
using?namespace?cv;

Mat?R_T2HomogeneousMatrix（const?Mat&?R?const?Mat&?T）;
void?HomogeneousMtr2RT（Mat&?HomoMtr?Mat&?R?Mat&?T）;
bool?isRotatedMatrix（Mat&?R）;
Mat?eulerAngleToRotateMatrix（const?Mat&?eulerAngle?const?std::string&?seq）;
Mat?quaternionToRotatedMatrix（const?Vec4d&?q）;
Mat?attitudeVectorToMatrix（const?Mat&?m?bool?useQuaternion?const?string&?seq）;
//數據使用的已有的值
//相機中13組標定板的位姿，xyz，rxryrz
Mat_?CalPose?=?（cv::Mat_（9?6）?<<
	-2.602917e+01?-2.058909e+01?2.001158e+02?-2.222308e+00?-2.200891e+00?1.038314e-02
	-2.109788e+01?-1.966766e+01?1.820841e+02?2.145852e+00?2.143222e+00?3.322979e-01
	-2.571942e+01?-1.872411e+01?2.013287e+02?-2.108920e+00?-2.134951e+00?1.931600e-01
	-2.488133e+01?-1.828509e+01?1.956522e+02?-2.047373e+00?-2.027428e+00?4.624128e-02
	-2.920918e+01?-2.170171e+01?2.143397e+02?2.174664e+00?2.123216e+00?-1.765171e-01
	-2.518449e+01?-1.630078e+01?2.110271e+02?2.229811e+00?2.162015e+00?-2.832562e-01
	-2.729219e+01?-1.855265e+01?1.884390e+02?-2.130600e+00?-2.041023e+00?2.669174e-02
	-2.754954e+01?-1.465493e+01?1.886429e+02?-2.204029e+00?-2.089585e+00?-1.583544e-01
	-2.716471e+01?-1.718814e+01?1.924382e+02?-2.287743e+00?-2.088326e+00?-2.886300e-01）;

//機械臂末端13組位姿xyzrxryrz
Mat_?ToolPose?=?（cv::Mat_（9?6）?<<
	1437.754?385.919?40.011?-8.792?18.557?-17.376
	1437.714?387.617?34.178?-23.661?16.950?-21.540
	1440.563?382.431?36.997?0.829?22.108?-12.817
	1440.555?385.317?45.598?-1.599?30.965?-14.361
	1433.627?385.860?37.482?-4.907?10.576?-16.978
	1439.363?390.483?34.553?0.388?13.051?-15.847
	1438.171?387.592?36.215?-4.281?27.707?-17.485
	1442.171?388.187?41.308?-13.644?25.154?-21.693
	1439.222?392.200?48.119?-19.865?22.540?-25.981）;

int?main（int?argc?char**?argv）
{
	//數據聲明
	vector?R_gripper2base;
	vector?T_gripper2base;
	vector?R_target2cam;
	vector?T_target2cam;
	Mat?R_cam2gripper?=?Mat（3?3?CV_64FC1）;				//相機與機械臂末端坐標系的旋轉矩陣與平移矩陣
	Mat?T_cam2gripper?=?Mat（3?1?CV_64FC1）;
	Mat?Homo_cam2gripper?=?Mat（4?4?CV_64FC1）;

	vector?Homo_target2cam;
	vector?Homo_gripper2base;
	Mat?tempR?tempT?temp;

	for?（int?i?=?0;?i?	{
		temp?=?attitudeVectorToMatrix（CalPose.row（i）?false?““）;	//注意seq為空，相機與標定板間的為旋轉向量
		Homo_target2cam.push_back（temp）;
		HomogeneousMtr2RT（temp?tempR?tempT）;
		/*cout?<		cout?<		cout?<		R_target2cam.push_back（tempR）;
		T_target2cam.push_back（tempT）;
	}
	for?（int?j?=?0;?j?	{
		temp?=?atti