VC2017+OPENCV4.30實(shí)現(xiàn)機(jī)器人與傳感器的手眼標(biāo)定，親測可用

使用VC2017結(jié)合OPENCV4.30實(shí)現(xiàn)機(jī)器人坐標(biāo)系與傳感器坐標(biāo)系的手眼標(biāo)定。可以計(jì)算出機(jī)器人的工具坐標(biāo)系和傳感器坐標(biāo)系的相對位置關(guān)系矩陣。自己親自測試，好用。

1. // HandEyeCalibrationTest.cpp : 此文件包含 "main" 函數(shù)。程序執(zhí)行將在此處開始并結(jié)束。
   
2. //
   
3. #include <opencv2/opencv.hpp>
   
4. #include <iostream>
   
5. #include <math.h>
   
6. 
   
7. //#pragma comment( lib, "opencv_world430.lib" )
   
8. 
   
9. using namespace std;
   
10. using namespace cv;
    
11. 
    
12. Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T);
    
13. void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T);
    
14. bool isRotatedMatrix(Mat& R);
    
15. Mat eulerAngleToRotateMatrix(const Mat& eulerAngle, const std::string& seq);
    
16. Mat quaternionToRotatedMatrix(const Vec4d& q);
    
17. Mat attitudeVectorToMatrix(const Mat& m, bool useQuaternion, const string& seq);
    
18. //數(shù)據(jù)使用的已有的值
    
19. //相機(jī)中13組標(biāo)定板的位姿，x,y,z，rx,ry,rz,
    
20. Mat_<double> CalPose = (cv::Mat_<double>(9, 6) <<
    
21.         -2.602917e+01, -2.058909e+01, 2.001158e+02, -2.222308e+00, -2.200891e+00, 1.038314e-02,
    
22.         -2.109788e+01, -1.966766e+01, 1.820841e+02, 2.145852e+00, 2.143222e+00, 3.322979e-01,
    
23.         -2.571942e+01, -1.872411e+01, 2.013287e+02, -2.108920e+00, -2.134951e+00, 1.931600e-01,
    
24.         -2.488133e+01, -1.828509e+01, 1.956522e+02, -2.047373e+00, -2.027428e+00, 4.624128e-02,
    
25.         -2.920918e+01, -2.170171e+01, 2.143397e+02, 2.174664e+00, 2.123216e+00, -1.765171e-01,
    
26.         -2.518449e+01, -1.630078e+01, 2.110271e+02, 2.229811e+00, 2.162015e+00, -2.832562e-01,
    
27.         -2.729219e+01, -1.855265e+01, 1.884390e+02, -2.130600e+00, -2.041023e+00, 2.669174e-02,
    
28.         -2.754954e+01, -1.465493e+01, 1.886429e+02, -2.204029e+00, -2.089585e+00, -1.583544e-01,
    
29.         -2.716471e+01, -1.718814e+01, 1.924382e+02, -2.287743e+00, -2.088326e+00, -2.886300e-01);
    
30. 
    
31. //機(jī)械臂末端13組位姿,x,y,z,rx,ry,rz
    
32. Mat_<double> ToolPose = (cv::Mat_<double>(9, 6) <<
    
33.         1437.754, 385.919, 40.011, -8.792, 18.557, -17.376,
    
34.         1437.714, 387.617, 34.178, -23.661, 16.950, -21.540,
    
35.         1440.563, 382.431, 36.997, 0.829, 22.108, -12.817,
    
36.         1440.555, 385.317, 45.598, -1.599, 30.965, -14.361,
    
37.         1433.627, 385.860, 37.482, -4.907, 10.576, -16.978,
    
38.         1439.363, 390.483, 34.553, 0.388, 13.051, -15.847,
    
39.         1438.171, 387.592, 36.215, -4.281, 27.707, -17.485,
    
40.         1442.171, 388.187, 41.308, -13.644, 25.154, -21.693,
    
41.         1439.222, 392.200, 48.119, -19.865, 22.540, -25.981);
    
42. 
    
43. int main(int argc, char** argv)
    
