特征检测+SIFT点匹配+PNP位姿确定

SIFT特征检测+FLANN点匹配+PNP位姿确定

软件环境

1. windows 10

2. vs2013

3. opencv3.1.0+opencv3.1.0 contrib

基本原理

​ 1.SIFT 特征点检测+匹配

​ 2. PNP位姿确定

代码实现

1. 参考博客

• sif+flann解决匹配问题
• solvepnp解决位姿估计

2. 修改

• 首先是更换库，3.x以上版本的SIFT算法detect和compute的调用和opencv2.x版本的不太一样，使用时如果包含库或者报错记得及时修改
• 使用的匹配算法和剔除算法都可以更改，相应的实例有很多
• 能力有限，findHomography已经能返回一个单应性矩阵，如果加以利用理论上也可以恢复位姿？但是我不太会用，所以把perspectiveTransform获得的四个目标像素角点重新输入了olvePnP解算相机和检测平面间的平移旋转
• 这个思路和用aruco或者二维码码块检测本质上是一致的，都是匹配检测获得四个角点然后用P3P解决位姿估计问题，差别是如果可以获得任意一个物体的正面图，我们就能实现对这个物体初步的检测和估计，而摆脱了对标记的需求。

3. 源码:

#include "stdafx.h"
#include <ctime>
#include <iostream>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp> 
#include<opencv2/core/core.hpp>
#include<opencv_modules.hpp>
#include <opencv2/xfeatures2d.hpp>	//包含 SiftFeatureDetector 的头文件
#include "opencv2/features2d/features2d.hpp"	//包含 FlannBasedMatcher 的头文件
#include "opencv2/calib3d/calib3d.hpp"			//包含 findHomography 的头文件

using namespace cv;
using namespace std;
using namespace cv::xfeatures2d;

vector<Vec3d> usePnP(vector<Point2f> scenePoints);


int main()
{
	
	//读图，匹配对象，待搜寻图
	cv::Mat imgObject = cv::imread("D:/Celeste/VS Code/SiftandFlann/101202.jpg", CV_LOAD_IMAGE_GRAYSCALE);
	cv::Mat imgScene = cv::imread("D:/Celeste/VS Code/SiftandFlann/101203.jpg", CV_LOAD_IMAGE_GRAYSCALE);

	if (imgObject.empty() || imgScene.empty())
	{
		std::cout << " --(!) Error reading images " << std::endl;
		return -1;
	}

	double begin = clock();


	Ptr<Feature2D> f2d = xfeatures2d::SIFT::create();
	///-- Step 1: 使用SIFT算子检测特征点
	//cv::SiftFeatureDetector detector;//( minHessian );
	//opencv3之后都使用对象指针
	std::vector<cv::KeyPoint> keypointsObject, keypointsScene;
	f2d->detect(imgObject, keypointsObject);
	f2d->detect(imgScene, keypointsScene);
	std::cout << "object--number of keypoints: " << keypointsObject.size() << std::endl;
	std::cout << "scene--number of keypoints: " << keypointsScene.size() << std::endl;

	///-- Step 2: 使用SIFT算子提取特征（计算特征向量）
	//cv::SiftDescriptorExtractor extractor;
	cv::Mat descriptorsObject, descriptorsScene;
	f2d->compute(imgObject, keypointsObject, descriptorsObject);
	f2d->compute(imgScene, keypointsScene, descriptorsScene);

	///-- Step 3: 使用FLANN法进行匹配
	cv::FlannBasedMatcher matcher;
	std::vector< cv::DMatch > allMatches;
	matcher.match(descriptorsObject, descriptorsScene, allMatches);
	std::cout << "number of matches before filtering: " << allMatches.size() << std::endl;

	//-- 计算关键点间的最大最小距离
	double maxDist = 0;
	double minDist = 100;
	for (int i = 0; i < descriptorsObject.rows; i++)
	{
		double dist = allMatches[i].distance;
		if (dist < minDist)
			minDist = dist;
		if (dist > maxDist)
			maxDist = dist;
	}
	printf("	max dist : %f \n", maxDist);
	printf("	min dist : %f \n", minDist);

	//-- 过滤匹配点，保留好的匹配点（这里采用的标准：distance<3*minDist）
	//也可以用RANSAC剔除
	std::vector< cv::DMatch > goodMatches;
	for (int i = 0; i < descriptorsObject.rows; i++)
	{
		if (allMatches[i].distance < 2 * minDist)
			goodMatches.push_back(allMatches[i]);
	}
	std::cout << "number of matches after filtering: " << goodMatches.size() << std::endl;

	//-- 显示匹配结果
	cv::Mat resultImg;
	drawMatches(imgObject, keypointsObject, imgScene, keypointsScene,
		goodMatches, resultImg, cv::Scalar::all(-1), cv::Scalar::all(-1), std::vector<char>(),
		cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS //不显示未匹配的点
		);
	//-- 输出匹配点的对应关系
	//for (size_t i = 0; i < goodMatches.size(); i++)
		//printf("	good match %d: keypointsObject [%d]  -- keypointsScene [%d]\n", i, goodMatches[i].queryIdx, goodMatches[i].trainIdx);

	///-- Step 4: 使用findHomography找出相应的透视变换
	//object和scene的赋值循环可以放到goodmatch剔点中直接写
	std::vector<cv::Point2f> object;
	std::vector<cv::Point2f> scene;
	for (size_t i = 0; i < goodMatches.size(); i++)
	{
		//-- 从好的匹配中获取关键点: 匹配关系是关键点间具有的一 一对应关系，可以从匹配关系获得关键点的索引
		//-- e.g. 这里的goodMatches[i].queryIdx和goodMatches[i].trainIdx是匹配中一对关键点的索引
		object.push_back(keypointsObject[goodMatches[i].queryIdx].pt);
		scene.push_back(keypointsScene[goodMatches[i].trainIdx].pt);
	}
	cv::Mat H = findHomography(object, scene, CV_RANSAC);
	//返回的H矩阵就是两个平面变换的单应性矩阵

	///-- Step 5: 使用perspectiveTransform映射点群，在场景中获取目标位置
	std::vector<cv::Point2f> objCorners(4);
	objCorners[0] = cvPoint(0, 0);
	objCorners[1] = cvPoint(imgObject.cols, 0);
	objCorners[2] = cvPoint(imgObject.cols, imgObject.rows);
	objCorners[3] = cvPoint(0, imgObject.rows);
	std::vector<cv::Point2f> sceneCorners(4);//目标的像素坐标
	cv::perspectiveTransform(objCorners, sceneCorners, H);
	

	//-- 在被检测到的目标四个角之间划线（左上第一个点，顺时针）
	//sceneCorners和像素点位置之间的关系,差一个参考图列数
	line(resultImg, sceneCorners[0] + cv::Point2f(imgObject.cols, 0), sceneCorners[1] + cv::Point2f(imgObject.cols, 0), cv::Scalar(0, 0, 0), 4);
	line(resultImg, sceneCorners[1] + cv::Point2f(imgObject.cols, 0), sceneCorners[2] + cv::Point2f(imgObject.cols, 0), cv::Scalar(0, 0, 0), 4);
	line(resultImg, sceneCorners[2] + cv::Point2f(imgObject.cols, 0), sceneCorners[3] + cv::Point2f(imgObject.cols, 0), cv::Scalar(0, 255, 0), 4);
	line(resultImg, sceneCorners[3] + cv::Point2f(imgObject.cols, 0), sceneCorners[0] + cv::Point2f(imgObject.cols, 0), cv::Scalar(255, 255, 255), 4);

	//-- 显示检测结果
	cv::imshow("detection result", resultImg);
	vector<int> compression_params;
	compression_params.push_back(IMWRITE_PNG_COMPRESSION);
	compression_params.push_back(3);
	//存图
	imwrite("D:/Celeste/VS Code/SiftandFlann/good.png", resultImg, compression_params);
	double end = clock();
	std::cout << "\nSIFT--elapsed time: " << (end - begin) / CLOCKS_PER_SEC * 1000 << " ms\n";

	//--计算位置姿态
	Vec3d rvec, tvec;	
	vector<Vec3d> res(2);
	//rvec是旋转向量
	//tvec是平移向量
	res=usePnP(sceneCorners);
	rvec = res[0];
	tvec = res[1];	
	cout << "rvec:" <<rvec<< endl;
	cout << "tvec" << tvec << endl;

	//后续计算矩阵转换关系、获得目标对象的世界坐标系

	cv::waitKey(0);
	return 0;

}

