手眼标定eyes to hand

手眼标定eyes to hand

1、写在前面

线扫做了一些，现在希望将重建出来的模型转换到机器人坐标系下，进行机器人编程，加工，看了不少大神写的原理，代码，勉强把代码调通了，记录一下，还有些原理没懂，后面懂了再来填坑

2、大概思路

1）. 测出标定板在相机坐标系位姿
根据棋盘格角点实际坐标和相机检测到的角点坐标，利用opencv的

solvePnP(objectPoints, cornerL, cameraMatrix, distCoeffs, rvec, T);

第一个参数是棋盘格实际坐标，
第二参数是检测到的角点坐标，
第三个参数是相机内参，
第四个参数是相机畸变参数，
后面两个就是棋盘格坐标系相对于相机坐标系的旋转平移矩阵
计算rvec,T，这个时候的rvec是雅可比形式的rvec，需要一个转换

2） 获取机器人末端的位姿
这个位姿是相对于机器人坐标系的，直接用示教器读出来就可以了

这时候将示教器读出的位姿换算成齐次矩阵

3）. 运用Navy解AX=XB完成手眼标定

3、完整代码

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>

#include <vector>
#include <math.h>
#include <iostream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
//#include <string>

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

Size square_size = Size(6, 6);  /* 实际测量得到的标定板上每个棋盘格的大小 */
cv::Size board_size(8, 11);//标定板的棋格

//获取标定板在相机上的位姿
void getRT(vector<Point2f>& cornerL, Mat& cameraMatrix, Mat &distCoeffs, Mat&R, Mat&T) {
	//首先获取到世界坐标的点
	if (cornerL.empty() || cameraMatrix.empty() || distCoeffs.empty())return;
	vector<Point3f> objectPoints;
	for (int i = 0; i<board_size.height; i++)
	{
		for (int j = 0; j<board_size.width; j++)
		{
			Point3f realPoint;
			/* 假设标定板放在世界坐标系中z=0的平面上 */
			realPoint.x = (float)i*(float)(square_size.width);
			realPoint.y = (float)j*(float)(square_size.width);
			realPoint.z = 0;
			objectPoints.push_back(realPoint);
		}
	}
	Mat rvec;
	solvePnP(objectPoints, cornerL, cameraMatrix, distCoeffs, rvec, T);//计算rvec,T，这个时候的rvec是雅可比形式的rvec，需要一个转换
	Rodrigues(rvec, R); //这个时候的R是3x3的一个矩阵。

}

void sqrtm(Matrix3d& R, Matrix3cd& sqrtR)
{
    sqrtR = Matrix3cd::Zero();
    ComplexSchur<Matrix3d> schur(3);
    schur.compute(R);
    Matrix3cd Q = schur.matrixU();
    Matrix3cd T = schur.matrixT();
    Vector3cd diagT = T.diagonal();
    bool flag = true;
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            if (i != j && abs(T(i, j)) > 0.0000001)
            {
                flag = false;
                break;
            }
        }
    }
    if (flag)
    {
        Matrix3cd sqT;
        for (int i = 0; i < 9; i++)
        {
            sqT(i / 3, i % 3) = sqrt(diagT(i % 3));
        }
        sqrtR = (Q.array()*sqT.array()).matrix()*Q.transpose();
        sqrtR = (sqrtR + sqrtR.transpose())*0.5;
    }
    else
    {
        Matrix3cd U = Matrix3cd::Zero();
        for (int j = 0; j < 3; j++)
        {
            U(j, j) = sqrt(T(j, j));
            for (int i = j - 1; i >= 0; i--)
            {
                complex<double> s;
                s = (0.0, 0.0);
                for (int k = i + 1; k <= j - 1; k++)
                {
                    s += U(i, k)*U(k, j);
                }
                U(i, j) = (T(i, j) - s) / (U(i, i) + U(j, j));
            }
        }
        sqrtR = Q*U*Q.transpose();
    }
}
Navy计算方法
bool Navy_HandEye(Matrix4d& Hcg, vector<Matrix4d> Hgij, vector<Matrix4d> Hcij)
{
    if (Hgij.size() != Hcij.size())
    {
        cout << "Input Wrong Matries, Please Check Their Sizes  ..." << endl;
        return false;
    }

    Mat Temp_R;
    Matrix3d temp_Rotation;
    Mat rvec;
    Vector3d rVecHg, rVecHc;

    int n = Hgij.size();
    Matrix3d M = Matrix3d::Zero();

    for (int i = 0; i < n; i++)
    {
        temp_Rotation = (Hgij[i]).block(0, 0, 3, 3);
        eigen2cv(temp_Rotation, Temp_R);
        Rodrigues(Temp_R, rvec);
        cv2eigen(rvec, rVecHg);

        temp_Rotation = (Hcij[i]).block(0, 0, 3, 3);
        eigen2cv(temp_Rotation, Temp_R);
        Rodrigues(Temp_R, rvec);
        cv2eigen(rvec, rVecHc);

        M =  rVecHc*rVecHg.transpose() + M;
    }

    Matrix3d MtM = M.transpose()*M;
    Matrix3cd sqrtMM;
    sqrtm(MtM, sqrtMM);
    Hcg.block(0, 0, 3, 3) = (sqrtMM.real()).inverse()*M.transpose();

    MatrixXd C = MatrixXd::Zero(3 * n, 3);
    VectorXd d = VectorXd::Zero(3 * n);
    Vector3d temp_d;
    Matrix3d I3 = Matrix3d::Identity();
    for (int i = 0; i < n; i++)
    {
        temp_d = Hcg.block(0, 0, 3, 3)*Hcij[i].block(0, 3, 3, 1);
        Vector3d temp_t = Hgij[i].block(0, 3, 3, 1);
        d(3 * i) = temp_t(0) - temp_d(0);
        d(3 * i + 1) = temp_t(1) - temp_d(1);
        d(3 * i + 2) = temp_t(2) - temp_d(2);
        for (int j = 0; j < 3; j++)
        {
            Matrix3d temp_R = Hgij[i].block(0, 0, 3, 3);
            C(3 * i, j) = I3(0, j) - temp_R(0, j);
            C(3 * i + 1, j) = I3(1, j) - temp_R(1, j);
            C(3 * i + 2, j) = I3(2, j) - temp_R(2, j);
        }
    }
    Hcg.block(0, 3, 3, 1) = (C.transpose()*C).inverse()*C.transpose()*d;

    return true;
}



