(二)面扫描仪与机械臂的手眼标定（眼在手外：九点标定）

1、配置opencv库与Eigen库
需用到opencv中矩阵运算
需用到Eigen中SVD分解算法
2、获取点
点相对于标定板坐标系的坐标
30 30 0
90 30 0
180 30 0
30 120 0
90 120 0
180 120 0
30 210 0
90 210 0
180 210 0
存为board.txt文件

点相对于相机坐标系的坐标
-24.6 115.6 869.1
-85.7 115.6 869.1
-171.3 119.7 869.1
-24.5 21.9 872.1
-85.9 21.9 872.1
-175.5 26.0 872.1
-28.6 -68.1 874.0
-90.0 -64.0 874.0
-176.0 -64.0 874.0
存为camera.txt文件
可以复制一份存为camera1.txt用于测试

获取点相对于机械臂基地坐标系的坐标
465.09 -109.00 70.64
405.09 -106.00 71.64
314.09 -101.00 72.64
473.09 -31.00 70.64
413.09 -30.00 71.64
321.09 -28.00 72.64
483.09 50.00 70.64
424.09 50.00 71.64
337.09 48.00 72.64
存为robot.txt

3、求解程序

#include <vector>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <math.h>
#include <map>
#include <fstream>
#include <sstream>
#include <Eigen/SVD>
#include <Eigen/Core>
#include<opencv2/core/eigen.hpp>


using namespace std;
using namespace cv;
using namespace Eigen;

void Solve_board2target(string target, Mat& T_board2camera, Mat& R_board2camera);
void test_camera2robot(cv::Mat H_camera2robot);

class coordinate {
public:
	double x;
	double y;
	double z;
};


int main()
{
	cv::Mat tempR, tempT;
	cv::Mat R_board2camera, T_board2camera;
	cv::Mat R_board2robot,  T_board2robot;
	cv::Mat H_board2camera = Mat::eye(4, 4, CV_64FC1);
	cv::Mat H_board2robot  = Mat::eye(4, 4, CV_64FC1);

	string target;

	//求板到机械臂末端的转换矩阵
	target = "robot";
	Solve_board2target(target, T_board2robot, R_board2robot);
	tempT = T_board2robot.clone();
	tempR = R_board2robot.clone();
	tempR({ 0,0,3,3 }).copyTo(H_board2robot({ 0,0,3,3 }));
	tempT({ 0,0,1,3 }).copyTo(H_board2robot({ 3,0,1,3 }));
	cout << "board2robot:" << endl;
	cout << H_board2robot << endl;
	
	cout << "______________________________________" << endl;

	//求板到相机的转换矩阵
	target = "camera";
	Solve_board2target(target, T_board2camera, R_board2camera);
	tempT = T_board2camera.clone();
	tempR = R_board2camera.clone();
	tempR({ 0,0,3,3 }).copyTo(H_board2camera({ 0,0,3,3 }));
	tempT({ 0,0,1,3 }).copyTo(H_board2camera({ 3,0,1,3 }));
	cout << "board2camera:" << endl;
	cout << H_board2camera << endl;
	
    //求逆，相机到相板的转换矩阵
	cv::Mat H_camera2board = H_board2camera.inv();
	cout << "H_camera2board:" << endl;
	cout << H_camera2board << endl;

	cout << "______________________________________" << endl;

	cv::Mat H_camera2robot = H_board2robot*H_camera2board;
	cout << "camera2robot:" << endl;
	cout << H_camera2robot << endl;

	cout << "______________________________________" << endl;

	test_camera2robot(H_camera2robot);
	
	system("pause");
	return 0;
}


