工业机器人手眼标定-九点标定法的C++实现

0,，九点标定法的具体实现⽅法参见，https://cloud.tencent.com/developer/article/1835302，本⽂只接受取到数据后的处理⽅法1，我是将两个数据存放在两个txt⽂件内，CameraPos.txt存放是的相机坐标，RobotPos存在的是对应的机器⼈坐标2，定义⼀个结构体存储标定后结果，定义两个vector存储读取到的点坐标

public :

struct CalResult {

double A_x; double B_x; double C_x; double A_y; double B_y; double C_y; }myCalResult;public:

vector points_camera; vector points_robot;

3，读取两个txt⾥的值，分别保存到两个vector

char path[256];

GetModuleFileNameA(NULL, path, 256); string filePath = path;

filePath=filePath.substr(0, filePath.rfind('\\\\'));

filePath = filePath + \"\\\\\"+ \"CalData\" + \"\\\\\" + \"CameraPos.txt\"; ifstream cameraFile;

cameraFile.open(filePath); assert(cameraFile.is_open()); cv::Point2d temp;

while (cameraFile.good() && !cameraFile.eof()) {

cameraFile >> temp.x >> temp.y; points_camera.push_back(temp); }

filePath = filePath.substr(0, filePath.rfind('\\\\')); filePath = filePath + \"\\\\\" + \"RobotPos.txt\"; ifstream robotFile;

robotFile.open(filePath); assert(robotFile.is_open());

while (robotFile.good() && !robotFile.eof()) {

robotFile >> temp.x >> temp.y; points_robot.push_back(temp); }

4，实现计算的函数

void getCalResult(vector points_camera, vector points_robot, CalResult a){

if (points_camera.size()!= calPointCount || points_robot.size()!= calPointCount) {

::MessageBox(NULL,TEXT(\"⼿眼标定错误\"),TEXT(\"错误\"),1); return; }

cv::Mat dst = cv::Mat(3, 3, CV_32F, cv::Scalar(0));//初始化系数矩阵A cv::Mat out_x = cv::Mat(3, 1, CV_32F, cv::Scalar(0));//初始化矩阵b cv::Mat out_y = cv::Mat(3, 1, CV_32F, cv::Scalar(0));//初始化矩阵b for (int i = 0; i < points_camera.size(); i++) {

//计算3*3的系数矩阵

dst.at(0, 0) = dst.at(0, 0) + pow(points_camera[i].x, 2);

dst.at(0, 1) = dst.at(0, 1) + points_camera[i].x*points_camera[i].y; dst.at(0, 2) = dst.at(0, 2) + points_camera[i].x;

dst.at(1, 0) = dst.at(1, 0) + points_camera[i].x*points_camera[i].y; dst.at(1, 1) = dst.at(1, 1) + pow(points_camera[i].y, 2); dst.at(1, 2) = dst.at(1, 2) + points_camera[i].y; dst.at(2, 0) = dst.at(2, 0) + points_camera[i].x; dst.at(2, 1) = dst.at(2, 1) + points_camera[i].y; dst.at(2, 2) = points_camera.size(); //x计算3*1的结果矩阵

out_x.at(0, 0) = out_x.at(0, 0) + points_camera[i].x*points_robot[i].x; out_x.at(1, 0) = out_x.at(1, 0) + points_camera[i].y*points_robot[i].x; out_x.at(2, 0) = out_x.at(2, 0) + points_robot[i].x; //y计算3*1的结果矩阵

out_y.at(0, 0) = out_y.at(0, 0) + points_camera[i].x*points_robot[i].y; out_y.at(1, 0) = out_y.at(1, 0) + points_camera[i].y*points_robot[i].y; out_y.at(2, 0) = out_y.at(2, 0) + points_robot[i].y;

}

//判断矩阵是否奇异

double determ = determinant(dst); if (abs(determ) < 0.001) {

::MessageBox(NULL, TEXT(\"X标定求解奇异\"), TEXT(\"错误\"), 1); return; }

cv::Mat inv;

cv::invert(dst, inv);//求矩阵的逆

cv::Mat output = inv * out_x;//计算输出

//X坐标计算结果,robotX=A_x*Camera_X+B_x*Camera_Y+C_x a.A_x = output.at(0, 0); a.B_x = output.at(1, 0); a.C_x = output.at(2, 0);

output = inv * out_y;//计算输出

//Y坐标计算结果,robotY=A_y*Camera_X+B_y*Camera_Y+C_y a.A_y = output.at(0, 0); a.B_y = output.at(1, 0); a.C_y = output.at(2, 0);}

6 计算结果验证 https://zhuanlan.zhihu.com/p/3919387

7 算法参考:https://blog.csdn.net/AlonewaitingNew/article/details/95217730