44. {
    
45.         //數(shù)據(jù)聲明
    
46.         vector<Mat> R_gripper2base;
    
47.         vector<Mat> T_gripper2base;
    
48.         vector<Mat> R_target2cam;
    
49.         vector<Mat> T_target2cam;
    
50.         Mat R_cam2gripper = Mat(3, 3, CV_64FC1);                                //相機(jī)與機(jī)械臂末端坐標(biāo)系的旋轉(zhuǎn)矩陣與平移矩陣
    
51.         Mat T_cam2gripper = Mat(3, 1, CV_64FC1);
    
52.         Mat Homo_cam2gripper = Mat(4, 4, CV_64FC1);
    
53. 
    
54.         vector<Mat> Homo_target2cam;
    
55.         vector<Mat> Homo_gripper2base;
    
56.         Mat tempR, tempT, temp;
    
57. 
    
58.         for (int i = 0; i < CalPose.rows; i++)                                //計(jì)算標(biāo)定板與相機(jī)間的齊次矩陣（旋轉(zhuǎn)矩陣與平移向量）
    
59.         {
    
60.                 temp = attitudeVectorToMatrix(CalPose.row(i), false, "");        //注意seq為空，相機(jī)與標(biāo)定板間的為旋轉(zhuǎn)向量
    
61.                 Homo_target2cam.push_back(temp);
    
62.                 HomogeneousMtr2RT(temp, tempR, tempT);
    
63.                 /*cout << i << "::" << temp << endl;
    
64.                 cout << i << "::" << tempR << endl;
    
65.                 cout << i << "::" << tempT << endl;*/
    
66.                 R_target2cam.push_back(tempR);
    
67.                 T_target2cam.push_back(tempT);
    
68.         }
    
69.         for (int j = 0; j < ToolPose.rows; j++)                                //計(jì)算機(jī)械臂末端坐標(biāo)系與機(jī)器人基坐標(biāo)系之間的齊次矩陣
    
70.         {
    
71.                 temp = attitudeVectorToMatrix(ToolPose.row(j), false, "xyz");  //注意seq不是空，機(jī)械臂末端坐標(biāo)系與機(jī)器人基坐標(biāo)系之間的為歐拉角
    
72.                 Homo_gripper2base.push_back(temp);
    
73.                 HomogeneousMtr2RT(temp, tempR, tempT);
    
74.                 /*cout << j << "::" << temp << endl;
    
75.                 cout << j << "::" << tempR << endl;
    
76.                 cout << j << "::" << tempT << endl;*/
    
77.                 R_gripper2base.push_back(tempR);
    
78.                 T_gripper2base.push_back(tempT);
    
79.         }
    
80.         //TSAI計(jì)算速度最快
    
81.         calibrateHandEye(R_gripper2base, T_gripper2base, R_target2cam, T_target2cam, R_cam2gripper, T_cam2gripper, CALIB_HAND_EYE_TSAI);
    
82. 
    
83.         Homo_cam2gripper = R_T2HomogeneousMatrix(R_cam2gripper, T_cam2gripper);
    
84.         cout << Homo_cam2gripper << endl;
    
85.         cout << "Homo_cam2gripper 是否包含旋轉(zhuǎn)矩陣：" << isRotatedMatrix(Homo_cam2gripper) << endl;
    
86. 
    
87.         FileStorage fs(".\\Homo_cam2gripper.xml", FileStorage::WRITE);
    
88.         fs << "Homo_cam2gripper" << Homo_cam2gripper;
    
89.         fs.release();
    
90. 
    
91.         ///////////////////////////////////////////////////////////////////////////////////////////////////////////
    
92. 
    
93.         /**************************************************
    
94.         * @note   手眼系統(tǒng)精度測試，原理是標(biāo)定板在機(jī)器人基坐標(biāo)系中位姿固定不變，
    
95.         *                  可以根據(jù)這一點(diǎn)進(jìn)行演算
    
96.         **************************************************/
    
97.         //使用1,2組數(shù)據(jù)驗(yàn)證  標(biāo)定板在機(jī)器人基坐標(biāo)系中位姿固定不變
    
98.         cout << "1 : " << Homo_gripper2base[0] * Homo_cam2gripper * Homo_target2cam[0] << endl;
    
99.         cout << "2 : " << Homo_gripper2base[1] * Homo_cam2gripper * Homo_target2cam[1] << endl;
    
100.         //標(biāo)定板在相機(jī)中的位姿
     
101.         cout << "3 : " << Homo_target2cam[1] << endl;
     
102.         cout << "4 : " << Homo_cam2gripper.inv() * Homo_gripper2base[1].inv() * Homo_gripper2base[0] * Homo_cam2gripper * Homo_target2cam[0] << endl;
     
103. 
     
104.         cout << "----手眼系統(tǒng)測試-----" << endl;
     
105.         cout << "機(jī)械臂下標(biāo)定板XYZ為：" << endl;
     
106.         for (size_t i = 0; i < Homo_target2cam.size(); i++)
     
107.         {
     
108.                 Mat chessPos{ 0.0,0.0,0.0,1.0 };  //4*1矩陣，單獨(dú)求機(jī)械臂坐標(biāo)系下，標(biāo)定板XYZ
     
109.                 Mat worldPos = Homo_gripper2base[i] * Homo_cam2gripper * Homo_target2cam[i] * chessPos;
     
110.                 cout << i << ": " << worldPos.t() << endl;
     
111.         }
     
112.         waitKey(0);
     
113. 
     
114.         return 0;
     
115. }
     