/**************************************************
* @brief   将旋转矩阵与平移向量合成为齐次矩阵
* @note
* @param   Mat& R   3*3旋转矩阵
* @param   Mat& T   3*1平移矩阵
* @return  Mat      4*4齐次矩阵
**************************************************/
Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T)
{
	Mat HomoMtr;
	Mat_<double> R1 = (Mat_<double>(4, 3) <<
		R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
		R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
		R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
		0, 0, 0);
	Mat_<double> T1 = (Mat_<double>(4, 1) <<
		T.at<double>(0, 0),
		T.at<double>(1, 0),
		T.at<double>(2, 0),
		1);
	cv::hconcat(R1, T1, HomoMtr);		//矩阵拼接
	return HomoMtr;
}

/**************************************************
* @brief    齐次矩阵分解为旋转矩阵与平移矩阵
* @note
* @param	const Mat& HomoMtr  4*4齐次矩阵
* @param	Mat& R              输出旋转矩阵
* @param	Mat& T				输出平移矩阵
* @return
**************************************************/
void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T)
{
	//Mat R_HomoMtr = HomoMtr(Rect(0, 0, 3, 3)); //注意Rect取值
	//Mat T_HomoMtr = HomoMtr(Rect(3, 0, 1, 3));
	//R_HomoMtr.copyTo(R);
	//T_HomoMtr.copyTo(T);
	/*HomoMtr(Rect(0, 0, 3, 3)).copyTo(R);
	HomoMtr(Rect(3, 0, 1, 3)).copyTo(T);*/
	Rect R_rect(0, 0, 3, 3);
	Rect T_rect(3, 0, 1, 3);
	R = HomoMtr(R_rect);
	T = HomoMtr(T_rect);

}
/**************************************************
* @brief	检查是否是旋转矩阵
* @note
* @param
* @param
* @param
* @return  true : 是旋转矩阵， false : 不是旋转矩阵
**************************************************/
bool isRotatedMatrix(Mat& R)		//旋转矩阵的转置矩阵是它的逆矩阵，逆矩阵 * 矩阵 = 单位矩阵
{
	Mat temp33 = R({ 0,0,3,3 });	//无论输入是几阶矩阵，均提取它的三阶矩阵
	Mat Rt;
	transpose(temp33, Rt);  //转置矩阵
	Mat shouldBeIdentity = Rt * temp33;//是旋转矩阵则乘积为单位矩阵
	Mat I = Mat::eye(3, 3, shouldBeIdentity.type());

	return cv::norm(I, shouldBeIdentity) < 1e-6;
}

