(一)面扫描仪与机械臂的手眼标定(眼在手上)

1、配置opencv库与Eigen库
需用到opencv中求解手眼标定的算法
需用到Eigen中SVD分解算法
2、获取点
点相对于标定板坐标系的坐标
30 30 0
30 60 0
30 150 0
60 30 0
60 150 0
150 30 0
150 60 0
150 150 0
存为board1.txt文件

点相对于相机坐标系的坐标
63.5676 355.2545 81.8691
32.552 356.6499 79.4237
-56.0291 359.4486 74.1348
66.4339 356.5228 50.7242
-53.22 360.1268 42.9512
71.6217 356.7818 -37.7328
40.2484 357.923 -40.4321
-48.236 360.6457 -45.6185
存为camera1.txt文件

改变机械臂位姿,再获取点相对于相机坐标系的坐标
存为camera2.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;



//机械臂末端位姿,x,y,z,rx,ry,rz
Mat_<double> ToolPose = (cv::Mat_<double>(7, 6) <<
	350, 80, 480, 0, 178, -86,
	355, 55, 500, 0, 184, -94,
	365, 65, 490, 0, 179, -82,
	385, 90, 465, 0, 183, -96,
	385, 70, 485, 0, 181, -94,
	370, 80, 470, 0, 178, -80,
	365, 85, 480, 0, 182, -82);
	



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

//求解标定板到相机的旋转矩阵和平移矩阵
void Solve_board2camera(int num, Mat& T_board2camera, Mat& R_board2camera);
//求解机械臂末端到基底的旋转矩阵和平移矩阵
void Solve_gripper2base(const Mat& m, Mat& T_gripper2base, Mat& R_gripper2base);
//求解欧拉角转旋转矩阵
Mat eulerAngleToRotateMatrix(const Mat& eulerAngle);
//测试
void testResult(Mat& R_camera2gripper, Mat& T_camera2gripper, Mat& tempH_T);

vector<Mat> v_H_board2camera, v_H_camera2gripper, v_H_gripper2base;
vector<Mat> v_Pc; //存储相机中点坐标


