使用云钥相机进行九点手眼标定

简介

我们在做图像处理时，获取的是图像坐标系中的坐标。为了能让机器能正常工作，我们需要将图像坐标转换到机器坐标系中。我们可以采用九点手眼标定的方法，进行坐标转换。九点手眼标定的原理这里就不详细讲解了，大家请看《基于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网络工 业相机，支持软硬件定制开发