简介
我们在做图像处理时,获取的是图像坐标系中的坐标。为了能让机器能正常工作,我们需要将图像坐标转换到机器坐标系中。我们可以采用九点手眼标定的方法,进行坐标转换。九点手眼标定的原理这里就不详细讲解了,大家请看《基于OpenCv的机器人手眼标定(九点标定法)》。
代码
这里我们已经打印了9个圆,并已知9个圆心的机器坐标。然后使用云钥工业相机实时采集图像,使用opencv找到九个圆心坐标,然后使用estimateAffine2D函数得到转换矩阵。
#include <stdio.h>
#include <string>
#include <opencv2/opencv.hpp>
#include "CKCameraInterface.h"
using namespace std;
using namespace cv;
int g_threshold = 40;
Mat g_transMat;
void camera_point_transform_to_real(Point2d cameraPoint, Mat &transMat, Point2d &realPoint)
{
realPoint.x = transMat.at<double>(0, 0) * cameraPoint.x + transMat.at<double>(0, 1) * cameraPoint.y + transMat.at<double>(0, 2);
realPoint.y = transMat.at<double>(1, 0) * cameraPoint.x + transMat.at<double>(1, 1) * cameraPoint.y + transMat.at<double>(1, 2);
}
void ResizeMat(const cv::Mat &srcMat, cv::Mat &dstMat, int cols = 800)
{
int rows = cols * srcMat.rows / srcMat.cols;
cv::Size dstSize(cols, rows);
dstMat.create(rows, cols, srcMat.type());
cv::resize(srcMat, dstMat, dstSize);
}
int calibrate_by_nine_points(Mat &image, vector<cv::Point2f> calibrationPoints, Mat &output)
{
Mat binaryImage;
vector<vector<cv::Point>> contours;
vector<cv::Vec4i> hierarchy;
threshold(image, binaryImage, g_threshold, 255, THRESH_BINARY);
findContours(binaryImage, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
int index = 0;
vector<cv::Point2f> cameraCircleCenter;
int i = 1;
Mat rgbImage;
cv::cvtColor(binaryImage, rgbImage, COLOR_GRAY2BGR);
for (; index >= 0; index = hierarchy[index][0])
{
if (contourArea(contours[index]) < 3000)
{
continue;
}
if (contours[index].size() > 5)
{
cv::Mat pointsf;
cv::Mat(contours[index]).convertTo(pointsf, CV_32F);
cv::RotatedRect circle = cv::fitEllipse(pointsf);
double biteOfWH = circle.size.width / circle.size.height;
double biteOfArea = contourArea(contours[index]) / (circle.size.width * circle.size.height);
if (abs(biteOfWH - 1) < 0.1 && abs(biteOfArea - 3.1415926 / 4) < 0.05)
{
cameraCircleCenter.push_back(circle.center);
i++;
//绘制轮廓
for(size_t i = 0; i < contours[index].size(); i++)
{
cv::Point point1 = contours[index][i % contours[index].size()];
cv::Point point2 = contours[index][i % contours[index].size()];
cv::line(rgbImage, point1, point2, Scalar(0, 0, 255), 3);
}
//绘制圆心
cv::Point point1 = cv::Point((int)round(circle.center.x) - 10, (int)round(circle.center.y));
cv::Point point2 = cv::Point((int)round(circle.center.x) + 10, (int)round(circle.center.y));
cv::line(rgbImage, point1, point2, Scalar(0, 255, 0), 3);
point1 = cv::Point((int)round(circle.center.x), (int)round(circle.center.y - 10));
point2 = cv::Point((int)round(circle.center.x), (int)round(circle.center.y + 10));
cv::line(rgbImage, point1, point2, Scalar(0, 255, 0), 3);
}
}
}
image = rgbImage;
if (cameraCircleCenter.size() != 9)
return -1;
//输入的机器坐标是从左到右从上到下排列的。因为得到的9个圆点的图像坐标可能不是从左向右,从上向下排列,所以我们对cameraCircleCenter中的坐标从左向右,从上向下排列。
//先对上下列进行排序
std::sort(cameraCircleCenter.begin(), cameraCircleCenter.end(), [](cv::Point2f &point1, cv::Point2f &point2) {
return point1.y < point2.y;
});
//再分别对每行进行排序
std::sort(cameraCircleCenter.begin(), cameraCircleCenter.begin() + 3, [](cv::Point2f &point1, cv::Point2f &point2) {
return point1.x < point2.x;
});
std::sort(cameraCircleCenter.begin() + 3, cameraCircleCenter.begin() + 6, [](cv::Point2f &point1, cv::Point2f &point2) {
return point1.x < point2.x;
});
std::sort(cameraCircleCenter.begin() + 6, cameraCircleCenter.end(), [](cv::Point2f &point1, cv::Point2f &point2) {
return point1.x < point2.x;
});
for (auto point : cameraCircleCenter)
printf("(%lf, %lf)\n", point.x, point.y);
output = cv::estimateAffine2D(cameraCircleCenter, calibrationPoints);
if (output.empty())
return -1;
return 0;
}
void process_ckcamera()
{
HANDLE hCamera;
int cameraNum;
//查找相机
int ret = CameraEnumerateDevice(&cameraNum);
if (ret < 0 || cameraNum == 0)
return;
//打开相机
ret = CameraInit(&hCamera, 0);
if (ret != CAMERA_STATUS_SUCCESS)
return;
//设置相机输出灰度图
CameraSetIspOutFormat(hCamera, CAMERA_MEDIA_TYPE_MONO8);
cv::namedWindow("image");
cv::createTrackbar("threshold", "image", &g_threshold, 255);
//已经测量的9个圆心的机器坐标
vector<cv::Point2f> calibrationPoints;
calibrationPoints.push_back(cv::Point2f(-20, 20));
calibrationPoints.push_back(cv::Point2f(0, 20));
calibrationPoints.push_back(cv::Point2f(20, 20));
calibrationPoints.push_back(cv::Point2f(-20, 0));
calibrationPoints.push_back(cv::Point2f(0, 0));
calibrationPoints.push_back(cv::Point2f(20, 0));
calibrationPoints.push_back(cv::Point2f(-20, 20));
calibrationPoints.push_back(cv::Point2f(0, 20));
calibrationPoints.push_back(cv::Point2f(20, 20));
//相机开始取图
CameraPlay(hCamera);
while (true)
{
stImageInfo imgInfo;
//获取图像
unsigned char *pbuff = CameraGetImageBufferEx(hCamera, &imgInfo, 1000);
if (pbuff == nullptr)
continue;
cv::Mat image(imgInfo.iHeight, imgInfo.iWidth, CV_8UC1, pbuff);
if (image.empty())
continue;
//进行九点标点
int ret = calibrate_by_nine_points(image, calibrationPoints, g_transMat);
cv::Mat dstImage;
ResizeMat(image, dstImage, 1024);
cv::imshow("image", dstImage);
int key = cv::waitKey(10);
if(key == 'y' && ret == 0)
{
printf("calibrate parameter: %lf,%lf,%lf,%lf,%lf,%lf\n", g_transMat.at<double>(0, 0), g_transMat.at<double>(0, 1),
g_transMat.at<double>(0, 2), g_transMat.at<double>(1, 0),
g_transMat.at<double>(1, 1), g_transMat.at<double>(1, 2));
break;
}
}
//相机结束取图
CameraPause(hCamera);
//关闭相机
CameraUnInit(hCamera);
}
int main(int argc, char *argv[])
{
process_ckcamera();
if(!g_transMat.empty())
{
Point2d cameraPoint(100, 100);
Point2d realPoint;
camera_point_transform_to_real(cameraPoint, g_transMat, realPoint);
printf("real point: (%lf, %lf)\n", realPoint.x, realPoint.y);
}
getchar();
return 0;
}
程序运行界面
拖动trackbar直到9个圆心都绘制出来。
欢迎大家使用云钥科技的相机,有USB2.0、USB3.0、GIGE网络工 业相机,支持软硬件定制开发