int main()
{
	cv::Mat T_gripper2base;
	cv::Mat R_gripper2base;
	cv::Mat H_gripper2base = Mat::eye(4, 4, CV_64FC1);
	cv::Mat T_board2camera;
	cv::Mat R_board2camera;
	cv::Mat H_board2camera = Mat::eye(4, 4, CV_64FC1);
	cv::Mat T_camera2gripper;
	cv::Mat R_camera2gripper;
	cv::Mat H_camera2gripper = Mat::eye(4, 4, CV_64FC1);

	cv::Mat tempR, tempT, tempH;

	vector<Mat> v_T_gripper2base;
	vector<Mat> v_R_gripper2base;
	vector<Mat> v_T_board2camera;
	vector<Mat> v_R_board2camera;


	//计算board2camera
	//计算板到相机的转换关系
	int num = 0;
	for (int i = 0; i < ToolPose.rows; i++)
	{
		num = i + 1;//用于打开文件

		//求解标定板到相机的旋转矩阵和平移矩阵
		Solve_board2camera(num, T_board2camera, R_board2camera);
		tempT = T_board2camera.clone();
		tempR = R_board2camera.clone();

		//存储标定板到相机的旋转矩阵和平移矩阵
		v_T_board2camera.push_back(tempT);
		v_R_board2camera.push_back(tempR);

		//存储转换矩阵
		tempR({ 0,0,3,3 }).copyTo(H_board2camera({ 0,0,3,3 }));
		tempT({ 0,0,1,3 }).copyTo(H_board2camera({ 3,0,1,3 }));
		tempH = H_board2camera;
		v_H_board2camera.push_back(tempH);
	}
	

	cout << "**************************************************************" << endl;
	//计算gripper2base
	//计算机械臂末端到基底的转换关系
	for (int i = 0; i < ToolPose.rows; i++)				
	{
		    //求解机械臂末端到基底的旋转矩阵和平移矩阵
			Solve_gripper2base(ToolPose.row(i), T_gripper2base, R_gripper2base);
			tempT = T_gripper2base.clone();
			tempR = R_gripper2base.clone();

			//存储机械臂末端到基底的旋转矩阵和平移矩阵
			v_T_gripper2base.push_back(tempT);
			v_R_gripper2base.push_back(tempR);

			//存储转换矩阵
			tempR({ 0,0,3,3 }).copyTo(H_gripper2base({ 0,0,3,3 }));
			tempT({ 0,0,1,3 }).copyTo(H_gripper2base({ 3,0,1,3 }));
			tempH = H_gripper2base.clone();
			v_H_gripper2base.push_back(tempH);
	}

	//将旋转矩阵转成选装向量,再将旋转向量转成旋转矩阵,看转化前后是否一样
	//Mat temp_R1;
	//Mat temp_R2;
	//Mat temp;
	//for (int i = 0; i < v_R_board2camera.size(); i++)
	//{
	//	cout << "_________________" << i+1 << "_________________" << endl;
	//	temp_R1 = v_R_board2camera[i];
	//	cout << "temp_R1: " << temp_R1 << endl;
	//	Rodrigues(temp_R1, temp);
	//	cout << "temp: " << temp << endl;
	//	Rodrigues(temp, temp_R2);
	//	cout << "temp_R2: " << temp_R2 << endl;

	//}
	//system("pause");

	
	//求解相机
	//opencv提供了五种方法: 
	cout << "***************************求解***************************" << endl;
	//cv::calibrateHandEye(v_R_gripper2base, v_T_gripper2base, v_R_board2camera, v_T_board2camera, R_camera2gripper, T_camera2gripper, CALIB_HAND_EYE_TSAI);
	//cv::calibrateHandEye(v_R_gripper2base, v_T_gripper2base, v_R_board2camera, v_T_board2camera, R_camera2gripper, T_camera2gripper, CALIB_HAND_EYE_PARK);
	cv::calibrateHandEye(v_R_gripper2base, v_T_gripper2base, v_R_board2camera, v_T_board2camera, R_camera2gripper, T_camera2gripper, CALIB_HAND_EYE_HORAUD);
	//cv::calibrateHandEye(v_R_gripper2base, v_T_gripper2base, v_R_board2camera, v_T_board2camera, R_camera2gripper, T_camera2gripper, CALIB_HAND_EYE_ANDREFF);
	//cv::calibrateHandEye(v_R_gripper2base, v_T_gripper2base, v_R_board2camera, v_T_board2camera, R_camera2gripper, T_camera2gripper, CALIB_HAND_EYE_DANIILIDIS);
	
	cout << "R_camera2gripper: " << endl;
	cout << R_camera2gripper << endl;

	cout << "T_camera2gripper: " << endl;
	cout << T_camera2gripper << endl;

	tempR = R_camera2gripper.clone();
	tempT = T_camera2gripper.clone();

	cout << "***************测试*******************" << endl;
	//测试结果
	testResult(tempR, tempT,tempH);

	system("pause");
	return 0;
}


