VC2017+OPENCV4.30實現機器人與傳感器的手眼標定，親測可用 - ELEOK

// HandEyeCalibrationTest.cpp : 此文件包含 "main" 函數。程序執行將在此處開始并結束。
//
#include <opencv2/opencv.hpp>
#include <iostream>
#include <math.h>
//#pragma comment( lib, "opencv_world430.lib" )
using namespace std;
using namespace cv;
Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T);
void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T);
bool isRotatedMatrix(Mat& R);
Mat eulerAngleToRotateMatrix(const Mat& eulerAngle, const std::string& seq);
Mat quaternionToRotatedMatrix(const Vec4d& q);
Mat attitudeVectorToMatrix(const Mat& m, bool useQuaternion, const string& seq);
//數據使用的已有的值
//相機中13組標定板的位姿，x,y,z，rx,ry,rz,
Mat_<double> CalPose = (cv::Mat_<double>(9, 6) <<
-2.602917e+01, -2.058909e+01, 2.001158e+02, -2.222308e+00, -2.200891e+00, 1.038314e-02,
-2.109788e+01, -1.966766e+01, 1.820841e+02, 2.145852e+00, 2.143222e+00, 3.322979e-01,
-2.571942e+01, -1.872411e+01, 2.013287e+02, -2.108920e+00, -2.134951e+00, 1.931600e-01,
-2.488133e+01, -1.828509e+01, 1.956522e+02, -2.047373e+00, -2.027428e+00, 4.624128e-02,
-2.920918e+01, -2.170171e+01, 2.143397e+02, 2.174664e+00, 2.123216e+00, -1.765171e-01,
-2.518449e+01, -1.630078e+01, 2.110271e+02, 2.229811e+00, 2.162015e+00, -2.832562e-01,
-2.729219e+01, -1.855265e+01, 1.884390e+02, -2.130600e+00, -2.041023e+00, 2.669174e-02,
-2.754954e+01, -1.465493e+01, 1.886429e+02, -2.204029e+00, -2.089585e+00, -1.583544e-01,
-2.716471e+01, -1.718814e+01, 1.924382e+02, -2.287743e+00, -2.088326e+00, -2.886300e-01);
//機械臂末端13組位姿,x,y,z,rx,ry,rz
Mat_<double> ToolPose = (cv::Mat_<double>(9, 6) <<
1437.754, 385.919, 40.011, -8.792, 18.557, -17.376,
1437.714, 387.617, 34.178, -23.661, 16.950, -21.540,
1440.563, 382.431, 36.997, 0.829, 22.108, -12.817,
1440.555, 385.317, 45.598, -1.599, 30.965, -14.361,
1433.627, 385.860, 37.482, -4.907, 10.576, -16.978,
1439.363, 390.483, 34.553, 0.388, 13.051, -15.847,
1438.171, 387.592, 36.215, -4.281, 27.707, -17.485,
1442.171, 388.187, 41.308, -13.644, 25.154, -21.693,
1439.222, 392.200, 48.119, -19.865, 22.540, -25.981);
int main(int argc, char** argv)
{
//數據聲明
vector<Mat> R_gripper2base;
vector<Mat> T_gripper2base;
vector<Mat> R_target2cam;
vector<Mat> T_target2cam;
Mat R_cam2gripper = Mat(3, 3, CV_64FC1); //相機與機械臂末端坐標系的旋轉矩陣與平移矩陣
Mat T_cam2gripper = Mat(3, 1, CV_64FC1);
Mat Homo_cam2gripper = Mat(4, 4, CV_64FC1);
vector<Mat> Homo_target2cam;
vector<Mat> Homo_gripper2base;
Mat tempR, tempT, temp;
for (int i = 0; i < CalPose.rows; i++) //計算標定板與相機間的齊次矩陣（旋轉矩陣與平移向量）
{
temp = attitudeVectorToMatrix(CalPose.row(i), false, ""); //注意seq為空，相機與標定板間的為旋轉向量
Homo_target2cam.push_back(temp);
HomogeneousMtr2RT(temp, tempR, tempT);
/*cout << i << "::" << temp << endl;
cout << i << "::" << tempR << endl;
cout << i << "::" << tempT << endl;*/
R_target2cam.push_back(tempR);
T_target2cam.push_back(tempT);
}
for (int j = 0; j < ToolPose.rows; j++) //計算機械臂末端坐標系與機器人基坐標系之間的齊次矩陣
{
temp = attitudeVectorToMatrix(ToolPose.row(j), false, "xyz"); //注意seq不是空，機械臂末端坐標系與機器人基坐標系之間的為歐拉角
Homo_gripper2base.push_back(temp);
HomogeneousMtr2RT(temp, tempR, tempT);
/*cout << j << "::" << temp << endl;
cout << j << "::" << tempR << endl;
cout << j << "::" << tempT << endl;*/
R_gripper2base.push_back(tempR);
T_gripper2base.push_back(tempT);
}
//TSAI計算速度最快
calibrateHandEye(R_gripper2base, T_gripper2base, R_target2cam, T_target2cam, R_cam2gripper, T_cam2gripper, CALIB_HAND_EYE_TSAI);
Homo_cam2gripper = R_T2HomogeneousMatrix(R_cam2gripper, T_cam2gripper);
cout << Homo_cam2gripper << endl;
cout << "Homo_cam2gripper 是否包含旋轉矩陣：" << isRotatedMatrix(Homo_cam2gripper) << endl;
FileStorage fs(".\\Homo_cam2gripper.xml", FileStorage::WRITE);
fs << "Homo_cam2gripper" << Homo_cam2gripper;
fs.release();
///////////////////////////////////////////////////////////////////////////////////////////////////////////
/**************************************************
* @note 手眼系統精度測試，原理是標定板在機器人基坐標系中位姿固定不變，
* 可以根據這一點進行演算
**************************************************/
//使用1,2組數據驗證標定板在機器人基坐標系中位姿固定不變
cout << "1 : " << Homo_gripper2base[0] * Homo_cam2gripper * Homo_target2cam[0] << endl;
cout << "2 : " << Homo_gripper2base[1] * Homo_cam2gripper * Homo_target2cam[1] << endl;
//標定板在相機中的位姿
cout << "3 : " << Homo_target2cam[1] << endl;
cout << "4 : " << Homo_cam2gripper.inv() * Homo_gripper2base[1].inv() * Homo_gripper2base[0] * Homo_cam2gripper * Homo_target2cam[0] << endl;
cout << "----手眼系統測試-----" << endl;
cout << "機械臂下標定板XYZ為：" << endl;
for (size_t i = 0; i < Homo_target2cam.size(); i++)
{
Mat chessPos{ 0.0,0.0,0.0,1.0 }; //4*1矩陣，單獨求機械臂坐標系下，標定板XYZ
Mat worldPos = Homo_gripper2base[i] * Homo_cam2gripper * Homo_target2cam[i] * chessPos;
cout << i << ": " << worldPos.t() << endl;
}
waitKey(0);
return 0;
}
/**************************************************
* @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; //返回轉換結束的齊次矩陣
}

復制代碼

国产免费AV|泡泡玛特欧洲总部将设在伦敦|中文天堂网www新版资源在线|一本久道综合在线中文|国精产品一二三产区的使用方法|香蕉鱼在线观看|www.27eee

ELEOK