由于项目需要,我之前做了一个基于圆形图案的标记识别,其目的是实现单目相机对标记物的识别和定位。标记如下所示:
这个东西有四个圆形组成,打印出来后,测量其尺寸,就可以通过识别其图像位置,再使用P4P解算,就可以得到该平面相对于相机的三维方向和倾角。
首先是识别:
ima = ardrone.getImage();
SimpleBlobDetector::Params params;
params.minThreshold = 40;
params.maxThreshold = 160;
params.thresholdStep = 5;
params.minArea = 100;
params.minConvexity = 0.7f;
params.minInertiaRatio = 0.8f;
params.maxArea = 8000;
params.filterByCircularity = true;
params.minCircularity = 0.5;
params.maxCircularity = 1.5;
params.maxInertiaRatio = 1;
params.maxConvexity = 1;
cv::Ptr<cv::SimpleBlobDetector> detector = cv::SimpleBlobDetector::create(params);
cvtColor(ima, gray, CV_BGR2GRAY); // smooth it, otherwise a lot of false circles may be detected
//normalize(gray, gray, 0, 255, 32);
//cout << gray.rows <<' '<<gray.cols<< endl;
std::vector<KeyPoint> keypoints;
if (keypoints.size() != pointnum)continue;
int fail = 0;
for (int i = 1; i < pointnum; i++)
{
if (abs(keypoints[0].size - keypoints[i].size) / keypoints[0].size > 0.3)fail = 1;
}
if (fail == 1)continue;
vector<vector<int>>dis;
for (int i = 0; i < pointnum; i++)
{
vector<int> temp;
for (int j = 0; j < pointnum; j++)
{
if (j == i)continue;
temp.push_back(GetDis(keypoints[i].pt, keypoints[j].pt));
}
dis.push_back(temp);
}
int pa = -1, pb = -1, pc = -1, pd = -1;
for (int i = 0; i < pointnum; i++)
{
int a = dis[i][0] - dis[i][1];
int b = dis[i][2] - dis[i][1];
int c = dis[i][0] - dis[i][2];
if ((float)abs(a) / (float)dis[i][0] < 0.2 && (float)abs(c) / (float)dis[i][0] < 0.2)
pa = i;
else if ((float)abs(a) / (float)dis[i][0] <0.2 && (float)abs(c) / (float)dis[i][0] >0.5&&c<0)
pb = i;
else if ((float)abs(c) / (float)dis[i][0] <0.2 && (float)abs(a) / (float)dis[i][0] >0.5&&a<0)
pb = i;
else if ((float)abs(c) / (float)dis[i][0] > 0.5 && (float)abs(a) / (float)dis[i][0] > 0.5 && (float)abs(a) / (float)dis[i][1] < 0.2)
pb = i;
else if ((float)dis[i][1] / (float)dis[i][0] <0.8 && (float)dis[i][2] / (float)dis[i][0] <0.8 && (float)abs(b) / (float)dis[i][1] <0.2)
pb = i;
}
if (pa < 0 || pb < 0)continue;
drawArrow(ima, keypoints[pa].pt, keypoints[pb].pt, 10, 60, Scalar(0, 0, 255));
putText(ima, "0", keypoints[pa].pt, 0, 1.0f, CV_RGB(255, 0, 0), 2);
putText(ima, "1", keypoints[pb].pt, 0, 1.0f, CV_RGB(255, 0, 0), 2);
detector->detect(gray, keypoints);
这里使用了OPRENCV自带的斑点检测法进行检验,设定参数后,得到比较准确的斑点。
然后对斑点的相对位置进行检验,得到点的相对位置和整个图案的指向方向。
if (keypoints.size() != pointnum)continue;
int fail = 0;
for (int i = 1; i < pointnum; i++)
{
if (abs(keypoints[0].size - keypoints[i].size) / keypoints[0].size > 0.3)fail = 1;
}
if (fail == 1)continue;
vector<vector<int>>dis;
for (int i = 0; i < pointnum; i++)
{
vector<int> temp;
for (int j = 0; j < pointnum; j++)
{
if (j == i)continue;
temp.push_back(GetDis(keypoints[i].pt, keypoints[j].pt));
}
dis.push_back(temp);
}
int pa = -1, pb = -1, pc = -1, pd = -1;
for (int i = 0; i < pointnum; i++)
{
int a = dis[i][0] - dis[i][1];
int b = dis[i][2] - dis[i][1];
int c = dis[i][0] - dis[i][2];
if ((float)abs(a) / (float)dis[i][0] < 0.2 && (float)abs(c) / (float)dis[i][0] < 0.2)
pa = i;
else if ((float)abs(a) / (float)dis[i][0] <0.2 && (float)abs(c) / (float)dis[i][0] >0.5&&c<0)
pb = i;
else if ((float)abs(c) / (float)dis[i][0] <0.2 && (float)abs(a) / (float)dis[i][0] >0.5&&a<0)
pb = i;
else if ((float)abs(c) / (float)dis[i][0] > 0.5 && (float)abs(a) / (float)dis[i][0] > 0.5 && (float)abs(a) / (float)dis[i][1] < 0.2)
pb = i;
else if ((float)dis[i][1] / (float)dis[i][0] <0.8 && (float)dis[i][2] / (float)dis[i][0] <0.8 && (float)abs(b) / (float)dis[i][1] <0.2)
pb = i;
}
if (pa < 0 || pb < 0)continue;
drawArrow(ima, keypoints[pa].pt, keypoints[pb].pt, 10, 60, Scalar(0, 0, 255));
putText(ima, "0", keypoints[pa].pt, 0, 1.0f, CV_RGB(255, 0, 0), 2);
putText(ima, "1", keypoints[pb].pt, 0, 1.0f, CV_RGB(255, 0, 0), 2);
然后就是计算距离了,先构造四个点的相机坐标系坐标,然后在图上找到四个点的像素坐标,然后带入solvePnP计算,就可以得到一个平移和旋转矩阵,从平移和旋转矩阵中就可以计算出该平面相对与相机的位置了。代码如下:
if (DIS)
{
for (int i = 0; i < pointnum; i++)
{
if (i != pa&&i != pb)
if (LinePoint(keypoints[pa].pt, keypoints[pb].pt, keypoints[i].pt) > 0)
pc = i;
else pd = i;
}
if (pc < 0 || pd < 0)continue;
putText(ima, "2", keypoints[pc].pt, 0, 1.0f, CV_RGB(255, 0, 0), 2);
putText(ima, "3", keypoints[pd].pt, 0, 1.0f, CV_RGB(255, 0, 0), 2);
vector<Point2f> centers;
centers.push_back(keypoints[pa].pt);
centers.push_back(keypoints[pb].pt);
centers.push_back(keypoints[pc].pt);
centers.push_back(keypoints[pd].pt);
vector<Point2f> tempshape;
tempshape.push_back(keypoints[pa].pt);
tempshape.push_back(keypoints[pc].pt);
tempshape.push_back(keypoints[pb].pt);
tempshape.push_back(keypoints[pd].pt);
double matchresult = matchShapes(myshape, tempshape, 1, 0);
if (matchresult > 0.5)
{
cout << "Shape not match:" << matchresult << endl;
continue;
}
solvePnP(realpos, centers, cameraMatrix, distCoeffs, rvecs, tvecs);
Rodrigues(rvecs, R);
Ang_X = asin(R.at<double>(1, 0) / cos(asin(-R.at<double>(2, 0)))) / pi * 180;
Ang_Y = asin(-R.at<double>(2, 0)) / pi * 180;
Ang_Z = asin(R.at<double>(2, 1) / cos(asin(-R.at<double>(2, 0)))) / pi * 180;
X = R.at<double>(0, 0) *realpos[0].x + R.at<double>(0, 1) * realpos[0].y + R.at<double>(0, 2) * realpos[0].z + tvecs.at<double>(0, 0);
Y = R.at<double>(1, 0) *realpos[0].x + R.at<double>(1, 1) * realpos[0].y + R.at<double>(1, 2) * realpos[0].z + tvecs.at<double>(1, 0);
Z = R.at<double>(2, 0) *realpos[0].x + R.at<double>(2, 1) * realpos[0].y + R.at<double>(2, 2) * realpos[0].z + tvecs.at<double>(2, 0);
putText(ima, "X:" + to_string(X), { 1, 50 }, 0, 1.0f, CV_RGB(255, 0, 0), 2);
putText(ima, "Y:" + to_string(Y), { 1, 150 }, 0, 1.0f, CV_RGB(0, 255, 0), 2);
putText(ima, "Z:" + to_string(Z), { 1, 250 }, 0, 1.0f, CV_RGB(0, 0, 255), 2);
putText(ima, "Ang_X:" + to_string(Ang_X), { 300, 50 }, 0, 1.0f, CV_RGB(255, 0, 0), 2);
putText(ima, "Ang_Y:" + to_string(Ang_Y), { 300, 150 }, 0, 1.0f, CV_RGB(0, 255, 0), 2);
putText(ima, "Ang_Z:" + to_string(Ang_Z), { 300, 250 }, 0, 1.0f, CV_RGB(0, 0, 255), 2);
}
这个代码是应用在了一个无人机上,所以图像获取部分等其他代码就不放了,但是核心原理都在上面了。
再放一下识别结果: