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<cv::Point2f> points_camera;
vector<cv::Point2f> 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<cv::Point2f> points_camera, vector<cv::Point2f> 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<float>(0, 0) = dst.at<float>(0, 0) + pow(points_camera[i].x, 2);
dst.at<float>(0, 1) = dst.at<float>(0, 1) + points_camera[i].x*points_camera[i].y;
dst.at<float>(0, 2) = dst.at<float>(0, 2) + points_camera[i].x;
dst.at<float>(1, 0) = dst.at<float>(1, 0) + points_camera[i].x*points_camera[i].y;
dst.at<float>(1, 1) = dst.at<float>(1, 1) + pow(points_camera[i].y, 2);
dst.at<float>(1, 2) = dst.at<float>(1, 2) + points_camera[i].y;
dst.at<float>(2, 0) = dst.at<float>(2, 0) + points_camera[i].x;
dst.at<float>(2, 1) = dst.at<float>(2, 1) + points_camera[i].y;
dst.at<float>(2, 2) = points_camera.size();
//x计算3*1的结果矩阵
out_x.at<float>(0, 0) = out_x.at<float>(0, 0) + points_camera[i].x*points_robot[i].x;
out_x.at<float>(1, 0) = out_x.at<float>(1, 0) + points_camera[i].y*points_robot[i].x;
out_x.at<float>(2, 0) = out_x.at<float>(2, 0) + points_robot[i].x;
//y计算3*1的结果矩阵
out_y.at<float>(0, 0) = out_y.at<float>(0, 0) + points_camera[i].x*points_robot[i].y;
out_y.at<float>(1, 0) = out_y.at<float>(1, 0) + points_camera[i].y*points_robot[i].y;
out_y.at<float>(2, 0) = out_y.at<float>(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<float>(0, 0);
a.B_x = output.at<float>(1, 0);
a.C_x = output.at<float>(2, 0);
output = inv * out_y;//计算输出
//Y坐标计算结果,robotY=A_y*Camera_X+B_y*Camera_Y+C_y
a.A_y = output.at<float>(0, 0);
a.B_y = output.at<float>(1, 0);
a.C_y = output.at<float>(2, 0);
}
6 计算结果验证 https://zhuanlan.zhihu.com/p/391938754
7 算法参考:https://blog.csdn.net/AlonewaitingNew/article/details/95217730
原文链接: https://www.cnblogs.com/ysc2021/p/15393419.html
欢迎关注
微信关注下方公众号,第一时间获取干货硬货;公众号内回复【pdf】免费获取数百本计算机经典书籍
原创文章受到原创版权保护。转载请注明出处:https://www.ccppcoding.com/archives/214289
非原创文章文中已经注明原地址,如有侵权,联系删除
关注公众号【高性能架构探索】,第一时间获取最新文章
转载文章受原作者版权保护。转载请注明原作者出处!