VC2017+OPENCV4.30实现机器人与传感器的手眼标定，亲测可用

使用VC2017结合OPENCV4.30实现机器人坐标系与传感器坐标系的手眼标定。可以计算出机器人的工具坐标系和传感器坐标系的相对位置关系矩阵。自己亲自测试，好用。

1. // HandEyeCalibrationTest.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
   
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. //数据使用的已有的值
    
19. //相机中13组标定板的位姿，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. //机械臂末端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.         //数据声明
    
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);                                //相机与机械臂末端坐标系的旋转矩阵与平移矩阵
    
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++)                                //计算标定板与相机间的齐次矩阵（旋转矩阵与平移向量）
    
59.         {
    
60.                 temp = attitudeVectorToMatrix(CalPose.row(i), false, "");        //注意seq为空，相机与标定板间的为旋转向量
    
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++)                                //计算机械臂末端坐标系与机器人基坐标系之间的齐次矩阵
    
70.         {
    
71.                 temp = attitudeVectorToMatrix(ToolPose.row(j), false, "xyz");  //注意seq不是空，机械臂末端坐标系与机器人基坐标系之间的为欧拉角
    
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计算速度最快
    
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 是否包含旋转矩阵：" << 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   手眼系统精度测试，原理是标定板在机器人基坐标系中位姿固定不变，
    
95.         *                  可以根据这一点进行演算
    
96.         **************************************************/
    
97.         //使用1,2组数据验证  标定板在机器人基坐标系中位姿固定不变
    
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.         //标定板在相机中的位姿
     
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 << "----手眼系统测试-----" << endl;
     
105.         cout << "机械臂下标定板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矩阵，单独求机械臂坐标系下，标定板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   将旋转矩阵与平移向量合成为齐次矩阵
     
119. * @note
     
120. * @param   Mat& R   3*3旋转矩阵
     
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    齐次矩阵分解为旋转矩阵与平移矩阵
     
143. * @note
     
144. * @param        const Mat& HomoMtr  4*4齐次矩阵
     
145. * @param        Mat& R              输出旋转矩阵
     
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        检查是否是旋转矩阵
     
166. * @note
     
167. * @param
     
168. * @param
     
169. * @param
     
170. * @return  true : 是旋转矩阵， false : 不是旋转矩阵
     
171. **************************************************/
     
172. bool isRotatedMatrix(Mat& R)                //旋转矩阵的转置矩阵是它的逆矩阵，逆矩阵 * 矩阵 = 单位矩阵
     
173. {
     
174.         Mat temp33 = R({ 0,0,3,3 });        //无论输入是几阶矩阵，均提取它的三阶矩阵
     
175.         Mat Rt;
     
176.         transpose(temp33, Rt);  //转置矩阵
     
177.         Mat shouldBeIdentity = Rt * temp33;//是旋转矩阵则乘积为单位矩阵
     
178.         Mat I = Mat::eye(3, 3, shouldBeIdentity.type());
     
179. 
     
180.         return cv::norm(I, shouldBeIdentity) < 1e-6;
     
181. }
     
182. 
     
183. /**************************************************
     
184. * @brief   欧拉角转换为旋转矩阵
     
185. * @note
     
186. * @param    const std::string& seq  指定欧拉角的排列顺序；（机械臂的位姿类型有xyz,zyx,zyz几种，需要区分）
     
187. * @param    const Mat& eulerAngle   欧拉角（1*3矩阵）, 角度值
     
188. * @param
     
189. * @return   返回3*3旋转矩阵
     
190. **************************************************/
     
191. Mat eulerAngleToRotateMatrix(const Mat& eulerAngle, const std::string& seq)
     
192. {
     
193.         CV_Assert(eulerAngle.rows == 1 && eulerAngle.cols == 3);//检查参数是否正确
     
194. 
     
195.         eulerAngle /= (180 / CV_PI);                //度转弧度
     
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方向的旋转矩阵
     
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.         //按顺序合成后的旋转矩阵
     
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))                //欧拉角特殊情况下会出现死锁
     
228.         {
     
229.                 cout << "Euler Angle convert to RotatedMatrix failed..." << endl;
     
230.                 exit(-1);
     
231.         }
     
232.         return rotMat;
     
233. }
     
234. 
     
235. /**************************************************
     
236. * @brief   将四元数转换为旋转矩阵
     
237. * @note
     
238. * @param   const Vec4d& q   归一化的四元数: q = q0 + q1 * i + q2 * j + q3 * k;
     
239. * @return  返回3*3旋转矩阵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.         //根据公式得来
     
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.         //这种形式等价
     
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      将采集的原始数据转换为齐次矩阵（从机器人控制器中获得的）
     
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      原始数据是否使用四元数表示
     
266. * @param          string& seq         原始数据使用欧拉角表示时，坐标系的旋转顺序
     
267. * @return          返回转换完的齐次矩阵
     
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)        //转置矩阵为行矩阵
     
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 });   //读取存储的四元数
     
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 });   //读取存储的欧拉角
     
288.                 }
     
289.                 if (m.total() == 10)
     
290.                 {
     
291.                         rotVec = m({ 7,0,3,1 });
     
292.                 }
     
293.                 //如果seq为空，表示传入的是3*1旋转向量，否则，传入的是欧拉角
     
294.                 if (0 == seq.compare(""))
     
295.                 {
     
296.                         Rodrigues(rotVec, temp({ 0,0,3,3 }));   //罗德利斯转换
     
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;   //返回转换结束的齐次矩阵
     
306. }

复制代码

【必读】版权免责声明

1、本主题所有言论和内容纯属会员个人意见，与本论坛立场无关。2、本站对所发内容真实性、客观性、可用性不做任何保证也不负任何责任，网友之间仅出于学习目的进行交流。3、对提供的数字内容不拥有任何权利，其版权归原著者拥有。请勿将该数字内容进行商业交易、转载等行为，该内容只为学习所提供，使用后发生的一切问题与本站无关。 4、本网站不保证本站提供的下载资源的准确性、安全性和完整性;同时本网站也不承担用户因使用这些下载资源对自己和他人造成任何形式的损失或伤害。 5、本网站所有软件和资料均为网友推荐收集整理而来，仅供学习用途使用，请务必下载后两小时内删除，禁止商用。6、如有侵犯你版权的，请及时联系我们（电子邮箱1370723259@qq.com）指出，本站将立即改正。