Jack Huang's Blog


  • 首页

  • 标签

  • 归档

  • 搜索

求解射线与三角形交点的算法

发表于 2019-06-04

求解射线与三角形的交点,在光线追踪、碰撞检测、目标拾取等场景中经常使用,是计算机图形学中最基本的操作。下面介绍常用的求解射线与三角形交点的算法。

问题定义

求解射线与三角形交点示意图如图1所示。

求解射线与三角形交点示意图 求解射线与三角形交点示意图

图1 求解射线与三角形交点示意图

射线的参数方程如下,其中O是射线的起点,D是射线的方向,t是常数。

$$O+Dt$$

该方程的含义是一个点从起点O开始,沿着方向D移动任意长度,得到终点R,根据t值的不同,得到的R值也不同,所有这些不同的R值便构成了整条射线,比如下面的射线,起点是P0,方向是u,p0 + tu也就构成了整条射线。

射线方程示意图

图2 射线方程示意图

三角形的参数方程如下,其中$V_0$,$V_1$和$V_2$是三角形的三个点,$u, v$是$V_1$和$V_2$的权重,$1-u-v$是$V_0$的权重,并且满足$u>=0, v >= 0,u+v<=1$。

$$(1-u-v)V_0+uV_1+vV_2$$

三角形方程示意图

图3 三角形方程示意图

直观方法

求解射线与三角形的交点最直观的方法如下:

  1. 判断射线是否与平面相交
  2. 判断点是否在三角形内

但该方法需要额外计算三角形所在平面,效率不高。

Moller-Trumbore方法(Journal of Graphic Tools, 1997)

Moller-Trumbore方法中,求射线与三角形的交点即求解如下方程:

$$O+Dt=(1-u-v)V_0+uV_1+vV_2$$

其中t,u,v是未知数,其他都是已知的。

移项并整理,将t,u,v提取出来作为未知数,得到下面的线性方程组:

$$\begin{bmatrix}
-D& V_1-V_0 &V_2-V_0
\end{bmatrix}\begin{bmatrix}
t\u\v
\end{bmatrix}=O-V_0$$

现在开始解这个方程组,这里要用到两个知识点,一是克莱姆法则,二是向量的混合积。

令$E_1 = V_1 - V_0,E_2 = V_2 - V_0,T = O - V_0$上式可以改写成:

$$\begin{bmatrix}
-D& E_1 & E_2
\end{bmatrix}\begin{bmatrix}
t\u\v
\end{bmatrix}=T$$

根据克莱姆法则,可得到t,u,v的解为:

$$
\begin{bmatrix}
t\u\v
\end{bmatrix}
=\frac{1}{\begin{vmatrix}
-D & E_1 & E_2
\end{vmatrix} }
\begin{vmatrix}
T&E_1&E_2\
-D& T& E_2\
-D & E_1& T
\end{vmatrix}
$$

根据混合积公式:

$$\begin{vmatrix}
a&b&c
\end{vmatrix}
=a\times{b}\cdot{c}$$

上式改写为:

$$
\begin{bmatrix}
t\u\v
\end{bmatrix}
=\frac{1}{\begin{bmatrix}
-D \times E_2 \cdot E_1
\end{bmatrix} }
\begin{vmatrix}
T \times E_1 \cdot E_2\
D \times E_2 \cdot T\
T \times E_1 \cdot D
\end{vmatrix}
$$

令$P=D \times E_2$,$Q=T \times E_1$,得到最终的公式:

$$\begin{bmatrix}
t\u\v
\end{bmatrix}
=\frac{1}{\begin{bmatrix}
P \cdot E_1
\end{bmatrix} }
\begin{vmatrix}
Q \cdot E_2\
P \cdot T\
Q \cdot D
\end{vmatrix}
$$

之所以提炼出P和Q是为了避免重复计算。

参考链接

  1. 射线和三角形的相交检测(ray triangle intersection test),by zdd.
  2. 光线-三角形求交测试算法[译], by PKUWWT.
  3. 克莱姆法则,by wikipedia.
  4. 混合积,by wikipedia.
  5. 重心坐标,by wikipedia.
  6. 重心坐标(Barycentric coordinates),by 杨超.

PLY文件格式分析

发表于 2019-06-04

PLY文件是一种存储3D模型的文件格式,全名为多边形档案(Polygon File Format)或 史丹佛三角形档案(Stanford Triangle Format)。

该格式主要用以储存立体扫描结果的三维数值,透过多边形片面的集合描述三维物体,与其他格式相较之下这是较为简单的方法。它可以储存的资讯包含颜色、透明度、表面法向量、材质座标与资料可信度,并能对多边形的正反两面设定不同的属性。

在档案内容的储存上PLY有两种版本,分别是纯文字(ASCII)版本与二元码(binary)版本,其差异在储存时是否以ASCII编码表示元素资讯。

通过分析PLY文件格式,我们可以进而初窥存储3D模型的文件奥秘。

文件格式

各种文件格式通常分成文件头和文件内容,PLY格式也不例外。

每个PLY档都包含档头(header),用以设定网格模型的“元素”与“属性”,以及在档头下方接着一连串的元素“数值资料”。一般而言,网格模型的“元素”就是顶点(vertices)、面(faces),另外还可能包含有边(edges)、深度图样本(samples of range maps)与三角带(triangle strips)等元素。无论是纯文字与二元码的PLY档,档头资讯都是以ASCII编码编写,接续其后的数值资料才有编码之分。

PLY档案以此行作为开头,以识别PLY格式:

1
ply

接着第二行是版本资讯,目前有三种写法:

1
2
3
format ascii 1.0
format binary_little_endian 1.0
format binary_big_endian 1.0

其中ascii, binary_little_endian, binary_big_endian是档案储存的编码方式,而1.0是遵循的标准版本(现阶段仅有PLY 1.0版)。在档头中可使用’comment’作为一行的开头以编写注解,例如:

1
comment This is a comment!

描述元素及属性,必须使用’element’及’property’的关键字,一般的格式为element下方接着属性列表,例如:

1
2
3
4
element <element name> <number in file>
property <data_type> <property name 1>
property <data_type> <property name 2>
property <data_type> <property name 3>

‘property’不仅定义了资料的型态,其出现顺序亦定义了资料的顺序。内定的资料形态有两种写法:一种是char uchar short ushort int uint float double,另外一种是具有位元长度的int8 uint8 int16 uint16 int32 uint32 float32 float64。 例如,描述一个包含12个顶点的物体,每个顶点使用3个单精度浮点数 (x,y,z)代表点的座标,使用3个unsigned char代表顶点颜色,颜色顺序为 (B, G, R),则档头的写法为:

1
2
3
4
5
6
7
element vertex 12
property float x
property float y
property float z
property uchar blue
property uchar green
property uchar red

其中vertex是内定的元素类型,接续的6行property描述构成vertex元素的数值字段顺序代表的意义,及其资料形态。

另一个常使用的元素是面。由于一个面是由3个以上的顶点所组成,因此使用一个“顶点列表”即可描述一个面, PLY格式使用一个特殊关键字’property list’定义之。 例如,一个具有10个面的物体,其PLY档头可能包含:

1
2
element face 10
property list uchar int vertex_indices

‘property list’表示该元素face的特性是由一行的顶点列表来描述。列表开头以uchar型态的数值表示列表的项目数,后面接着资料型态为int的顶点索引值(vertex_indices),顶点索引值从0开始。

最后,标头必须以此行结尾:

1
end_header

档头后接着的是元素资料(端点座标、拓朴连结等)。在ASCII格式中各个端点与面的资讯都是以独立的一行描述,而二元编码格式则连续储存这些资料,载入时须以’element’定义的元素数目以及’property’中设定的资料形态计算各笔字段的长度。

示例

一个典型的PLY文件结构分成三部分:

1
2
3
文件头 (从ply开始到end_header)
顶点元素列表
面元素列表

其中的顶点元素列表一般以x y z方式排列,形态如文件头所定义;而面元素列表是以下列格式表示。

1
<组成面的端点數N> <端点#1的索引> <端点#2的索引> ... <端点#N的索引>

以存储一个立方体模型的PLY文件为例,其内容为:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
ply
format ascii 1.0
comment made by anonymous
comment this file is a cube
element vertex 8
property float32 x
property float32 y
property float32 z
element face 12
property list uint8 int32 vertex_index
end_header
0 0 0
0 25.8 0
18.9 0 0
18.9 25.8 0
0 0 7.5
0 25.8 7.5
18.9 0 7.5
18.9 25.8 7.5
3 5 1 0
3 5 4 0
3 4 0 2
3 4 6 2
3 7 5 4
3 7 6 4
3 3 2 1
3 1 2 0
3 5 7 1
3 7 1 3
3 7 6 3
3 6 3 2

PLY文件可用blender软件打开。

参考链接

  1. PLY,by wikipedia.
  2. Opengl学习笔记:(一).Ply文件文件格式和文件读取,by 为何走到这里.

图像特征检测与描述

发表于 2019-06-02

特征检测(英语:Feature detection)是计算机视觉和图像处理中的一个概念。它指的是使用计算机提取图像信息,决定每个图像的点是否属于一个图像特征。特征检测的结果是把图像上的点分为不同的子集,这些子集往往属于孤立的点、连续的曲线或者连续的区域。

图像特征分类

特征的精确定义往往由问题或者应用类型决定。特征是一个数字图像中“有趣”的部分,它是许多计算机图像分析算法的起点。因此一个算法是否成功往往由它使用和定义的特征决定。因此特征检测最重要的一个特性是“可重复性”:同一场景的不同图像所提取的特征应该是相同的。

特征检测是图象处理中的一个初级运算,也就是说它是对一个图像进行的第一个运算处理。它检查每个像素来确定该像素是否代表一个特征。假如它是一个更大的算法的一部分,那么这个算法一般只检查图像的特征区域。作为特征检测的一个前提运算,输入图像一般通过高斯模糊核在尺度空间中被平滑。此后通过局部导数运算来计算图像的一个或多个特征。

常用的图像特征分成以下四类:

  • 边缘

边缘指组成两个图像区域之间边界(或边缘)的像素。一般一个边缘的形状可以是任意的,还可能包括交叉点。在实践中边缘一般被定义为图像中拥有大的梯度的点组成的子集。一些常用的算法还会把梯度高的点联系起来来构成一个更完善的边缘的描写。

  • 角

角指图像中点似的特征,在局部它有两维结构。早期的算法首先进行边缘检测,然后分析边缘的走向来寻找边缘突然转向(角)。后来发展的算法不再需要边缘检测这个步骤,而是可以直接在图像梯度中寻找高度曲率。

  • 区域

与角不同的是区域描写一个图像中的一个区域性的结构,但是区域也可能仅由一个像素组成,因此许多区域检测也可以用来监测角。一个区域监测器检测图像中一个对于角监测器来说太平滑的区域。

区域检测可以被想象为把一张图像缩小,然后在缩小的图像上进行角检测。

  • 脊

长条形的物体被称为脊。在实践中脊可以被看作是代表对称轴的一维曲线,此外局部针对于每个脊像素有一个脊宽度。从灰梯度图像中提取脊要比提取边缘、角和区域困难。在空中摄影中往往使用脊检测来分辨道路,在医学图像中它被用来分辨血管。

边缘检测

边缘检测(英语:Edge detection)是图像处理和计算机视觉中的基本问题,边缘检测的目的是标识数字图像中亮度变化明显的点。图像属性中的显著变化通常反映了属性的重要事件和变化。这些包括(i)深度上的不连续、(ii)表面方向不连续、(iii)物质属性变化和(iv)场景照明变化。 边缘检测是图像处理和计算机视觉中,尤其是特征检测中的一个研究领域。

图像边缘检测大幅度地减少了数据量,并且剔除了可以认为不相关的信息,保留了图像重要的结构属性。有许多方法用于边缘检测,它们的绝大部分可以划分为两类:基于查找一类和基于零穿越的一类。基于查找的方法通过寻找图像一阶导数中的最大和最小值来检测边界,通常是将边界定位在梯度最大的方向。基于零穿越的方法通过寻找图像二阶导数零穿越来寻找边界,通常是Laplacian过零点或者非线性差分表示的过零点。

边缘检测的方法

有许多用于边缘检测的方法,他们大致可分为两类:

  • 基于搜索的边缘检测

基于搜索的边缘检测方法首先计算边缘强度,通常用一阶导数表示,例如梯度模;然后,用计算估计边缘的局部方向,通常采用梯度的方向,并利用此方向找到局部梯度模的最大值.

  • 基于零交叉的边缘检测

基于零交叉的方法找到由图像得到的二阶导数的零交叉点来定位边缘.通常用拉普拉斯算子或非线性微分方程的零交叉点,我们将在后面的小节中描述.

滤波做为边缘检测的预处理通常是必要的,通常采用高斯滤波.

已发表的边缘检测方法应用计算边界强度的度量,这与平滑滤波有本质的不同.正如许多边缘检测方法依赖于图像梯度的计算,他们用不同种类的滤波器来估计x-方向和y-方向的梯度.

角检测

角检测(英语:Corner detection)或兴趣点检测(interest point detection),是计算机视觉系统中用来提取特征以及推测图像内容的一种方法.角检测的应用很广,经常用在运动检测,跟踪,图像镶嵌(image mosaicing),全景图缝合(panorama stiching),三维建模以及物体识别中.

问题定义

两条边的交点形成一个角(点)。而图像的要点(也称为受关注点)是指图像中具有代表性以及稳健性(即指该点能够在有噪声干扰的情况下也能稳定的被定位,在大陆亦被称为:鲁棒性)的点。也就是说,要点可以是角(点),也可以不是,例如局部亮点或暗点,线段终点,或者曲线上的曲率最大值点。在实际应用中,很多所谓的(角)点检测算法其实是检测要点,而不仅仅是角(点)。所以,如果我们只想检测角的话,还需要对检测出的要点进一步分析。

角检测的方法

在现实世界中,角点对应于物体的拐角,道路的十字路口、丁字路口等。从图像分析的角度来定义角点可以有以下两种定义:

  • 角点可以是两个边缘的角点;
  • 角点是邻域内具有两个主方向的特征点;
  • 前者往往需要对图像边缘进行编码,这在很大程度上依赖于图像的分割与边缘提取,具有相当大的难度和计算量,且一旦待检测目标局部发生变化,很可能导致操作的失败。早期主要有Rosenfeld和Freeman等人的方法,后期有CSS等方法。

基于图像灰度的方法通过计算点的曲率及梯度来检测角点,避免了第一类方法存在的缺陷,此类方法主要有Moravec算子、Forstner算子、Harris算子、SUSAN算子等。

角检测方法发展历程

图1 角检测方法发展历程

Harris算法

Harris角点检测基本原理如图2所示。人眼对角点的识别通常是在一个局部的小区域或小窗口完成的。如果在各个方向上移动这个特征的小窗口,窗口内区域的灰度发生了较大的变化,那么就认为在窗口内遇到了角点。如果这个特定的窗口在图像各个方向上移动时,窗口内图像的灰度没有发生变化,那么窗口内就不存在角点;如果窗口在某一个方向移动时,窗口内图像的灰度发生了较大的变化,而在另一些方向上没有发生变化,那么,窗口内的图像可能就是一条直线的线段。

Harris角点检测基本原理

图2 Harris角点检测基本原理

SIFT算法

SIFT(Scale-invariant features transform, 尺度不变特征变换)是一种检测局部特征的算法,该算法通过求一幅图中的特征点(interest points,or corner points)及其有关scale 和 orientation 的描述子得到特征并进行图像特征点匹配,获得了良好效果。SIFT特征不只具有尺度不变性,即使改变旋转角度,图像亮度或拍摄视角,仍然能够得到好的检测效果。该算法由 David Lowe在1999年所发表,2004年完善总结。

SIFT算子是把图像中检测到的特征点用一个128维的特征向量进行描述,因此一幅图像经过SIFT算法后表示为一个128维的特征向量集,该特征向量集具有对图像缩放,平移,旋转不变的特征,对于光照、仿射和投影变换也有一定的不变性,是一种非常优秀的局部特征描述算法。

SIFT算法的流程分别为:

  1. 尺度空间极点检测
  2. 关键点精确定位
  3. 关键点的方向确定
  4. 特征向量的生成

ORB算法

ORB的全称是ORiented Brief,是文章ORB: an efficient alternative to SIFT or SURF中提出的一种新的角点检测与特征描述算法。实际上,ORB算法是将FAST角点检测与BRIEF特征描述结合并进行了改进。

ORB特征由关键点和描述子两部分组成。它的关键点称为“Oriented Fast”,是一种改进的FAST角点。描述子称为“BRIEF(Binary Robust Independent Elementary Feature)”。因此提取ORB特征分为如下两个步骤:

  • 提取FAST角点(相较于原版FAST角点,ORB中计算了特征点的主方向,为后续的BRIEF描述子增加了旋转不变性)
  • 计算BRIEF描述子

参考链接

  1. 特征检测,by wikipedia.
  2. 边缘检测,by wikipedia.
  3. 角检测,by wikipedia.
  4. Harris角点,by Ronny.
  5. 特征点匹配——SIFT算法详解,by lhanchao.
  6. ORB特征提取、匹配及实现,by zhaoxuhui.
  7. 传统计算机视觉中图像特征匹配方法的原理介绍(SIFT 和 ORB),by Zhang Bin.

相机位姿估计入门

发表于 2019-05-29 | 更新于 2019-06-01

相机位姿估计是指给定若干图像,估计其中相机运动的问题。求解方法通常分特征点法和直接法两种。下面主要介绍特征点法。

特征点法的思路是先从图像当中提取许多特征,然后在图像间进行特征匹配,这样就得到许多匹配好的点,再根据这些点进行相机位姿的求解。根据传感器形式的不同,可以分成三种情况:

  • 2D-2D,单目相机获取的影像,只能获得像素坐标
  • 3D-3D配对点,RGBD或双目相机,可以获取深度信息
  • 3D-2D,已知一张图中的3D信息,另一张图只有2D信息

问题分析

在两帧各自的相机坐标系中,设P点的相机坐标系的坐标分别为$P_1$、$P_2$,如图1所示。

相机位姿估计示意图

图1 相机位姿估计示意图

其中:

$$P_{1}=\begin{pmatrix}
X_{1}\
Y_{1}\
Z_{1}
\end{pmatrix},P_{2}=\begin{pmatrix}
X_{2}\
Y_{2}\
Z_{2}
\end{pmatrix}$$

相机从第一帧移动到第二帧,其旋转矩阵设为R,平移向量设为t。这里的R表示的旋转是相对于第一帧的姿态改变量。那么有:

$$P_{2}=RP_{1}+t \tag{1}$$

相机位姿估计的最终目的就是要根据这个运动方程,求解出相机的运动,也就是R、t。注意这里的R、t并不是相机外参!

由于$P_1$、$P_2$的坐标未知,但其在像素坐标系的坐标$p_1$、$p_2$已知,因此,根据小孔成像模型寻找$p_1$、$p_2$和$P_1$、$P_2$之间的关系。

小孔成像模型

小孔成像模型的示意图如图2所示。设P点在世界坐标系下坐标为$P_w$在相机坐标系下坐标为$P_c$,物理成像平面对应坐标为 $P’$, 像素平面对应坐标为p, 有:

$$p=\begin{pmatrix}
u\
v
\end{pmatrix},P^{‘}=\begin{pmatrix}
X^{‘}\
Y^{‘}\
Z^{‘}
\end{pmatrix},P_c=\begin{pmatrix}
X_c\
Y_c\
Z_c
\end{pmatrix},P_w=\begin{pmatrix}
X_w\
Y_w\
Z_w
\end{pmatrix}$$

小孔成像模型示意图

图2 小孔成像模型示意图

根据小孔成像模型,有如下公式:

$$
\begin{pmatrix}
u\
v\
1
\end{pmatrix}=\frac{1}{Z_{c}}\begin{pmatrix}
f_{x} & 0 & c_{x}\
0 & f_{y} & c_{y}\
0 & 0 & 1
\end{pmatrix}\begin{pmatrix}
X_{c}\
Y_{c}\
Z_{c}
\end{pmatrix} \tag{2}$$

该公式给出了点P相机坐标系下坐标与像素平面坐标之间的关系。

2D-2D估计

设相机内参矩阵为K,即

$$
K=\begin{pmatrix}
f_{x} & 0 & c_{x}\
0 & f_{y} & c_{y}\
0 & 0 & 1
\end{pmatrix}
$$

将公式(2)代入公式(1),则

$$K^{-1}Z_{2}p_{2}=RK^{-1}Z_{1}p_{1}+t \tag{3}$$

内参矩阵K短期内不变,但$Z_1$很明显一般情况下不等于$Z_2$,除非是相机绕着P点为圆心旋转。现在已知的是K、p1、p2,待求R,t。 然而这里还有$Z_1$、$Z_2$是未知的,因此必须想办法将$Z$消去。

将相机坐标系下坐标$P_c$归一化,即

$$
P_{c}^{‘}=\begin{pmatrix}
{X_{c}}/{Z_{c}}\
{Y_{c}}/{Z_{c}}\
1
\end{pmatrix}
$$

由公式(2)可得:

$$
\begin{pmatrix}
u\
v\
1
\end{pmatrix}=\begin{pmatrix}
f_{x} & 0 & c_{x}\
0 & f_{y} & c_{y}\
0 & 0 & 1
\end{pmatrix}\begin{pmatrix}
{X_{c}}/{Z_{c}}\
{Y_{c}}/{Z_{c}}\
1
\end{pmatrix} =K\begin{pmatrix}
{X_{c}}/{Z_{c}}\
{Y_{c}}/{Z_{c}}\
1
\end{pmatrix} \tag{4}$$

