相机位姿估计1_1:OpenCV:solvePnP二次封装与性能测试

发布时间:2017-5-28 20:39:28 编辑:www.fx114.net 分享查询网我要评论
本篇文章主要介绍了"相机位姿估计1_1:OpenCV:solvePnP二次封装与性能测试 ",主要涉及到相机位姿估计1_1:OpenCV:solvePnP二次封装与性能测试 方面的内容,对于相机位姿估计1_1:OpenCV:solvePnP二次封装与性能测试 感兴趣的同学可以参考一下。

关键词:OpenCV::solvePnP

文章类型:方法封装、测试

@Author:VShawn([email protected])

@Date:2016-11-27

@Lab: [email protected]

前言

今天给大家带来的是一篇关于程序功能、性能测试的文章,读过《相机位姿估计1:根据四个特征点估计相机姿态》一文的同学应该会发现,直接使用OpenCV的solvePnP来估计相机位姿,在程序调用上相当麻烦,从一开始的参数设定到最后将计算出的矩阵转化为相机的位姿参数,需要花费近两百行代码。因此为了更方便地调用程序,今天我就给大家带来一个我自己对solvePnP的封装类PNPSolver,顺便将OpenCV自带的三种求解方法测试一遍。

类的封装

封装的思路我就不写了,由于博客更新速度赶不上我写程序的速度,现在发上来的类已经修改过好几次了,思路也换了几次。不过大的方向没变,目的就是只需要输入参数,输入坐标点后直接可以得到相机在世界坐标系的坐标。

类的调用顺序:

1.初始化PNPSolver类;

2.调用SetCameraMatrix(),SetDistortionCoefficients()设置好相机内参数与镜头畸变参数;

3.向Points3D,Points2D中添加一一对应的特征点对;

4.调用Solve()方法运行计算;

5.从属性Theta_C2W中提取旋转角,从Position_OcInW中提取出相机在世界坐标系下的坐标。

以下是类体:

PNPSolver.h

#pragma once
#include <opencv2\opencv.hpp>
#include <math.h>
#include <iostream>
#include <fstream>
using namespace std;


// 本类用于快速解决PNP问题,顺带解决空间绕轴旋转以及图像系、相机系、世界系三系坐标投影问题
// 默认使用Gao的P3P+重投影法,要求输入4个特征点
// 调用顺序:
// 1.初始化本类
// 2.调用SetCameraMatrix(),SetDistortionCoefficients()设置好相机内参数与镜头畸变参数
// 3.向Points3D,Points2D中添加一一对应的特征点对
// 4.调用Solve()方法运行计算
// 5.从RoteM, TransM, W2CTheta等属性中提出结果
//
// 原理参见:http://www.cnblogs.com/singlex/category/911880.html
// Author:VShawn
// Ver:2016.11.25.0
class PNPSolver
{
public:
	PNPSolver();
	//带参数初始化
	PNPSolver(double fx, double fy, double u0, double v0, double k_1, double  k_2, double  p_1, double  p_2, double k_3);
	~PNPSolver();

	enum METHOD
	{
		CV_ITERATIVE = CV_ITERATIVE,
		CV_P3P = CV_P3P,
		CV_EPNP = CV_EPNP
	};

	/***********************位姿估计所用特征点**************************/
	vector<cv::Point3f> Points3D;//存储四个点的世界坐标
	vector<cv::Point2f> Points2D;//存储四个点的图像坐标

	/***********************位姿估计结果**************************/
	//最后求出的旋转矩阵与平移矩阵
	cv::Mat RoteM, TransM;
	//世界系到相机系的三轴旋转欧拉角,世界系照此旋转后可以与相机坐标系完全平行。
	//旋转顺序为x、y、z
	cv::Point3f Theta_W2C;
	//相机系到世界系的三轴旋转欧拉角,相机坐标系照此旋转后可以与世界坐标系完全平行。
	//旋转顺序为z、y、x
	cv::Point3f Theta_C2W;
	//相机坐标系中,世界坐标系原点Ow的坐标
	cv::Point3f Position_OwInC;
	//世界坐标系中,相机坐标系原点Oc的坐标
	cv::Point3f Position_OcInW;


	/*********************公有方法*****************************/

	//解PNP问题,获得位姿信息
	//调用后在RoteM, TransM, W2CTheta等属性中提取计算结果,属性说明参见注释
	//输出参数:CV_ITERATIVE,CV_P3P(默认),CV_EPNP,具体参见Opencv documentation.
	//实测
	//CV_ITERATIVE迭代法似乎只能用4个共面特征点求解,5个点或非共面4点解不出正确的解
	//CV_P3P的Gao的方法可以使用任意四个特征点,特征点数量不能少于4也不能多于4
	//CV_EPNP方法可以实现特征点数>=4的问题求解,不需要4点共面
	//返回值:
	//0正确
	//-1相机内参数或畸变参数未设置
	//-2未提供足够的特征点,或特征点数目不匹配
	//-3输入的点数据有误,详见printf信息
	int Solve(METHOD method = METHOD::CV_P3P);
	
	//根据计算出的结果将世界坐标重投影到图像,返回像素坐标点集
	//使用前需要先用Solve()解出相机位姿
	//输入为世界坐标系的点坐标集合
	//输出为点投影到图像上的图像坐标集合
	vector<cv::Point2f> WordFrame2ImageFrame(vector<cv::Point3f> WorldPoints);



	//根据输入的参数将图像坐标转换到相机坐标中
	//使用前需要先用Solve()解出相机位姿
	//输入为图像上的点坐标
	//double F为镜头焦距
	//输出为点在焦距=F时的相机坐标系坐标
	cv::Point3f ImageFrame2CameraFrame(cv::Point2f p, double F);




	//设置相机内参数矩阵
	void SetCameraMatrix(double fx, double fy, double u0, double v0)
	{
		camera_matrix = cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(0));
		camera_matrix.ptr<double>(0)[0] = fx;
		camera_matrix.ptr<double>(0)[2] = u0;
		camera_matrix.ptr<double>(1)[1] = fy;
		camera_matrix.ptr<double>(1)[2] = v0;
		camera_matrix.ptr<double>(2)[2] = 1.0f;
	}
	//设置畸变系数矩阵
	void SetDistortionCoefficients(double k_1, double  k_2, double  p_1, double  p_2, double k_3)
	{
		distortion_coefficients = cv::Mat(5, 1, CV_64FC1, cv::Scalar::all(0));
		distortion_coefficients.ptr<double>(0)[0] = k_1;
		distortion_coefficients.ptr<double>(1)[0] = k_2;
		distortion_coefficients.ptr<double>(2)[0] = p_1;
		distortion_coefficients.ptr<double>(3)[0] = p_2;
		distortion_coefficients.ptr<double>(4)[0] = k_3;
	}

上一篇:设置Highchart柱子最大宽度( 让 highcharts支持maxPointWidth属性)
下一篇:centos7安装python-pip

相关文章

相关评论

本站评论功能暂时取消,后续此功能例行通知。

一、不得利用本站危害国家安全、泄露国家秘密,不得侵犯国家社会集体的和公民的合法权益,不得利用本站制作、复制和传播不法有害信息!

二、互相尊重,对自己的言论和行为负责。