//计算H_board2camera
//**************************************************************************
void Solve_board2camera(int num,Mat& T_board2camera, Mat& R_board2camera)
{
	//源点集p、A为P_board   目标点集q、B为P_camera
	//计算P_board到P_camera的转换矩阵

	string str = to_string(num);

	coordinate P_board, P_camera, P_centroid;
	vector<coordinate> v_board, v_camera;

	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 H = Mat::zeros(3, 3, CV_64FC1);
	cv::Mat tempD, tempU, tempV;
	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("..//board1.txt", ios::in);
	while (!fin.eof())
	{
		fin >> P_board.x >> P_board.y >> P_board.z;

		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();

	//读取相机中点的坐标
	P_centroid.x = 0;
	P_centroid.y = 0;
	P_centroid.z = 0;
	fin.open("..//camera"+str +".txt", ios::in);
	while (!fin.eof())
	{
		fin >> P_camera.x >> P_camera.y >> P_camera.z;
		//P_camera.x = P_camera.x * 1000;
		//P_camera.y = P_camera.y * 1000;
		//P_camera.z = P_camera.z * 1000;

		//cout << P_camera.x << "  " << P_camera.y << "  " << P_camera.z << endl;

		v_camera.push_back(P_camera); //存储
		P_centroid.x = P_centroid.x + P_camera.x;
		P_centroid.y = P_centroid.y + P_camera.y;
		P_centroid.z = P_centroid.z + P_camera.z;
	}
	fin.close();

	//计算质心
	centroidB.at<double>(0.0) = P_centroid.x / v_camera.size();
	centroidB.at<double>(1.0) = P_centroid.y / v_camera.size();
	centroidB.at<double>(2.0) = P_centroid.z / v_camera.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_camera[i].x;
		PB.at<double>(1, 0) = v_camera[i].y;
		PB.at<double>(2, 0) = v_camera[i].z;

		H = H + (PA - centroidA)*((PB - centroidB).t());
	}

	Eigen::MatrixXd SVD_H(3, 3);
	cv2eigen(H, SVD_H);  //Mat矩阵转Eigen矩阵
	//SVD分解(H) 并计算旋转矩阵与平移矩阵
	//------------------------------------------------------------------
	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);
	
	保存所有相机中点到v_Pc中,为后面的测试用
	cv::Mat PC(4, 1, CV_64FC1);
	cv::Mat PD;
	for (int i = 0; i < v_camera.size(); i++)
	{
		///cout << i << endl;
		PC.at<double>(0, 0) = v_camera[i].x;
		PC.at<double>(1, 0) = v_camera[i].y;
		PC.at<double>(2, 0) = v_camera[i].z;
		PC.at<double>(3, 0) = 1;
		PD = PC.clone();
		v_Pc.push_back(PD);
	}
	
	//测试相机中的点在标定板中点坐标
	//cout << "测试*******************************" << endl;
	//R_board2camera({ 0,0,3,3 }).copyTo(tempH({ 0,0,3,3 }));
	//T_board2camera({ 0,0,1,3 }).copyTo(tempH({ 3,0,1,3 })); //得到机械臂末端到机械臂基底的转换矩阵

	//tempH = tempH.inv();
	//double Point[1][4] = { -31.8856, -29.4328, 352.0000 ,1 };
	//cv::Mat tempPoint(4, 1, CV_64FC1, Point);
	//Mat temp;
	//temp = tempH*tempPoint;
	//cout << "temp: " << temp << endl;

	//system("pause");

	/*ofstream fout;
	fout.open("..//testc2b.txt", ios::app);
	double tempAA[1][3];
	Mat temp;
	tempH = tempH.inv();
	for (int i = 0; i < v_camera.size(); i++)
	{
		cout << i << endl;
		PC.at<double>(0, 0) = v_camera[i].x;
		PC.at<double>(1, 0) = v_camera[i].y;
		PC.at<double>(2, 0) = v_camera[i].z;
		PC.at<double>(3, 0) = 1;
		PD = PC.clone();
		v_Pc.push_back(PD);
		temp = tempH * PD;
		tempAA[0][0] = temp.at<double>(0, 0);
		tempAA[0][1] = temp.at<double>(1, 0);
		tempAA[0][2] = temp.at<double>(2, 0);
		fout << tempAA[0][0] << "   " << tempAA[0][1] << "  " << tempAA[0][2] << endl;
		
	}
	v_camera.clear();
	fout.close();*/
	//system("pause");

}


//计算H_gripper2base
//***********************************************************************
void Solve_gripper2base(const Mat& m, Mat& T_gripper2base, Mat& R_gripper2base)
{
	cv::Mat EulerAngle;

	T_gripper2base = m({ 0,0,3,1 }).t();
	EulerAngle = m({ 3,0,3,1 });
	R_gripper2base = eulerAngleToRotateMatrix(EulerAngle);
	
}


