boolsRotatedMatrix(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;}
cv::Vec3f rotationMatrixToEulerAngles(cv::Mat &R){assert(isRotatedMatrix(R));float sy =sqrt(R.at<double>(0,0)* R.at<double>(0,0)+ R.at<double>(1,0)* R.at<double>(1,0));bool singular = sy <1e-6;// Iffloat x, y, z;if(!singular){
x =atan2(R.at<double>(2,1), R.at<double>(2,2));
y =atan2(-R.at<double>(2,0), sy);
z =atan2(R.at<double>(1,0), R.at<double>(0,0));}else{
x =atan2(-R.at<double>(1,2), R.at<double>(1,1));
y =atan2(-R.at<double>(2,0), sy);
z =0;}return cv::Vec3f(z, y, x);}
voidTsai_HandEye(Mat& Hcg, vector<Mat> Hgij, vector<Mat> Hcij){CV_Assert(Hgij.size()== Hcij.size());int nStatus = Hgij.size();
Mat Rgij(3,3, CV_64FC1);
Mat Rcij(3,3, CV_64FC1);
Mat rgij(3,1, CV_64FC1);
Mat rcij(3,1, CV_64FC1);double theta_gij;double theta_cij;
Mat rngij(3,1, CV_64FC1);
Mat rncij(3,1, CV_64FC1);
Mat Pgij(3,1, CV_64FC1);
Mat Pcij(3,1, CV_64FC1);
Mat tempA(3,3, CV_64FC1);
Mat tempb(3,1, CV_64FC1);
Mat A;
Mat b;
Mat pinA;
Mat Pcg_prime(3,1, CV_64FC1);
Mat Pcg(3,1, CV_64FC1);
Mat PcgTrs(1,3, CV_64FC1);
Mat Rcg(3,3, CV_64FC1);
Mat eyeM =Mat::eye(3,3, CV_64FC1);
Mat Tgij(3,1, CV_64FC1);
Mat Tcij(3,1, CV_64FC1);
Mat tempAA(3,3, CV_64FC1);
Mat tempbb(3,1, CV_64FC1);
Mat AA;
Mat bb;
Mat pinAA;
Mat Tcg(3,1, CV_64FC1);for(int i =0; i < nStatus; i++){
Hgij[i](Rect(0,0,3,3)).copyTo(Rgij);
Hcij[i](Rect(0,0,3,3)).copyTo(Rcij);Rodrigues(Rgij, rgij);Rodrigues(Rcij, rcij);
theta_gij =norm(rgij);
theta_cij =norm(rcij);
rngij = rgij / theta_gij;
rncij = rcij / theta_cij;
Pgij =2*sin(theta_gij /2)*rngij;
Pcij =2*sin(theta_cij /2)*rncij;
tempA =skew(Pgij + Pcij);
tempb = Pcij - Pgij;
A.push_back(tempA);
b.push_back(tempb);}//Compute rotationinvert(A, pinA, DECOMP_SVD);
Pcg_prime = pinA * b;
Pcg =2* Pcg_prime /sqrt(1+norm(Pcg_prime)*norm(Pcg_prime));
PcgTrs = Pcg.t();
Rcg =(1-norm(Pcg)*norm(Pcg)/2)* eyeM +0.5*(Pcg * PcgTrs +sqrt(4-norm(Pcg)*norm(Pcg))*skew(Pcg));//Compute Translation for(int i =0; i < nStatus; i++){
Hgij[i](Rect(0,0,3,3)).copyTo(Rgij);
Hcij[i](Rect(0,0,3,3)).copyTo(Rcij);
Hgij[i](Rect(3,0,1,3)).copyTo(Tgij);
Hcij[i](Rect(3,0,1,3)).copyTo(Tcij);
tempAA = Rgij - eyeM;
tempbb = Rcg * Tcij - Tgij;
AA.push_back(tempAA);
bb.push_back(tempbb);}invert(AA, pinAA, DECOMP_SVD);
Tcg = pinAA * bb;
Rcg.copyTo(Hcg(Rect(0,0,3,3)));
Tcg.copyTo(Hcg(Rect(3,0,1,3)));
Hcg.at<double>(3,0)=0.0;
Hcg.at<double>(3,1)=0.0;
Hcg.at<double>(3,2)=0.0;
Hcg.at<double>(3,3)=1.0;}
//R和T转RT矩阵
Mat R_T2RT(Mat &R, Mat &T){
Mat RT;
Mat_<double> R1 =(cv::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.0,0.0);
cv::Mat_<double> T1 =(cv::Mat_<double>(4,1)<< T.at<double>(0,0), T.at<double>(1,0), T.at<double>(2,0),1.0);
cv::hconcat(R1, T1, RT);//C=A+B左右拼接return RT;}
标定板三维点构建方式:
//生成标定板三维点,标定板纵向角点数量mInitParams.CalibrateBoardSize.height,例如(8)//标定板横向角点数量mInitParams.CalibrateBoardSize.width,例如(11)//标定板一个格子的大小CalibrateBoardLength,例如(15)。
mvStandardCalibrateBoard.resize(0);for(int i =0; i < mInitParams.CalibrateBoardSize.height; i++){for(int j =0; j < mInitParams.CalibrateBoardSize.width; j++){
mvStandardCalibrateBoard.push_back(cv::Point3f(float(j * mInitParams.CalibrateBoardLength),float(i * mInitParams.CalibrateBoardLength),0));}}