眼在手外标定

手眼标定之眼在手外
标定机器人末端工具坐标系，操作机器人触碰机器人工作空间内不共线的五个点，得到这五个点在机器人基座坐标系下的坐标。双目相机or深度相机对应读取这五个点在相机坐标系的坐标，解方程AX=B，得到手眼矩阵X。
解方程的方法可借助opencv：：solve()函数，
示例代码见下：

#include <opencv2/imgproc/imgproc.hpp> 
#include<opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"

using namespace cv;
using namespace std;


int main{

Mat A = (Mat_<double>(5, 4) <<
	InputOriPosition1[0], InputOriPosition1[1], InputOriPosition1[2], 1,
	InputOriPosition2[0], InputOriPosition2[1], InputOriPosition2[2], 1,
	InputOriPosition3[0], InputOriPosition3[1], InputOriPosition3[2], 1,
	InputOriPosition4[0], InputOriPosition4[1], InputOriPosition4[2], 1,
	InputOriPosition5[0], InputOriPosition5[1], InputOriPosition5[2], 1
	);
Mat B = (Mat_<double>(5, 3) <<
	OutputNewPosition1[0], OutputNewPosition1[1], OutputNewPosition1[2],
	OutputNewPosition2[0], OutputNewPosition2[1], OutputNewPosition2[2],
	OutputNewPosition3[0], OutputNewPosition3[1], OutputNewPosition3[2],
	OutputNewPosition4[0], OutputNewPosition4[1], OutputNewPosition4[2],
	OutputNewPosition5[0], OutputNewPosition5[1], OutputNewPosition5[2]
	);
Mat matrix = Mat(4, 3, CV_32F);
solve(A, B, matrix, DECOMP_SVD);
matrix = matrix.t();
Mat RT_h = (Mat_<double>(1, 4) << 0, 0, 0, 1);
vconcat(matrix, RT_h, matrix);//4*4
std::cout << "matrix: " << matrix << std::endl;

}