116. 
     
117. /**************************************************
     
118. * @brief   將旋轉(zhuǎn)矩陣與平移向量合成為齊次矩陣
     
119. * @note
     
120. * @param   Mat& R   3*3旋轉(zhuǎn)矩陣
     
121. * @param   Mat& T   3*1平移矩陣
     
122. * @return  Mat      4*4齊次矩陣
     
123. **************************************************/
     
124. Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T)
     
125. {
     
126.         Mat HomoMtr;
     
127.         Mat_<double> R1 = (Mat_<double>(4, 3) <<
     
128.                 R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
     
129.                 R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
     
130.                 R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
     
131.                 0, 0, 0);
     
132.         Mat_<double> T1 = (Mat_<double>(4, 1) <<
     
133.                 T.at<double>(0, 0),
     
134.                 T.at<double>(1, 0),
     
135.                 T.at<double>(2, 0),
     
136.                 1);
     
137.         cv::hconcat(R1, T1, HomoMtr);                //矩陣拼接
     
138.         return HomoMtr;
     
139. }
     
140. 
     
141. /**************************************************
     
142. * @brief    齊次矩陣分解為旋轉(zhuǎn)矩陣與平移矩陣
     
143. * @note
     
144. * @param        const Mat& HomoMtr  4*4齊次矩陣
     
145. * @param        Mat& R              輸出旋轉(zhuǎn)矩陣
     
146. * @param        Mat& T                                輸出平移矩陣
     
147. * @return
     
148. **************************************************/
     
149. void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T)
     
150. {
     
151.         //Mat R_HomoMtr = HomoMtr(Rect(0, 0, 3, 3)); //注意Rect取值
     
152.         //Mat T_HomoMtr = HomoMtr(Rect(3, 0, 1, 3));
     
153.         //R_HomoMtr.copyTo(R);
     
154.         //T_HomoMtr.copyTo(T);
     
155.         /*HomoMtr(Rect(0, 0, 3, 3)).copyTo(R);
     
156.         HomoMtr(Rect(3, 0, 1, 3)).copyTo(T);*/
     
157.         Rect R_rect(0, 0, 3, 3);
     
158.         Rect T_rect(3, 0, 1, 3);
     
159.         R = HomoMtr(R_rect);
     
160.         T = HomoMtr(T_rect);
     
161. 
     
162. }
     
163. 
     
164. /**************************************************
     
165. * @brief        檢查是否是旋轉(zhuǎn)矩陣
     
166. * @note
     
167. * @param
     
168. * @param
     
169. * @param
     
170. * @return  true : 是旋轉(zhuǎn)矩陣， false : 不是旋轉(zhuǎn)矩陣
     
171. **************************************************/
     
172. bool isRotatedMatrix(Mat& R)                //旋轉(zhuǎn)矩陣的轉(zhuǎn)置矩陣是它的逆矩陣，逆矩陣 * 矩陣 = 單位矩陣
     
173. {
     
174.         Mat temp33 = R({ 0,0,3,3 });        //無論輸入是幾階矩陣，均提取它的三階矩陣
     
175.         Mat Rt;
     
176.         transpose(temp33, Rt);  //轉(zhuǎn)置矩陣
     
177.         Mat shouldBeIdentity = Rt * temp33;//是旋轉(zhuǎn)矩陣則乘積為單位矩陣
     
178.         Mat I = Mat::eye(3, 3, shouldBeIdentity.type());
     
179. 
     
180.         return cv::norm(I, shouldBeIdentity) < 1e-6;
     
181. }
     
182. 
     
183. /**************************************************
     
184. * @brief   歐拉角轉(zhuǎn)換為旋轉(zhuǎn)矩陣
     
185. * @note
     
186. * @param    const std::string& seq  指定歐拉角的排列順序；（機(jī)械臂的位姿類型有xyz,zyx,zyz幾種，需要區(qū)分）
     
187. * @param    const Mat& eulerAngle   歐拉角（1*3矩陣）, 角度值
     
188. * @param
     
189. * @return   返回3*3旋轉(zhuǎn)矩陣
     
190. **************************************************/
     