//使用pnp求解位置，输入是四个顶点的像素坐标，输出是旋转平移向量
vector<Vec3d> usePnP(vector<Point2f> scenePoints)
{
	//参考物体在实际空间中的位置，以参考物体左上角为原点，顺时针方向，point3d用mm单位真实数据，pnp解得得R\T就是相机相对参考物体的位姿旋转
	//根据实际物体修改参数
	vector<Point3f> objectPoints(4);
	objectPoints[0] = CvPoint3D32f(0, 0, 0.0f);
	objectPoints[1] = CvPoint3D32f(230.0f, 0.0f, 0.0f);
	objectPoints[2] = CvPoint3D32f(230.0f, 289.0f, 0.0f);
	objectPoints[3] = CvPoint3D32f(0.0f, 289.0f, 0.0f);
	//相机内参、畸变矩阵重新填写
	cv::Mat cameraMatrix, distCoeffs;
	cameraMatrix = (Mat_<double>(3, 3) << 598.29493, 0, 304.76898, 0, 597.56086, 233.34673, 0, 0, 1);
	distCoeffs = (Mat_<double>(5, 1) << -0.53572, 1.35993, -0.00244, 0.00620, 0.00000);
	//参数返回值
	Vec3d rvec, tvec;
	//计算rvec\tvec;采用p3p模式
	bool IsSolve=cv::solvePnP(objectPoints, scenePoints, cameraMatrix, distCoeffs, rvec, tvec, false, CV_P3P);
	vector<Vec3d> res(2);
	if (IsSolve)
	{
		cout << "solvePnP successfully！" << endl;
		res[0] = rvec;
	    res[1] = tvec;

	}
	else
	{
		cout << " solvePnP unsuccessfully……check your data again" << endl;
		res[0] = Vec3d(0, 0, 0);
		res[1] = Vec3d(0, 0, 0);
	}
	return res;

}

4. 效果演示