Opencv 九点标定
Opencv 3.4.2
项目位置:
F:\vision\Yolo\testProject\ExeComAcs\MFCTest MThread\MFCTest
代码实现:
#include "include/opencv2/video/tracking.hpp" void CMFCTestDlg::OnBnClickedBtn9pointcalib() { std::vector<Point2f> points_camera; std::vector<Point2f> points_robot; /*points_camera.push_back(Point2f(1372.36f, 869.00F)); points_camera.push_back(Point2f(1758.86f, 979.07f)); points_camera.push_back(Point2f(2145.75f, 1090.03f)); points_camera.push_back(Point2f(2040.02f, 1461.64f)); points_camera.push_back(Point2f(1935.01f, 1833.96f)); points_camera.push_back(Point2f(1546.79f, 1724.21f)); points_camera.push_back(Point2f(1158.53f, 1613.17f)); points_camera.push_back(Point2f(1265.07f, 1240.49f)); points_camera.push_back(Point2f(1652.6f, 1351.27f)); points_robot.push_back(Point2f(98.884f, 320.881f)); points_robot.push_back(Point2f(109.81f, 322.27f)); points_robot.push_back(Point2f(120.62f, 323.695f)); points_robot.push_back(Point2f(121.88f, 313.154f)); points_robot.push_back(Point2f(123.11f, 302.671f)); points_robot.push_back(Point2f(112.23f, 301.107f)); points_robot.push_back(Point2f(101.626f, 299.816f)); points_robot.push_back(Point2f(100.343f, 310.447f)); points_robot.push_back(Point2f(111.083f, 311.665f));*/ points_camera.push_back(Point2f(1516.14f,1119.48f)); points_camera.push_back(Point2f(1516.24f,967.751f)); points_camera.push_back(Point2f(1668.55f,967.29f)); points_camera.push_back(Point2f(1668.44f,1118.93f)); points_camera.push_back(Point2f(1668.54f,1270.57f)); points_camera.push_back(Point2f(1516.22f,1271.37f)); points_camera.push_back(Point2f(1364.21f,1272.15f)); points_camera.push_back(Point2f(1364.08f,1120.42f)); points_camera.push_back(Point2f(1364.08f,968.496f)); points_robot.push_back(Point2f(35.7173f,18.8248f)); points_robot.push_back(Point2f(40.7173f, 18.8248f)); points_robot.push_back(Point2f(40.7173f, 13.8248f)); points_robot.push_back(Point2f(35.7173f, 13.8248f)); points_robot.push_back(Point2f(30.7173f, 13.8248f)); points_robot.push_back(Point2f(30.7173f, 18.8248f)); points_robot.push_back(Point2f(30.7173f, 23.8248f)); points_robot.push_back(Point2f(35.7173f, 23.8248f)); points_robot.push_back(Point2f(40.7173f, 23.8248f)); /*for (int i = 0;i < 9;i++) { points_camera.push_back(Point2f(i+1, i+1)); points_robot.push_back(Point2f(2*(i + 1), 2*(i + 1))); }*/ //变换成 2*3 矩阵 Mat affine; estimateRigidTransform(points_camera, points_robot, true).convertTo(affine, CV_32F); if (affine.empty()) { affine = LocalAffineEstimate(points_camera, points_robot, true); } float A, B, C, D, E, F; A = affine.at<float>(0, 0); B = affine.at<float>(0, 1); C = affine.at<float>(0, 2); D = affine.at<float>(1, 0); E = affine.at<float>(1, 1); F = affine.at<float>(1, 2); //坐标转换 //Point2f src = Point2f(1652.6f, 1351.27f); //Point2f src = Point2f(100, 100); Point2f src = Point2f(1516.14f, 1119.48f); Point2f dst; dst.x = A * src.x + B * src.y + C; dst.y = D * src.x + E * src.y + F; //RMS 标定偏差 std::vector<Point2f> points_Calc; double sumX = 0, sumY = 0; for (int i = 0;i < points_camera.size();i++) { Point2f pt; pt.x = A * points_camera[i].x + B * points_camera[i].y + C; pt.y = D * points_camera[i].x + E * points_camera[i].y + F; points_Calc.push_back(pt); sumX += pow(points_robot[i].x - points_Calc[i].x,2); sumY += pow(points_robot[i].y - points_Calc[i].y, 2); } double rmsX, rmsY; rmsX = sqrt(sumX / points_camera.size()); rmsY = sqrt(sumY / points_camera.size()); CString str; str.Format("偏差值:%0.3f,%0.3f", rmsX, rmsY); AfxMessageBox(str); } Mat CMFCTestDlg::LocalAffineEstimate(const std::vector<Point2f>& shape1, const std::vector<Point2f>& shape2, bool fullAfine) { Mat out(2, 3, CV_32F); int siz = 2 * (int)shape1.size(); if (fullAfine) { Mat matM(siz, 6, CV_32F); Mat matP(siz, 1, CV_32F); int contPt = 0; for (int ii = 0; ii<siz; ii++) { Mat therow = Mat::zeros(1, 6, CV_32F); if (ii % 2 == 0) { therow.at<float>(0, 0) = shape1[contPt].x; therow.at<float>(0, 1) = shape1[contPt].y; therow.at<float>(0, 2) = 1; therow.row(0).copyTo(matM.row(ii)); matP.at<float>(ii, 0) = shape2[contPt].x; } else { therow.at<float>(0, 3) = shape1[contPt].x; therow.at<float>(0, 4) = shape1[contPt].y; therow.at<float>(0, 5) = 1; therow.row(0).copyTo(matM.row(ii)); matP.at<float>(ii, 0) = shape2[contPt].y; contPt++; } } Mat sol; solve(matM, matP, sol, DECOMP_SVD); out = sol.reshape(0, 2); } else { Mat matM(siz, 4, CV_32F); Mat matP(siz, 1, CV_32F); int contPt = 0; for (int ii = 0; ii<siz; ii++) { Mat therow = Mat::zeros(1, 4, CV_32F); if (ii % 2 == 0) { therow.at<float>(0, 0) = shape1[contPt].x; therow.at<float>(0, 1) = shape1[contPt].y; therow.at<float>(0, 2) = 1; therow.row(0).copyTo(matM.row(ii)); matP.at<float>(ii, 0) = shape2[contPt].x; } else { therow.at<float>(0, 0) = -shape1[contPt].y; therow.at<float>(0, 1) = shape1[contPt].x; therow.at<float>(0, 3) = 1; therow.row(0).copyTo(matM.row(ii)); matP.at<float>(ii, 0) = shape2[contPt].y; contPt++; } } Mat sol; solve(matM, matP, sol, DECOMP_SVD); out.at<float>(0, 0) = sol.at<float>(0, 0); out.at<float>(0, 1) = sol.at<float>(1, 0); out.at<float>(0, 2) = sol.at<float>(2, 0); out.at<float>(1, 0) = -sol.at<float>(1, 0); out.at<float>(1, 1) = sol.at<float>(0, 0); out.at<float>(1, 2) = sol.at<float>(3, 0); } return out; }
posted on 2020-08-03 13:24 strangeman 阅读(2665) 评论(1) 编辑 收藏 举报