将公式(4)代入公式(1),则有:

$$K^{-1}p_{2}=RK^{-1}p_{1}+t$$

这样便能求解了。但是也导致了一个问题,因为不同帧对应的相机坐标系中P的Z值并不相等。在实际操作中分别除以其本身从而将Z分量归一化为1,这其实就丢失了真实的位置信息,不同帧的缩放是不等的!从而导致2D-2D估计两两帧之间,每次估计的尺度都是不同的。这也就是单目SLAM的尺度不确定性。

通过2D-2D,最终可获得P点像素坐标 $p$ 对应相机坐标系下归一化坐标$P_{c}^{‘}$。

三角测量

三角测量在三角学与几何学上是一借由测量目标点与固定基准线的已知端点的角度,测量目标距离的方法。而不是直接测量特定位置的距离(三边量测法)。当已知一个边长及两个观测角度时,观测目标点可以被标定为一个三角形的第三个点。

基于两固定角度之距离量测

假设一量测目标点及两个已知座标的参考点可形成一个三角形,则借由计算三角形其中参考边的长度,量测两参考点与目标点形成的角度,即可找出目标点的距离及座标。

基于两固定角度之距离量测示意图

图3 基于两固定角度之距离量测示意图

三角测量可用来计算岸边与船只之间的距离及座标。A顶点的观察者测量岸边与船只之间的角度α,B点的观察者则依同理测量出角度β,由长度l或已知的A及B点座标,则可由正弦定理取得在C点船只的座标及距离d。

计算过程如下:

$$ \ell = \frac{d}{\tan \alpha} + \frac{d}{\tan \beta}$$

根据三角恒等式${\displaystyle \tan \alpha ={\frac {\sin \alpha }{\cos \alpha }}}$和${\displaystyle \sin \left(\alpha +\beta \right)=\sin \alpha \cos \beta +\cos \alpha \sin \beta }$,此式可等于:

$${\displaystyle \ell =d\left({\frac {\cos \alpha }{\sin \alpha }}+{\frac {\cos \beta }{\sin \beta }}\right)}$$

$${\displaystyle \ell =d\ {\frac {\sin(\alpha +\beta )}{\sin \alpha \sin \beta }}}$$

因此,

$${\displaystyle d=\ell \ {\frac {\sin \alpha \sin \beta }{\sin(\alpha +\beta )}}}$$

由此便可简单定义出一未知点与观察点间的距离,以及与观察点往东西、南北向相差的位移量,终得完整座标。

2D-2D估计获得特征点在相机坐标系下的归一化3D坐标,结合三角测量,可获得特征点在相机坐标系下的深度,两者结合即获得特征点在相机坐标系下的3D坐标。

3D-2D估计

3D-2D估计本质是PnP一个问题,即给定世界坐标系中n个3D点及其在图像中的相应2D投影的情况下,估计校准相机的姿势的问题。相机姿势由6个自由度(DOF)组成,其由旋转(滚动,俯仰和偏航)以及相机相对于世界的3D平移构成。该问题源于相机校准,并且在计算机视觉和其他领域中具有许多应用,包括3D姿态估计、机器人和增强现实。 对于n = 3,存在一个常用的问题解决方案,称为P3P,并且许多解决方案适用于n≥3的一般情况。

P3P

P3P仅需要使用三对匹配点,就可以完成相机的位姿估计。

假设空间中有A,B,C三点,投影到成像平面中有a,b,c三点,在PnP问题中,A,B,C在世界坐标系下的坐标是已知的,但是在相机坐标系下的坐标是未知的。a,b,c的坐标是已知的。PnP的目的就是要求解A,B,C在相机坐标系下的坐标值。如下图所示。需要注意的是三角形abc和三角形ABC不一定是平行的。

P3P问题示意图

图4 P3P问题示意图

根据余弦定理有:

$$OA^2 + OB^2 - 2OA \cdot OB \cdot \cos(a,b) = AB^2 \ OB^2 + OC^2 - 2OB \cdot OC \cdot \cos(b,c) = BC^2 \ OA^2 + OC^2 - 2OA \cdot OC \cdot \cos(a,c) = AC^2$$

记$x=\dfrac{OA}{OC}$,$y=\dfrac{OB}{OC}$,因为A,B,C在相机坐标系中的坐标未知,因此x,y是未知的。

另记$u=\dfrac{BC^2}{AB^2}$,$w=\dfrac{AC}{AB}$, 根据A,B,C的世界坐标,u,w是可以求出的。

通过一系列的转化可以得到两个等式:

$$(1-u)y^2-ux^2-\cos(b,c)y+2uxy \cos(a,b) +1 = 0 \ (1-w)x^2-wy^2-\cos(a,c)x+2wxy \cos(a,b) +1 = 0$$

该方程组是关于x,y的一个二元二次方程,可以通过吴消元法求解。最多可能得到四个解,因此在三个点之外还需要一组匹配点进行验证。

至此,通过x和y就可以求得A,B,C在相机坐标下的坐标值。因此3D-2D问题转变成了3D-3D的位姿估计问题。而带有匹配信息的3D-3D位姿求解非常容易。

Bundle Adjustment

假设某空间点坐标为$P_i = [X_i, Y_i, Z_i]$, 其投影的像素坐标为$p_i=[u_i,v_i]$。这些在PnP问题里都是已知的。在相机坐标系下有$c=[x_i, y_i, z_i]$,这个坐标通过P3P或者其他解法有了粗略的估计。根据针孔相机模型可得:

$$z_i p_i = KTP_i = K \exp([\xi]_{\times})P_i$$

根据这个等式可以构造出一个最小二乘问题:

$$\xi^* = \arg \min \limits {\xi} \dfrac{1}{2} \sum\limits _{i=1} ^n \begin{Vmatrix} p_i - \dfrac{1}{z_i} K \exp([\xi]{\times})P_i \end{Vmatrix} _2 ^2$$

该问题的误差项,是将像素坐标与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差,所以称之为重投影误差。如图5所示。

重投影误差示意图

图5 重投影误差示意图

这个最小二乘问题主要优化两个变量,第一是对相机位姿的优化,也就是对李代数的优化,第二是对空间点P的优化,也就是P点的优化。

3D-3D估计

3D-3D的位姿估计问题是指,对于空间中的某一点,我们知道这个点在两个相机坐标系中的三维坐标,如何利用这两个三维坐标来求解这两个相机坐标系的运动就是3D-3D的位姿评估问题。这个问题通常用迭代最近点(Iterative Closest Point,ICP)求解。

假设空间中的一系列点在第一个相机坐标系下的三维坐标为$C={c_1,…,c_n}$,在第二个相机坐标系下匹配的三维坐标为$C’={c_1’,…,c’_n}$。则有:

$$\forall i, \ c_i=Rc_i’+t$$

对于ICP的求解主要分为两种方式:利用线性代数的求解和利用非线性优化方式求解。

线性代数求解

构造误差项:

$$e_i = c_i - (Rc_i’+t)$$

将这个误差项构造成一个最小二乘问题:

$$\min \limits _{R,t} J= \dfrac{1}{2} \sum \limits _{i=1} ^n \begin{Vmatrix} c_i - (Rc_i’+t) \end{Vmatrix} _2 ^2$$