191. Mat eulerAngleToRotateMatrix(const Mat& eulerAngle, const std::string& seq)
     
192. {
     
193.         CV_Assert(eulerAngle.rows == 1 && eulerAngle.cols == 3);//檢查參數(shù)是否正確
     
194. 
     
195.         eulerAngle /= (180 / CV_PI);                //度轉(zhuǎn)弧度
     
196. 
     
197.         Matx13d m(eulerAngle);                                //<double, 1, 3>
     
198. 
     
199.         auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
     
200.         auto rxs = sin(rx), rxc = cos(rx);
     
201.         auto rys = sin(ry), ryc = cos(ry);
     
202.         auto rzs = sin(rz), rzc = cos(rz);
     
203. 
     
204.         //XYZ方向的旋轉(zhuǎn)矩陣
     
205.         Mat RotX = (Mat_<double>(3, 3) << 1, 0, 0,
     
206.                 0, rxc, -rxs,
     
207.                 0, rxs, rxc);
     
208.         Mat RotY = (Mat_<double>(3, 3) << ryc, 0, rys,
     
209.                 0, 1, 0,
     
210.                 -rys, 0, ryc);
     
211.         Mat RotZ = (Mat_<double>(3, 3) << rzc, -rzs, 0,
     
212.                 rzs, rzc, 0,
     
213.                 0, 0, 1);
     
214.         //按順序合成后的旋轉(zhuǎn)矩陣
     
215.         cv::Mat rotMat;
     
216. 
     
217.         if (seq == "zyx") rotMat = RotX * RotY * RotZ;
     
218.         else if (seq == "yzx") rotMat = RotX * RotZ * RotY;
     
219.         else if (seq == "zxy") rotMat = RotY * RotX * RotZ;
     
220.         else if (seq == "yxz") rotMat = RotZ * RotX * RotY;
     
221.         else if (seq == "xyz") rotMat = RotZ * RotY * RotX;
     
222.         else if (seq == "xzy") rotMat = RotY * RotZ * RotX;
     
223.         else
     
224.         {
     
225.                 cout << "Euler Angle Sequence string is wrong...";
     
226.         }
     
227.         if (!isRotatedMatrix(rotMat))                //歐拉角特殊情況下會(huì)出現(xiàn)死鎖
     
228.         {
     
229.                 cout << "Euler Angle convert to RotatedMatrix failed..." << endl;
     
230.                 exit(-1);
     
231.         }
     
232.         return rotMat;
     
233. }
     
234. 
     
235. /**************************************************
     
236. * @brief   將四元數(shù)轉(zhuǎn)換為旋轉(zhuǎn)矩陣
     
237. * @note
     
238. * @param   const Vec4d& q   歸一化的四元數(shù): q = q0 + q1 * i + q2 * j + q3 * k;
     
239. * @return  返回3*3旋轉(zhuǎn)矩陣R
     
240. **************************************************/
     
241. Mat quaternionToRotatedMatrix(const Vec4d& q)
     
242. {
     
243.         double q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
     
244. 
     
245.         double q0q0 = q0 * q0, q1q1 = q1 * q1, q2q2 = q2 * q2, q3q3 = q3 * q3;
     
246.         double q0q1 = q0 * q1, q0q2 = q0 * q2, q0q3 = q0 * q3;
     
247.         double q1q2 = q1 * q2, q1q3 = q1 * q3;
     
248.         double q2q3 = q2 * q3;
     
249.         //根據(jù)公式得來
     
250.         Mat RotMtr = (Mat_<double>(3, 3) << (q0q0 + q1q1 - q2q2 - q3q3), 2 * (q1q2 + q0q3), 2 * (q1q3 - q0q2),
     
251.                 2 * (q1q2 - q0q3), (q0q0 - q1q1 + q2q2 - q3q3), 2 * (q2q3 + q0q1),
     
252.                 2 * (q1q3 + q0q2), 2 * (q2q3 - q0q1), (q0q0 - q1q1 - q2q2 + q3q3));
     
253.         //這種形式等價(jià)
     
254.         /*Mat RotMtr = (Mat_<double>(3, 3) << (1 - 2 * (q2q2 + q3q3)), 2 * (q1q2 - q0q3), 2 * (q1q3 + q0q2),
     
255.                                                                                  2 * (q1q2 + q0q3), 1 - 2 * (q1q1 + q3q3), 2 * (q2q3 - q0q1),
     
256.                                                                                  2 * (q1q3 - q0q2), 2 * (q2q3 + q0q1), (1 - 2 * (q1q1 + q2q2)));*/
     
257. 
     
258.         return RotMtr;
     
259. }
     