//测试结果
void testResult(Mat& R_camera2gripper, Mat& T_camera2gripper, Mat& tempH_T)
{
	cv::Mat R_vector;//机械臂位置中的欧拉角
	cv::Mat T_vector;//机械臂的平移向量
	cv::Mat R_girpper2base;
	cv::Mat T_gripper2base;
	cv::Mat H_gripper2base = Mat::eye(4, 4, CV_64FC1);
	cv::Mat H_camera2gripper = Mat::eye(4, 4, CV_64FC1);
	cv::Mat temp;

	//计算相机到机械臂末端的转换矩阵
	R_camera2gripper({ 0,0,3,3 }).copyTo(H_camera2gripper({ 0,0,3,3 }));
	T_camera2gripper({ 0,0,1,3 }).copyTo(H_camera2gripper({ 3,0,1,3 })); 
	cout << "H_camera2gripper" << H_camera2gripper << endl;
	
	把相机到机械臂末端的转换矩阵保存到文件中
	ofstream fout;
	double H_cam2gri[4][4];
	fout.open("..//H_camera2gripper.txt", ios::trunc);
	for (int i = 0; i < 4; i++)
	{
		H_cam2gri[i][0] = H_camera2gripper.at<double>(i, 0);
		H_cam2gri[i][1] = H_camera2gripper.at<double>(i, 1);
		H_cam2gri[i][2] = H_camera2gripper.at<double>(i, 2);
		H_cam2gri[i][3] = H_camera2gripper.at<double>(i, 3);
		fout << H_cam2gri[i][0] << '\t' << H_cam2gri[i][1] << '\t' << H_cam2gri[i][2] << '\t' << H_cam2gri[i][3] << endl;

	}
	fout.close();


	//++++++++++++++++++++++++++++++++++++++++++++++++++
    //把测试结果保存到文件中
	//测试相机中点在基底坐标戏中的坐标
	cv::Mat mat_P(4, 1, CV_64FC1);
	double tempAA[1][3];
	int n;
	fout.open("..//test123.txt", ios::trunc);
	Mat tempH;
	for (int i = 0; i < v_H_gripper2base.size(); i++) //v_H_gripper2base机械臂末端到基底的转换矩阵
	{

		cout << "------------------------------------" << endl;
		for (int j = 0; j < 8; j++) //每个机械臂姿态,保存了9个相机中的点
		{
			n = i * 8 + j;
			temp = v_H_gripper2base[i] * H_camera2gripper *v_Pc[n];
			
			//cout << "temp: " << temp << endl;
			tempAA[0][0] = temp.at<double>(0, 0);
			tempAA[0][1] = temp.at<double>(1, 0);
			tempAA[0][2] = temp.at<double>(2, 0);
			fout << tempAA[0][0] << "  " << tempAA[0][1] << "  " << tempAA[0][2] << endl;
		}
	}
	fout.close();	

	cout << "计算结束!" << endl;

	/*cout << "++++++++++++++++++++++++++++++++++++++++++++++++" << endl;
	cout << "++++++++++++++++++++++++++++++++++++++++++++++++" << endl;
	cout << "++++++++++++++++++++++++++++++++++++++++++++++++" << endl;
	double test_dou[3][3] = { 180,180,90 };
	cv::Mat test_mat(1, 3, CV_64FC1,test_dou);
	cout << test_mat << endl;
	cv::Mat test_result = eulerAngleToRotateMatrix(test_mat);
	cout << "test_result: " << test_result << endl;*/

}



//欧拉角转旋转矩阵
//********************************************************************************************
Mat eulerAngleToRotateMatrix(const Mat& eulerAngle)
{

	eulerAngle /= (180 / CV_PI);		//度转弧度
	
	Matx13d m(eulerAngle);				//<double, 1, 3>

	auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
	auto rxs = sin(rx), rxc = cos(rx);
	auto rys = sin(ry), ryc = cos(ry);
	auto rzs = sin(rz), rzc = cos(rz);

	//XYZ方向的旋转矩阵
	/*Mat RotX = (Mat_<double>(3, 3) << 1, 0, 0,
		0, rxc, -rxs,
		0, rxs, rxc);*/
	Mat RotX = (Mat_<double>(3, 3) << rxc, -rxs, 0,
		rxs, rxc, 0,
		0, 0, 1);
	Mat RotY = (Mat_<double>(3, 3) << ryc, 0, rys,
		0, 1, 0,
		-rys, 0, ryc);
	Mat RotZ = (Mat_<double>(3, 3) << rzc, -rzs, 0,
		rzs, rzc, 0,
		0, 0, 1);


	//按顺序合成后的旋转矩阵
	cv::Mat rotMat;
	rotMat = RotX * RotY * RotZ;


	return rotMat;

}

  • 1
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值