通过求解这个最小二乘问题,我们可以得到R和t。

总结

相机位姿估计最终目标是获得表征相机运动的旋转矩阵R和平移向量t,可分成两种方法:一种是2D-2D估计加三角测量,另一种是3D-2D估计加3D-3D估计。

参考链接

  1. SLAM相机位姿估计(1),by zhaoxuhui.
  2. 2D-2D相机位姿估计,by 金戈大王.
  3. 单目相机中的对极几何,by 一索哥传奇.
  4. 三角测量,by 金戈大王.
  5. 三角测量,by wikipedia.
  6. 单目相机中的三角化测量,by 一索哥传奇.
  7. 3D-2D相机位姿估计,by 金戈大王.
  8. 3D-3D相机位姿估计,by 金戈大王.
  9. 相机位姿求解问题?, by zhihu.
  10. 3D-2D的运动估计,by 一索哥传奇.
  11. 3D-3D的运动估计,by 一索哥传奇.
  12. 相机位姿求解——P3P问题,by 达达MFZ.
  13. 图像二维坐标转世界三维坐标,by 橙子.

光线追踪基本概念入门

发表于 2019-05-26 | 更新于 2022-05-11

光线追踪(Ray tracing)是三维计算机图形学中的特殊渲染算法,跟踪从眼睛发出的光线而不是光源发出的光线,通过这样一项技术生成编排好的场景的数学模型显现出来。光线追踪的优点可以提供更为真实的光影效果,缺点是计算量巨大。

基本概念

光线追踪与光栅化渲染作为相对的两个概念,理解光栅化渲染更能解释光线追踪的概念。

光栅化渲染是将向量图形格式表示的图像转换成位图以用于显示器或者打印机输出的过程,如图1所示。

光线追踪示意图

图1 光栅化渲染示意图

光线追踪的示意图如图2所示。[Whitted 1980]提出了使用光线跟踪来在计算机上生成图像的方法,这一方法后来也被称为经典光线跟踪方法或Whitted-style 光线跟踪方法。其主要思想是从视点向成像平面上的像素发射光线,找到与该光线相交的最近物体的交点,如果该点处的表面是散射面,则计算光源直接照射该点产生的颜色;如果该点处表面是镜面或折射面,则继续向反射或折射方向跟踪另一条光线,如此递归下去,直到光线逃逸出场景或达到设定的最大递归深度。这种经典的方法可以产生镜面反射、折射、阴影等效果,不过不能实现其他的全局光照的效果。

光线追踪示意图

图2 光线追踪示意图

辐射度学基本量

图形学模拟可见光与各种材质的交互,这个过程涉及到能量的传输。辐射度学(Radiometry)是度量电磁辐射能量传输的学科,也是基于物理着色模型的基础。

  • 能量

能量(Energy),用符号Q表示,单位焦耳(J),每个光子都具有一定量的能量,和频率相关,频率越高,能量也越高。

  • 功率

功率(Power),单位瓦特(Watts),或者焦耳/秒(J/s)。辐射度学中,辐射功率也被称为辐射通量(Radiant Flux)或者通量(Flux),指单位时间内通过表面或者空间区域的能量的总量,用符号\Phi 表示,定义 $\Phi = \frac{ dQ}{dt}$。

  • 辐照度和辐出度

辐照度(Irradiance),指单位时间内到达单位面积的辐射能量,或到达单位面积的辐射通量,也就是通量对于面积的密度。用符号E表示,单位 $W / m^{2}$ 。定义为 $E = \frac{d\Phi }{dA}$。

辐出度(Radiant Existance),也称为辐射出射度、辐射度(Radiosity),用符号M表示。辐出度与辐照度类似,唯一的区别在辐出度衡量的是离开表面的通量密度,辐照度衡量的是到达表面的通量密度。辐照度和辐出度都可以称为辐射通量密度(Radiant Flux Density)。

  • 辐射强度

立体角则是度量三维角度的量,用符号 $\omega$ 表示,单位为立体弧度(也叫球面度,Steradian,简写为sr),等于立体角在单位球上对应的区域的面积(实际上也就是在任意半径的球上的面积除以半径的平方 $\omega = \frac {s}{r^{2} }$ ),单位球的表面积是 $4\pi$ ,所以整个球面的立体角也是$4\pi$ 。

辐射强度(Radiant Intensity),指通过单位立体角的辐射通量。用符号I表示,单位 $W / sr$,定义为 $I = \frac{d \Phi }{d \omega }$ 。之所以引入辐射强度,是因为有时候要度量通过一个点的通量的密度,但因为点的面积是0,无法使用辐照度,所以引入辐射强度。辐射强度不会随距离变化而变化,不像点光源的辐照度会随距离增大而衰减,这是因为立体角不会随距离变化而变化。

  • 辐射率

辐射率(Radiance),指每单位面积每单位立体角的辐射通量密度。用符号 $L$ 表示,单位 $W/m^{2} sr$ ,定义为 $L = \frac{d \Phi }{d\omega d A^{\bot } }$ 。其中 $dA^{\bot}$是微分面积dA在垂直于光线方向的投影。

渲染方程(The Rendering Equation)

[Kajiya 1986]第一次将渲染方程引入图形学,使用它来解释光能传输的产生的各种现象。这一方程描述了场景中光能传输达到稳定状态以后,物体表面某个点在某个方向上的辐射亮度(Radiance)与入射辐射亮度等的关系。

$$L_o(x,w_o)=L_e(x,w_0)+\int_\Omega{f_r(x,w_i,w_0)L_i(x,w_i)cos\theta_idw_i}$$

其中,$L_o(x,w_o)$ 表示物体表面点 $x$ 处在方向 $𝜔_𝑜$ 上出射的辐射亮度,$𝐿𝑒(𝑥, 𝜔_0)$ 表示在该点该方向上自辐射的亮度。$𝐿𝑖(𝑥,𝜔_𝑖)$ 表示该点处 $𝜔_𝑖$ 方向入射的辐射亮度,$𝑓𝑟(𝑥, 𝜔_𝑖, 𝜔_𝑜)$ 是双向反射分布函数(BRDF),描述的是入射方向的辐射亮度对出射方向的贡献,$cos 𝜃𝑖$ 是$𝜔_𝑖$与表面法向的点积。在这一方程的基础上,辐射度方法和蒙特卡罗光线跟踪的方法就可以看成是对方程中积分的不同的数值求解方法。

BRDF描述的是表面本身的性质,比如它的光滑程度,导电程度等等。由于四面八方的光线都会作用在这个表面,所以我们需要对所有方向进行积分,也就是一个球面上的积分,考虑到积分项中的 $(w_i\cdot n)$ ,那么只有位于正半空间的方向才会对最终积分有贡献,所以最后这个球面的积分就变成了一个半球的积分,如图3所示。

光线追踪示意图

图3 BRDF示意图

双向反射分布函数(bidirectional reflectance distribution function, BRDF)

在计算机图形学领域,着色(Shading)是指根据表面或者多边形相对光源和相机的角度和距离来计算它的颜色的过程。不同的用途可以使用不同的着色算法,CAD等追求响应速度的交互式图形领域可以使用简单快速的着色算法,卡通油画等艺术效果可以使用非真实感(Nonphotorealistic)着色算法,而追求真实感的CG电影或游戏则可以使用基于物理建模的着色算法。而BRDF是基于物理建模的着色算法的理论基础。

