相机旋转矩阵求解三个姿态角
http://www.cnblogs.com/singlex/p/pose_estimation_1.html
https://www.cnblogs.com/singlex/p/RotateMatrix2Euler.html
关键词:位姿估计 OpenCV::solvePnP
用途:各种位姿估计
文章类型:原理、流程、Demo示例
@Author:VShawn(singlex@foxmail.com)
@Date:2016-11-18
@Lab: CvLab202@CSU
目录
- 《相机位姿估计0:基本原理之如何解PNP问题》
- 《相机位姿估计1:根据四个特征点估计相机姿态》
- 《相机位姿估计1_1:OpenCV:solvePnP二次封装与性能测试》
- 《相机位姿估计2:[应用]实时位姿估计与三维重建相机姿态》
- 《相机位姿估计3:根据两幅图像的位姿估计结果求某点的世界坐标》
转载该博客 https://blog.csdn.net/gaotihong/article/details/79088361
前言
本文通过迭代法解PNP问题,得到相机坐标系关于世界坐标系的旋转矩阵R与平移矩阵T后,根据之前的文章《根据相机旋转矩阵求解三个轴的旋转角》获得相机坐标系的三轴旋转角,实现了对相机位姿的估计。知道相机在哪后,我们就可以通过两张照片,计算出照片中某个点的高度,实现对环境的测量。
先看演示视频:
原理简介
相机位姿估计就是通过几个已知坐标的特征点,以及他们在相机照片中的成像,求解出相机位于坐标系内的坐标与旋转角度,其核心问题就在于对PNP问题的求解,这部分本文不再啰嗦,参见本人之前的博客文章《相机位姿估计0:基本原理之如何解PNP问题》。本文中对pnp问题的求解直接调用了OpenCV的库函数"solvePnP",其函数原型为:
bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )
第一个输入objectPoints为特征点的世界坐标,坐标值需为float型,不能为double型,可以输入mat类型,也可以直接输入vector<point3f> 。
第二个输入imagePoints为特征点在图像中的坐标,需要与前面的输入一一对应。同样可以输入mat类型,也可以直接输入vector<point3f> 。
第三个输入cameraMatrix为相机内参数矩阵,大小为3×3,形式为:
第四个输入distCoeffs输入为相机的畸变参数,为1×5的矩阵。
第五个rvec为输出矩阵,输出解得的旋转向量。
第六个tvec为输出平移向量。
第七个设置为true后似乎会对输出进行优化。
最后的输入参数有三个可选项:
CV_ITERATIVE,默认值,它通过迭代求出重投影误差最小的解作为问题的最优解。
CV_P3P则是使用非常经典的Gao的P3P问题求解算法。
CV_EPNP使用文章《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》中的方法求解。
流程
1.从函数的原型看出函数需要相机的内参数与畸变参数,于是相机标定是必不可少的,通过OpenCV自带例程或者Matlab的相机标定工具箱都可以很方便地求出相机标定参数。
2.准备好四个特征点的世界坐标,存入Mat矩阵
1
2
3
4
5
|
vector<cv::Point3f> Points3D;
Points3D.push_back(cv::Point3f(0, 0, 0)); //P1 三维坐标的单位是毫米
Points3D.push_back(cv::Point3f(0, 200, 0)); //P2
Points3D.push_back(cv::Point3f(150, 0, 0)); //P3
Points3D.push_back(cv::Point3f(150, 200, 0)); //P4
|
3.准备好四个特征点在图像上的对应点坐标,这个坐标在实验中我是通过PhotoShop数出来的。注意,输入坐标的顺序一定要与之前输入世界坐标的顺序一致,就是说点与点要对应上,OpenCV的函数无法解决点与点匹配的问题(对应搜索问题)。
1
2
3
4
5
|
vector<cv::Point2f> Points2D;
Points2D.push_back(cv::Point2f(3062, 3073)); //P1 单位是像素
Points2D.push_back(cv::Point2f(3809, 3089)); //P2
Points2D.push_back(cv::Point2f(3035, 3208)); //P3
Points2D.push_back(cv::Point2f(3838, 3217)); //P4
|
4.创建输出变量,即旋转矩阵跟平移矩阵的变量。最后调用函数。
1
2
3
4
5
6
7
8
|
//初始化输出矩阵
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
//三种方法求解
solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false , CV_ITERATIVE); //实测迭代法似乎只能用共面特征点求位置
//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_P3P); //Gao的方法可以使用任意四个特征点
//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_EPNP);
|
5将输出的旋转向量转变为旋转矩阵
1
2
3
4
|
//旋转向量变旋转矩阵
double rm[9];
cv::Mat rotM(3, 3, CV_64FC1, rm);
Rodrigues(rvec, rotM);
|
6.最后根据《根据相机旋转矩阵求解三个轴的旋转角》一文求出相机的三个旋转角,根据《子坐标系C在父坐标系W中的旋转》求出相机在世界坐标系中的位置。
至此,我们就求出了相机的位姿。
实验
本人在实验中先后使用了两台相机做测试,一台是畸变较小的sony a6000微单+35mm定焦镜头,另一台是畸变较重的130w的工业相机+6mm定焦广角镜头,实验中两台相机都得到了正确的位姿结果,此处为了方便只用α6000微单做演示说明。
如上图所示,四个特征点P1-P4的世界坐标与像素坐标都已在图中标明,P5用于重投影验证位姿解是否正确。
相机实际位姿大约为:
粗略读出卷尺读数,得到相机的世界坐标大约为(520,0,330)。细心的读者应该发现了,上面几张图的特征点不一样了,其实是我中途重新做了一张特征点图,重新安放实验装置的时候已经尽量按照(520,0,330)这个坐标去安放了,但误差肯定是不可避免的。
把参数输入例程中,得到结果,计算出相机的世界坐标:
也就是(528.6,-2.89,358.6),跟实际情况还是差不多的。
同时还得到了x y z轴的三个旋转角
自己动手转一转相机,发现也是对的。
对P5点重投影,投影公式为:
结果为:
误差在10pix以内,结果也是正确的,于是验证完毕。
P.S.经本人测试发现,solvePnP提供的三种算法都能对相机位姿进行估计,虽然三者直接解出的结果略有不同,但都在误差范围之内。其中solvePnP的默认方法迭代法,似乎只能使用共面的四个特征点求位姿,一旦有一个点不共面,解出的结果就会不对。
例程
最后给出例程,例程基于VS2013开发,使用的是OpenCV2.4.X,大家运行前需要将opencv的路径重新配置成自己电脑上的,不懂的话参考我的博客《OpenCV2+入门系列(一):OpenCV2.4.9的安装与测试》。例程中提供两张照片,其中DSC03323就是"实验"中所用图片,例程在计算完成后,会在D盘根目录下生成两个txt,分别存储:相机在世界坐标系的坐标、相机的三个旋转角。
下载地址:
CSDN:http://download.csdn.net/detail/wx2650/9688155
GIT:https://github.com/vshawn/Shawn_pose_estimation_by_opencv
最后
我现在在外地出差,演示视频里的东西就下一篇文章中再说了,敬请期待。
http://www.cnblogs.com/singlex/p/pose_estimation_1.html
关键词:位姿估计 OpenCV::solvePnP
用途:各种位姿估计
文章类型:原理、流程、Demo示例
@Author:VShawn(singlex@foxmail.com)
@Date:2016-11-18
@Lab: CvLab202@CSU
目录
- 《相机位姿估计0:基本原理之如何解PNP问题》
- 《相机位姿估计1:根据四个特征点估计相机姿态》
- 《相机位姿估计1_1:OpenCV:solvePnP二次封装与性能测试》
- 《相机位姿估计2:[应用]实时位姿估计与三维重建相机姿态》
- 《相机位姿估计3:根据两幅图像的位姿估计结果求某点的世界坐标》
前言
本文通过迭代法解PNP问题,得到相机坐标系关于世界坐标系的旋转矩阵R与平移矩阵T后,根据之前的文章《根据相机旋转矩阵求解三个轴的旋转角》获得相机坐标系的三轴旋转角,实现了对相机位姿的估计。知道相机在哪后,我们就可以通过两张照片,计算出照片中某个点的高度,实现对环境的测量。
先看演示视频:
原理简介
相机位姿估计就是通过几个已知坐标的特征点,以及他们在相机照片中的成像,求解出相机位于坐标系内的坐标与旋转角度,其核心问题就在于对PNP问题的求解,这部分本文不再啰嗦,参见本人之前的博客文章《相机位姿估计0:基本原理之如何解PNP问题》。本文中对pnp问题的求解直接调用了OpenCV的库函数"solvePnP",其函数原型为:
bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )
第一个输入objectPoints为特征点的世界坐标,坐标值需为float型,不能为double型,可以输入mat类型,也可以直接输入vector<point3f> 。
第二个输入imagePoints为特征点在图像中的坐标,需要与前面的输入一一对应。同样可以输入mat类型,也可以直接输入vector<point3f> 。
第三个输入cameraMatrix为相机内参数矩阵,大小为3×3,形式为:
第四个输入distCoeffs输入为相机的畸变参数,为1×5的矩阵。
第五个rvec为输出矩阵,输出解得的旋转向量。
第六个tvec为输出平移向量。
第七个设置为true后似乎会对输出进行优化。
最后的输入参数有三个可选项:
CV_ITERATIVE,默认值,它通过迭代求出重投影误差最小的解作为问题的最优解。
CV_P3P则是使用非常经典的Gao的P3P问题求解算法。
CV_EPNP使用文章《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》中的方法求解。
流程
1.从函数的原型看出函数需要相机的内参数与畸变参数,于是相机标定是必不可少的,通过OpenCV自带例程或者Matlab的相机标定工具箱都可以很方便地求出相机标定参数。
2.准备好四个特征点的世界坐标,存入Mat矩阵
1
2
3
4
5
|
vector<cv::Point3f> Points3D;
Points3D.push_back(cv::Point3f(0, 0, 0)); //P1 三维坐标的单位是毫米
Points3D.push_back(cv::Point3f(0, 200, 0)); //P2
Points3D.push_back(cv::Point3f(150, 0, 0)); //P3
Points3D.push_back(cv::Point3f(150, 200, 0)); //P4
|
3.准备好四个特征点在图像上的对应点坐标,这个坐标在实验中我是通过PhotoShop数出来的。注意,输入坐标的顺序一定要与之前输入世界坐标的顺序一致,就是说点与点要对应上,OpenCV的函数无法解决点与点匹配的问题(对应搜索问题)。
1
2
3
4
5
|
vector<cv::Point2f> Points2D;
Points2D.push_back(cv::Point2f(3062, 3073)); //P1 单位是像素
Points2D.push_back(cv::Point2f(3809, 3089)); //P2
Points2D.push_back(cv::Point2f(3035, 3208)); //P3
Points2D.push_back(cv::Point2f(3838, 3217)); //P4
|
4.创建输出变量,即旋转矩阵跟平移矩阵的变量。最后调用函数。
1
2
3
4
5
6
7
8
|
//初始化输出矩阵
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
//三种方法求解
solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false , CV_ITERATIVE); //实测迭代法似乎只能用共面特征点求位置
//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_P3P); //Gao的方法可以使用任意四个特征点
//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_EPNP);
|
5将输出的旋转向量转变为旋转矩阵
1
2
3
4
|
//旋转向量变旋转矩阵
double rm[9];
cv::Mat rotM(3, 3, CV_64FC1, rm);
Rodrigues(rvec, rotM);
|
6.最后根据《根据相机旋转矩阵求解三个轴的旋转角》一文求出相机的三个旋转角,根据《子坐标系C在父坐标系W中的旋转》求出相机在世界坐标系中的位置。
至此,我们就求出了相机的位姿。
实验
本人在实验中先后使用了两台相机做测试,一台是畸变较小的sony a6000微单+35mm定焦镜头,另一台是畸变较重的130w的工业相机+6mm定焦广角镜头,实验中两台相机都得到了正确的位姿结果,此处为了方便只用α6000微单做演示说明。
如上图所示,四个特征点P1-P4的世界坐标与像素坐标都已在图中标明,P5用于重投影验证位姿解是否正确。
相机实际位姿大约为:
粗略读出卷尺读数,得到相机的世界坐标大约为(520,0,330)。细心的读者应该发现了,上面几张图的特征点不一样了,其实是我中途重新做了一张特征点图,重新安放实验装置的时候已经尽量按照(520,0,330)这个坐标去安放了,但误差肯定是不可避免的。
把参数输入例程中,得到结果,计算出相机的世界坐标:
也就是(528.6,-2.89,358.6),跟实际情况还是差不多的。
同时还得到了x y z轴的三个旋转角
自己动手转一转相机,发现也是对的。
对P5点重投影,投影公式为:
结果为:
误差在10pix以内,结果也是正确的,于是验证完毕。
P.S.经本人测试发现,solvePnP提供的三种算法都能对相机位姿进行估计,虽然三者直接解出的结果略有不同,但都在误差范围之内。其中solvePnP的默认方法迭代法,似乎只能使用共面的四个特征点求位姿,一旦有一个点不共面,解出的结果就会不对。
例程
最后给出例程,例程基于VS2013开发,使用的是OpenCV2.4.X,大家运行前需要将opencv的路径重新配置成自己电脑上的,不懂的话参考我的博客《OpenCV2+入门系列(一):OpenCV2.4.9的安装与测试》。例程中提供两张照片,其中DSC03323就是"实验"中所用图片,例程在计算完成后,会在D盘根目录下生成两个txt,分别存储:相机在世界坐标系的坐标、相机的三个旋转角。
下载地址:
CSDN:http://download.csdn.net/detail/wx2650/9688155
GIT:https://github.com/vshawn/Shawn_pose_estimation_by_opencv
相机旋转矩阵求解三个姿态角相关推荐
- 捷联惯导算法(三)姿态角和姿态矩阵
前言 文中算法公式摘自<捷联惯导算法与组合导航原理>(严恭敏.翁浚 编著).<惯性导航>(秦永元 编著),其他理解仅代表个人观点.本文是对姿态角和姿态矩阵之间转化的理解. 一. ...
- IMU内参标定以及初始化(9轴IMU,比6轴多三个姿态角信息)
IMU内参标定以及初始化(绕8字) 一.IMU内参标定 1.6轴(角速度+线加速度)信息初始化(标定噪声和bias) 2.三轴姿态信息初始化(绕8子) 二.IMU模块ROS配置 注意事项: 因为三个方 ...
- STM32实现四驱小车(三)传感任务——姿态角解算
目录 一. 绪论 二. 惯性传感器测量原理 1. 三轴加速度计 2. 三轴陀螺仪 3. 三轴磁力计 三. 状态估计 1. 姿态估计 (1)线性互补滤波器 (2)非线性互补滤波器 (3)卡尔曼滤波器 2 ...
- 利用MPU6050三轴加速度获取欧拉姿态角
最近用到了MPU6050进行姿态估计,现将其中MPU6050三轴加速度读数求解欧拉姿态角的推导过程记录如下: 首先将MPU6050固联的载体坐标系b系与导航坐标系n系重合.即将图1的姿态旋转至图2中的 ...
- PX4飞控中利用EKF估计姿态角代码详解
PX4飞控中利用EKF估计姿态角代码详解 PX4飞控中主要用EKF算法来估计飞行器三轴姿态角,具体c文件在px4\Firmware\src\modules\attitude_estimator_ekf ...
- 固定翼飞机姿态角Backstepping反步法控制
固定翼飞机姿态角Backstepping反步法控制 1. 数学建模 2. 系统解耦 3. 反步法控制律设计 4. 仿真结果 5. 仿真结果分析 6. 参考文献 在文章固定翼飞机数学建模入门(姿态角篇) ...
- 树莓派pico mpu6050 一阶互补滤波四元数法 解算姿态角
micro-python:一阶互补滤波&四元数法 代码 2.系统方案 2.1.组成 本系统由供电部分, 主控部分, 姿态传感器与通信部份组成 2.2.供电部分 电池为一节14500锂电池, 容 ...
- 导航中姿态角与欧拉角的联系
先引用教科书中的一段关于姿态角的定义: Note: 请仔细理解三个姿态角的定义, 需要注意的是航向角(yaw)与俯仰角(pitch)与当地水平面有关, 而横滚角(roll)的定义与水平面无关, 这样就 ...
- 重建大师 | 姿态角辅助功能,“又快又好”实现倾斜空三处理
倾斜摄影数据处理 作为新型基础测绘的重要技术手段,倾斜摄影测量技术已经得到极其广泛的应用,其技术核心构成包括:飞行载具.倾斜相机.POS(Position and Orientation System ...
最新文章
- 两度延期,K项目终于要开工了!
- 服务器SSL/TLS快速检测工具TLLSSLed
- 国产GPU为何“一夜杀到老黄城下”?
- SAP与其他系统交互的方式
- java map判断是否有键_检查Java HashMap中是否存在给定键
- React 实现一个漂亮的 Table
- java明文发送_使用java MD5加密网络明文
- Windbg 查看SSDT表
- 如何在 Active Directory 中还原已删除的用户帐户及其组成员身份
- python3.5安装步骤-pycharm安装步骤
- 开放域对话中粗粒度响应选择的上下文细到粗蒸馏
- 小D课堂-SpringBoot 2.x微信支付在线教育网站项目实战_6-3.微信网站扫码支付介绍...
- java.util.Scanner的日常用法
- c99变长数组_第九章 C99可变长数组VLA详解
- 富途证券招股书解读:近2个月客户资产下降
- (软考)系统分析师——标准化知识
- linux 的 绘画软件,Drawing Linux(简单画图工具)
- 手写原笔迹输入_原笔迹真实展现 E人E本 M1手写功能体验
- 每个人都可以做到:月入30000的秘籍!
- 数据库优化(超级详细),转文,值得一读
热门文章
- Windows 通过编辑注册表设置左右手使用习惯更改 Popup 弹出位置
- 推荐几款实用软件工具
- Jira使用简介 HP ALM使用简介
- f2fs mkfs 格式化过程系列 0
- MSTP的原理以及实验
- Biopython操作DNA,RNA和蛋白质序列
- ODL:OpenDayLight子项目之MD-SAL
- 港科夜闻|香港科大新研究显示预期接种疫苗和加强针后产生的T细胞免疫屏障将能继续保护人体并减少被Omicron等病毒变异的感染机率...
- AUTOSAR开发工具DaVinci Configurator里的Modules
- 刷PAT啦1008~1010