/**************************************************
* @brief   欧拉角转换为旋转矩阵
* @note
* @param    const std::string& seq  指定欧拉角的排列顺序；（机械臂的位姿类型有xyz,zyx,zyz几种，需要区分）
* @param    const Mat& eulerAngle   欧拉角（1*3矩阵）, 角度值
* @param
* @return   返回3*3旋转矩阵
**************************************************/
Mat eulerAngleToRotateMatrix(const Mat& eulerAngle, const std::string& seq)
{
	CV_Assert(eulerAngle.rows == 1 && eulerAngle.cols == 3);//检查参数是否正确

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

	if (seq == "zyx") rotMat = RotX * RotY * RotZ;
	else if (seq == "yzx") rotMat = RotX * RotZ * RotY;
	else if (seq == "zxy") rotMat = RotY * RotX * RotZ;
	else if (seq == "yxz") rotMat = RotZ * RotX * RotY;
	else if (seq == "xyz") rotMat = RotZ * RotY * RotX;
	else if (seq == "xzy") rotMat = RotY * RotZ * RotX;
	else
	{
		cout << "Euler Angle Sequence string is wrong...";
	}
	if (!isRotatedMatrix(rotMat))		//欧拉角特殊情况下会出现死锁
	{
		cout << "Euler Angle convert to RotatedMatrix failed..." << endl;
		exit(-1);
	}
	return rotMat;
}
/**************************************************
* @brief   将四元数转换为旋转矩阵
* @note
* @param   const Vec4d& q   归一化的四元数: q = q0 + q1 * i + q2 * j + q3 * k;
* @return  返回3*3旋转矩阵R
**************************************************/
Mat quaternionToRotatedMatrix(const Vec4d& q)
{
	double q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];

	double q0q0 = q0 * q0, q1q1 = q1 * q1, q2q2 = q2 * q2, q3q3 = q3 * q3;
	double q0q1 = q0 * q1, q0q2 = q0 * q2, q0q3 = q0 * q3;
	double q1q2 = q1 * q2, q1q3 = q1 * q3;
	double q2q3 = q2 * q3;
	//根据公式得来
	Mat RotMtr = (Mat_<double>(3, 3) << (q0q0 + q1q1 - q2q2 - q3q3), 2 * (q1q2 + q0q3), 2 * (q1q3 - q0q2),
		2 * (q1q2 - q0q3), (q0q0 - q1q1 + q2q2 - q3q3), 2 * (q2q3 + q0q1),
		2 * (q1q3 + q0q2), 2 * (q2q3 - q0q1), (q0q0 - q1q1 - q2q2 + q3q3));
	//这种形式等价
	/*Mat RotMtr = (Mat_<double>(3, 3) << (1 - 2 * (q2q2 + q3q3)), 2 * (q1q2 - q0q3), 2 * (q1q3 + q0q2),
	2 * (q1q2 + q0q3), 1 - 2 * (q1q1 + q3q3), 2 * (q2q3 - q0q1),
	2 * (q1q3 - q0q2), 2 * (q2q3 + q0q1), (1 - 2 * (q1q1 + q2q2)));*/

	return RotMtr;
}


/**************************************************
* @brief      将采集的原始数据转换为齐次矩阵（从机器人控制器中获得的）
* @note
* @param	  Mat& m    1*6//1*10矩阵 ， 元素为： x,y,z,rx,ry,rz  or x,y,z, q0,q1,q2,q3,rx,ry,rz
* @param	  bool useQuaternion      原始数据是否使用四元数表示
* @param	  string& seq         原始数据使用欧拉角表示时，坐标系的旋转顺序
* @return	  返回转换完的齐次矩阵
**************************************************/
Mat attitudeVectorToMatrix(const Mat& m, bool useQuaternion, const string& seq)
{
	CV_Assert(m.total() == 6 || m.total() == 10);
	//if (m.cols == 1)	//转置矩阵为行矩阵
	//	m = m.t();	

	Mat temp = Mat::eye(4, 4, CV_64FC1);

	if (useQuaternion)
	{
		Vec4d quaternionVec = m({ 3,0,4,1 });   //读取存储的四元数
		quaternionToRotatedMatrix(quaternionVec).copyTo(temp({ 0,0,3,3 }));
	}
	else
	{
		Mat rotVec;
		if (m.total() == 6)
		{
			rotVec = m({ 3,0,3,1 });   //读取存储的欧拉角
		}
		if (m.total() == 10)
		{
			rotVec = m({ 7,0,3,1 });
		}
		//如果seq为空，表示传入的是3*1旋转向量，否则，传入的是欧拉角
		if (0 == seq.compare(""))
		{
			Rodrigues(rotVec, temp({ 0,0,3,3 }));   //罗德利斯转换
		}
		else
		{
			eulerAngleToRotateMatrix(rotVec, seq).copyTo(temp({ 0,0,3,3 }));
		}
	}
	//存入平移矩阵
	temp({ 3,0,1,3 }) = m({ 0,0,3,1 }).t();
	return temp;   //返回转换结束的齐次矩阵
}

	//机械臂末端位姿,x,y,z,rx,ry,rz