260. 
     
261. /**************************************************
     
262. * @brief      將采集的原始數(shù)據(jù)轉(zhuǎn)換為齊次矩陣（從機(jī)器人控制器中獲得的）
     
263. * @note
     
264. * @param          Mat& m    1*6//1*10矩陣 ， 元素為： x,y,z,rx,ry,rz  or x,y,z, q0,q1,q2,q3,rx,ry,rz
     
265. * @param          bool useQuaternion      原始數(shù)據(jù)是否使用四元數(shù)表示
     
266. * @param          string& seq         原始數(shù)據(jù)使用歐拉角表示時(shí)，坐標(biāo)系的旋轉(zhuǎn)順序
     
267. * @return          返回轉(zhuǎn)換完的齊次矩陣
     
268. **************************************************/
     
269. Mat attitudeVectorToMatrix(const Mat& m, bool useQuaternion, const string& seq)
     
270. {
     
271.         CV_Assert(m.total() == 6 || m.total() == 10);
     
272.         //if (m.cols == 1)        //轉(zhuǎn)置矩陣為行矩陣
     
273.         //        m = m.t();        
     
274. 
     
275.         Mat temp = Mat::eye(4, 4, CV_64FC1);
     
276. 
     
277.         if (useQuaternion)
     
278.         {
     
279.                 Vec4d quaternionVec = m({ 3,0,4,1 });   //讀取存儲(chǔ)的四元數(shù)
     
280.                 quaternionToRotatedMatrix(quaternionVec).copyTo(temp({ 0,0,3,3 }));
     
281.         }
     
282.         else
     
283.         {
     
284.                 Mat rotVec;
     
285.                 if (m.total() == 6)
     
286.                 {
     
287.                         rotVec = m({ 3,0,3,1 });   //讀取存儲(chǔ)的歐拉角
     
288.                 }
     
289.                 if (m.total() == 10)
     
290.                 {
     
291.                         rotVec = m({ 7,0,3,1 });
     
292.                 }
     
293.                 //如果seq為空，表示傳入的是3*1旋轉(zhuǎn)向量，否則，傳入的是歐拉角
     
294.                 if (0 == seq.compare(""))
     
295.                 {
     
296.                         Rodrigues(rotVec, temp({ 0,0,3,3 }));   //羅德利斯轉(zhuǎn)換
     
297.                 }
     
298.                 else
     
299.                 {
     
300.                         eulerAngleToRotateMatrix(rotVec, seq).copyTo(temp({ 0,0,3,3 }));
     
301.                 }
     
302.         }
     
303.         //存入平移矩陣
     
304.         temp({ 3,0,1,3 }) = m({ 0,0,3,1 }).t();
     
305.         return temp;   //返回轉(zhuǎn)換結(jié)束的齊次矩陣
     
306. }

復(fù)制代碼

【必讀】版權(quán)免責(zé)聲明

1、本主題所有言論和內(nèi)容純屬會(huì)員個(gè)人意見，與本論壇立場無關(guān)。2、本站對所發(fā)內(nèi)容真實(shí)性、客觀性、可用性不做任何保證也不負(fù)任何責(zé)任，網(wǎng)友之間僅出于學(xué)習(xí)目的進(jìn)行交流。3、對提供的數(shù)字內(nèi)容不擁有任何權(quán)利，其版權(quán)歸原著者擁有。請勿將該數(shù)字內(nèi)容進(jìn)行商業(yè)交易、轉(zhuǎn)載等行為，該內(nèi)容只為學(xué)習(xí)所提供，使用后發(fā)生的一切問題與本站無關(guān)。 4、本網(wǎng)站不保證本站提供的下載資源的準(zhǔn)確性、安全性和完整性;同時(shí)本網(wǎng)站也不承擔(dān)用戶因使用這些下載資源對自己和他人造成任何形式的損失或傷害。 5、本網(wǎng)站所有軟件和資料均為網(wǎng)友推薦收集整理而來，僅供學(xué)習(xí)用途使用，請務(wù)必下載后兩小時(shí)內(nèi)刪除，禁止商用。6、如有侵犯你版權(quán)的，請及時(shí)聯(lián)系我們（電子郵箱1370723259@qq.com）指出，本站將立即改正。