单⽬相机三维姿态解算（opencv）
钢管工艺单⽬相机三维姿态解算
Abstract:This passage mainly describes how to solve pose(Yaw,Pitch,Roll)with signal camera in three-dimensional,which bases on OpenCV library.
Key words: OpenCV; Pose;Signalcamera
摘 要本⽂主要描述的是利⽤开源计算机视觉库OpenCV解决单⽬相机的三维姿态解算问题，最终能求出单⽬相机相对于已知物体的三维姿态欧式⾓，即俯仰⾓，偏航⾓，滚轮⾓。 displayport转hdmi关键词： OpenCV；三维姿态
1 总体设计⽅案
本⽂的单⽬相机姿态解算能应⽤于⽆⼈机、机器⼈姿态解算系统，能测量出相机相对于被检测物体平⾯的三维姿态⾓，即俯仰⾓、偏航⾓、滚轮⾓。
单⽬相机三维姿态解算的⼤致过程：⾸先，通过相机标定得出相机的内参数，然后利⽤已知物体在三维
空间的坐标，在图像上⼀⼀对应的图像像素坐标以及相机内参数进⽽求解出此时相机相对于空间已知物体的外参数，即旋转向量以及平移向量。最后对旋转向量进⾏数据分析处理，求解出此时相机相对于已知物体空间坐标的三维姿态欧⽒⾓，即俯仰⾓，偏航⾓，滚轮⾓。 2 相机标定
在这⾥，摄像头的成像模型是以针孔成像模型为准的，但是由于透镜本⾝以及相机制造⼯艺等问题，致使成像模型不能百分百的按照针孔成像模型输出图像，输出图像必然存在畸变。因此，我们需要对相机进⾏标定，标定的⽬的是求解出相机内参数，内参数包括相机基本参数（图像光轴主点，X,Y⽅向焦距）以及畸变系数向量（切向畸变系数，径向畸变 系数）。有了相机的内参数，我们就可以知道图像像素平⾯和物体物理平⾯的关系，以及可以对相机进⾏去畸变。
2.1针孔摄像机模型
针孔摄像机模型⼜名⼩孔成像模型，其简要概念就是通过透镜聚集更多的光线，然后，这些光线都只能穿过这个⼩孔，进⽽投影形成图像。如下图所⽰：其中图像平⾯（Image plane）到针孔平⾯（Pinhole plane）的距离为焦距f（focal length），Z为物体到⼩孔平⾯的距离，x为物体通过成像仪投影到图⽚的像素距离，X为物体的物理长度。由其，我们可以得出关系式：-x = f * (X/Z)。猴车
图⼀针孔成像模型
2.1棋盘标定法
本⽂使⽤棋盘标定法进⾏相机的标定。棋盘标定法⼜称“张⽒标定”，是由张正有于1988年提出的单⾯棋盘格的摄像机标定⽅法。碱性硅溶胶
图⼆棋盘图
棋盘标定法的基本思想是通过三维场景中拍摄同⼀棋盘标定板在不同⽅向，不同位置的多张棋盘图⽚，因为每⼀张棋盘图⽚的⾓点都是等间隔的，换⾔之，棋盘⾓点的空间三维坐标是已知的（三维空间坐标系相对于每⼀张棋盘物体⽽⾔），然后，计算出每张棋盘图像在图像平⾯的像素坐标，有了每张棋盘图的三维空间坐标以及对应的图像像素平⾯的⼆维像素坐标⼀⼀对应投影关系，进⽽求出相机的内参数。
图三标定过程
OpenCV提供calibrateCamera()函数进⾏标定，利⽤该函数我们能得出相机的内参数，包括相机基本矩阵
A ，畸变系数向量D:
其中k1，k2为径向畸变，p1，p2为切向畸变，对普通摄像头我们⼀般只需要前四个系数，但对于畸变很⼤的摄像头，如鱼眼摄像头，我们应该采⽤5-8个系数向量。
3 相机姿态估计
相机姿态估计的基本思想是利⽤相机的内参数以及已知物体在空间坐标与与之⼀⼀对应的图像坐标的投影关系来求出此时物体相对于已知物体在三维空间坐标的外参数，也即是旋转矩阵以及平移向量。
此关键算法是解决N点透视投影问题，也称作PNP（Perspective-N-Point）问题。
3.1 3D点(X,Y,Z)与2D(x,y)点的联系
图四针孔成像模型的计算
在这⾥，我们以针孔成像模型为准。O为投影中⼼，主点（u0，v0） 为穿过光轴的图像主点。（X,Y,Z）为空间坐标系下的物体坐标。（注意这⾥的参考坐标系为相机的投影中⼼），（x,y,z）为图像像素坐标。（图像像素坐标的原点为左上⾓）。
根据上述的针孔投影关系：x = f * (X/Z)，我们可以容易地得出下列投影关系：（这⾥我们引⼊了fx：⽔平像素表⽰的焦距，fy：竖直像素表⽰的焦距，因为像素在⼤多情况下为矩形⽽不是正⽅形）
在这⾥，我们使⽤矩阵形式表⽰上式，有：
联合签名入口当参考坐标系并不位于相机的投影中⼼时，如下图所⽰：
模杯
图五图像坐标与世界坐标的关系
因此，我们需要引⼊旋转平移矩阵[R|t]，其中，R为3*3旋转矩阵，t为3*1平移向量。有下列矩阵
我们⽤x表⽰图像像素平⾯的某⼀点的矩阵，X表⽰为世界坐标系的某⼀点的矩阵，M为相机基本矩阵，即有：x =M*[R|t]*X。
3.2 PNP（Prespective-N-Point）问题
OpenCV提供solvePnP以及solvePnPRanssac函数来求解出相机相对于已知物体的三维空间坐标系的旋转和平移向量。
根据上式x = M*[R|t]*X，要求出外参数[R|t]，我们必须知道相机基本矩阵M，以及已知物体在三维空间坐标点X，与之对应的图像像素坐标点x。
图六 PNP问题的理解（已知特征点在图像坐标与空间坐标的关系）
4 三维姿态欧式⾓解算
综上所述，我们可以通过相机标定来获取相机内参数（基础矩阵，畸变向量），已知物体三维空间坐标系的点以及对应的图像像素坐标点。知道这些参数我们可以利⽤PNP算法通过迭代求出重投影误差
最⼩的解作为问题的最优解进⽽求出相机相对于已知物体的三维空间坐标系的外参数。即我们可以得出旋转矩阵。
4.1 旋转矩阵R
三个坐标轴的旋转矩阵：
⼀个绕X轴旋转 的矩阵：
⼀个绕Y轴旋转 的矩阵：
⼀个绕Z轴旋转 的矩阵：
我们可以利⽤⼀个旋转矩阵可以表⽰为上⾯三个坐标轴矩阵：
因此，如果我们知道旋转矩阵R，我们就可以求出旋转⾓度：
我们⽤以下矩阵表⽰上述旋转矩阵R：
通过计算我们可以⽤以下简单代码表⽰，其中，atan2(y,x)的做法：当 x 的绝对值⽐ y 的绝对值⼤时使⽤ atan(y/x)；反之使⽤atan(x/y)。这样就保证了数值稳定性，actan（y/x）是求反三⾓函数。
可以⽤下⾯代码表⽰的以上的旋转矩阵转换成三维姿态⾓（滚轮⾓、偏航⾓、俯仰⾓）的过程：
图七 MATLAB旋转矩阵解算出三维姿态⾓过程代码
4.2 相机三维姿态欧式⾓
图⼋飞机的三维姿态坐标轴（相机坐标系）
4.2.1 偏航⾓Yaw
由上图我们可以知道，偏航⾓就是绕Yaw Axis(Z轴)旋转的⾓度。
4.2.2 滚轮⾓Roll
由上图我们可以知道，偏航⾓就是绕Roll Axis(Y轴)旋转的⾓度。
4.2.3 俯仰⾓Pitch
由上图我们可以知道，偏航⾓就是绕Pitch Axis(X轴)旋转的⾓度。
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议