Mat_<double> CalPose = (cv::Mat_<double>(8, 6) <<
	-1069.762, -994.112, -97.337, -104.476, -90.071, -40.314,
	- 1083.791, -1005.756, -92.969, -105.890, 89.054, -39.633,
	- 1035.899, -1005.886, -41.036, -101.964, 81.756, 38.862,
	- 1028.416, -1020.080, -99.108, -99.261, 89.824, 41.777,
	-1110.911,-1002.588,-79.360,-114.913,88.142,-34.318,
	-1077.360,-1016.106,-112.105,-107.618,92.751,-39.349,
	-1041.973,-997.952,-117.855,-99.714,93.099,-43.031,
	-1086.080,-989.188,-186.423,-106.190,103.040,-44.306
	);
	
int main()
{
	//Hgij是机器人末端的位姿，Hcij是标定板坐标系在机器人中的位姿，
	//标定板坐标系选取第一个点作为坐标原点，x,y轴对应标定板xy，z轴垂直于标定板且z为0
    vector<Eigen::Matrix4d> Hgij,Hcij;
	//获取标定板在相机坐标下的位姿
	vector<Mat>calPOS;//所有标定板的位姿
	Mat H(4, 4, CV_32F);//每一个标定板的位姿
	//循环计算标定板位姿，以第一个点为坐标原点
	for (int i = 0; i < 8; i++)
	{
		char filename[100];
		/*读取左边的图像*/
		sprintf_s(filename, "F:\\相机驱动 - 副本(未删减)\\3、应用程序\\2、源代码\\Right\\Right%04d.bmp", i + 1);
		Mat ImageIn = imread(filename);

		if (!ImageIn.data)
		{
			cout << "Fail to read image ..." << endl;
			return -1;
		}

		vector<cv::Point2f> centres;//标定板上识别到的角点
		bool isFind = findChessboardCorners(ImageIn, board_size, centres, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE);
		cv::Mat cameraMatrix(3, 3, CV_32F);//内参
		cv::Mat cameraDistcoeff;//畸变矩阵
								//读取内参和畸变矩阵
		FileStorage file_storage2("F:\\vs2015Data\\cal\\test2\\intrinsicsl.yml", FileStorage::READ);
		file_storage2["cameraMatrix"] >> cameraMatrix;
		file_storage2["cameraDistcoeff"] >> cameraDistcoeff;
		file_storage2.release();


		cv::Mat rvec, tvec;//标定出来的旋转平移矩阵
						   //标定
		getRT(centres, cameraMatrix, cameraDistcoeff, rvec, tvec);
		//将旋转平移矩阵换为齐次矩阵
		H = R_T2HomogeneousMatrix(rvec, tvec);
		Eigen::Matrix4d transMat;
		cv2eigen(H, transMat);
		Hcij.push_back(transMat);
	
	}

	Eigen::Matrix4d transMat;//标定板每一个姿态的位姿

	//将在示教器上读出的机器人末端位姿换成齐次矩阵
	Mat temp;
	for (int i = 0; i < CalPose.rows; i++)//计算标定板与相机间的齐次矩阵（旋转矩阵与平移向量）
	{
		temp = attitudeVectorToMatrix(CalPose.row(i), false, "xyz");	
		cv2eigen(temp, transMat);
		Hgij.push_back(transMat);
	}

    Eigen::Matrix4d Hcg = Eigen::Matrix4d::Identity();
	cout << Hgij.size() << "" << Hcij.size() << endl;
    Navy_HandEye(Hcg,Hgij,Hcij);

    cout << "The transform matrix is " << Hcg << endl;

    if(!SaveMatrix("EyeHand.yaml", Hcg)){
        cout << "Fail to save transform matrix ..." << endl;
        return -1;
    }
	system("pause");
    return 0;

}