我们看到一个表面,实际上是周围环境的光照射到表面上,然后表面将一部分光反射到我们眼睛里。双向反射分布函数BRDF(Bidirectional Reflectance Distribution Function)就是描述表面入射光和反射光关系的。

对于一个方向的入射光,表面会将光反射到表面上半球的各个方向,不同方向反射的比例是不同的,我们用BRDF来表示指定方向的反射光和入射光的比例关系,BRDF定义为:

$$f(l,v)=\frac{dL_o(v)}{dE(l)}$$

其中,$f$就是BRDF,$l$是入射光方向,$v$是观察方向,也就是我们关心的反射光方向。$d L_o(v)$ 是表面反射到$v$方向的反射光的微分辐射率。表面反射到$v$方向的反射光的辐射率为$L_o(v)$,来自于表面上半球所有方向的入射光线的贡献,而微分辐射率 $d L_o(v)$ 特指来自方向 $l$ 的入射光贡献的反射辐射率。$dE(l)$是表面上来自入射光方向 $l$ 的微分辐照度。表面接收到的辐照度为 $E$ ,来自上半球所有方向的入射光线的贡献,而微分辐照度 $dE(l)$ 特指来自于方向 $l$ 的入射光。

光照模型(illumination model)

当光照射到物体表面时,物体对光会发生反射、透射、吸收、衍射、折射、和干涉,其中被物体吸收的部分转化为热,反射、透射的光进入人的视觉系统,使我们能看见物体。为模拟这一现象,我们建立一些数学模型来替代复杂的物理模型,这些模型就称为明暗效应模型或者光照明模型。

局部光照模型

在真实感图形学中,仅处理光源直接照射物体表面的光照明模型被称为局部光照明模型。局部光照明模型的分类如图4所示。

局部光照模型分类

图4 局部光照模型分类

局部光照模型是一种比较简单的光照模型,它是与光栅化渲染算法相适应的,光栅化算法一次只考虑一个像素的光照强度,因此局部光照模型不能计算某像素受其他像素影响的光照强度部分。也就是说,局部光照模型只对物体进行直接光照的计算,而不考虑其他的间接影响。

全局光照模型

全局光照模型是基于光学物理原理的,光照强度的计算依赖于光能在现实世界中的传播情况,考虑光线与整个场景中各物体表面及物体表面间的相互影响,包括多次反射 、透射 、散射等。因此,与局部光照模型相比,全局光照模型需要相当大的计算量 ,但同时也能取得非常逼真的真实效果 。全局光照模型分类如图5所示。

全局光照模型分类

图5 全局光照模型分类

求交检测

在光线追踪过程中,从眼睛发出的光线与3D模型的三角面求交是一个复杂问题。通常精致的3D模型可能由几十万至上百万三角面构成,如果采用穷举法求交点,其时间复杂度将是O(n), 过于复杂。为了减少不必要的求交检测,应采用空间划分技术,最常用的是平衡kdtree算法,提高求交检测的效率。

k-d tree

在计算机科学里,k-d树( k-维树的缩写)是在k维欧几里德空间组织点的数据结构。k-d树可以使用在多种应用场合,如多维键值搜索(例:范围搜寻及最邻近搜索)。k-d树是空间二分树(Binary space partitioning )的一种特殊情况。

k-d树是每个节点都为k维点的二叉树。所有非叶子节点可以视作用一个超平面把空间分割成两个半空间。节点左边的子树代表在超平面左边的点,节点右边的子树代表在超平面右边的点。选择超平面的方法如下:每个节点都与k维中垂直于超平面的那一维有关。因此,如果选择按照x轴划分,所有x值小于指定值的节点都会出现在左子树,所有x值大于指定值的节点都会出现在右子树。这样,超平面可以用该x值来确定,其法线为x轴的单位向量。

参考链接

  1. 光线追踪,by wikipedia.
  2. 栅格化,by wikipedia.
  3. 渲染,by wikipedia.
  4. 光线追踪基本概念与代码实现,by 鹅城惊喜师爷.
  5. 一篇光线追踪的入门,by 洛城.
  6. 基于蒙特卡罗的光线跟踪绘制方法,by 严俊.
  7. 基于物理着色:BRDF,by Maple.
  8. Monte-Carlo Ray Tracing System (一)原理以及设计,by 已退逼乎.
  9. 冯氏光照模型–镜面光的计算,by MooAiFighting.
  10. 什么是光照模型,by 黄琦.
  11. 蒙特卡洛光线追踪,by sunacmer.
  12. k-d树,by wikipedia.
  13. 辐射强度、辐亮度、辐照度——一文搞定,by 三眼二郎.
  14. 从光栅化到光线追踪,by CrazyEngineCo.
  15. windows下没有srand48和drand48的解决方法,by 查志强.
  16. 辐射照度、辐射强度、光照度、发光强度(差异以及如何相互转换)(易懂讲解),by 三眼二郎.

SLAM中位姿估计的数学基础

发表于 2019-05-20 | 更新于 2021-05-10

同步定位与地图构建(SLAM或Simultaneous localization and mapping)是一种概念:希望机器人从未知环境的未知地点出发,在运动过程中通过重复观测到的地图特征(比如,墙角,柱子等)定位自身位置和姿态,再根据自身位置增量式的构建地图,从而达到同时定位和地图构建的目的。

SLAM的核心工作有两个:

  • 地图构建

研究如何把从一系列传感器收集到的信息,集成到一个一致性的模型上的问题。它可以被描述为第一核心问题:这个世界长什么样?地图构建的核心部分是环境的表达方式以及传感器数据的解释。

  • 定位

研究在地图上估测机器人的坐标和姿势形态的问题;换而言之,机器人需要回答这里的第二核心问题,我在哪?典型的解包含以下两个方面:追踪——通常机器人的初始位置已知;全局定位——通常只给出很少,甚至不给出有关于起始位置环境特征的先验信息。

下面主要介绍SLAM定位研究中涉及的数学知识。

向量点积

有如下两个向量:

$$a=\begin{pmatrix}
a_{1}\
a_{2}\
a_{3}
\end{pmatrix},b=\begin{pmatrix}
b_{1}\
b_{2}\
b_{3}
\end{pmatrix}$$

它们的点乘定义如下:

$$a\cdot b=a^{T}\cdot b = \sum_{i=1}^{3}a_{i}b_{i}=\left | a \right |\left | b \right |cos<a,b>$$

即两个向量点乘等于一个向量的转置乘以另一个向量。

向量叉积

两个向量做叉积结果还是一个向量,这个向量垂直于这两个做叉积的向量所组成的平面。

$$
a\times b=\begin{vmatrix}
i & j & k\
a_{1} & a_{2} & a_{3}\
b_{1} & b_{2} & b_{3}
\end{vmatrix}=(a_{2}b_{3}-a_{3}b_{2})i+(a_{3}b_{1}-a_{1}b_{3})j+(a_{1}b_{2}-a_{2}b_{1})k $$

为了计算方便,将两个向量写成行列式的形式,然后按对角线法则计算行列式的值。i、j、k分量分别即代表x、y、z轴方向的分量。写成列向量的形式如下:

$$
\begin{pmatrix}
a_{2}b_{3}-a_{3}b_{2}\
a_{3}b_{1}-a_{1}b_{3}\
a_{1}b_{2}-a_{2}b_{1}
\end{pmatrix}=\begin{pmatrix}
0\cdot b_{1}-a_{3}b_{2}+a_{2}b_{3}\
a_{3}b_{1}-0\cdot b_{2}-a_{1}b_{3}\
-a{2}b_{1}+a_{1}b_{2}+0\cdot b_{3}
\end{pmatrix}=\begin{pmatrix}
0 & -a_{3} & a_{2}\
a_{3} & 0 & -a_{1}\
-a_{2} & a_{1} & 0
\end{pmatrix}\begin{pmatrix}
b_{1}\
b_{2}\
b_{3}
\end{pmatrix} $$

再将其写成与b相乘的矩阵形式,提取系数矩阵。这个系数矩阵就称作向量a的反对称矩阵,用a^记。所谓反对称矩阵,是指满足下面这个条件的矩阵。

$$A^{-1}=-A$$

因为向量叉积的结果是一个向量,因此可以用来表示旋转,结果向量的方向为旋转轴,大小为旋转角。结果可以看作一个旋转向量。

对极几何

两个相机在不同位置拍摄同一物体,两张照片中的景物有重叠部分,那么理论上这两张照片会存在一定的对应关系,而对极几何就是探索描述它们之间对应关系的工具。相机在不同位置拍摄同一物体示意图如图1所示。

相机在不同位置拍摄同一物体示意图

图1 相机在不同位置拍摄同一物体示意图

图中的概念有:

  • 极点e:分别是左边相机中心在右图像平面上的像,右相机中心在左像平面上的像。

  • 极平面:两个相机中心和空间中某店p形成的平面。

  • 极线l:极平面分别和两个像平面的交线。

对极几何则是描述这几个量之间的对应关系。直观讲,从左图的角度看,如果不知道p点的深度信息,射线op是这个点可能出现的空间位置,因为该射线上的点都会投影到同一个像素点,同时,如果不知道p点的具体位置,那么当在右图的位置看时,极线 l’ 就是点p可能出现的位置,即在这条线上的某个地方。如图2所示:

物体位置示意图

图2 物体位置示意图

在计算机视觉中,基础矩阵(Fundamental matrix) F 是一个3×3的矩阵,表达了立体像对的像点之间的对应关系。在对极几何中,对于立体像对中的一对同名点,它们的齐次化图像坐标分别为 $p$ 与 $p’$, ${\displaystyle \mathrm {F} p}$ 表示一条必定经过 $p’$的直线(极线)。这意味着立体像对的所有同名点对都满足:

$${\displaystyle p’^{\top }\mathrm {F} p=0.}$$

F矩阵中蕴含了立体像对的两幅图像在拍摄时相互之间的空间几何关系(外参数)以及相机检校参数(内参数),包括旋转、位移、像主点坐标和焦距。因为 F 矩阵的秩为2,并且可以自由缩放(尺度化),所以只需7对同名点即可估算出F的值。

基础矩阵这一概念由Q. T. Luong在他那篇很有影响力的博士毕业论文中提出。Faugeras则是在1992年发表的著作中以上面的关系式给出了 F 矩阵的定义。尽管Longuet-Higgins提出的本质矩阵也满足类似的关系式,但本质矩阵中并不蕴含相机检校参数。本质矩阵与基础矩阵之间的关系可由下式表达:

$$ {\displaystyle \mathrm {E} =\mathrm {K’^{\top }} \mathrm {FK} .}$$

其中 $K$和 $K’$分别为两个相机的内参数矩阵。

参考链接

  1. SLAM数学基础,by zhaoxuhui.
  2. 即时定位与地图构建,by wikipedia.
  3. 大牛讲堂|SLAM第一篇:基础知识, by 地平线HorizonRobotics.
  4. 学习SLAM需要哪些预备知识?,by wikipedia.
  5. 计算机视觉中的数学方法,by 吴福朝.
  6. 对极几何及单应矩阵, by Rap_God.
    ​

PX4无人机MAVROS外部控制教程

发表于 2019-05-15

PX4中的offboard模式能够接受来自外部的控制指令,搭配机载或支持MAVROS的协同计算机,可在PX4飞控平台上加入视觉处理或人工智能,以实现无人机自动控制功能。下面即介绍如何在基于MAVROS的协同计算机上运行程序控制PX4无人机。具体过程如下:

PX4固件参数设置

在开始无人机外部控制开发之前,您需要在PX4固件中设置两件事:

  • 将RC开关映射到场外模式激活

要执行此操作,请在QGroundControl中加载参数并查找RC_MAP_OFFB_SW参数,您可以为其分配要用于激活offboard模式的RC通道。 以这样一种方式映射事物是非常有用的。当你脱离offboard模式时,你会进入位置控制。

虽然此步骤不是强制性的,因为您可以使用MAVLink消息激活offboard模式。 我们认为这种方法更安全。

  • 启用配套计算机接口

查找SYS_COMPANION参数并将其设置为921600(推荐)或57600. 此参数将激活Telem2端口上的MAVLink流,其中包含特定于板载模式的数据流以及相应的波特率(921600 8N1或57600 8N1)。

本人实验测试时SYS_COMPANION参数设置成Companion Link(57600) 。此外要注意通过Telem2端口的无线收发装置与地面支持MAVROS的具有无线收发装置的协同计算机连接。并长按安全锁手动解锁无人机。

安装MAVROS

在地面协同计算机上安装MAVROS请参考Ubuntu16.04安装MAVROS,完成后使用如下命令启动MAVROS:

1
2
3
$ roscore
# 启动MAVROS,链接到本地ROS
$ roslaunch mavros px4.launch fcu_url:=serial:///dev/ttyUSB0:57600

构建Offboard示例程序

ROS节点Offboard示例程序的构建请参考ROS构建Package和Node的教程,完成后使用如下命令启动Offboard示例程序:

1
2
3
$ cd catkin_ws/
$ source devel/setup.bash
$ rosrun offboard offboard_node

参考链接

  1. PX4软件在环仿真之MAVROS外部控制,by jackhuang.
  2. Offboard Control, by dronecode.

PX4软件在环仿真之MAVROS外部控制

发表于 2019-05-15

PX4软件在环仿真之MAVROS外部控制将使用基于MAVROS的PX4外部控制ROS节点,控制在Gazebo仿真环境中的四悬翼无人机,如图1所示。

px4软件在环仿真

图1 px4软件在环仿真

具体步骤如下:

构建PX4自驾仪

请参考PX4编译开发环境配置和Development Environment on Linux配置PX4编译开发环境。需要指出的是,为使用Gazebo仿真环境,PX4开发工具链使用脚本ubuntu_sim_ros_gazebo.sh构建。

PX4编译开发环境配置完成后,使用如下命令启动:

1
2
$ cd ~/src/Firmware
$ make posix_sitl_default gazebo

安装MAVROS

安装MAVROS请参考Ubuntu16.04安装MAVROS,完成后使用如下命令启动MAVROS:

1
2
3
$ roscore
# 启动MAVROS,链接到本地ROS
$ roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

构建Offboard示例程序

ROS节点Offboard示例程序的构建请参考ROS构建Package和Node的教程,完成后使用如下命令启动Offboard示例程序:

1
2
3
$ cd catkin_ws/
$ source devel/setup.bash
$ rosrun offboard offboard_node

最终能够在Gazebo仿真环境中观察到四悬翼无人机垂直起飞到2米的高度。

参考链接

  1. Ubuntu16.04安装MAVROS,by jackhuang.
  2. ROS构建Package和Node的教程,by jackhuang.
  3. PX4编译开发环境配置,by jackhuang.
  4. Development Environment on Linux, by dronecode.

ROS构建Package和Node的教程

发表于 2019-05-15

ROS(机器人操作系统,Robot Operating System),是专为机器人软件开发所设计出来的一套电脑操作系统架构。它是一个开源的元级操作系统(后操作系统),提供类似于操作系统的服务,包括硬件抽象描述、底层驱动程序管理、共用功能的执行、程序间消息传递、程序发行包管理,它也提供一些工具和库用于获取、建立、编写和执行多机融合的程序。

ROS的运行架构是一种使用ROS通信模块实现模块间P2P的松耦合的网络连接的处理架构,它执行若干种类型的通讯,包括:

  • 基于服务的同步RPC(远程过程调用)通讯;
  • 基于Topic的异步数据流通讯,还有参数服务器上的数据存储。

ROS可与PX4一起用于飞行器的外部控制, 它使用MAVROS节点与在硬件上运行的PX4或使用Gazebo Simulator进行通信。MAVROS节点的安装请参考Ubuntu16.04安装MAVROS,下面以编写基于MAVROS的外部控制节点为例,介绍ROS包和节点的构建过程。

ROS包构建方法

catkin和rosbuild是构建ROS包的两种方法。目前多用catkin构建ros包。

catkin包可以构建为独立项目,与构建普通cmake项目的方式相同,但catkin还提供了工作空间的概念,您可以在其中同时构建多个相互依赖的包。

ROS包构成

ROS包是其代码组织的一种高级结构好哦,最简单的catkin包的构成如下:

1
2
3
my_package/
CMakeLists.txt
package.xml

其中:

  • 包必须包含一个符合catkin的package.xml文件。
    • 该package.xml文件提供有关包的元信息。
  • 包必须包含一个使用catkin的CMakeLists.txt。
    • 如果它是catkin元数据包,它必须具有相关的样板CMakeLists.txt文件。
  • 包都必须有自己的文件夹。
    • 这意味着没有嵌套包,也没有多个包共享同一目录。

catkin工作空间的构成

使用catkin软件包的推荐方法是使用catkin工作区,但您也可以独立构建catkin软件包。 一个简单的工作空间可能如下所示:

1
2
3
4
5
6
7
8
9
10
workspace_folder/        -- WORKSPACE
src/ -- SOURCE SPACE
CMakeLists.txt -- 'Toplevel' CMake file, provided by catkin
package_1/
CMakeLists.txt -- CMakeLists.txt file for package_1
package.xml -- Package manifest for package_1
...
package_n/
CMakeLists.txt -- CMakeLists.txt file for package_n
package.xml -- Package manifest for package_n

创建catkin包

通常使用catkin_create_pkg命令创建新的catkin包,具体步骤如下:

1
2
3
4
5
6
7
8
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
# 创建名为offboard的ros包,它依赖roscpp、mavros和geometry_msgs
$ catkin_create_pkg offboard roscpp mavros geometry_msgs
$ cd offboard/src/
# 创建offboard_node.cpp,将官方PX4外部控制例程代码写入该文件
# 该文件也是offboard包的第一个node
$ touch offboard_node.cpp

offboard_node.cpp文件的具体内容如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
/**
* @file offb_node.cpp
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
* Stack and tested in Gazebo SITL
*/

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}

int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");

//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);

// wait for FCU connection
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}

geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;

//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}

mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";

mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;

ros::Time last_request = ros::Time::now();

while(ros::ok()){
if( current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
} else {
if( !current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
}

local_pos_pub.publish(pose);

ros::spinOnce();
rate.sleep();
}

return 0;
}

之后需要修改~/catkin_ws/src/offboard目录下的CMakeLists.txt文件。取消掉一些注释,生成相应节点(否则会出现找不到节点的错误)。

修改CMakeLists.txt文件

构建catkin包

在catkin工作空间(catkin_ws)中,使用catkin_make工具从源文件构建和安装一个包:

1
2
3
4
5
6
7
$ cd ~/catkin_ws
$ catkin_make
$ catkin_make install
# 配置catkin工作空间,使ros能找到刚生成的offboard包
$ source devel/setup.bash
# 查看ros包路径环境变量是否配置好
$ echo $ROS_PACKAGE_PATH

参考链接

  1. Ubuntu16.04安装MAVROS,by jackhuang.
  2. Creating a ROS Package, by ros homepage.
  3. Building a ROS Package, by ros homepage.
  4. Understanding ROS Nodes, by ros homepage.
  5. catkin 与 rosbuild解析及两者区别和联系,by Xuefeng_BUPT.
  6. Pixhawk原生固件PX4之SITL软件在环仿真,by FantasyJXF.
  7. Pixhawk原生固件PX4之offboard,by FantasyJXF.
  8. MAVROS Offboard control example, by dronecode.

使用nvm进行nodejs版本管理

发表于 2019-05-11 | 更新于 2022-05-21

nvm是一个简单bash脚本,用于管理多个活动的nodejs版本。在linux操作系统中使用nvm安装和管理nodejs十分方便。在windows操作系统下则使用nvm-windows对nodejs版本进行管理。下面简单介绍ubuntu 16.04操作系统下nvm的安装使用。

安装脚本

在Ubuntu 16.04的终端中,直接运行如下命令:

1
wget -qO- https://raw.githubusercontent.com/nvm-sh/nvm/v0.34.0/install.sh | bash

管理Nodejs

  • 安装最新nodejs长期支持版
    1
    2
    3
    4
    5
    6
    7
    8
    # in linux
    nvm ls-remote
    # in windows
    nvm ls available
    # install node and use
    nvm install --lts
    nvm install 12.22.12
    nvm use 12.22.12
  • 卸载最新nodejs长期支持版
    1
    nvm uninstall --lts

nvm设置代理

在 nvm 的安装路径下,找到 settings.txt,在后面加上这两行:

1
2
node_mirror: https://npm.taobao.org/mirrors/node/
npm_mirror: https://npm.taobao.org/mirrors/npm/

nvm离线安装

无法连接互联网的Windows环境下,使用nvm的前提是,需要在有互联网环境下将自己所需要的 Node版本,使用 nvm install 版本号 下载到本地,然后将 C:\Users\Administrator\AppData\Roaming\nvm 目录下的对应版本文件拷贝到离线安装后的文件夹中即可。

参考链接

  1. Node Version Manager, by nvm-sh.
  2. nvm-windows,by nvm-windows.
  3. nvm 在 Windows 中的使用及离线安装,by tyronchen.
  4. Windows上安装nodejs版本管理器nvm,by weiqinl.
  5. nvm 设置下载 node 的镜像地址,by xhlwill.
  6. 使用 nvm 管理不同版本的 node 与 npm,by runoob.
上一页1…404142…53下一页

Jack Huang

523 日志
67 标签
© 2025 Jack Huang
由 Hexo 强力驱动
|
主题 — NexT.Muse