单目相机三维姿态解算
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解决单目相机的三维姿态解算问题，最终能求出单目相机相对于已知物体的三维姿态欧式角，即俯仰角，偏航角，滚轮角。
关键词： 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轴)旋转的角度。
5 实验结果
5.1 相机标定得出内参数
利用张正友棋盘法计算出相机内参数，包括相机基础矩阵M1，畸变向量D1：
图九 相机内参数
5.2 求解物体在图像上的三维空间坐标轴（即PNP问题）
首先利用已知的相机内参数（上面通过相机标定得出）以及已知2D图像坐标点（通过提取物体里面的正方形角点特征得出角点坐标）与空间对应的3D空间点（自定义，但顺序要与2D角点顺序一致）求解出相机的外参数（即旋转向量，平移向量）。本文使用OpenCV提供的solvePnP函数求解出外参数
最后利用相机内参数，上面的外参数以及自定义的3D空间坐标点(0.0,0.0, 0.0)，(1.5, 0.0,0.0)，(0.0, 1.5,0.0) ，(0.0, 0.0,1.5)，即分别对应空间原点，空间X轴，空间Y轴，空间Z轴，求解出一一对应的图像2D坐标点。
本文使用OpenCV提供的projectPoints函数解算出已知空间轴坐标点对应的图像坐标上的坐标点。最后将对应图像坐标点连在一起即为物体的空间坐标系。
图十 OpenCV代码
图十一 物体的三维空间坐标系在图像中的表示（红色为X轴，绿色为Y轴，蓝色为Z轴）
5.3 求解三维姿态角
实验情况一：相机位于已知物体的上方，与物体空间坐标轴平行，没有偏转。即：偏航角，滚轮角，俯仰角理论上都为0度。
图十二 相机位于已知物体的上方（与物体空间坐标轴平行，没有偏转）
实验情况二：相机平面与物体平面成45度角但左右没有偏转。即：理论上，偏航角为0度，滚轮角为0度，俯仰角为45度。
图十三 相机平面与物体平面成45度（与物体空间坐标X轴Y 轴平行，与Z轴成45度）
6 结束语
从实验结果中可以看出，误差正负5°左右，个人认为误差主要是标定结果（内参数）以及角点特征提取不准造成的。
参考文献：
[1] OpenCV.2.Computer.Vision.Application.Programming.Cookbook.pdf
[2] Kaehler A., Bradski G. - Learning OpenCV3 Computer Vision in C++ with the OpenCV Library - 2017.pdf
[3] Gregory G.Slabaugh - Computing Euler angles from a rotation matrix
转载自：https://blog.csdn.net/chenmohousuiyue/article/details/78157509