//计算H_board2camera
//**************************************************************************
void Solve_board2target(string target, Mat& T_board2camera, Mat& R_board2camera)
{
	//计算A2B

	coordinate P_board, P_target, P_centroid;
	vector<coordinate> v_board, v_target;

	cv::Mat PA(3, 1, CV_64FC1);
	cv::Mat PB(3, 1, CV_64FC1);
	cv::Mat centroidA(3, 1, CV_64FC1);
	cv::Mat centroidB(3, 1, CV_64FC1);
	cv::Mat tempD, tempU, tempV;
	cv::Mat H     = Mat::zeros(3, 3, CV_64FC1);
	cv::Mat tempH = Mat::eye(4, 4, CV_64FC1);


	//读取点信息 并计算质心坐标
	//------------------------------------------
	P_centroid.x = 0;
	P_centroid.y = 0;
	P_centroid.z = 0;

	//读取板上点的坐标并计算质心
	ifstream fin;
	fin.open("..//board.txt", ios::in);
	while (!fin.eof())
	{
		fin >> P_board.x >> P_board.y >> P_board.z;
		///cout << P_board.x << "  " << P_board.y << "  " << P_board.z << endl;
		v_board.push_back(P_board); //存储
		P_centroid.x = P_centroid.x + P_board.x;
		P_centroid.y = P_centroid.y + P_board.y;
		P_centroid.z = P_centroid.z + P_board.z;
	}
	fin.close();

	//计算质心
	centroidA.at<double>(0.0) = P_centroid.x / v_board.size();
	centroidA.at<double>(1.0) = P_centroid.y / v_board.size();
	centroidA.at<double>(2.0) = P_centroid.z / v_board.size();
	///cout << "centroidA:" << endl;
	///cout << centroidA << endl;

	//读取相机中点的坐标
	P_centroid.x = 0;
	P_centroid.y = 0;
	P_centroid.z = 0;

	if (target == "camera")
	{
		fin.open("..//camera.txt", ios::in);
	}
	else if (target == "robot")
	{
		fin.open("..//robot.txt", ios::in);
	}
	else
	{
		cout << "targe输入有误!!" << endl; 
		return;
	}
	while (!fin.eof())
	{
		fin >> P_target.x >> P_target.y >> P_target.z;
		///cout << P_target.x << "  "  << P_target.y << "  " << P_target.z << endl;
		v_target.push_back(P_target); //存储
		P_centroid.x = P_centroid.x + P_target.x;
		P_centroid.y = P_centroid.y + P_target.y;
		P_centroid.z = P_centroid.z + P_target.z;
	}
	fin.close();

	//计算质心
	centroidB.at<double>(0.0) = P_centroid.x / v_target.size();
	centroidB.at<double>(1.0) = P_centroid.y / v_target.size();
	centroidB.at<double>(2.0) = P_centroid.z / v_target.size();

	//计算H矩阵
	//--------------------------------------------------------------
	for (int i = 0; i < v_board.size(); i++)
	{
		PA.at<double>(0, 0) = v_board[i].x;
		PA.at<double>(1, 0) = v_board[i].y;
		PA.at<double>(2, 0) = v_board[i].z;

		PB.at<double>(0, 0) = v_target[i].x;
		PB.at<double>(1, 0) = v_target[i].y;
		PB.at<double>(2, 0) = v_target[i].z;

		H = H + (PA - centroidA)*((PB - centroidB).t());
	}
	///cout << "H:" << endl;
	///cout << H << endl;
	Eigen::MatrixXd SVD_H(3, 3);
	cv2eigen(H, SVD_H);  //Mat矩阵转Eigen矩阵

	//SVD分解(H) 并计算旋转矩阵与平移矩阵
	//------------------------------------------------------------------
	///cout << "SVD_H:" << endl;
	///cout << SVD_H << endl;
	SVD::compute(H, tempD, tempU, tempV, 0);
	//-------------------------------------------------------------------
	// 进行svd分解
	Eigen::JacobiSVD<Eigen::MatrixXd> svd_holder(SVD_H,
		Eigen::ComputeThinU |
		Eigen::ComputeThinV);
	// 构建SVD分解结果
	Eigen::MatrixXd U = svd_holder.matrixU();
	Eigen::MatrixXd V = svd_holder.matrixV();
	Eigen::MatrixXd D = svd_holder.singularValues();
	//--------------------------------------------------------------------
	//eigen矩阵转Mat矩阵
	eigen2cv(U, tempU);
	eigen2cv(V, tempV);
	eigen2cv(D, tempD);

	//计算标定板到相机的旋转矩阵和平移矩阵
	R_board2camera = tempV * (tempU.t());
	T_board2camera = centroidB - (R_board2camera * centroidA);
	
}

//测试camera2robot
void test_camera2robot(cv::Mat H_camera2robot)
{
	coordinate P_camera;
	vector<coordinate> v_camera;

	cv::Mat PTemp  (4, 1, CV_64FC1);
	cv::Mat PCamera(4, 1, CV_64FC1);
	cv::Mat PRobot (4, 1, CV_64FC1);
	
	ifstream fin;
	fin.open("..//camera1.txt", ios::in);
	while (!fin.eof())
	{
		fin >> P_camera.x >> P_camera.y >> P_camera.z;
		cout << P_camera.x << "  " << P_camera.y << "  " << P_camera.z << endl;
		v_camera.push_back(P_camera); //存储
	}
	fin.close();
	double tempAA[1][3];

	for (int i = 0; i < v_camera.size(); i++)
	{
		cout << i << endl;
		PTemp.at<double>(0, 0) = v_camera[i].x;
		PTemp.at<double>(1, 0) = v_camera[i].y;
		PTemp.at<double>(2, 0) = v_camera[i].z;
		PTemp.at<double>(3, 0) = 1;

		PCamera = PTemp.clone();
		///cout << "PCamera:" << endl;
		///cout << PCamera << endl;

		///cout << "H_camera2robot" << endl;
		///cout << H_camera2robot << endl;
		//cout << H_camera2robot << endl;

		PRobot = H_camera2robot * PCamera;
		///cout << "PRobot:" << endl;
		///cout << PRobot << endl;

		tempAA[0][0] = PRobot.at<double>(0, 0);
		tempAA[0][1] = PRobot.at<double>(1, 0);
		tempAA[0][2] = PRobot.at<double>(2, 0);
		cout << tempAA[0][0] << "   " << tempAA[0][1] << "  " << tempAA[0][2] << endl;
	}
}