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

[复制链接]
查看57 | 回复0 | 6 天前 | 显示全部楼层 |阅读模式
使用VC2017结合OPENCV4.30实现机器人坐标系与传感器坐标系的手眼标定。可以计算出机器人的工具坐标系和传感器坐标系的相对位置关系矩阵。自己亲自测试,好用。
  1. // HandEyeCalibrationTest.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
  2. //
  3. #include <opencv2/opencv.hpp>
  4. #include <iostream>
  5. #include <math.h>

  6. //#pragma comment( lib, "opencv_world430.lib" )

  7. using namespace std;
  8. using namespace cv;

  9. Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T);
  10. void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T);
  11. bool isRotatedMatrix(Mat& R);
  12. Mat eulerAngleToRotateMatrix(const Mat& eulerAngle, const std::string& seq);
  13. Mat quaternionToRotatedMatrix(const Vec4d& q);
  14. Mat attitudeVectorToMatrix(const Mat& m, bool useQuaternion, const string& seq);
  15. //数据使用的已有的值
  16. //相机中13组标定板的位姿,x,y,z,rx,ry,rz,
  17. Mat_<double> CalPose = (cv::Mat_<double>(9, 6) <<
  18.         -2.602917e+01, -2.058909e+01, 2.001158e+02, -2.222308e+00, -2.200891e+00, 1.038314e-02,
  19.         -2.109788e+01, -1.966766e+01, 1.820841e+02, 2.145852e+00, 2.143222e+00, 3.322979e-01,
  20.         -2.571942e+01, -1.872411e+01, 2.013287e+02, -2.108920e+00, -2.134951e+00, 1.931600e-01,
  21.         -2.488133e+01, -1.828509e+01, 1.956522e+02, -2.047373e+00, -2.027428e+00, 4.624128e-02,
  22.         -2.920918e+01, -2.170171e+01, 2.143397e+02, 2.174664e+00, 2.123216e+00, -1.765171e-01,
  23.         -2.518449e+01, -1.630078e+01, 2.110271e+02, 2.229811e+00, 2.162015e+00, -2.832562e-01,
  24.         -2.729219e+01, -1.855265e+01, 1.884390e+02, -2.130600e+00, -2.041023e+00, 2.669174e-02,
  25.         -2.754954e+01, -1.465493e+01, 1.886429e+02, -2.204029e+00, -2.089585e+00, -1.583544e-01,
  26.         -2.716471e+01, -1.718814e+01, 1.924382e+02, -2.287743e+00, -2.088326e+00, -2.886300e-01);

  27. //机械臂末端13组位姿,x,y,z,rx,ry,rz
  28. Mat_<double> ToolPose = (cv::Mat_<double>(9, 6) <<
  29.         1437.754, 385.919, 40.011, -8.792, 18.557, -17.376,
  30.         1437.714, 387.617, 34.178, -23.661, 16.950, -21.540,
  31.         1440.563, 382.431, 36.997, 0.829, 22.108, -12.817,
  32.         1440.555, 385.317, 45.598, -1.599, 30.965, -14.361,
  33.         1433.627, 385.860, 37.482, -4.907, 10.576, -16.978,
  34.         1439.363, 390.483, 34.553, 0.388, 13.051, -15.847,
  35.         1438.171, 387.592, 36.215, -4.281, 27.707, -17.485,
  36.         1442.171, 388.187, 41.308, -13.644, 25.154, -21.693,
  37.         1439.222, 392.200, 48.119, -19.865, 22.540, -25.981);

  38. int main(int argc, char** argv)
  39. {
  40.         //数据声明
  41.         vector<Mat> R_gripper2base;
  42.         vector<Mat> T_gripper2base;
  43.         vector<Mat> R_target2cam;
  44.         vector<Mat> T_target2cam;
  45.         Mat R_cam2gripper = Mat(3, 3, CV_64FC1);                                //相机与机械臂末端坐标系的旋转矩阵与平移矩阵
  46.         Mat T_cam2gripper = Mat(3, 1, CV_64FC1);
  47.         Mat Homo_cam2gripper = Mat(4, 4, CV_64FC1);

  48.         vector<Mat> Homo_target2cam;
  49.         vector<Mat> Homo_gripper2base;
  50.         Mat tempR, tempT, temp;

  51.         for (int i = 0; i < CalPose.rows; i++)                                //计算标定板与相机间的齐次矩阵(旋转矩阵与平移向量)
  52.         {
  53.                 temp = attitudeVectorToMatrix(CalPose.row(i), false, "");        //注意seq为空,相机与标定板间的为旋转向量
  54.                 Homo_target2cam.push_back(temp);
  55.                 HomogeneousMtr2RT(temp, tempR, tempT);
  56.                 /*cout << i << "::" << temp << endl;
  57.                 cout << i << "::" << tempR << endl;
  58.                 cout << i << "::" << tempT << endl;*/
  59.                 R_target2cam.push_back(tempR);
  60.                 T_target2cam.push_back(tempT);
  61.         }
  62.         for (int j = 0; j < ToolPose.rows; j++)                                //计算机械臂末端坐标系与机器人基坐标系之间的齐次矩阵
  63.         {
  64.                 temp = attitudeVectorToMatrix(ToolPose.row(j), false, "xyz");  //注意seq不是空,机械臂末端坐标系与机器人基坐标系之间的为欧拉角
  65.                 Homo_gripper2base.push_back(temp);
  66.                 HomogeneousMtr2RT(temp, tempR, tempT);
  67.                 /*cout << j << "::" << temp << endl;
  68.                 cout << j << "::" << tempR << endl;
  69.                 cout << j << "::" << tempT << endl;*/
  70.                 R_gripper2base.push_back(tempR);
  71.                 T_gripper2base.push_back(tempT);
  72.         }
  73.         //TSAI计算速度最快
  74.         calibrateHandEye(R_gripper2base, T_gripper2base, R_target2cam, T_target2cam, R_cam2gripper, T_cam2gripper, CALIB_HAND_EYE_TSAI);

  75.         Homo_cam2gripper = R_T2HomogeneousMatrix(R_cam2gripper, T_cam2gripper);
  76.         cout << Homo_cam2gripper << endl;
  77.         cout << "Homo_cam2gripper 是否包含旋转矩阵:" << isRotatedMatrix(Homo_cam2gripper) << endl;

  78.         FileStorage fs(".\\Homo_cam2gripper.xml", FileStorage::WRITE);
  79.         fs << "Homo_cam2gripper" << Homo_cam2gripper;
  80.         fs.release();

  81.         ///////////////////////////////////////////////////////////////////////////////////////////////////////////

  82.         /**************************************************
  83.         * @note   手眼系统精度测试,原理是标定板在机器人基坐标系中位姿固定不变,
  84.         *                  可以根据这一点进行演算
  85.         **************************************************/
  86.         //使用1,2组数据验证  标定板在机器人基坐标系中位姿固定不变
  87.         cout << "1 : " << Homo_gripper2base[0] * Homo_cam2gripper * Homo_target2cam[0] << endl;
  88.         cout << "2 : " << Homo_gripper2base[1] * Homo_cam2gripper * Homo_target2cam[1] << endl;
  89.         //标定板在相机中的位姿
  90.         cout << "3 : " << Homo_target2cam[1] << endl;
  91.         cout << "4 : " << Homo_cam2gripper.inv() * Homo_gripper2base[1].inv() * Homo_gripper2base[0] * Homo_cam2gripper * Homo_target2cam[0] << endl;

  92.         cout << "----手眼系统测试-----" << endl;
  93.         cout << "机械臂下标定板XYZ为:" << endl;
  94.         for (size_t i = 0; i < Homo_target2cam.size(); i++)
  95.         {
  96.                 Mat chessPos{ 0.0,0.0,0.0,1.0 };  //4*1矩阵,单独求机械臂坐标系下,标定板XYZ
  97.                 Mat worldPos = Homo_gripper2base[i] * Homo_cam2gripper * Homo_target2cam[i] * chessPos;
  98.                 cout << i << ": " << worldPos.t() << endl;
  99.         }
  100.         waitKey(0);

  101.         return 0;
  102. }

  103. /**************************************************
  104. * @brief   将旋转矩阵与平移向量合成为齐次矩阵
  105. * @note
  106. * @param   Mat& R   3*3旋转矩阵
  107. * @param   Mat& T   3*1平移矩阵
  108. * @return  Mat      4*4齐次矩阵
  109. **************************************************/
  110. Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T)
  111. {
  112.         Mat HomoMtr;
  113.         Mat_<double> R1 = (Mat_<double>(4, 3) <<
  114.                 R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
  115.                 R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
  116.                 R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
  117.                 0, 0, 0);
  118.         Mat_<double> T1 = (Mat_<double>(4, 1) <<
  119.                 T.at<double>(0, 0),
  120.                 T.at<double>(1, 0),
  121.                 T.at<double>(2, 0),
  122.                 1);
  123.         cv::hconcat(R1, T1, HomoMtr);                //矩阵拼接
  124.         return HomoMtr;
  125. }

  126. /**************************************************
  127. * @brief    齐次矩阵分解为旋转矩阵与平移矩阵
  128. * @note
  129. * @param        const Mat& HomoMtr  4*4齐次矩阵
  130. * @param        Mat& R              输出旋转矩阵
  131. * @param        Mat& T                                输出平移矩阵
  132. * @return
  133. **************************************************/
  134. void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T)
  135. {
  136.         //Mat R_HomoMtr = HomoMtr(Rect(0, 0, 3, 3)); //注意Rect取值
  137.         //Mat T_HomoMtr = HomoMtr(Rect(3, 0, 1, 3));
  138.         //R_HomoMtr.copyTo(R);
  139.         //T_HomoMtr.copyTo(T);
  140.         /*HomoMtr(Rect(0, 0, 3, 3)).copyTo(R);
  141.         HomoMtr(Rect(3, 0, 1, 3)).copyTo(T);*/
  142.         Rect R_rect(0, 0, 3, 3);
  143.         Rect T_rect(3, 0, 1, 3);
  144.         R = HomoMtr(R_rect);
  145.         T = HomoMtr(T_rect);

  146. }

  147. /**************************************************
  148. * @brief        检查是否是旋转矩阵
  149. * @note
  150. * @param
  151. * @param
  152. * @param
  153. * @return  true : 是旋转矩阵, false : 不是旋转矩阵
  154. **************************************************/
  155. bool isRotatedMatrix(Mat& R)                //旋转矩阵的转置矩阵是它的逆矩阵,逆矩阵 * 矩阵 = 单位矩阵
  156. {
  157.         Mat temp33 = R({ 0,0,3,3 });        //无论输入是几阶矩阵,均提取它的三阶矩阵
  158.         Mat Rt;
  159.         transpose(temp33, Rt);  //转置矩阵
  160.         Mat shouldBeIdentity = Rt * temp33;//是旋转矩阵则乘积为单位矩阵
  161.         Mat I = Mat::eye(3, 3, shouldBeIdentity.type());

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

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

  175.         eulerAngle /= (180 / CV_PI);                //度转弧度

  176.         Matx13d m(eulerAngle);                                //<double, 1, 3>

  177.         auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
  178.         auto rxs = sin(rx), rxc = cos(rx);
  179.         auto rys = sin(ry), ryc = cos(ry);
  180.         auto rzs = sin(rz), rzc = cos(rz);

  181.         //XYZ方向的旋转矩阵
  182.         Mat RotX = (Mat_<double>(3, 3) << 1, 0, 0,
  183.                 0, rxc, -rxs,
  184.                 0, rxs, rxc);
  185.         Mat RotY = (Mat_<double>(3, 3) << ryc, 0, rys,
  186.                 0, 1, 0,
  187.                 -rys, 0, ryc);
  188.         Mat RotZ = (Mat_<double>(3, 3) << rzc, -rzs, 0,
  189.                 rzs, rzc, 0,
  190.                 0, 0, 1);
  191.         //按顺序合成后的旋转矩阵
  192.         cv::Mat rotMat;

  193.         if (seq == "zyx") rotMat = RotX * RotY * RotZ;
  194.         else if (seq == "yzx") rotMat = RotX * RotZ * RotY;
  195.         else if (seq == "zxy") rotMat = RotY * RotX * RotZ;
  196.         else if (seq == "yxz") rotMat = RotZ * RotX * RotY;
  197.         else if (seq == "xyz") rotMat = RotZ * RotY * RotX;
  198.         else if (seq == "xzy") rotMat = RotY * RotZ * RotX;
  199.         else
  200.         {
  201.                 cout << "Euler Angle Sequence string is wrong...";
  202.         }
  203.         if (!isRotatedMatrix(rotMat))                //欧拉角特殊情况下会出现死锁
  204.         {
  205.                 cout << "Euler Angle convert to RotatedMatrix failed..." << endl;
  206.                 exit(-1);
  207.         }
  208.         return rotMat;
  209. }

  210. /**************************************************
  211. * @brief   将四元数转换为旋转矩阵
  212. * @note
  213. * @param   const Vec4d& q   归一化的四元数: q = q0 + q1 * i + q2 * j + q3 * k;
  214. * @return  返回3*3旋转矩阵R
  215. **************************************************/
  216. Mat quaternionToRotatedMatrix(const Vec4d& q)
  217. {
  218.         double q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];

  219.         double q0q0 = q0 * q0, q1q1 = q1 * q1, q2q2 = q2 * q2, q3q3 = q3 * q3;
  220.         double q0q1 = q0 * q1, q0q2 = q0 * q2, q0q3 = q0 * q3;
  221.         double q1q2 = q1 * q2, q1q3 = q1 * q3;
  222.         double q2q3 = q2 * q3;
  223.         //根据公式得来
  224.         Mat RotMtr = (Mat_<double>(3, 3) << (q0q0 + q1q1 - q2q2 - q3q3), 2 * (q1q2 + q0q3), 2 * (q1q3 - q0q2),
  225.                 2 * (q1q2 - q0q3), (q0q0 - q1q1 + q2q2 - q3q3), 2 * (q2q3 + q0q1),
  226.                 2 * (q1q3 + q0q2), 2 * (q2q3 - q0q1), (q0q0 - q1q1 - q2q2 + q3q3));
  227.         //这种形式等价
  228.         /*Mat RotMtr = (Mat_<double>(3, 3) << (1 - 2 * (q2q2 + q3q3)), 2 * (q1q2 - q0q3), 2 * (q1q3 + q0q2),
  229.                                                                                  2 * (q1q2 + q0q3), 1 - 2 * (q1q1 + q3q3), 2 * (q2q3 - q0q1),
  230.                                                                                  2 * (q1q3 - q0q2), 2 * (q2q3 + q0q1), (1 - 2 * (q1q1 + q2q2)));*/

  231.         return RotMtr;
  232. }

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

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

  247.         if (useQuaternion)
  248.         {
  249.                 Vec4d quaternionVec = m({ 3,0,4,1 });   //读取存储的四元数
  250.                 quaternionToRotatedMatrix(quaternionVec).copyTo(temp({ 0,0,3,3 }));
  251.         }
  252.         else
  253.         {
  254.                 Mat rotVec;
  255.                 if (m.total() == 6)
  256.                 {
  257.                         rotVec = m({ 3,0,3,1 });   //读取存储的欧拉角
  258.                 }
  259.                 if (m.total() == 10)
  260.                 {
  261.                         rotVec = m({ 7,0,3,1 });
  262.                 }
  263.                 //如果seq为空,表示传入的是3*1旋转向量,否则,传入的是欧拉角
  264.                 if (0 == seq.compare(""))
  265.                 {
  266.                         Rodrigues(rotVec, temp({ 0,0,3,3 }));   //罗德利斯转换
  267.                 }
  268.                 else
  269.                 {
  270.                         eulerAngleToRotateMatrix(rotVec, seq).copyTo(temp({ 0,0,3,3 }));
  271.                 }
  272.         }
  273.         //存入平移矩阵
  274.         temp({ 3,0,1,3 }) = m({ 0,0,3,1 }).t();
  275.         return temp;   //返回转换结束的齐次矩阵
  276. }
复制代码
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则