单目相机三维姿态解算

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

我们可以利用一个旋转矩阵可以表示为上面三个坐标轴矩阵：

因此，如果我们知道旋转矩阵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

单目相机三维姿态解算相关推荐

相机矫正_实战 | 我用位姿解算实现单目相机测距
在项目过程中,总遇到需要单目视觉给出目标测距信息的情况,其实单目相机本不适合测距,即使能给出,精度也有限,只能在有限制的条件下或者对精度要求很不高的情况下进行应用.该文结合SLAM方法,通过3D-2D ...
实战 | 巧用位姿解算实现单目相机测距
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达在项目过程中,总遇到需要单目视觉给出目标测距信息的情况,其实单目相 ...
九七的视觉工程 | 普通单目相机成像过程及相机内参详解
相机成像过程及相机内参详解一.定义介绍本节以小孔成像模型为例,详细介绍了普通单目相机的成像过程,相机内参及内参矩阵的详细含义.本节所述的成像原理与大部分相机成像原理类似,读懂此节可满足大部分与 ...
基于OpenCV的单目相机标定与三维定位
相机是产生图像数据的硬件,广泛应用于消费电子.汽车.安防等领域.围绕着相机衍生出一系列的研究与应用领域,包括传统的图像处理和基于深度学习的智能应用等.目前大火的自动驾驶中相机也是重要的硬件组成,如环视 ...
python捷联惯导的姿态解算_自动驾驶中高精地图的大规模生产：视觉惯导技术在高德的应用...
导读:导航.驾驶辅助.自动驾驶等技术的不断发展对地图的精细程度提出了更高的要求.常规的道路级地图对于智能交通系统存在很多不足,针对自动驾驶应用的需求,我们提出了利用视觉惯导技术制作高精地图的方法. 本 ...
重磅直播|GDR-Net：基于几何信息指导的单目6D物体姿态估计算法（CVPR2021作者开讲啦）...
大家好,本公众号现已开启线上视频公开课,主讲人通过B站直播间,对3D视觉领域相关知识点进行讲解,并在微信群内完成答疑. 本期由清华大学自动化系BBNC Lab王谷博士分享,分享的主题为<基于几何 ...
单目相机空间定位文献阅读
关键词单目相机:Monocular camera 单目相机空间定位:Monocular camera geolocation 大范围单目PTZ相机空间定位:Large-range Monocular ...
基于单目相机的2D地图构建
第一章引言 1.1 研究背景同时定位和建图(SLAM)是机器人和自主导航中的一个重要领域.它是指机器人在未知环境中移动并使用其传感器和里程计信息构建环境地图并同时估计其在此地图中位置的过程.SLA ...
华为诺亚方舟加拿大实验室提出BANet，双向视觉注意力机制用于单目相机深度估计...
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达这篇文章的作者是华为加拿大诺亚方舟实验室,是一篇将双向注意力机制用于弹幕深度估计的工作.主要的创新点在 ...

单目相机三维姿态解算