Simon Shi的小站

人工智能,机器学习, 强化学习,大模型,自动驾驶

0%

loguru.hpp

office web: GitHub - emilk/loguru: A lightweight C++ logging library

Docments

loguru一共只需要两个源文件: loguru.hpp 和 loguru.cpp.
链接的时候还需要: -lpthread -ldl

系统配置

1
2
3
loguru::g_stderr_verbosity = 9;                            // print everything
// 不想打印到console
loguru::g_stderr_verbosity = loguru::Verbosity_OFF; // not print anthing

日志配置

1
loguru::add_file("everything.log", loguru::Append, loguru::Verbosity_MAX);

标准输出,后缀_F类似于printf

1
LOG_F(INFO, "I'm hungry for some %.3f!", 3.14159);

判断程序,如果真,则输出

1
LOG_IF_F(ERROR, true, "Will only show if badness happens");

断言,会中断程序

1
CHECK_F(1 == 0, "Assertion 1 == 0 failed.\n '%s'\n '%d' ", a.c_str(), 1000);

Demo

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
/// g++ demo.cpp loguru.cpp -lpthread -ldl

#include "loguru.hpp"
#include <cstdio>
#include <string>

int main(int argc,char *argv[]){
loguru::g_stderr_verbosity = 9;

// Optional, but useful to time-stamp the start of the log.
// Will also detect verbosity level on command line as -v.
// loguru::init(argc, argv);

// Put every log message in "everything.log":
loguru::add_file("everything.log", loguru::Append, loguru::Verbosity_MAX);

// Only log INFO, WARNING, ERROR and FATAL to "latest_readable.log":
loguru::add_file("latest_readable.log", loguru::Truncate, loguru::Verbosity_INFO);

// Only show most relevant things on stderr:
// loguru::g_stderr_verbosity = 1;

LOG_SCOPE_F(INFO, "Will indent all log messages within this scope.");
LOG_F(INFO, "I'm hungry for some %.3f!", 3.14159);
LOG_F(2, "Will only show if verbosity is 2 or higher");
// VLOG_F(get_log_level(), "Use vlog for dynamic log level (integer in the range 0-9, inclusive)");
LOG_IF_F(ERROR, true, "Will only show if badness happens");
// auto fp = fopen(filename, "r");
std::string a = "Here is a string.";
//CHECK_F(1 == 0, "Assertion 1 == 0 failed.\n '%s'\n '%d' ", a.c_str(), 1000);
/// CHECK_GT_F(length, 0); // Will print the value of `length` on failure.
/// CHECK_EQ_F(a, b, "You can also supply a custom message, like to print something: %d", a + b);

// Each function also comes with a version prefixed with D for Debug:
/// DCHECK_F(expensive_check(x)); // Only checked #if !NDEBUG
DLOG_F(INFO, "Only written in debug-builds");

// Turn off writing to stderr:
loguru::g_stderr_verbosity = loguru::Verbosity_OFF;

// Turn off writing err/warn in red:
loguru::g_colorlogtostderr = false;
return 0;
}

参考:简书-logurn

Win窗口快捷键

Win + ←:最大化窗口到左侧的屏幕上(与开始屏幕应用无关)
Win + →:最大化窗口到右侧的屏幕上(与开始屏幕应用无关)

Win+ ↑:最大化窗口(与开始屏幕应用无关)
Win+ ↓:最小化窗口(与开始屏幕应用无关)

Win+ SHIFT +↑:垂直拉伸窗口,宽度不变(与开始屏幕应用无关)
Win+ SHIFT +↓:垂直缩小窗口,宽度不变(与开始屏幕应用无关)

Win+SHIFT+←:将活动窗口移至左侧显示器 (与开始屏幕应用无关)
Win+SHIFT+→:将活动窗口移至右侧显示器(与开始屏幕应用无关)

WIN桌面快捷键

Ctrl + Win + ← 切换到上一个桌面

Ctrl + Win + → 切换到下一个桌面

WPS

编辑
复制格式 Ctrl+Shift+C
粘贴格式 Ctrl+Shift+V
撤销 Ctrl+Z
恢复 Ctrl+Y
插入分页符 Ctrl+Enter
插入换行符 Shift+Enter
插入空域 Ctrl+F9
插入超链接 Ctrl+K
删除行 Ctrl+”-“
格式
增大字号 Ctrl+Shift+. 或者
Ctrl+]
减小字号 Ctrl+Shift+, 或者
Ctrl+[
上标 Ctrl+Shift+=
下标 Ctrl++
筛选 Ctrl+Shift+L
合并单元格 Ctrl + M
替换 Ctrl + H
运算 求和 Alt + “=”

VS Code

Ctrl+K Ctrl+L 全部展开
Ctrl+K Ctrl+0 全部折叠
Ctrl + shift + U 切换输出视图
Ctrl + shift + down
Ctrl + shift + down
Alt + shift + down
Alt + shift + down
Alt + shift + down
Alt + shift + down
Alt + shift + down

目标:

  • 理解图像特征点的意义,掌握在单张图像中提取出特征点,掌握多副图像匹配特征点的方法

  • 理解对极几何的原理,利用对极几何的约束,恢复图像之间摄像机的三维运动

  • 理解PNP(3D-2D)问题,利用已知三维结构与图像对应关系,求解摄像机的三维运动

  • 理解ICP问题(3D-3D),利用点云匹配关系,求解摄像机的三维运动

  • 理解如何通过三角化,获得二维图像上对应点的三维结构。

视觉里程计(VO)算法 现状
特征点法 目前的主流算法
直接法 追赶者的角色

特征点提取算法

方法
角点 Harris 角点、FAST 角点 、GFTT 角点
局部图像特征 SIFT 、SURF 、ORB 速度: ORB < SIFI
关键点 描述子
ORB特征 Oriented FAST and Rotated BRIEF BRIEF

匹配算法
- 暴力匹配

    - 汉明距离

    - 快速近似最近邻

1
2
3
4
5
6
7
st=>start: Start
e=>end: End
op1=>operation: 特征点 | past
op2=>operation: ORB匹配 | past
op3=>operation: 特征匹配 | past

st(right)->op1(right)->op2(right)->op3(right)->e

1、特征点法

一个 SLAM 系统分为前端和后端,其中是视觉SLAM前端也称为视觉里程计(VO)。VO根据相邻图像的信息估计出粗略的相机运动,给后端提供较好的初始值。VO的算法主要分为两个大类:特征点法直接法。基于特征点法的前端,被认为是视觉里程计的主流方法。它具有稳定,对光照、动态物体不敏感的优势,是目前比较成熟的解决方案。如何提取、匹配图像特征点,然后估计两帧之间的相机运动和场景结构,从而实现一个两帧间视觉里程计。这类算法也称为两视图几何(Two-view geometry)

1.1 特征点

VO的核心问题是如何根据图像来估计相机运动。然而,图像本身是一个由亮度和色彩组成的矩阵,如果直接从矩阵层面考虑运动估计,将会非常困难。所以,比较方便的做法是:首先,从图像中选取比较有代表性的点。这些点在相机视角发生少量变化后会保持不变,于是能在各个图像中找到相同的点。然后,在这些点的基础上,讨论相机位姿估计问题,以及这些点的定位问题。在经典SLAM模型中称这些点为路标(Landmark)。而在视觉 SLAM 中,路标则是指图像特征(Feature)。其定义为:一组与计算任务相关的信息,计算任务取决于具体的应用。简而言之,特征是图像信息的另一种数字表达形式。一组好的特征对于在指定任务上的最终表现至关重要。数字图像在计算机中以灰度值矩阵的方式存储,所以最简单的单个图像像素也是一种“特征”。但是,在视觉里程计中希望特征点在相机运动之后保持稳定,而灰度值受光照、形变、物体材质的影响严重,在不同图像间变化非常大,不够稳定。理想的情况是,当场景和相机视角发生少量改变时,算法还能从图像中判断哪些地方是同一个点。所以,仅凭灰度值是不够的,需要对图像提取特征点。 特征点是图像里一些特别的地方,可以把图像中的角点、边缘和区块都当成图像中有代表性的地方。不过更容易精确地指出,某两幅图像中出现了同一个角点;同一 个边缘则稍微困难一些,因为沿着该边缘前进,图像局部是相似的;同一个区块则是最困难的。可以发现,图像中的角点、边缘相比于像素区块而言更加“特别”,在不同图像之间的辨识度更强。所以,一种直观的提取特征的方式就是在不同图像间辨认角点,确定它们的对应关系。在这种做法中,角点就是所谓的特征。角点的提取算法有很多,例如Harris 角点、FAST 角点 、GFTT 角点等等。然而,在大多数应用中,单纯的角点依然不能满足很多需求。例如,从远处看上去是角点的地方,当相机走近之后,可能就不显示为角点了。或者,当旋转相机时,角点的外观会发生变化,也就不容易辨认出那是同一个角点。为此,就提出了许多更加稳定的局部图像特征,如著名的SIFT 、SURF 、ORB ,等等。相比于普通角点,这些人工设计的特征点能够拥有如下的性质:

  1. 可重复性(Repeatability):相同的特征可以在不同的图像中找到。
  2. 可区别性(Distinctiveness):不同的特征有不同的表达。
  3. 高效率(Efficiency):同一图像中,特征点的数量应远小于像素的数量。
  4. 本地性(Locality):特征仅与一小片图像区域相关。

特征点由关键点(Key-point)和描述子(Descriptor)两部分组成。关键点是指该特征点在图像里的位置,有些特征点还具有朝向、大小等信息。描述子通常是一个向量,按照某种人为设计的方式,描述了该关键点周围像素的信息。描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。因此,只要两个特征点的描述子在向量空间上的距离相近,就可以认为它们是同样的特征点。SIFT(尺度不变特征变换,Scale-Invariant FeatureTransform):充分考虑了在图像变换过程中出现的光照、尺度、旋转等变化,但随之而来的是极大的计算量。 FAST关键点则考虑适当降低精度和鲁棒性,以提升计算的速度,属于计算特别快的一种特征点(没有描述子);而ORB(Oriented FASTand Rotated BRIEF)特征则是改进了 FAST 检测子不具有方向性的问题,并采用速度极快的二进制描述子BRIEF,使整个图像特征提取的环节大大加速。根据作者在论文中所述测试,在同一幅图像中同时提取约 1000 个特征点的情况下,ORB 约要花费 15.3ms,SIFT 约花费 5228.7ms。由此可以看出,ORB在保持了特征子具有旋转、尺度不变性的同时,速度方面提升明显,对于实时性要求很高的SLAM来说是一个很好的选择。大部分特征提取都具有较好的并行性,可以通过 GPU 等设备来加速计算。经过GPU加速后的SIFT,就可以满足实时计算要求。但是,引入GPU将带来整个SLAM成本的提升。在目前的 SLAM方案中,ORB 是质量与性能之间较好的折中。

1.2 ORB特征

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

  1. FAST 角点提取:找出图像中的“角点”。相较于原版的 FAST,ORB中计算了特征点的主方向,为后续的BRIEF描述子增加了旋转不变特性。
  2. BRIEF 描述子:对前一步提取出特征点的周围图像区域进行描述。ORB 对BRIEF进行了一些改进,主要是指在 BRIEF中使用了先前计算的方向信息。

FAST 关键点

FAST 是一种角点,主要检测局部像素灰度变化明显的地方,以速度快著称。它的思想是:如果一个像素与邻域的像素差别较大(过亮或过暗),那么它更可能是角点。相比于其他角点检测算法,FAST 只需比较像素亮度的大小,十分快捷。它的检测过程如下:

  1. 在图像中选取像素p,假设它的亮度为Ip。
  2. 设置一个阈值T (比如,Ip 的20%)。
  3. 以像素p为中心,选取半径为3的圆上的16个像素点。
  4. 假如选取的圆上有连续的N个点的亮度大于 Ip + T 或小于 Ip − T,那么像素p可以被认为是特征点(N通常取12,即为 FAST-12。其他常用的 N 取值为 9 和 11,它们分别被称为FAST-9 和 FAST-11)。
  5. 循环以上四步,对每一个像素执行相同的操作。

在 FAST-12 算法中,为了更高效,可以添加一项预测试操作,以快速地排除绝大多数不是角点的像素。具体操作为,对于每个像素,直接检测邻域圆上的第 1, 5, 9, 13 个像素的亮度。只有当这 4个像素中有 3 个同时大于 Ip + T 或小于 Ip − T 时,当前像素才有可能是一个角点,否则应该直接排除。这样的预测试操作大大加速了角点检测。此外,原始的 FAST角点经常出现“扎堆”的现象。所以在第一遍检测之后,还需要用非极大值抑制(Non-maximal suppression),在一定区域内仅保留响应极大值的角点,避免角点集中的问题。

FAST特征点的计算仅仅是比较像素间亮度的差异,所以速度非常快,但它也有重复性不强,分布不均匀的缺点。此外,FAST角点不具有方向信息。同时,由于它固定取半径为3的圆,存在尺度问题:远处看着像是角点的地方,接近后看可能就不是角点了。针对FAST角点不具有方向性和尺度的弱点,ORB添加了尺度和旋转的描述。尺度不变性由构建图像金字塔,并在金字塔的每一层上检测角点来实现。而特征的旋转是由灰度质心法(Intensity Centroid)实现的。 金字塔是计算图视觉中常用的一种处理方法,如下图。

金字塔底层是原始图像。每往上一层,就对图像进行一个固定倍率的缩放,这样就有了不同分辨率的图像。较小的图像可以看成是远处看过来的场景。在特征匹配算法中可以匹配不同层上的图像,从而实现尺度不变性。 例如,如果相机在后退,那么应该能够在上一个图像金字塔的上层和下一个图像的下层中找到匹配。 在旋转方面,计算特征点附近的图像灰度质心。所谓质心是指以图像块灰度值作为权重的中心。其具体操作步骤如下:

  1. 在一个小的图像块B中,定义图像块的矩为

  1. 通过矩可以找到图像块的质心:

  1. 连接图像块的几何中心O与质心 C,得到一个方向向量 OC,于是特征点的方向可以定义为

$$
\theta = \arctan(m_{01}/m_{10})
$$

通过以上方法,FAST角点便具有了尺度与旋转的描述,从而大大提升了其在不同图像之间表述的鲁棒性。ORB 中,把这种改进后的FAST称为 Oriented FAST。

注:金字塔是指对图像进行不同层次的降采样,以获得不同分辨率的图像。

BRIEF 描述子

在提取 Oriented FAST 关键点后,对每个点计算其描述子。ORB使用改进的BRIEF特征描述。

BRIEF 是一种二进制描述子,其描述向量由许多个0和1组成,这里的0和1编码了关键点附近两个随机像素(比如 p 和 q)的大小关系:如果 p 比 q 大,则取 1,反之就取 0。如果取了 128 个这样的 p, q,最后就得到128维由 0、1 组成的向量。BRIEF 使用了随机选点的比较,速度非常快,而且由于使用了二进制表达,存储起来也十分方便,适用于实时的图像匹配。原始的 BRIEF描述子不具有旋转不变性,因此在图像发生旋转时容易丢失。而 ORB 在 FAST 特征点提取阶段计算了关键点的方向,所以可以利用方向信息,计算了旋转之后的“Steer BRIEF”特征使ORB 的描述子具有较好的旋转不变性。 由于考虑到了旋转和缩放,使得 ORB 在平移、旋转和缩放的变换下仍有良好的表现。同时,FAST和BREIF的组合也非常高效,使得ORB特征在实时SLAM中非常受欢迎。

1.3 特征匹配

特征匹配是视觉 SLAM 中极为关键的一步,解决了SLAM中的数据关联问题(data association),即确定当前看到的路标与之前看到的路标之间的对应关系。通过对图像与图像或者图像与地图之间的描述子进行准确匹配,可以为后续的姿态估计、优化等操作减轻大量负担。然而,由于图像特征的局部特性,误匹配的情况广泛存在,成为视觉SLAM中制约性能提升的一大瓶颈。部分原因是场景中经常存在大量的重复纹理,使得特征描述非常相似。在这种情况下,仅利用局部特征解决误匹配是非常困难的。

考虑两个时刻的图像。如果在图像It中提取到特征点

$$
x_t^m,m = 1,2…,M
$$

在图像 It+1 中也提取到特征点xnt+1。如何寻找这两个集合元素的对应关系?最简单的特征匹配方法就是暴力匹配(Brute-Force Matcher)。即对每一个特征点 xm t 与所有的 xn t+1 测量描述子的距离,然后排序,取最近的一 个作为匹配点。描述子距离表示了两个特征之间的相似程度,不过在实际运用中还可以取不同的距离度量范数。对于浮点类型的描述子,使用欧氏距离进行度量即可。而对于二进制的描述子(比如BRIEF),往往使用汉明距离(Hamming distance)作为度量——两个二进制串之间的汉明距离,指的是其不同位数的个数。 然而,当特征点数量很大时,暴力匹配法的运算量将变得很大,特别是当想要匹配某个帧和一张地图的时候。这不符合在SLAM中的实时性需求。此时快速近似最近邻(FLANN)算法更加适合于匹配点数量极多的情况。这些匹配算法理论已经成熟,实现上集成到OpenCV了。

[47] M. Muja and D. G. Lowe, “Fast approximate nearest neighbors with automatic algorithm configuration.,” in VISAPP (1), pp. 331–340, 2009.

2、实践:特征提取和匹配

OpenCV集成了多数主流的图像特征,可以很方便地进行调用。

2.1 OpenCV 的 ORB 特征

调用OpenCV来提取和匹配ORB。图像位于slambook2/ch7/下的1.png 和 2.png,可以看到相机发生了微小的运动。程序演示如何提取ORB特征并进行匹配:

slambook/ch7/orb_cv.cpp

首先要安装g2o,github下载20200410版本的:https://github.com/RainerKuemmerle/g2o.git

对ch7文件夹中的cmake工程进行编译,报错:

1
undefined reference to `vtable for fmt::v7::format_error‘

链接上fmt 库:

1
target_link_libraries("可执行文件" ${Sophus_LIBRARIES} fmt)

运行结果:

未筛选的匹配中带有大量的误匹配。经过一次筛选之后,匹配数量减少了许多,大多数匹配都是正确的。这里筛选的依据是汉明距离小于最小距离的两倍,这是一种工程上的经验方法。尽管在示例图像中能够筛选出正确的匹配,但仍然不能保证在所有其他图像中得到的匹配都是正确的。因此,在后面的运动估计中,还需要使用去除误匹配的算法。

2.2 手写 ORB 特征

代码:slambook/ch7/orb_self.cpp

运行结果:

在计算中用256位的二进制描述,即对应到8个32位的unsigned int数据,用 typedef 将它表示成 DescType。然后计算 FAST 特征点的角度,再使用该角度计算描述子。此代码中通过三角函数的原理回避了复杂的 arctan 以及 sin、cos 计算,从而达到加速的效果。在 BfMatch 函数中还使用了 SSE 指令集中的_mm_popcnt_u32 函数来计算一个 unsigned int 变量中1的个数,从而达到计算汉明距离的效果。

这个程序中,通过一些简单的算法修改,对ORB的提取加速了数倍。如果对提取特征部分进一步并行化处理,算法还可以有加速的空间。

2.3 计算相机运动

有了匹配好的点对后要根据点对来估计相机的运动。

  1. 当相机为单目时,只知道2D的像素坐标,因而问题是根据两组2D点估计运动。该问题用对极几何来解决。
  2. 当相机为双目、RGB-D 时,或者通过某种方法得到了距离信息,那么问题就是根据两组3D点估计运动。该问题通常用 ICP 来解决。
  3. 如果一组为 3D,一组为 2D,即得到了一些 3D 点和它们在相机的投影位置,也能估计相机的运动。该问题通过 PnP 求解。

3、2D−2D: 对极几何(Epipolar Geometry)

描述的是两幅视图之间的内在射影关系,与外部场景无关,只依赖于摄像机内参数和这两幅试图之间的的相对姿态。

对极几何(Epipolar Geometry)是Structure from Motion问题中,在两个相机位置产生的两幅图像的之间存在的一种特殊几何关系,是sfm问题中2D-2D求解两帧间相机姿态的基本模型。

3.1对极约束

如果有若干对这样的匹配点,就可以通过这些二维图像点的对应关系,恢复出在两帧之间摄像机的运动。求取两帧图像I1, I2之间的运动,设第一帧到第二帧的运动为R,t。两个相机中心分别为O1,O2。如果I1中有一个特征点p1 ,它在I2中对应着特征点p2,两者是通过特征匹配得到的。如果匹配正确,说明它们是同一个空间点在两个成像平面上的投影。首先,连线O1p1和连线O2p2在三维空间中会相交于点P。这时候点O1, O2 ,P三个点可以确定一个平面,称为极平面(Epipolar plane) 。O1O2连线与像平面I1, I2 的交点分别为 e1, e2 。e1, e2 称为极点(Epipoles) ,O1O2被称为基线(Baseline) 。称极平面与两个像平面 I1, I2之间的相交线 L1, L2极线(Epipolar line)

直观讲,从第一帧的角度看,射线O1p1是某个像素可能出现的空间位置——因为该射线上的所有点都会投影到同一个像素点。同时,如果不知道P的位置,那么当在第二幅图像上看时,连线e2p2(也就是第二幅图像中的极线)就是P可能出现的投影的位置,也就是射线O1p1在第二个相机中的投影。由于通过特征点匹配确定了p2的像素位置,所以能够推断P的空间位置,以及相机的运动。如果没有特征匹配,就没法确定p2到底在极线的哪个位置了。

从代数角度来看一下这里的几何关系。在第一帧的坐标系下,设P的空间位置为P = [X, Y, Z]T.根据针孔相机模型知道两个像素点 p1, p2的像素位置为:

这里K为相机内参矩阵,R, t 为两个坐标系的相机运动。具体来说,这里计算的是 R21和 t21 ,因为是把第一个坐标系下的坐标转换到第二个坐标系下。在使用齐次坐标时,一个向量等于它自身乘上任意的非零常数。这通常用于表达一个投影关系。例如 s1p1 和 p1 成投影关系,它们在齐次坐标的意义下是相等的。称这种相等关系为尺度意义下相等(equal up to a scale),记作:sp ≃ p.那么,上述两个投影关系可写为:p1 ≃ KP ,p2 ≃ K(RP + t) .现在取:

这里的x1,x2是两个像素点的归一化平面上的坐标。代入上式,得:x2 ≃ Rx1 + t.

两边同时左乘 t∧ 。根据∧的定义,这相当于两侧同时与t做外积:

然后,两侧同时左乘

得:

观察等式左侧

是一个与 t 和x2都垂直的向量。把它再和x2做内积时,将得到0。由于等式左侧严格为零,那么乘以任意非零常数之后也为零,于是可以把 ≃ 写成通常的等号。因此,就得到了:

重新代入 p1, p2 ,有:

这两个式子都称为对极约束。它的几何意义是O1, P, O2三者共面。对极约束中同时包含了平移和旋转。把中间部分记作两个矩阵:基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E,于是可以进一步简化对极约束:

对极约束简洁地给出了两个匹配点的空间位置关系。于是,相机位姿估计问题变为以下两步:

  1. 根据配对点的像素位置求出 E 或者 F 。
  2. 根据E或者F求出 R, t。由于 E 和 F 只相差了相机内参,而内参在SLAM中通常是已知的,所以实践当中往往使用形式更简单的E。

3.2本质矩阵E

根据定义,本质矩阵 E = t∧R。它是一个3 × 3的矩阵,内有 9 个未知数。从E的构造方式上看,有以下值得注意的地方:

• 本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束,所以对E乘以任意非零常数后,对极约束依然满足。这称为E在不同尺度下是等价的。

• 根据 E = t∧R,可以证明,本质矩阵E的奇异值必定是 [σ, σ, 0]T的形式。这称为本质矩阵的内在性质。

• 另一方面,由于平移和旋转各有 3 个自由度,故 t∧R 共有 6 个自由度。但由于尺度等价性,故 E 实际上有 5 个自由度。表明最少可以用5对点来求解E。但是,E的内在性质是一种非线性性质,在估计时会带来麻烦,因此,也可以只考虑它的尺度等价性,使用8对点来估计E——这就是经典的八点法(Eight-point-algorithm) 。八点法只利用了E的线性性质,因此可以在线性代数框架下求解。

考虑一对匹配点,它们的归一化坐标为 x1= [u1 , v1 , 1] T , x2 = [u2 , v2 , 1] T 。根据对极约束,有:

把矩阵 E 展开,写成向量的形式:e = [e1 , e2 , e3 , e4 , e5 , e6 , e7 , e8 , e9 ] T ,那么对极约束可以写成与e有关的线性形式:[u1u2 , u1v2 , u1, v1u2, v1v2, v1, u2, v2, 1] ·e = 0.同理,对于其他点对也有相同的表示。把所有点都放到一个方程中,变成线性方程组(ui , vi 表示第 i 个特征点,以此类推):

这八个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成,大小为 8 × 9。 e 位于该矩阵的零空间中。如果系数矩阵是满秩的(即秩为 8),那么它的零空间维数为 1, 也就是 e 构成一条线。这与 e 的尺度等价性是一致的。如果八对匹配点组成的矩阵满足秩为 8 的条件,那么E的各元素就可由上述方程解得。

接下来的问题是如何根据已经估得的本质矩阵E,恢复出相机的运动 R, t。这个过程是由奇异值分解(SVD)得到的。设 E 的 SVD 分解为:

其中 U,V 为正交阵,Σ 为奇异值矩阵。根据 E 的内在性质,可以知道 Σ = diag(σ, σ, 0)。 在 SVD 分解中,对于任意一个 E,存在两个可能的 t, R 与它对应:

其中

表示沿 Z 轴旋转 90 度得到的旋转矩阵。同时,由于 −E 和 E 等价,所以对任意一个 t 取负号,也会得到同样的结果。因此,从 E 分解到 t, R 时,一共存在四个可能的解。只有第一种解中,P 在两个相机中都具有正的深度。因此,只要把任意一点代入四种解中,检测该点在两个相机下的深度,就可以确定哪个解是正确的了。

如果利用 E 的内在性质,那么它只有五个自由度。所以最小可以通过五对点来求解相机运动。然而这种做法形式复杂,从工程实现角度考虑,由于平时通常会有几十对乃至上百对的匹配点,从八对减至五对意义并不明显。剩下的问题还有一个:根据线性方程解出的 E,可能不满足 E 的内在性质——它的奇异值不一定为 σ, σ, 0 的形式。这时,在做 SVD 时会刻意地把 Σ 矩阵调整成上面的样子。通常的做法是,对八点法求得的 E 进行 SVD 分解后,会得到奇异值矩阵 Σ = diag(σ1, σ2, σ3),不妨设 σ1 ≥ σ2 ≥ σ3。取:

这相当于是把求出来的矩阵投影到了 E 所在的流形上。更简单的做法是将奇异值矩阵取成 diag(1, 1, 0),因为 E 具有尺度等价性,这样做也是合理的。

3.3单应矩阵

单应矩阵(Homography)H ,描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上(比如墙,地面等),则可以通过单应性来进行运动估计。这种情况在无人机携带的俯视相机,或扫地机携带的顶视相机中比较常见。单应矩阵通常描述处于共同平面上的一些点,在两张图像之间的变换关系。考虑在图像I1和I2有一对匹配好的特征点p1和p2。这些特征点落在某平面上。设这个平面满足方程:

然后代入

得:

把直接描述图像坐标 p1 和 p2 之间的变换的中间这部分记为 H,于是 p2 = Hp1。它的定义与旋转、平移以及平面的参数有关。单应矩阵H是一个 3 × 3 的矩阵,求解时的思路可以先根据匹配点计算H,然后将它分解以计算旋转和平移。把上式展开,得:

注意这里的等号是在非零因子下成立的。在实际处理中,通常乘以一个非零因子使得 h9 = 1(在它取非零值时)。然后根据第三行,去掉这个非零因子,于是有:

这样一组匹配点对就可以构造出两项约束(事实上有三个等式,但是因为线性相关,只取前两个),于是自由度为 8 的单应矩阵可以通过 4 对匹配特征点算出(注意:这些特征点不能有三点共线的情况),即求解以下的线性方程组(当 h9 = 0 时,右侧为零):

这种做法把H矩阵看成了向量,通过解该向量的线性方程来恢复H,又称直接线性变换法(Direct Linear Transform)。求出单应矩阵以后需要对其进行分解,才可以得到相应的旋转矩阵 R 和平移向量 t。分解的方法包括数值法与解析法 。单应矩阵的分解会返回四组旋转矩阵与平移向量,并且同时可以计算出它们分别对应的场景点所在平面的法向量。如果已知成像的地图点的深度全为正值(即在相机前方),则又可以排除两组解。最后仅剩两组解,这时需要通过更多的先验信息进行判断。通常可以通过假设已知场景平面的法向量来解决,如场景平面 与相机平面平行,那么法向量n的理论值为1。

单应性在 SLAM 中具有重要意义。当特征点共面,或者相机发生纯旋转的时候,基础矩阵的自由度下降,这就出现了所谓的退化(degenerate)。现实中的数据总包含一些噪声, 这时候如果继续使用八点法求解基础矩阵,基础矩阵多余出来的自由度将会主要由噪 声决定。为了能够避免退化现象造成的影响,通常会同时估计基础矩阵 F 和单应矩阵 H,选择重投影误差比较小的那个作为最终的运动估计矩阵。

总结

2D-2D的情况下,只知道图像坐标之间的对应关系

  • 特征点在平面上的时候,使用H恢复R,t

  • 否则,使用E(相机内参已知,更方便,也更常用)或者F恢复R,t

求得R,t之后

  • 利用三角化计算特征点的3D位置(深度)

4、实践:对极约束求解相机运动(pose_esti_2d2d)

如何通过 Essential 矩阵求解相机运动?

使用匹配好的特征点来计算 E,F 和 H,进而分解 E 得 到 R, t。整个程序使用 OpenCV 提供的算法进行求解。

代码:slambook/ch7/pose_estimation_2d2d.cpp

pose_estimation_2d2d函数提供了从特征点求解相机运动的部分,在函数中输出了 E,F 和 H 的数值,然后验证对极约束是否成立,以及 t ∧R 和 E 在非零数乘下等价的事实。调用此程序的输出结果:

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
build/pose_estimation_2d2d 1.png 2.png
-- Max dist : 95.000000
-- Min dist : 4.000000
一共找到了 79 组匹配点
fundamental_matrix is
[4.844484382466111e-06, 0.0001222601840188731, -0.01786737827487386;
-0.0001174326832719333, 2.122888800459598e-05, -0.01775877156212593;
0.01799658210895528, 0.008143605989020664, 1]
essential_matrix is
[-0.0203618550523477, -0.4007110038118445, -0.03324074249824097;
0.3939270778216369, -0.03506401846698079, 0.5857110303721015;
-0.006788487241438284, -0.5815434272915686, -0.01438258684486258]
homography_matrix is
[0.9497129583105288, -0.143556453147626, 31.20121878625771;
0.04154536627445031, 0.9715568969832015, 5.306887618807696;
-2.81813676978796e-05, 4.353702039810921e-05, 1]
R is
[0.9985961798781875, -0.05169917220143662, 0.01152671359827873;
0.05139607508976055, 0.9983603445075083, 0.02520051547522442;
-0.01281065954813571, -0.02457271064688495, 0.9996159607036126]
t is
[-0.8220841067933337;
-0.03269742706405412;
0.5684264241053522]

t^R=
[0.02879601157010516, 0.5666909361828478, 0.04700950886436416;
-0.5570970160413605, 0.0495880104673049, -0.8283204827837456;
0.009600370724838804, 0.8224266019846683, 0.02034004937801349]
epipolar constraint = [0.002528128704106625]
epipolar constraint = [-0.001663727901710724]
epipolar constraint = [-0.0008009088410884102

可以看出,对极约束的满足精度约在 10−3 量级。OpenCV 使用三角化检测角点的深度是否为正,从而选出正确的解。 对极约束是从 x2 = Rx1 + t 得到的。这里的 R, t 组成的变换矩阵,是第一个图到第二个图的坐标变换矩阵: x2 = T21x1.

4.1 讨论

从演示程序中可以看到,输出的 E 和 F 相差了相机内参矩阵。从 E,F 和 H 都可以分解出运动,不过 H 需要假设特征点位于平面上。 由于 E 本身具有尺度等价性,它分解得到的 t, R 也有一个尺度等价性。而 R ∈ SO(3) 自身具有约束,所以认为 t 具有一个尺度。换言之,在分解过程中,对 t 乘以任意非零常数,分解都是成立的。因此通常把 t 进行归一化,让它的长度等于 1。

尺度不确定性

对 t 长度的归一化,直接导致了单目视觉的尺度不确定性(Scale Ambiguity)。例如,程序中输出的 t 第一维约 0.822。这个 0.822 究竟是指 0.822 米呢,还是 0.822 厘米 呢,是没法确定的。因为对 t 乘以任意比例常数后,对极约束依然是成立的。换言之, 在单目 SLAM 中,对轨迹和地图同时缩放任意倍数,得到的图像依然是一样的。 在单目视觉中,对两张图像的 t 归一化,相当于固定了尺度。虽然不知道它的实际长度为多少,但以这时的 t 为单位 1,计算相机运动和特征点的 3D 位置。这被称为单目 SLAM 的初始化。在初始化之后,就可以用 3D-2D 来计算相机运动了。初始化之后的轨迹和地图的单位,就是初始化时固定的尺度。因此,单目 SLAM 有一步不可避免的初始化。初始化的两张图像必须有一定程度的平移,而后的轨迹和地图都将以此步的平移为单位。 除了对 t 进行归一化之外,另一种方法是令初始化时所有的特征点平均深度为 1,也 可以固定一个尺度。相比于令 t 长度为 1 的做法,把特征点深度归一化可以控制场景的规模大小,使计算在数值上更稳定些。

初始化的纯旋转问题

从 E 分解到 R, t 的过程中,如果相机发生的是纯旋转,导致 t 为零,那么,得到的 E 也将为零,这将导致无从求解 R。不过,此时可以依靠 H 求取旋转,但仅有旋转时,无法用三角测量估计特征点的空间位置,于是,另一个结论是,单目初始化不能只有纯旋转,必须要有一定程度的平移。如果没有平移,单目将无法初始化。在实践当中,如果初始化时平移太小,会使得位姿求解与三角化结果不稳定, 从而导致失败。相对的,如果把相机左右移动而不是原地旋转,就容易让单目 SLAM 初始化。

多于八对点的情况

当给定的点数多于八对时,可以计算一个最小二乘解。对于线性化后的对极约束,我们把左侧的系数矩阵记为 A: Ae = 0

对于八点法,A 的大小为 8 × 9。如果给定的匹配点多于 8,该方程构成一个超定方程,即不一定存在 e 使得上式成立。因此,可以通过最小化一个二次型来求:

于是就求出了在最小二乘意义下的 E 矩阵。不过,当可能存在误匹配的情况时,会更倾向于使用随机采样一致性(Random Sample Concensus, RANSAC)来求,而不是最小二乘。RANSAC 是一种通用的做法,适用于很多带错误数据的情况,可以处理带有错误匹配的数据。

5、三角测量(三角化)

使用对极几何约束估计了相机运动之后,下一步需要用相机的运动估计特征点的空间位置。在单目SLAM中,仅通过单张图像无法获得像素的深度信息,需要通过三角测量(Triangulation)(或三角化)的方法来估计地图点的深度。

三角测量是指,通过在两处观察同一个点的夹角,确定该点的距离。三角测量最早由高斯提出并应用于测量学中,它在天文学、地理学的测量中都有应用。例如可以通过不同季节观察到星星的角度,估计它离我们的距离。在SLAM中主要用三角化来估计像素点的距离。

考虑图像 I1 和 I2,以左图为参考,右图的变换矩阵为 T。相机光心 为 O1 和 O2。在 I1 中有特征点 p1,对应 I2 中有特征点 p2。理论上直线 O1p1 与 O2p2 在场景中会相交于一点 P,该点即是两个特征点所对应的地图点在三维场景中的位置。然而由于噪声的影响,这两条直线往往无法相交。因此可以通过最二小乘去求解。 按照对极几何中的定义,设 x1, x2 为两个特征点的归一化坐标,那么它们满足: s1x1 = s2Rx2 + t.

已知 R, t,想要求解的是两个特征点的深度 s1, s2。这两个深度是可以分开求的,先算s2,那么先对上式两侧左乘一个 $ \hat{x_1} $得:

$$
s_1 \hat{x_1} x_1 = 0 = s_2 \hat{x_1} R x_2 + \hat{x_1} t
$$

该式左侧为零,右侧可看成 s2 的一个方程,可以根据它直接求得 s2。有了 s2,s1也可以求出。于是就得到了两个帧下的点的深度,确定了它们的空间坐标。由于噪声的存在,估得的 R, t不一定精确使方程为0,所以更常见的做法求最小二乘解而不是零解。

6、实践:三角测量

6.1 三角测量代码

代码演示了对之前根据对极几何求解的相机位姿通过三角化求出特征点的空间位置。调用 OpenCV 提供的 triangulation 函数进行三角化。

代码为:slambook/ch7/triangulation.cpp

同时在 main 函数中增加三角测量部分,并验证重投影关系。

代码输出某特征点的信息:

1
2
3
4
point in the first camera frame: [0.0844072, -0.0734976]
point projected from 3D [0.0843702, -0.0743606], d=14.9895
point in the second camera frame: [0.0431343, -0.0459876]
point reprojected from second frame: [0.04312769812378599, -0.04515455276163744, 1]

打印了每个空间点在两个相机坐标系下的投影坐标与像素坐标。这里对于这个特征点来说,通过两个图像下的一对像素坐标的三角化,能得到这个特征点的空间坐标,d为深度,利用这个空间坐标重投影能得到一个投影坐标,跟原始的像素坐标相比有一定的误差。由于误差的存在,它们会有一些微小的差异。误差的量级大约在小数点后第三位。可以看到,三角化特征点的距离大约为 15。但由于尺度不确定性,并不知道这里的 15 究竟是多少米。

6.2 讨论

三角测量是由平移得到的,有平移才会有对极几何中的三角形,才谈的上三角测量。因此,纯旋转是无法使用三角测量的,因为对极约束将永远满足。在平移存在的情况下,还要关心三角测量的不确定性,这会引出一个三角测量的矛盾。

如图所示。当平移很小时,像素上的不确定性将导致较大的深度不确定性。也就是说,如果特征点运动一个像素 δx,使得视线角变化了一个角度 δθ,那么测量到深度值将有 δd 的变化。从几何关系可以看到,当 t 较大时,δd 将明显变小,这说明平移较大时, 在同样的相机分辨率下,三角化测量将更精确。对该过程的定量分析可以使用正弦定理得到,但这里先考虑定性分析。 因此,要增加三角化的精度,其一是提高特征点的提取精度,也就是提高图像分辨率 ——但这会导致图像变大,提高计算成本。另一方式是使平移量增大。但是,平移量增大会导致图像的外观发生明显的变化,比如箱子原先被挡住的侧面显示出来了,比如反射光发生变化了,等等。外观变化会使得特征提取与匹配变得困难。总而言之,增大平移,会导致匹配失效;而平移太小,则三角化精度不够——这就是三角化的矛盾。

拓展:定量地计算每个特征点的位置及不确定性。假设特征点服从高斯分布,并且对它不断地进行观测,在信息正确的情况下,就能够期望它的方差会不断减小乃至收敛。这就得到了一个滤波器,称为深度滤波器(Depth Filter)。

7、3D-2D: PnP

PnP(Perspective-n-Point)是求解 3D 到 2D 点对运动的方法。它描述了当知道 n 个 3D 空间点以及它们的投影位置时,如何估计相机所在的位姿。前面的2D-2D 的对极几何方法需要八个或八个以上的点对(以八点法为例),且存在着初始化、纯旋转和尺度的问题。然而,如果两张图像中,其中一张特征点的 3D 位置已知,那么最少只需三个点对(需要至少一个额外点验证结果)就可以估计相机运动。特征点的 3D 位置可以由三角化,或者由RGB-D 相机的深度图确定。因此,在双目或 RGB-D 的视觉里程计中, 可以直接使用 PnP 估计相机运动。而在单目视觉里程计中,必须先进行初始化,然后才能使用 PnP。3D-2D 方法不需要使用对极约束,又可以在很少的匹配点中获得较好的运动估计,是最重要的一种姿态估计方法。 PnP 问题有很多种求解方法,例如用三对点估计位姿的P3P,直接线性变换(DLT), EPnP(Efficient PnP),UPnP等等。此外,还能用非线性优化的方式,构建最小二乘问题并迭代求解,也就是万金油式的 Bundle Adjustment。

7.1 直接线性变换

考虑某个空间点 P,它的齐次坐标为 P = (X, Y, Z, 1)T。在图像 I1 中,投影到特征点 x1 = (u1, v1, 1)T(以归一化平面齐次坐标表示)。此时相机的位姿 R, t 是未知的。与单应矩阵的求解类似,定义增广矩阵 [R|t] 为一个 3 × 4 的矩阵,包含了旋转与平移信息。展开形式列写如下:

用最后一行把 s 消去,得到两个约束:

定义 T 的行向量: t1 = (t1, t2, t3, t4) T , t2 = (t5, t6, t7, t8) T , t3 = (t9, t10, t11, t12) T , 于是有:

t 是待求的变量,可以看到每个特征点提供了两个关于 t 的线性约束。假设一共有 N 个特征点,可以列出线性方程组:

由于 t 一共有 12 维,因此最少通过六对匹配点,即可实现矩阵 T 的线性求解,这种方法称为直接线性变换(Direct Linear Transform,DLT)。当匹配点大于六对时,可以使用 SVD 等方法对超定方程求最小二乘解。 在 DLT 求解中,直接将 T 矩阵看成了 12 个未知数,忽略了它们之间的联系。因为旋转矩阵 R ∈ SO(3),用 DLT 求出的解不一定满足该约束,它是一个一般矩阵。平移向量属于向量空间,对于旋转矩阵 R,必须针对 DLT 估计的 T 的左边寻找一个最好的旋转矩阵对它进行近似。这可以由 QR 分解完成, 相当于把结果从矩阵空间重新投影到 SE(3) 流形上,转换成旋转和平移两部分。 这里的 x1 使用了归一化平面坐标,去掉了内参矩阵 K 的影响 ——这是因为内参 K 在 SLAM 中通常假设为已知。如果内参未知,也能用 PnP 去估计 K, R, t 三个量。然而由于未知量的增多,效果会差一些。

7.7.2 P3P

P3P仅使用三对匹配点,对数据要求较少,需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点。记 3D 点为 A, B, C,2D 点为 a, b, c,其中小写字母代表的点为大写字母在相机成像平面上的投影,如图所示。

此外,P3P 还需要使用一对验证点,以从可能的解出选出正确的那一个(类似于对极几何情形)。记验证点对为 D − d,相机光心为 O。注意,预知的是 A, B, C 在世界坐标系中的坐标,而不是在相机坐标系中的坐标。一旦3D 点在相机坐标系下的坐标能够算出,就得到了 3D-3D 的对应点,把 PnP 问题转换为了 ICP 问题。三角形之间存在对应关系:

利用余弦定理,有:

对上面三式全体除以

并且记 x = OA/OC, y = OB/OC,得:

有:

化为:

由于知道 2D 点的图像位置,三个余弦角 cos⟨a, b⟩, cos⟨b, c⟩, cos⟨a, c⟩ 是已知的。同时,

可以通过 A, B, C 在世界坐标系下的坐标算出,变换到相机坐标系下之后,并不改变这个比值。该式中的 x, y 是未知的,随着相机移动会发生变化。因此,该方程组是关于 x, y 的一个二元二次方程(多项式方程)。解析地求解该方程组是一个复杂的过程,需要用吴消元法。类似于分解 E 的情况,该方程最多可能得到四个解,但可以用验证点来计算最可能的解,得到 A, B, C 在相机坐标系下的 3D 坐标。然后,根据 3D-3D 的点对,计算相机的运动 R, t。

从 P3P 的原理上可以看出,为了求解 PnP,利用了三角形相似性质,求解投影点 a, b, c 在相机坐标系下的 3D 坐标,最后把问题转换成一个 3D 到 3D 的位姿估计问题。

P3P 也存在着一些问题:

  1. P3P 只利用三个点的信息。当给定的配对点多于 3 组时,难以利用更多的信息。

  2. 如果 3D 点或 2D 点受噪声影响,或者存在误匹配,则算法失效。 所以后续人们还提出了许多别的方法,如 EPnP、UPnP 等。它们利用更多的信息,而且用迭代的方式对相机位姿进行优化,以尽可能地消除噪声的影响。在 SLAM 当中,通常的做法是先使用 P3P/EPnP 等方法估计相机位姿,然后构建最小二乘优化问题对估计值进行调整(Bundle Adjustment)。

7.3 Bundle Adjustment

除了使用线性方法之外,可以把 PnP 问题构建成一个定义于李代数上的非线性最小二乘问题。前面说的线性方法,往往是先求相机位姿,再求空间点位置,而非线性优化则是把它们都看成优化变量,放在一起优化。这是一种非常通用的求解方式,可以用它对 PnP 或 ICP 给出的结果进行优化。在 PnP 中, 这个 Bundle Adjustment 问题,是一个最小化重投影误差(Reprojection error)的问题。

考虑 n 个三维空间点 P 和它们的投影 p,希望计算相机的位姿 R, t,它的李代数表示为 ξ。假设某空间点坐标为 Pi = [Xi , Yi , Zi ]T,其投影的像素坐标为 ui = [ui , vi ]T。 像素位置与空间点位置的关系如下:

写成矩阵形式是:

中间隐含着齐次坐标到非齐次的转换,否则按矩阵的乘法来说,维度是不对的。因为exp (ξ∧) Pi 结果是 4 × 1 的,而它左侧的 K 是 3 × 3 的,所以必须把 exp (ξ ∧) Pi 的前三维取出来,变成三维的非齐次坐标。 现在,由于相机位姿未知以及观测点的噪声,该等式存在一个误差。因此把误差求和,构建最小二乘问题,然后寻找最好的相机位姿,使它最小化:

该问题的误差项,是将像素坐标(观测到的投影位置)与 3D 点按照当前估计的位姿进行投影得到的位置相比较得到的误差,所以称之为重投影误差。使用齐次坐标时,这个误差有 3 维。不过,由于u最后一维为 1,该维度的误差一直为零,因而更多时候使用非齐次坐标,于是误差就只有 2 维了。

如图所示,通过特征匹配,知道了 p1 和 p2 是同一个空间点 P 的投影,但是不知道相机的位姿。在初始值中,P 的投影 pˆ2 与实际的 p2 之间有一定的距离。于是调整相机的位姿,使得这个距离变小。不过,由于这个调整需要考虑很多个点,所以最后每个点的误差通常都不会精确为零。

使用李代数,可以构建无约束的优化问题, 很方便地通过 G-N, L-M 等优化算法进行求解。不过,在使用 G-N 和 L-M 之前,需要知道每个误差项关于优化变量的导数,也就是线性化: e(x + ∆x) ≈ e(x) + J∆x.

这里的 J 的形式是关键所在。固然可以使用数值导数,但如果能够推导解析形式时,会优先考虑解析导数。现在,当 e 为像素坐标误差 (2 维),x 为相机位姿(6 维)时,J 将是一个 2 × 6 的矩阵。

使用扰动模型来求李代数的导数:

首先,记变换到相机坐标系下的空间点坐标为 P′,并且将其前三维取出来: P′ = (exp (ξ∧)P)1:3 = [X′,Y′,Z′]T

相机投影模型相对于P′则为: su = KP′

展开:

利用第 3 行消去 s(实际上就是 P′ 的距离),得:

这与相机模型是一致的。当求误差时,可以把这里的 u, v 与实际的测量值比较,求差。定义了中间变量后对 ξ∧左乘扰动量δξ,然后考虑 e 的变化关于扰动量的导数。利用链式法则,可以列写如下:

这里的⊕指李代数上的左乘扰动。第一项是误差关于投影点的导数,得:

第二项为变换后的点关于李代数的导数,得:

而在 P′ 的定义中,取出了前三维,于是得:

将这两项相乘,就得到了 2 × 6 的雅可比矩阵:

这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。保留了前面的负号,因为这是由于误差是由观测值减预测值定义的。它当然也可反过来,定义成 “预测减观测”的形式。在这种情况下,只要去掉前面的负号即可。此外,如果 se(3) 的定义方式是旋转在前,平移在后时,只要把这个矩阵的前三列与后三列对调即可。

另一方面,除了优化位姿,还希望优化特征点的空间位置。因此,需要讨论 e 关于空间点 P 的导数。仍利用链式法则,有:

第二项,按照定义 P′ = exp(ξ∧)P = RP + t.

发现 P′ 对 P 求导后只剩下 R。于是:

以上是观测相机方程关于相机位姿与特征点的两个导数矩阵。它们能够在优化过程中提供重要的梯度方向,指导优化的迭代。

8、实践:求解 PnP

8.1 使用 EPnP 求解位姿

首先用 OpenCV 提供的 EPnP 求 解 PnP 问题,然后通过 g2o 对结果进行优化。由于 PnP 需要使用 3D 点,为了避免初始化带来的麻烦,使用了 RGB-D 相机中的深度图(1_depth.png),作为特征点的 3D 位置。

代码为: slambook/ch7/pose_estimation_3d2d.cpp

在例程中,得到配对特征点后,在第一个图的深度图中寻找它们的深度,并求出空间位置。以此空间位置为 3D 点,再以第二个图像的像素位置为 2D 点,调用 EPnP 求解 PnP 问题。程序输出后可以看到,在有3D信息时,估计的 R 几乎是相同的,而 t 相差的较多。这是由于引入了新的深度信息所致。 不过,由于 Kinect 采集的深度图本身会有一些误差,所以这里的 3D 点也不是准确的。后面会希望把位姿 ξ 和所有三维特征点 P 同时优化。

8.2 使用 BA 优化

使用前一步的估计值作为初始值,把问题建模成一个最小二乘的图优化问题,如图所示。

在这个图优化中,节点和边的选择为:

  1. 节点:第二个相机的位姿节点 ξ ∈ se(3),以及所有特征点的空间位置 P ∈ R3。
  2. 边:每个 3D 点在第二个相机中的投影,以观测方程来描述: zj = h(ξ, Pj ).

由于第一个相机位姿固定为零,没有把它写到优化变量里。现在根据一组 3D 点和第二个图像中的 2D 投影,估计第二个相机的位姿。所以把第一个相机画成虚线,表明不希望考虑它。 g2o 提供了许多关于 BA 的节点和边,不必自己从头实现所有的计算。在 g2o/types/sba/types_six_dof_expmap.h 中提供了李代数表达的节点和边。文件中有VertexSE3Expmap(李代数位姿)、VertexSBAPointXYZ(空间点位置) 和 EdgeProjectXYZ2UV(投影方程边)这三个类。

VertexSE3Expmap的类定义:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

VertexSE3Expmap();

bool read(std::istream& is);

bool write(std::ostream& os) const;

virtual void setToOriginImpl() {
_estimate = SE3Quat();
}

virtual void oplusImpl(const double∗ update_) {
Eigen::Map<const Vector6d> update(update_);
setEstimate( SE3Quat::exp(update)∗estimate());
}
};

模板参数:

  1. 第一个参数 6 表示它内部存储的优化变量维度,可以看到这是一个 6 维的李代数。
  2. 第二参数是优化变量的类型,这里使用了 g2o 定义的相机位姿:SE3Quat。 这个类内部使用了四元数加位移向量来存储位姿,但同时也支持李代数上的运算,例如对数映射(log 函数)和李代数上增量(update 函数)等操作。
  3. 空间点位置类的维度为 3,类型是 Eigen 的 Vector3D。另一方面,边 EdgeProjectXYZ2UV 连接了两个前面说的两个顶点,它的观测值为 2 维,由 Vector2D 表示,实际上就是空间点的像素坐标。它的误差计算函数表达了投影方程的误差计算方法,也就是前面提到的 z − h(ξ, P ) 的方式。

EdgeProjectXYZ2UV 的 linearizeOplus 函数的实现用到了前面推导的雅可比矩阵:

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
void EdgeProjectXYZ2UV::linearizeOplus() {
VertexSE3Expmap ∗ vj = static_cast<VertexSE3Expmap ∗>(_vertices[1]);
SE3Quat T(vj−>estimate());
VertexSBAPointXYZ∗ vi = static_cast<VertexSBAPointXYZ∗>(_vertices[0]);
Vector3D xyz = vi−>estimate();
Vector3D xyz_trans = T.map(xyz);

double x = xyz_trans[0];
double y = xyz_trans[1];
double z = xyz_trans[2];
double z_2 = z∗z;

const CameraParameters ∗ cam = static_cast<const CameraParameters ∗>(parameter(0));

Matrix<double,2,3,Eigen::ColMajor> tmp;
tmp(0,0) = cam−>focal_length;
tmp(0,1) = 0;
tmp(0,2) = −x/z∗cam−>focal_length;

tmp(1,0) = 0;
tmp(1,1) = cam−>focal_length;
tmp(1,2) = −y/z∗cam−>focal_length;

_jacobianOplusXi = −1./z ∗ tmp ∗ T.rotation().toRotationMatrix();

_jacobianOplusXj(0,0) = x∗y/z_2 ∗cam−>focal_length;
_jacobianOplusXj(0,1) = −(1+(x∗x/z_2)) ∗cam−>focal_length;
_jacobianOplusXj(0,2) = y/z ∗cam−>focal_length;
_jacobianOplusXj(0,3) = −1./z ∗cam−>focal_length;
_jacobianOplusXj(0,4) = 0;
_jacobianOplusXj(0,5) = x/z_2 ∗cam−>focal_length;

_jacobianOplusXj(1,0) = (1+y∗y/z_2) ∗cam−>focal_length;
_jacobianOplusXj(1,1) = −x∗y/z_2 ∗cam−>focal_length;
_jacobianOplusXj(1,2) = −x/z ∗cam−>focal_length;
_jacobianOplusXj(1,3) = 0;
_jacobianOplusXj(1,4) = −1./z ∗cam−>focal_length;
_jacobianOplusXj(1,5) = y/z_2 ∗cam−>focal_length;
}

它与公式是一致的。成员变量“*- jacobianOplusXi”是误差到空间点的导数,“*jacobianOplusXj”是误差到相机位姿的导数,以李代数的左乘扰动表达。稍有差别的是,g2o 的相机里用 f 统一描述 fx, fy,并且 李代数定义顺序不同(g2o 是旋转在前,平移在后),所以矩阵前三列和后三列与上面的定义是颠倒的。

们在上一个 PnP 例程的基础上,加上 g2o 提供的 Bundle Adjustment:

首先声明了 g2o 图优化,配置优化求解器和梯度下降方法,然后根据估计到的特征点,将位姿和空间点放到图中。最后调用优化函数进 行求解。

优化结果:迭代 11 轮后,LM 发现优化目标函数接近不变,于是停止了优化。输出了最后得到位姿变换矩阵 T,对比之前直接做 PnP 的结果,大约在小数点后第三位发生了一些变化。这主要是由于同时优化了特征点和相机位姿导致的。

Bundle Adjustment 是一种通用的做法。它可以不限于两个图像。可以放入多个图像匹配到的位姿和空间点进行迭代优化,甚至可以把整个 SLAM 过程放进来。那种做法规模较大,主要在后端使用。在前端,我们通常 考虑局部相机位姿和特征点的小型 Bundle Adjustment 问题,希望实时对它进行求解和优化。

9、3D-3D: ICP

3D-3D的位姿估计问题。假设有一组配对好的3D点,

$$
P = { p_1, …, p_n} \quad P^{‘} = { p_1^{‘}, …, p_n^{‘}}
$$

我们想要找到一个欧式变换R,t使得:

$$
\forall_i, p_i = Rp_i^{‘} + t.
$$

这个问题可以用迭代最近点(Iterative Closest Point, ICP)求解。3D-3D位姿问题中,没有相机模型,也就是说,仅考虑两组3D点之间的变换时,跟相机并没有关系。在RGB-D SLAM中,可以用这种方式估计相机位姿。

两种求解方式:1、线性代数的求解(SVD)2、非线性优化方式(类Bundle Adjustment)

9.1 SVD方法

定义误差项:

$$
e_i = p_i - (Rp_i^{‘} + t ).
$$

构建最小二乘问题,求误差平方和达到最小的R,t;

$$
\min_{R,t}J = \frac{1}{2} \sum_{i=1}^n || p_i - (Rp_i^{‘} + t)||_2^{2}
$$

下面推导求解方法,定义两组质心:

$$
p = \frac{1}{n} \sum_{i=1}^{n} (p_i) ,
\qquad
p^{‘} = \frac{1}{n} \sum_{i=1}^{n} (p^{‘}_i)
$$

注意,质心没有下标的。随后带入误差函数,如下处理:

$$
\frac{1}{2} \sum_{i=1}^{n} || p_i - (Rp_i^{‘} + t )||^2 =
\frac{1}{2} \sum_{i=1}^{n} || p_i - Rp_i^{‘} -t -p +Rp^{‘} +p -Rp^{‘})||^2
\
\qquad \qquad \qquad \qquad \qquad
= \frac{1}{2} \sum_{i=1}^{n} || p_i -p - R(p_i^{‘} - p^{‘}) + (p - Rp^{‘} + t)||^2
\
\qquad \qquad \qquad \qquad \qquad \qquad
= \frac{1}{2} \sum_{i=1}^{n} || p_i -p - R(p_i^{‘} - p^{‘}) ||^2

  • ||(p - Rp^{‘} + t)||^2 +
    \ \qquad \qquad \qquad \qquad \qquad
    2(p_i^{‘} - p^{‘} - R(p_i^{‘} - p^{‘}))^T (p-Rp^{‘}-t)
    $$

注意到交叉项部分中, $(p_i^{‘} - p^{‘} - R(p_i^{‘} - p^{‘}))$ 在求和之后为零的,因此优化目标函数可以简化为:

$$
\min_{R,t} J = \frac{1}{2} \sum_{i=1}^{n} || p_i -p - R(p_i^{‘} - p^{‘}) ||^2

  • ||(p - Rp^{‘} + t)||^2
    $$

仔细观察左右两项,左边只和旋转矩阵R相关,右边既有R也有t,单是只跟质心相关,只要获取了R,令第二项为零,即可得到t。

因此分三个步骤:

9.2 非线性优化方法

于是,在非线性优化中只需不断迭代,我们就能找到极小值。而且,可以证明 [6] , ICP 问题存在唯一解或无穷多解的情况。
在唯一解的情况下,只要我们能找到极小值解,那么 这个极小值就是全局最优值——因此不会遇到局部极小而非全局最小的情况。
这也意味着 ICP 求解可以任意选定初始值。这是已经匹配点时求解 ICP 的一大好处。 需要说明的是,我们这里讲的 ICP ,
是指已经由图像特征给定了匹配的情况下,进行 位姿估计的问题。在匹配已知的情况下,这个最小二乘问题实际上具有解析解 [52, 53, 54] ,
所以并没有必要进行迭代优化。 ICP 的研究者们往往更加关心匹配未知的情况。不过,在 RGB-D SLAM 中,由于一个像素的深度数据可能测量不到,
所以我们可以混合着使用 PnP 和 ICP 优化:对于深度已知的特征点,用建模它们的 3D-3D 误差;对于深度未知的特征
点,则建模 3D-2D 的重投影误差。于是,可以将所有的误差放在同一个问题中考虑,使得 求解更加方便。

10、实践:求解ICP

引用:

SLAM_OV 第七讲-视觉里程计-1 特征点法和特征提取和匹配实践

SLAM_OV 第七讲-视觉里程计-2 对极几何和对极约束、本质矩阵、基础矩阵

SLAM OV 第七讲-视觉里程计-3,4 单应矩阵和实践

SLAM OV 第七讲-视觉里程计-5,6 视觉里程计-三角测量和实践

SLAM OV 第七讲-视觉里程计-7,8 第七讲-视觉里程计-PnP和实践

CDSN: 相机运动估计 三、 3D - 3D: ICP_贵在坚持,不忘初心的博客-CSDN博客

1、状态估计问题

1.1 批量状态估计与最大后验估计

经典 SLAM 模型由一个运动方程和一个观测方程构成:

$$
x_k = f (x_{k-1}, u_k) + w_k \
z_{k,j} = h(y_i, x_k) + v_{k,j}
$$

xk是相机的位姿变量,可以由Tk∈SE(3)表达。运动方程与输入的具体形式有关,在视觉SLAM中没有特殊性(和普通的机器人、车辆的情况一样)。观测方程则由针孔模型给定。假设在xk处对路标yj进行了一次观测,对应到图像上的像素位置zk,j,那么,观测方程可以表示成

$$
sz_{k,j} = K(R_ky_j + t_k)
$$

其中K为相机内参,s为像素点的距离,也是(Rkyj +tk)的第三个分量。如果使用变换矩阵Tk描述位姿,那么路标点yj必须以齐次坐标来描述,计算完成后要转换为非齐次坐标。

在运动和观测方程中,通常假设两个噪声项wk,vk,j满足零均值的高斯分布:

$$
w_k \sim N(0, R_k), v_k \sim N(0, Q_{k,j})
$$

其中N表示高斯分布,0表示零均值,Rk,Qk,j为协方差矩阵。在这些噪声的影响下,希望通过带噪声的数据z和u推断位姿x和地图y(以及它们的概率分布),这构成了一个状态估计问题。

处理这个状态估计问题的方法分成两种。

第一种:由于在SLAM过程中,这些数据是随时间逐渐过来的,所以在直观上,应该持有一个当前时刻的估计状态,然后用新的数据来更新它。这种方式称为增量(incremental)的方法,或者叫滤波器。在SLAM的早期研究,主要使用扩展卡尔曼滤波器(EKF)及其衍生方法来求解。

第二种:是把数据累加起来一并处理,这种方式称为批量(batch)的方法。例如,可以把0到k时刻所有的输入和观测数据都放在一起,求在这样的输入和观测下,如何估计整个0到k时刻的轨迹与地图?

增量方法仅关心当前时刻的状态估计xk,而对之前的状态则不多考虑;相对地,批量方法可以在更大的范围达到最优化,被认为优于传统的滤波器,成为当前视觉SLAM的主流方法。极端情况下,可以让机器人或无人机收集所有时刻的数据,再带回计算中心统一处理,这也正是SfM(Structure from Motion)的主流做法。这种极端情况不实时,不符合SLAM的运用场景。所以在SLAM中,实用的方法通常是一些折中的手段。比如,固定一些历史轨迹,仅对当前时刻附近的一些轨迹进行优化,这就是滑动窗口估计法。还有因子图增量平滑优化的方法,能够增量增加优化问题并进行动态调整,能够达到有滤波器的速度和图优化的精度。

先讨论批量方法,考虑从1到N的所有时刻,并假设有M个路标点。定义所有时刻的机器人位姿和路标点坐标为:

$$
x = {x_1, …, x_N}, y={y_1, …, y_M}
$$

用不带下标的u表示所有时刻的输入,z表示所有时刻的观测数据。对机器人状态的估计,从概率学的观点来看,就是已知输入数据u和观测数据z的条件下,求状态x*,*y的条件概率分布:

$$
P(x,y|z,u)
$$

特别地,当不知道控制输入,只有一张张图像时,即只考虑观测方程带来的数据时,相当于估计P(x*,*y|z)的条件概率分布,此问题也称为Structure from Motion(SfM),即如何从许多图像中重建三维空间结构。

为了估计状态变量的条件分布,利用贝叶斯法则,有:

$$
P(x,y|z,u) = \frac{P(z,u|x,y) P(x,y)}{P(z,u)} \infty
\underbrace{P(z,u|x,y)}{似然} \underbrace{P(x,y)}{先验}
$$

贝叶斯法则左侧称为后验概率,右侧的 P(z|x) 称为似然(Likehood),另一部分 P(x) 称为先验(Prior)。直接求后验分布是困难的,但是求一个状态最优估计,使得在该状态下后验概率最大化(Maximize a Posterior,MAP),则是可行的:

$$
(x, y)^*_{Map} = argmax P (x,y| z,u) = argmax P(z,u|x,y)P(x,y)
$$

贝叶斯法则的分母部分与待估计的状态x,y无关,因而可以忽略。贝叶斯法则说明,求解最大后验概率等价于最大化似然和先验的乘积。进一步,如果不知道机器人位姿或路标大概在什么地方,此时就没有了先验。那么,可以求解最大似然估计(Maximize Likelihood Estimation,MLE):

$$
(x,y)^*_{MLE} = argmax P(z,u| x,y)
$$

似然是指“在现在的位姿下,可能产生怎样的观测数据”。但是由于知道观测数据,所以最大似然估计可以理解成:“在什么样的状态下,最可能产生现在观测到的数据”。这就是最大似然估计的直观意义。

1.2 最小二乘的引出

如何求最大似然估计呢?在高斯分布的假设下,最大似然能够有较简单的形式。回顾观测模型,对于某一次观测:

$$
z_{k,j} = h(y_i, x_k) + v(k,j)
$$

假设噪声项vk ∼ N (0,Qk,j),观测数据的条件概率为:

$$
P(z_{j,k} | x_k, y_i) = N( h(y_i, x_k), Q_{k,j})
$$

它依然是一个高斯分布。考虑单次观测的最大似然估计,可以使用最小化负对数来求一个高斯分布的最大似然。

高斯分布在负对数下有较好的数学形式。考虑任意高维高斯分布x ∼ N(µ,Σ),它的概率密度函数展开形式为:

$$
P(x) = \frac{1}{\sqrt[]{ (2\pi)^N det(\Sigma)}}
\exp \big( -\frac{1}{2}(x-\mu)^T \Sigma^{-1}(x-\mu) \big)
$$

对其取负对数,则变为:

$$
-\ln(P(x))= \frac{1}{2} \ln \big({ (2\pi)^N det(\Sigma)} \big)

  • -\frac{1}{2}(x-\mu)^T \Sigma^{-1}(x-\mu)
    $$

因为对数函数是单调递增的,所以对原函数求最大化相当于对负对数求最小化。在最小化上式的x时,第一项与x无关,可以略去。于是,只要最小化右侧的二次型项,就得到了对状态的最大似然估计。代入SLAM的观测模型,相当于在求:

$$
b
$$

该式等价于最小化噪声项(即误差)的一个二次型。这个二次型称为马哈拉诺比斯距离(Mahalanobis distance),又叫马氏距离。它也可以看成是由(Qk,j)-1 加权之后的欧氏距离(二范数),这里(Qk,j)-1也叫做信息矩阵,即高斯分布协方差矩阵之逆。

现在考虑批量时刻的数据。通常假设各个时刻的输入和观测是相互独立的,这意味着各个输入之间是独立的,各个观测之间是独立的,并且输入和观测也是独立的。于是可以对联合分布进行因式分解:

$$
b
$$

这说明可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差:

$$
e_{u,k} = x_k - f(x_{k-1},u_k) \
e_{z,j,k} = z_{k,j} - h(x_k, y_i)
$$

那么,最小化所有时刻估计值与真实值之间的马氏距离,等价于求最大似然估计。负对数允许把乘积变成求和:

$$
\min J(x,y) = \sum_{k} e_{u,k}^T R_{k}^{-1} e_{u,k} +
\sum_{k}\sum_{j} e_{z,k,j}^T R_{k,j}^{-1} e_{z,k,j}
$$

这样就得到了一个最小二乘问题(Least Square Problem),它的解等价于状态的最大似然估计。直观上看,由于噪声的存在,当我们把估计的轨迹与地图代入 SLAM 的运动、观测方程中时,它们并不会完美地成立。这时对状态的估计值进行微调,使得整体的误差下降一些。当然这个下降也有限度,它一般会到达一个极小值。这就是一个典型非线性优化的过程。

SLAM 中的最小二乘问题具有一些特定的结构:

• 整个问题的目标函数由许多个误差的(加权的)二次型组成。虽然总体的状态变量维数很高,但每个误差项都是简单的,仅与一两个状态变量有关。例如,运动误差只与$x_{k−1}, x_k$ 有关,观测误差只与$x_k,y_j$有关。这种关系会让整个问题有一种稀疏的矩阵形式,计算量大大减少。

• 如果使用李代数表示增量,则该问题是无约束的最小二乘问题。但如果用旋转矩阵/变换矩阵描述位姿,则会引入旋转矩阵自身的约束,即需在问题中加入$R^TR = I$且$det(R) =1$的条件。额外的约束会使优化变得更困难。这体现了李代数的优势。

• 使用了二次型度量误差。误差的分布将影响此项在整个问题中的权重。例如,某次的观测非常准确,那么协方差矩阵就会“小”,而信息矩阵就会“大”,所以这个误差项会在整个问题中占有较高的权重。

1.3 例子:批量状态估计

考虑一个离散时间系统:

$$
x_k = x_{k-1} + u_k + w_k, \quad w_k \sim N(0, Q_k) \
z_k = x_{k} + n_k, \qquad\qquad n_k \sim N(0, R_k)
$$

这可以表达一辆沿 x 轴前进或后退的汽车。第一个公式为运动方程,uk 为输入,wk 为噪声;第二个公式为观测方程,zk 为对汽车位置的测量。取时间 k = 1,…,3,现希望根据已有的 v,y 进行状态估计。设初始状态 x0 已知,来推导批量(batch)状态的最大似然估计。

首先,令批量状态变量为$x = [x_0,x_1,x_2,x_3]^T$,令批量观测为$z = [z_1,z_2, z_3]^T$,定义$u = [u_1,u_2,u_3]^T$。最大似然估计为:

$$
x^*_{map} = \argmax P(x|u,z) = \argmax P(u,z|x)\
\quad
= \sum^{3}{k=1} (u_k| x{k-1, x_k})
\sum^{3}_{k=1} (z_k| )
$$

运动方程:

$$
P(u_k| x_{k-1}, x_k) = N (x_k - x_{k-1}, Q_k)
$$

观测方程:

$$
P(z_k, x_k) = N(x_k, R_k)
$$

构建误差变量:

$$
e_{u,k} = x_k - x_{k-1} - u_k, \quad
e_{z,k} = z_k - x_k
$$

于是最小二乘的目标函数为

$$
\min \sum_{k=1}^3 e_{u,k}^T Q_k^{-1} e_{u,k} +
\sum_{k=1}^3 e_{z,k}^T R_{k}^{-1} e_{z,k}
$$

这个系统是线性系统,将它写成向量形式。定义向量$y = [u,z]^T$,那么可以写出矩阵H,使得:

$$
y - Hx = e \sim N(0, \Sigma)
$$

那么:

$$
H = []

\
\Sigma = diag(Q_1, Q_2, Q_3, R_1, R_2, R_3)
$$

整个问题可以写成:

$$
x_{map}^* = \argmax e^T \Sigma^{-1}e.
$$

这个问题有唯一的解:

$$
x_{map}^* = (H^T \Sigma^{-1} H)^{-1} H^T \Sigma^{-1}y
$$

2、非线性最小二乘

考虑一个最小二乘问题:

$$
\min_{x} F(x) = \frac{1}{2} ||f(x)||^2_2
$$

其中,自变量$x\in R_n$,f是任意标量非线性函数f(x):Rn->R。注意这里的系数1/2是无关紧要的。如何求解这样一个优化问题:如果f是个数学形式上很简单的函数,那么该问题可以用解析形式来求。令目标函数的导数为零,然后求解x的最优值,就求二元函数的极值一样:

$$
\frac{dF}{dx} = 0
$$

解此方程,就得到了导数为零处的极值,可能是极大,极小或鞍点处的值。只要逐个比较函数值大小即可。如果 f 为简单的线性函数,那么这个问题就是简单的线性最小二乘问题,但是有些导函数形式复杂,使得该方程不容易求解。求解这个方程需要知道关于目标函数的全局性质,而通常这是不大可能的。对于不方便直接求解的最小二乘问题,可以用迭代的方式,从一个初始值出发,不断地更新当前的优化变量,使目标函数下降。具体步骤可列写如下:

  1. 给定某个初始值x0。
  2. 对于第 k 次迭代,寻找一个增量 ∆xk,使得

$$
|| f(x_k + \Delta x_k) ||^2_2
$$

达到极小值。

  1. 若 ∆xk 足够小,则停止。

  2. 否则,令x(k+1) = xk + ∆xk,返回第 2 步。

这让求解导函数为零的问题变成了一个不断寻找下降增量 ∆xk 的问题。由于可以对 f 进行线性化,增量的计算将简单很多。当函数下降直到增量非常小的时候,就认为算法收敛,目标函数达到了一个极小值。在这个过程中,问题在于如何找到每次迭代点的增量,而这是一个局部的问题,只需要关心 f 在迭代值处的局部性质而非全局性质。这类方法在最优化、机器学习等领域应用非常广泛。

下面是如何寻找这个增量 ∆xk的方法。

2.1 一阶和二阶梯度法

考虑第 k 次迭代,假设在xk处,想要找到增量 ∆xk,最直观的方式是将目标函数在xk附近进行泰勒展开:

$$
F
$$

其中J(xk) 是 F(x)关于x的一阶导数(也叫梯度、雅可比矩阵﹝Jacobian﹞),H 则是二阶导数(海塞﹝Hessian﹞矩阵),它们都在xk 处取值。可以选择保留泰勒展开的一阶或二阶项,那么对应的求解方法则称为一阶梯度或二阶梯度法。如果保留一阶梯度,取增量为反向的梯度,即可保证函数下降:∆x∗= −J(xk)

这只是个方向,通常还要再指定一个步长 λ。步长可以根据一定的条件来计算,在机器学习当中也有一些经验性质的方法。这种方法被称为最速下降法。它的直观意义是:只要沿着反向梯度方向前进,在一阶(线性)的近似下,目标函数必定会下降。

以上都是在第k次迭代时进行的,并不涉及其他的迭代信息。为了简化符号,省略下标k,并认为这些对任意一次迭代都成立。

选择保留二阶梯度信息,此时增量方程为:

$$
$$

右侧只含 ∆x的零次、一次和二次项。求右侧等式关于 ∆x的导数并令它为零,得到:

$$

$$
求解这个线性方程就得到了增量。此类方法又称为牛顿法。

一阶和二阶梯度法都十分直观,只要把函数在迭代点附近进行泰勒展开,并针对更新量做最小化即可。用一个一次或二次的函数近似原函数,然后用近似函数的最小值来估计原函数的极小值。只要原目标函数局部看起来像一次或二次函数,这类算法就是成立的。不过这两种方法也存在问题:最速下降法过于贪心,容易走出锯齿路线,反而增加了迭代次数。而牛顿法则需要计算目标函数的H矩阵,这在问题规模较大时非常困难,通常倾向于避免H的计算。所以引出了下面的一些方法。

2.2 高斯牛顿法

牛顿法是对目标函数 F(x) 进行一阶泰勒展开,而高斯牛顿法是对f(x)进行一阶泰勒展开:f (x + ∆x) ≈ f(x) + J(x)T∆x.

注:把 J(x) 写成列向量,那么它可以和 ∆x 进行内积,得到一个标量。

这里J(x)T为f(x)关于x的导数,为n×1的列向量。目标是寻找增量∆x,使得∥f(x+∆x)∥2达到最小。为了求 ∆x,需要解一个线性的最小二乘问题:

$$

F
$$

根据极值条件,将上述目标函数对 ∆x求导,并令导数为零。可以得到如下方程组:

$$
F
$$

这个方程是关于变量∆x的线性方程组,称它为增量方程,也可以称为高斯牛顿方程(GaussNewton equation)或者正规方程(Normal equation)。把左边的系数定义为H,右边定义为g,那么上式变为:H∆x = g.

对比牛顿法可见,高斯牛顿法用JJT作为牛顿法中二阶Hessian矩阵的近似,从而省略了计算H的过程。求解增量方程是整个优化问题的核心所在。高斯牛顿法的算法步骤可以写成:

  1. 给定初始值x0。

  2. 对于第 k 次迭代,求出当前的雅可比矩阵J(xk) 和误差f(xk)。

  3. 求解增量方程:H∆xk = g。

  4. 若 ∆xk 足够小,则停止。否则,令x(k+1) = xk+ ∆xk,返回第 2 步。

从算法步骤中,主要还是增量的求解。只要能够顺利解出增量,就能保证目标函数能够正确地下降。

为了求解增量方程,需要求解H−1,这需要H矩阵可逆,但实际数据中计算得到的JJT只有半正定性。也就是说,在使用高斯牛顿法时,可能出现JJT为奇异矩阵或者病态(ill-condition)的情况,此时增量的稳定性较差,导致算法不收敛。直观地说,原函数在这个点的局部近似不像一个二次函数。假设H非奇异也非病态,如果求出来的步长∆x太大,也会导致采用的局部近似式:f (x + ∆x) ≈ f(x) + J(x)T∆x.不够准确,这样一来无法保证它的迭代收敛。

在非线性优化领域,相当多的算法都可以归结为高斯牛顿法的变种。这些算法都借助了高斯牛顿法的思想并且通过改进修正其缺点。例如一些线搜索方法 (line search method) 加入了一个步长α,在确定了∆x后进一步找到 α 使得 ∥f(x + α∆x)∥2 达到最小,而不是简单地令 α = 1。

列文伯格—马夸尔特方法在一定程度上修正了这些问题,比高斯牛顿法更为鲁棒,但收敛速度会比高斯牛顿法更慢,被称为阻尼牛顿法(Damped Newton Method)。

2.3 列文伯格—马夸尔特方法

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果,所以应该给∆x添加一个范围,称为信赖区域(Trust Region)。这个范围定义了在什么情况下二阶近似是有效的,这类方法也称为信赖区域方法(Trust Region Method)。在信赖区域里边,近似是有效的;出了这个区域,近似可能会出问题。

那么如何确定这个信赖区域的范围呢?一个比较好的方法是根据近似模型跟实际函数之间的差异来确定:如果差异小,说明近似效果好,扩大近似的范围;反之,如果差异大,就缩小近似的范围。我们定义一个指标 ρ 来刻画近似的好坏程度:

$$
F
$$

ρ 的分子是实际函数下降的值,分母是近似模型下降的值。如果 ρ 接近于 1,则近似是好的。如果 ρ 太小,说明实际减小的值远少于近似减小的值,则认为近似比较差,需要缩小近似范围。反之,如果 ρ 比较大,则说明实际下降的比预计的更大,可以放大近似范围。

于是构建一个改良版的非线性优化框架,该框架会比高斯牛顿法有更好的效果:

  1. 给定初始值 x0,以及初始优化半径 µ。
  2. 对于第 k 次迭代,在高斯牛顿法的基础上加上信赖区域,求解:

$$
F
$$

  1. 其中 µ 是信赖区域的半径,D 为系数矩阵。
  2. 计算 ρ。
  3. 若 ρ > 3/4,则设置 µ = 2µ。
  4. 若 ρ < 1/4,则设置 µ = 0.5µ。
  5. 如果 ρ 大于某阈值,则认为近似可行。令 xk+1 = xk + ∆xk。
  6. 判断算法是否收敛。如不收敛则返回第 2 步,否则结束。

这里近似范围扩大的倍数和阈值都是经验值,可以替换成别的数值。

$$
F
$$

这个式子中,把增量限定于一个半径为 µ 的球中,认为只在这个球内才是有效的。带上D之后,这个球可以看成一个椭球。在列文伯格提出的优化方法中,把D取成单位阵I,相当于直接把 ∆xk约束在一个球中。随后,马夸尔特提出将D取成非负数对角阵——实际中通常用JTJ 的对角元素平方根,使得在梯度小的维度上约束范围更大一些。

在列文伯格—马夸尔特优化中,需要求解这个子问题来获得梯度。

这个子问题是带不等式约束的优化问题,用拉格朗日乘子把约束项放到目标函数中,构成拉格朗日函数:

$$
F
$$

这里 λ 为拉格朗日乘子。类似于高斯牛顿法中的做法,令该拉格朗日函数关于∆x的导数为零,它的核心仍是计算增量的线性方程:

$$
F
$$

可以看到,增量方程相比于高斯牛顿法,多了一项 λDTD。考虑它的简化形式,即D = I,那么相当于求解:

(H + λI)∆xk = g*.*

当参数λ比较小时,H占主要地位,这说明二次近似模型在该范围内是比较好的,列文伯格—马夸尔特方法更接近于高斯牛顿法。另一方面,当λ比较大时,λI占据主要地位,列文伯格—马夸尔特方法更接近于一阶梯度下降法(即最速下降),这说明附近的二次近似不够好。列文伯格—马夸尔特方法的求解方式,可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题,提供更稳定、更准确的增量 ∆x。

在实际中,还存在许多其他的方式来求解增量,例如

[31] J. Nocedal and S. Wright, Numerical Optimization. Springer Science & Business Media, 2006.

实际问题中,通常选择高斯牛顿法或列文伯格—马夸尔特方法其中之一作为梯度下降策略。当问题性质较好时,用高斯牛顿。如果问题接近病态,则用列文伯格—马夸尔特方法。

梯度下降法 场景
牛顿法
高斯-牛顿 问题质量较好
列文伯格-马夸尔特方法 问题接近病态?

小结

专门介绍数值优化的书籍

[31] J. Nocedal and S. Wright, Numerical Optimization. Springer Science & Business Media, 2006.

以高斯牛顿法和列文伯格—马夸尔特方法为代表的优化方法,在很多开源的优化库中都已经实现并提供给用户。最优化是处理许多实际问题的基本数学工具,不光在视觉 SLAM 中起着核心作用,在类似于深度学习等其他领域,它也是求解问题的核心方法之一(深度学习数据量很大,以一阶方法为主)。

无论是高斯牛顿法还是列文伯格—马夸尔特方法,在做最优化计算时,都需要提供变量的初始值。这个初始值不能随意设置。实际上非线性优化的所有迭代求解方案,都需要用户来提供一个良好的初始值。由于目标函数太复杂,导致在求解空间上的变化难以预测,对问题提供不同的初始值往往会导致不同的计算结果。这种情况是非线性优化的通病:大多数算法都容易陷入局部极小值。因此,无论是哪类科学问题,提供初始值都应该有科学依据,例如视觉 SLAM 问题中,会用 ICP、PnP 之类的算法提供优化初始值(视觉前端)。一个良好的初始值对最优化问题非常重要。

如何求解线性增量方程组呢?在视觉SLAM算法里,经常遇到∆x的维度很大,如果是要做大规模的视觉三维重建,就会经常发现这个维度可以轻易达到几十万甚至更高的级别。要对那么大个矩阵进行求逆是大多数处理器无法负担的,因此存在着许多针对线性方程组的数值求解方法。在不同的领域有不同的求解方式,但几乎没有一种方式是直接求系数矩阵的逆,会采用矩阵分解的方法来解线性方程,例如 QR、Cholesky 等分解方法。(工程数学)

视觉SLAM里这个矩阵往往有特定的稀疏形式,这为实时求解优化问题提供了可能性。利用稀疏形式的消元、分解,最后再进行求解增量,会让求解的效率大大提高。在很多开源的优化库上,比如GTSAM,维度为一万多的变量在一般的PC上就可以在几秒甚至更短的时间内被求解出来,其原因也是用了更加高级的数学工具(因子图增量平滑优化)。视觉SLAM算法现在能够实时地实现,多亏了系数矩阵是稀疏的,如果矩阵是稠密的,优化这类视觉SLAM算法不会被学界广泛采纳了。

3 实践:曲线拟合问题

3.1手写高斯牛顿法

考虑一条满足以下方程的曲线:

$$
y = \exp (ax^2 + bx + c) + w
$$

其中 a,b,c 为曲线的参数,w 为高斯噪声,满足 w ∼ (0*,σ*2)。假设有 N 个关于 x,y 的观测数据点,根据这些数据点求出曲线的参数。那么,可以求解下面的最小二乘问题来估计曲线参数:

$$
\min_{a,b,c} 1/2 \sum_{i=1}^N
|| y_i - \exp( ax_i^2 + bx_i +c ) || ^2
$$

在这个问题中,待估计的变量是 a,b,c,而不是 x。程序里先根据模型生成 x,y 的真值,然后在真值中添加高斯分布的噪声。随后,使用高斯牛顿法来从带噪声的数据(x,y)拟合参数模型。定义误差为:

$$
e_i = y_i - \exp(ax_i^2 + bx_i + c)
$$

那么可以求出每个误差项对于状态变量的导数:

$$
\frac{\partial e_i} {\partial a} = -x_i^2 \exp(ax_i^2 + bx_i + c)\
\frac{\partial e_i} {\partial b} = -x_i \exp(ax_i^2 + bx_i + c)\
\frac{\partial e_i} {\partial c} = - \exp(ax_i^2 + bx_i + c) \
$$

于是

$$
J_i = [\frac{\partial e_i}{\partial a}, \frac{\partial e_i}{\partial b}, \frac{\partial e_i}{\partial c}]
$$

高斯牛顿法的增量方程为:

$$
\big( \sum_{i=1}{100} J_i(\sigma^2)^{-1} J_i^T \big)
\Delta x_k =
\sum_{i=1}^{100} - J_i(\sigma^2)^-1 e_i

$$

也可以选择把所有的Ji 排成一列,将这个方程写成矩阵形式,它的含义与求和形式是一致的。下面的代码演示了这个过程是如何进行的:> slambook2/ch6/gaussNewton.cpp

在这个例子中演示了如何对一个简单的拟合问题进行迭代优化。该程序输出每一步迭代的目标函数值和更新量,如下:

整个问题的目标函数在迭代 9 次之后趋近收敛,更新量趋近于零。最终估计的值与真值接近,函数图像如下:

蓝色点为100个数据点,黑色线为理论模型,红色线为拟合的模型。

3.2使用 Ceres 进行曲线拟合

Google Ceres 是一个广泛使用的最小二乘问题求解库。在 Ceres 中,只需按照一定步骤定义待解的优化问题,然后交给求解器计算即可。Ceres 求解的最小二乘问题最一般的形式如下(带边界的核函数最小二乘):

$$
\min_{x} 1/2 \sum_{j} p_i (|| f_i (x_{i_1}, …, x_{i_n}) ||^2)
\
s.t. \qquad l_j \leq x_j \leq u_j.

$$

在这个问题中,x1*,··· ,xn 为优化变量,又称参数块(Parameter blocks),fi 称为代价函数(Cost function),也称为残差块(Residual blocks),在 SLAM 中也可理解为误差项。lj 和 uj 为第 j 个优化变量的上限和下限。在最简单的情况下,取 lj = −∞,uj* = ∞(不限制优化变量的边界)。此时,目标函数由许多平方项经过一个核函数 ρ(·) 之后求和组成。取 ρ 为恒等函数,那么目标函数即为许多项的平方和,就得到了无约束的最小二乘问题。为了让 Ceres 求解这个问题,需要做以下几件事:

  1. 定义每个参数块。参数块通常为向量,但是在SLAM里也可以定义成四元数、李代数这种特殊的结构。如果是向量,那么需要为每个参数块分配一个 double 数组,来存储变量的值。
  2. 然后定义残差块的计算方式。残差块通常关联若干个参数块,对它们进行一些自定义的计算,然后返回残差值。Ceres 对它们求平方和之后,作为目标函数的值。
  3. 残差块往往也需要定义雅可比的计算方式。在 Ceres 中,可以使用自动求导功能,也可以手动指定雅可比的计算过程。如果要使用自动求导,那么残差块需要按照特定的写法来书写:残差的计算过程是一个带模板的括号运算符。
  4. 最后,把所有的参数块和残差块加入 Ceres 定义的 Problem 对象中,调用 Solve 函数求解即可。求解之前,可以传入一些配置信息,例如迭代次数、终止条件等,也可以使用默认的配置。

安装 Ceres

Ceres 的 github 地址为:ceres-solver/ceres-solver

安装依赖项

1
2
3
4
5
6
sudo apt−get install liblapack−dev libsuitesparse−dev libcxsparse3 \
libgflags−dev libgoogle−glog−dev libgtest−dev


sudo apt install liblapack-dev libsuitesparse-dev libcxsparse3.1.4 \
        libgflags-dev libgoogle-glog-dev libgtest-dev

然后进入 Ceres 库目录下,使用 cmake 编译并安装。安装完成后,在/usr/local/include/ceres 下找到 Ceres 的头文件,并在/usr/local/lib/下找到名为 libceres.a 的库文件。有了这些文件,就可以使用 Ceres 进行优化计算了。

使用 Ceres 拟合曲线

下面的代码演示了如何使用 Ceres 求解同样的问题:slambook/ch6/ceresCurveFitting.cpp

利用 OpenCV 的噪声生成器生成了 100 个带高斯噪声的数据,随后利用 Ceres 进行拟合。这里Ceres用法有如下几项:

  1. 定义残差块的类。方法是书写一个类(或结构体),并在类中定义带模板参数的 () 运算符,这样该类就成为了一个拟函数(Functor),或者叫仿函数。这种定义方式使得 Ceres 可以像调用函数一样,对该类的某个对象(比如 a)调用 a() 方法。Ceres 会把雅可比矩阵作为类型参数传入此函数,从而实现自动求导的功能。
  2. 程序中的 double abc[3] 即为参数块,而对于残差块,对每一个数据构造 CURVE_FIT-TING_COST 对象,然后调用 AddResidualBlock 将误差项添加到目标函数中。由于优化需要梯度,有若干种选择:(1)使用 Ceres 的自动求导(Auto Diff);(2)使用数值求导(Numeric Diff);(3)自行推导解析的导数形式,提供给 Ceres。
  3. 自动求导需要指定误差项和优化变量的维度。这里的误差是标量,维度为 1;优化的是 a,b,c 三个量,维度为 3。于是,在自动求导类 AutoDiffCostFunction 的模板参数中设定变量维度为 1、3。
  4. 设定好问题后,调用 Solve 函数进行求解。可以在options里配置优化选项。例如,可以选择使用 Line Search 还是 Trust Region、迭代次数、步长,等等。可以查看 Options 的定义,看看有哪些优化方法可选,一般默认的配置已经可用于很广泛的问题了。

最终的优化值和手写的基本相同,但运行速度上 Ceres 要相对慢一些。

Ceres的优点是提供了自动求导工具,不必去计算很麻烦的雅可比矩阵。Ceres的自动求导是通过模板元实现的,在编译时期就可以完成自动求导工作,不过仍然是数值导数。此外,Ceres的优化过程配置也很丰富,使其适合很广泛的最小二乘优化问题,包括 SLAM 之外的各种问题。

注:自动求导也是用数值导数实现的。

  • 代码实现

3.3使用 g2o 进行曲线拟合

g2o(General Graphic Optimization,G2O)是在SLAM领域广为使用的优化库。它是一个基于图优化的库。图优化是一种将非线性优化与图论结合起来的理论。

图优化理论简介

前面介绍了非线性最小二乘的求解方式。它们是由很多个误差项之和组成的。然而,仅有一组优化变量和许多个误差项,并不清楚它们之间的关联。比如,某个优化变量xj存在于多少个误差项中呢?能保证对它的优化是有意义的吗?进一步,希望能够直观地看到该优化问题长什么样。于是,就引出了图优化。

图优化,是把优化问题表现成图(Graph)的一种方式。这里的图是图论意义上的图。一个图由若干个顶点(Vertex),以及连接着这些顶点的边(Edge)组成。进而,用顶点表示优化变量,用边表示误差项。于是,对任意一个上述形式的非线性最小二乘问题,可以构建与之对应的一个图。可以简单地称它为图,也可以用概率图里的定义,称之为贝叶斯图或因子图。

如下图:

用三角形表示相机位姿节点,用圆形表示路标点,它们构成了图优化的顶点;同时,实线表示相机的运动模型,虚线表示观测模型,它们构成了图优化的边。最基本的图优化是用图模型来表达一个非线性最小二乘的优化问题,可以利用图模型的某些性质做更好的优化。

g2o 是一个通用的图优化库,可以在g2o里求解任何能够表示为图优化的最小二乘问题,包括曲线拟合问题

g2o 的编译与安装

安装依赖:

1
sudo apt−get install qt5−qmake qt5−default libqglviewer−dev−qt5 libsuitesparse−dev libcxsparse3 libcholmod3

从GitHub下载并cmake安装:https://github.com/RainerKuemmerle/g2o

安装完成后, g2o 的头文件将位于/usr/local/g2o 下,库文件位于/usr/local/lib/下

使用 g2o 拟合曲线

首先要将曲线拟合问题抽象成图优化。这个过程中,节点为优化变量, 边为误差项。

在曲线拟合问题中,整个问题只有一个顶点:曲线模型的参数 a, b, c;而各个带噪声的数据点, 构成了一个个误差项,也就是图优化的边。这里的边是一元边(Unary Edge),即只连接一个顶点。事实上,图优化中一条边可以连接一个、两个或多个顶点,这主要反映每个误差与多少个优化变量有关。

主要步骤:

  1. 定义顶点和边的类型。
  2. 构建图。
  3. 选择优化算法。
  4. 调用g2o进行优化,返回结果。

程序:slambook/ch6/g2oCurveFitting.cpp

在这个程序中,从g2o派生出了用于曲线拟合的图优化顶点和边:CurveFittingVertex 和 CurveFittingEdge,这实质上扩展了g2o的使用方式。这两个类分别派生于BaseVertex和BaseUnaryEdge类。在派生类中,重写了重要的虚函数:

  1. 顶点的更新函数:oplusImpl。优化过程最重要的是增量∆x的计算,而该函数处理的是 xk+1 = xk + ∆x 的过程。在曲线拟合过程中,由于优化变量(曲线参数)本身位于向量空间中,这个更新计算就是简单的加法。但是当优化变量不在向量空间中时,比如x是相机位姿,它本身不一定有加法运算。这时就需要重新定义增量如何加到现有的估计上的行为了。
  2. 顶点的重置函数:setToOriginImpl。即把估计值置零即可。
  3. 边的误差计算函数:computeError。该函数需要取出边所连接的顶点的当前估计值,根据曲线模型,与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的。
  4. 边的雅可比计算函数:linearizeOplus。这个函数里计算了每条边相对于顶点的雅可比。
  5. 存盘和读盘函数:read、write。

定义了顶点和边之后,在main函数里声明了一个图模型,然后按照生成的噪声数据,往图模型中添加顶点和边,最后调用优化函数进行优化。g2o会给出优化的结果。

    第一版书代码Debug:

《视觉SLAM十四讲》 g2o实践代码报错解决方法(第六讲为例) - 代码先锋网

4 小结

非线性优化问题:由许多个误差项平方和组成的最小二乘问题。讨论了两种主要的梯度下降方式:高斯牛顿法和列文伯格 —马夸尔特方法。在实践部分中,分别使用了手写高斯牛顿法、Ceres 和 g2o 两种优化库求解同一个 曲线拟合问题,发现结果相似。 特别地,如果用g2o来拟合曲线,必须先把问题转换为图优化,定义新的顶点和边。相比之下,Ceres 定义误差项求曲线拟合问题则自然了很多,因为它本身即是一个优化库。然而,在 SLAM 中更多的问题是,一个带有许多个相机位姿和许多个空间点的优化问题如何求解。特别地,当相机位姿以李代数表示时,误差项关于相机位姿的导数如何计算。g2o提供了大量现成的顶点和边,非常便于相机位姿估计问题。而在 Ceres 中, 不得不自己实现每一个Cost Function,有一些不便。

Ceres 库提供了基于模板元的自动求导和运行时的数值求导,而 g2o 只提供了运行时数值求导这一种方式。但是对于大多数问题,如果能够推导出雅可比矩阵的解析形式并告诉优化库,就可以避免数值求导中的诸多问题。

非线性拟合(曲线拟合)

非线性优化
梯度下降法
高斯牛顿法
列文伯格-马夸尔特法
工具库 特性 求导 使用
Ceres - (自动求导)基于模板元的自动求导和运行时的数值求导
G2o 提供了大量现成的顶点和边,非常便于相机位姿估计问题 运行时数值求导这一种方式 必须先把问题转换为图优化,定义新的顶点和边

习题

  1. 证明线性方程 Ax = b 当系数矩阵 A 超定时,最小二乘解为 x = (ATA) −1ATb。
  2. 调研最速下降法、牛顿法、高斯牛顿法和列文伯格—马夸尔特方法各有什么优缺点。
  3. 为什么高斯牛顿法的增量方程系数矩阵可能不正定?不正定有什么几何含义?为什么在这种情况下解就不稳定了?
  4. DogLeg 是什么?它与高斯牛顿法和列文伯格—马夸尔特方法有何异同?
  5. 阅读 Ceres 的教学材料(http://ceres-solver.org/tutorial.html)以更好地掌握其用法。
  6. 阅读 g2o 自带的文档 。
  7. 更改曲线拟合实验中的曲线模型,并用 Ceres 和 g2o 进行优化实验。

笔记总结

笔记总结(1)—非线性优化原理

https://zhuanlan.zhihu.com/p/462463343

笔记总结(2)—非线性优化应用

https://zhuanlan.zhihu.com/p/462932847

参考

视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题

视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题

视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合

 笔记总结(1)—非线性优化原理

 笔记总结(2)—非线性优化应用

outlook:

[TOC]

数据转换

C++: byte和int的相互转化_PuttyTree的博客-CSDN博客_c++ byte转int

C++ sockt

recv

1
2
3
const int bufLen = 256;
char buffer[bufLen];
int resultLen = recv(sclient, buffer, bufLen, 0);

FIX-01 “undefined reference to __imp_WSAStartup’”

1
2
3
4
<!--# VS task.xml配置修改:-->
"args": [
"C:/Program Files (x86)/Windows Kits/10/Lib/10.0.19041.0/um/x64/WS2_32.Lib",
]

Windows 网络编程

阻塞模式开发(执行I/O操作时,线程被阻塞调用)

非阻塞模式开发(执行I/O操作时,立即返回,但是要处理返回错误的问题)

套接字Select模型(常见的I/O模型)

UDP (Send/Recv Text)

Sever

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
#include <iostream>
#include <winsock2.h>
#include <string>
#include <fstream>
#include "windows.h"
#include "stdio.h"

#pragma comment(lib, "ws2_32.lib")

using namespace std;

int main()
{
WSADATA wsd;
SOCKET s;

// init
if (WSAStartup(MAKEWORD(2,2), &wsd) != 0)
{
cout << "WSA Start UP faild" << endl;
return 1;
}

// socket creat
s = socket(AF_INET, SOCK_DGRAM, 0);
if (s == INVALID_SOCKET)
{
cout << "socket() failed " << WSAGetLastError() << endl;
WSACleanup();
return 1;
}
// socket bind
SOCKADDR_IN servAddr;
servAddr.sin_family = AF_INET;
servAddr.sin_port = htons((short) 5000); // PORT
servAddr.sin_addr.s_addr = htonl(INADDR_ANY); // IP
if (bind(s, (SOCKADDR*)&servAddr, sizeof(servAddr)) == SOCKET_ERROR)
{
cout << "bind() failed " << WSAGetLastError() << endl;
closesocket(s);
WSACleanup();
return 1;
}

// recv
int BUF_SIZE = 64;
char buf[BUF_SIZE];
// recv
SOCKADDR_IN clientAddr;
int nClientLen = sizeof(clientAddr);
if (recvfrom(s, buf, BUF_SIZE, 0, (SOCKADDR*)&clientAddr, &nClientLen) == SOCKET_ERROR)
{
cout << "recefrom() failed " << WSAGetLastError() << endl;
closesocket(s);
WSACleanup();
return 1;
}
cout << clientAddr.sin_addr.s_addr << endl;
cout << "recv : " << buf << endl;


}

client

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
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
#include <iostream>
#include <winsock2.h>
#include <string>
#include <fstream>
#include "windows.h"
#include "stdio.h"

using namespace std;

string constructBody(string args[2], string file[2]);
string readFile(string fileName);

#define PORT 5000
#define IP "127.0.0.1"
#define HOST "localhost"
#define RECEIVER "receive_data"
#define COMPNAME "compname"
#define PROGRAM "program"
#define FILENAME "file"
#define BOUNDARY "--------FVIAlihiu"
#define DUMMY_DATA "-todo"
#define DUMMY_FILE "dummy.txt"

/// @brief Windows SOcket 网络开发
/// ref : http://9v.lt/blog/sending-multipart-post-requests-c/
/// @return
int main()
{
SOCKET dataSock;
WSADATA wasData;
int error = WSAStartup(0x0202, &wasData);
if (error != 0)
{
WSACleanup();
exit(1);
}


// dataSock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
dataSock = socket(AF_INET, SOCK_DGRAM, 0);
if (dataSock == INVALID_SOCKET)
{
exit(1);
}

SOCKADDR_IN target;
target.sin_family = AF_INET;
target.sin_port = htons(PORT);
target.sin_addr.s_addr = inet_addr(IP);

connect(dataSock, (SOCKADDR*)&target, sizeof(target));

string programNames[1][2] = {{"KULVERTOP", "Chrome"}};
string file[2] = {FILENAME, "Default.txt"};

int a = sizeof( programNames)/sizeof( programNames[0]);
for (int i = 0; i < a; i++)
{
cout << "Send data for " << programNames[i][1] << endl;
string body = constructBody(programNames[i], file);
char header[1024];
// cout << header
sprintf(header, "POST %s HTTP1.1 \r\n"
"Host: %s\r\n"
"Content-Length: %d\r\n"
"Content-Type: multipart/from-data; boundary=%s\r\n"
"Accept-Charset: utf-8\r\n\r\n",
RECEIVER, IP, strlen(body.c_str()), BOUNDARY);

cout << header << endl;
// int p = send(dataSock, header, strlen(header), 0);
// int k = send(dataSock, body.c_str(), strlen(body.c_str()), 0);
char buf[64] = "My UDP";
// int t = send(dataSock, buf, strlen(buf), 0);
int t = send(dataSock, buf, 64, 0);

}

closesocket(dataSock);
WSACleanup();
}

string readFile(string file)
{
string fileContents;
ifstream tmp(file.c_str());
getline(tmp, fileContents);
tmp.close();

return fileContents;
}

string constructBody(string args[2], string file[2])
{
string body;
string CRLF = "\r\n";

// first we add the args
body.append("--" + string(BOUNDARY) + CRLF);
body.append("Content-Disposition: from-data; name=\"" + string(COMPNAME) + "\"" + CRLF);
body.append(CRLF);
body.append(args[0] + CRLF);
body.append("--" + string(BOUNDARY) + CRLF);
body.append("Content-Disposition: from-data; name=\"" + string(PROGRAM) + "\"" + CRLF);
body.append(CRLF);
body.append(args[1] + CRLF);

// now we add the file
body.append("--" + string(BOUNDARY) + CRLF);
body.append("Content-Disposition: from-data; name=\"" + string(FILENAME) + "\"; filename=\"" + string(DUMMY_FILE) + "\"" + CRLF);
body.append("Content-Type:text/plain" + CRLF);
body.append("--" + string(BOUNDARY) + "--" + CRLF);
body.append(CRLF);

return body;
}

demo::Socket文件传输

Client Server
Method-1 发送4字节(的文件长度)+文件的内容String 接受对应格式的文件
Method-2

LINUX网络编程

基础知识

c++网络编程基础知识总结_你最特别17的博客-CSDN博客_网络口c++

EPOLLIN , EPOLLOUT , EPOLLPRI, EPOLLERR 和 EPOLLHUP事件

poll()函数ref

1
2
3
4
5
6
7
8
9
10
POLLIN:                有普通数据或者优先数据可读
POLLRDNORM: 有普通数据可读
POLLRDBAND: 有优先数据可读
POLLPRI: 有紧急数据可读
POLLOUT: 有普通数据可写
POLLWRNORM: 有普通数据可写
POLLWRBAND: 有紧急数据可写
POLLERR: 有错误发生
POLLHUP: 有描述符挂起事件发生
POLLNVAL: 描述符非法

Client

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
//client Linux 
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <cerrno>
#include <cstring>
#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <unistd.h>
#include <arpa/inet.h>

using namespace std;

int main()
{
cout << "this is client" <<endl;
int client = socket(AF_INET, SOCK_STREAM, 0);
if(client == -1)
{
cout << " socket err " <<endl;
return 0;
}
struct sockaddr_in serverAddr;
serverAddr.sin_family = AF_INET;
serverAddr.sin_port = htons(7637);
serverAddr.sin_addr.s_addr = inet_addr("127.0.0.1");

if(connect(client, (struct sockaddr_in*) &serverAddr, sizeof(serverAddr)) < 0)
{
cout << "connect err" <<endl;
return 0;
}
cout << "connect ...." <<endl;
char data[255];
char buf[255];
while (true)
{
cin >> data;
send(client, data, strlen(data), 0);
if(strcmp(data, "exit") == 0 || strcmp(data, "EXIT") == 0)
{
cout << "client disconnect ..." <<endl;
break;
}
memset(buf, 0 ,sizeof(buf));
int len = recv(client, buf, sizeof(buf), 0);
buf[len] = '\0';

cout << buf << endl;
}
close(client);
return 0;
}

Server

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
//server linux 
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <cerrno>
#include <cstring>
#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <unistd.h>
#include <arpa/inet.h>

using namespace std;


int main()
{
cout << "this is server" <<endl;
int listenfd = socket(AF_INET, SOCK_STREAM, 0);
if (listenfd == -1)
{
cout << "socker err " <<endl;
return 0;
}
struct sockaddr_in addr;
addr.sin_family = AF_INET;
addr.sin_port = htons(7637);
addr.sin_addr.s_addr = INADDR_ANY;

if(bind(listenfd, (struct sockaddr*) &addr,sizeof(addr)) == -1)
{
cout << "bind err" <<endl;
return 0;
}
if(listen(listenfd , 5) == -1)
{
cout <<"listen err" <<endl;
}

int conn;
char clientIP[INET_ADDRSTRLEN] = "";
struct sockaddr_in clientAddr;
socklen_t clinetAddrLen = sizeof(clientAddr);
while (true)
{
cout << "listen ...." <<endl;
conn = accept(listenfd, (struct sockaddr*)&clientAddr, &clinetAddrLen);
if(conn < 0)
{
cout <<"conn accept err " <<endl;
continue;
}

inet_ntop(AF_INET, &clientAddr.sin_addr, clientIP, INET_ADDRSTRLEN);
cout << " connect " << clientIP << ":" << ntohs(clientAddr.sin_port) << endl;

char buf[255];
while (true)
{
memset(buf, 0 , sizeof(buf));
int len = recv(conn, buf, sizeof(buf), 0);
buf[len] = '\0';
if(strcmp(buf,"exit") == 0 || strcmp(buf,"EXIT") == 0)
{
cout << "disconnect " << clientIP << ":" <<ntohs(clientAddr.sin_port) << endl;
break;
}
cout << buf << endl;
send(conn, buf, len,0)
}
close(conn);
}
close(listenfd);
return 0;

}

C++ socket demo_andyleung520的博客-CSDN博客_c++ socket demo

C++ Http

C++ 发送HTTP请求_BUG·搬运工的博客-CSDN博客_c++ http请求

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
#include "HTTPRequest.hpp" //相关源码在文章末尾

...

//注意:URI地址必须以 http:// 开头,否则不符合头文件校验规则
string uri = "http://en.xxx.com/api-ctl/client/health/";
string method = "POST";
string arguments = string_To_UTF8((char*)encryptStr.c_str());
auto protocol = http::InternetProtocol::V4;
http::Request req{ uri, protocol };

json responseJson;
try {
const auto response = req.send(method, arguments, {
{"Content-Type", "application/json"},
{"User-Agent", "runscope/0.1"},
{"Accept", "*/*"}
}, std::chrono::seconds(2));
responseJson = json::parse(string{ response.body.begin(), response.body.end() });
}
catch (exception& e) {
//捕获请求失败异常,处理逻辑自行添加
}

HTTPRequest.hpp

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
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
//  HTTPRequest

#ifndef HTTPREQUEST_HPP
#define HTTPREQUEST_HPP

#include <cctype>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <algorithm>
#include <array>
#include <chrono>
#include <functional>
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <system_error>
#include <type_traits>
#include <vector>

#if defined(_WIN32) || defined(__CYGWIN__)
# pragma push_macro("WIN32_LEAN_AND_MEAN")
# pragma push_macro("NOMINMAX")
# ifndef WIN32_LEAN_AND_MEAN
# define WIN32_LEAN_AND_MEAN
# endif // WIN32_LEAN_AND_MEAN
# ifndef NOMINMAX
# define NOMINMAX
# endif // NOMINMAX
# include <winsock2.h>
# if _WIN32_WINNT < _WIN32_WINNT_WINXP
extern "C" char *_strdup(const char *strSource);
# define strdup _strdup
# include <wspiapi.h>
# endif // _WIN32_WINNT < _WIN32_WINNT_WINXP
# include <ws2tcpip.h>
# pragma pop_macro("WIN32_LEAN_AND_MEAN")
# pragma pop_macro("NOMINMAX")
#else
# include <errno.h>
# include <fcntl.h>
# include <netinet/in.h>
# include <netdb.h>
# include <sys/select.h>
# include <sys/socket.h>
# include <sys/types.h>
# include <unistd.h>
#endif // defined(_WIN32) || defined(__CYGWIN__)

namespace http
{
class RequestError final: public std::logic_error
{
public:
using std::logic_error::logic_error;
};

class ResponseError final: public std::runtime_error
{
public:
using std::runtime_error::runtime_error;
};

enum class InternetProtocol: std::uint8_t
{
V4,
V6
};

struct Uri final
{
std::string scheme;
std::string user;
std::string password;
std::string host;
std::string port;
std::string path;
std::string query;
std::string fragment;
};

struct HttpVersion final
{
uint16_t major;
uint16_t minor;
};

struct Status final
{
// RFC 7231, 6. Response Status Codes
enum Code: std::uint16_t
{
Continue = 100,
SwitchingProtocol = 101,
Processing = 102,
EarlyHints = 103,

Ok = 200,
Created = 201,
Accepted = 202,
NonAuthoritativeInformation = 203,
NoContent = 204,
ResetContent = 205,
PartialContent = 206,
MultiStatus = 207,
AlreadyReported = 208,
ImUsed = 226,

MultipleChoice = 300,
MovedPermanently = 301,
Found = 302,
SeeOther = 303,
NotModified = 304,
UseProxy = 305,
TemporaryRedirect = 307,
PermanentRedirect = 308,

BadRequest = 400,
Unauthorized = 401,
PaymentRequired = 402,
Forbidden = 403,
NotFound = 404,
MethodNotAllowed = 405,
NotAcceptable = 406,
ProxyAuthenticationRequired = 407,
RequestTimeout = 408,
Conflict = 409,
Gone = 410,
LengthRequired = 411,
PreconditionFailed = 412,
PayloadTooLarge = 413,
UriTooLong = 414,
UnsupportedMediaType = 415,
RangeNotSatisfiable = 416,
ExpectationFailed = 417,
MisdirectedRequest = 421,
UnprocessableEntity = 422,
Locked = 423,
FailedDependency = 424,
TooEarly = 425,
UpgradeRequired = 426,
PreconditionRequired = 428,
TooManyRequests = 429,
RequestHeaderFieldsTooLarge = 431,
UnavailableForLegalReasons = 451,

InternalServerError = 500,
NotImplemented = 501,
BadGateway = 502,
ServiceUnavailable = 503,
GatewayTimeout = 504,
HttpVersionNotSupported = 505,
VariantAlsoNegotiates = 506,
InsufficientStorage = 507,
LoopDetected = 508,
NotExtended = 510,
NetworkAuthenticationRequired = 511
};

HttpVersion httpVersion;
std::uint16_t code;
std::string reason;
};

using HeaderField = std::pair<std::string, std::string>;
using HeaderFields = std::vector<HeaderField>;

struct Response final
{
Status status;
HeaderFields headerFields;
std::vector<std::uint8_t> body;
};

inline namespace detail
{
#if defined(_WIN32) || defined(__CYGWIN__)
class WinSock final
{
public:
WinSock()
{
WSADATA wsaData;
const auto error = WSAStartup(MAKEWORD(2, 2), &wsaData);
if (error != 0)
throw std::system_error{error, std::system_category(), "WSAStartup failed"};

if (LOBYTE(wsaData.wVersion) != 2 || HIBYTE(wsaData.wVersion) != 2)
{
WSACleanup();
throw std::runtime_error{"Invalid WinSock version"};
}

started = true;
}

~WinSock()
{
if (started) WSACleanup();
}

WinSock(WinSock&& other) noexcept:
started{other.started}
{
other.started = false;
}

WinSock& operator=(WinSock&& other) noexcept
{
if (&other == this) return *this;
if (started) WSACleanup();
started = other.started;
other.started = false;
return *this;
}

private:
bool started = false;
};
#endif // defined(_WIN32) || defined(__CYGWIN__)

inline int getLastError() noexcept
{
#if defined(_WIN32) || defined(__CYGWIN__)
return WSAGetLastError();
#else
return errno;
#endif // defined(_WIN32) || defined(__CYGWIN__)
}

constexpr int getAddressFamily(const InternetProtocol internetProtocol)
{
return (internetProtocol == InternetProtocol::V4) ? AF_INET :
(internetProtocol == InternetProtocol::V6) ? AF_INET6 :
throw RequestError{"Unsupported protocol"};
}

class Socket final
{
public:
#if defined(_WIN32) || defined(__CYGWIN__)
using Type = SOCKET;
static constexpr Type invalid = INVALID_SOCKET;
#else
using Type = int;
static constexpr Type invalid = -1;
#endif // defined(_WIN32) || defined(__CYGWIN__)

explicit Socket(const InternetProtocol internetProtocol):
endpoint{socket(getAddressFamily(internetProtocol), SOCK_STREAM, IPPROTO_TCP)}
{
if (endpoint == invalid)
throw std::system_error{getLastError(), std::system_category(), "Failed to create socket"};

#if defined(_WIN32) || defined(__CYGWIN__)
ULONG mode = 1;
if (ioctlsocket(endpoint, FIONBIO, &mode) != 0)
{
close();
throw std::system_error{WSAGetLastError(), std::system_category(), "Failed to get socket flags"};
}
#else
const auto flags = fcntl(endpoint, F_GETFL);
if (flags == -1)
{
close();
throw std::system_error{errno, std::system_category(), "Failed to get socket flags"};
}

if (fcntl(endpoint, F_SETFL, flags | O_NONBLOCK) == -1)
{
close();
throw std::system_error{errno, std::system_category(), "Failed to set socket flags"};
}
#endif // defined(_WIN32) || defined(__CYGWIN__)

#ifdef __APPLE__
const int value = 1;
if (setsockopt(endpoint, SOL_SOCKET, SO_NOSIGPIPE, &value, sizeof(value)) == -1)
{
close();
throw std::system_error{errno, std::system_category(), "Failed to set socket option"};
}
#endif // __APPLE__
}

~Socket()
{
if (endpoint != invalid) close();
}

Socket(Socket&& other) noexcept:
endpoint{other.endpoint}
{
other.endpoint = invalid;
}

Socket& operator=(Socket&& other) noexcept
{
if (&other == this) return *this;
if (endpoint != invalid) close();
endpoint = other.endpoint;
other.endpoint = invalid;
return *this;
}

void connect(const struct sockaddr* address, const socklen_t addressSize, const std::int64_t timeout)
{
#if defined(_WIN32) || defined(__CYGWIN__)
auto result = ::connect(endpoint, address, addressSize);
while (result == -1 && WSAGetLastError() == WSAEINTR)
result = ::connect(endpoint, address, addressSize);

if (result == -1)
{
if (WSAGetLastError() == WSAEWOULDBLOCK)
{
select(SelectType::write, timeout);

char socketErrorPointer[sizeof(int)];
socklen_t optionLength = sizeof(socketErrorPointer);
if (getsockopt(endpoint, SOL_SOCKET, SO_ERROR, socketErrorPointer, &optionLength) == -1)
throw std::system_error{WSAGetLastError(), std::system_category(), "Failed to get socket option"};

int socketError;
std::memcpy(&socketError, socketErrorPointer, sizeof(socketErrorPointer));

if (socketError != 0)
throw std::system_error{socketError, std::system_category(), "Failed to connect"};
}
else
throw std::system_error{WSAGetLastError(), std::system_category(), "Failed to connect"};
}
#else
auto result = ::connect(endpoint, address, addressSize);
while (result == -1 && errno == EINTR)
result = ::connect(endpoint, address, addressSize);

if (result == -1)
{
if (errno == EINPROGRESS)
{
select(SelectType::write, timeout);

int socketError;
socklen_t optionLength = sizeof(socketError);
if (getsockopt(endpoint, SOL_SOCKET, SO_ERROR, &socketError, &optionLength) == -1)
throw std::system_error{errno, std::system_category(), "Failed to get socket option"};

if (socketError != 0)
throw std::system_error{socketError, std::system_category(), "Failed to connect"};
}
else
throw std::system_error{errno, std::system_category(), "Failed to connect"};
}
#endif // defined(_WIN32) || defined(__CYGWIN__)
}

std::size_t send(const void* buffer, const std::size_t length, const std::int64_t timeout)
{
select(SelectType::write, timeout);
#if defined(_WIN32) || defined(__CYGWIN__)
auto result = ::send(endpoint, reinterpret_cast<const char*>(buffer),
static_cast<int>(length), 0);

while (result == -1 && WSAGetLastError() == WSAEINTR)
result = ::send(endpoint, reinterpret_cast<const char*>(buffer),
static_cast<int>(length), 0);

if (result == -1)
throw std::system_error{WSAGetLastError(), std::system_category(), "Failed to send data"};
#else
auto result = ::send(endpoint, reinterpret_cast<const char*>(buffer),
length, noSignal);

while (result == -1 && errno == EINTR)
result = ::send(endpoint, reinterpret_cast<const char*>(buffer),
length, noSignal);

if (result == -1)
throw std::system_error{errno, std::system_category(), "Failed to send data"};
#endif // defined(_WIN32) || defined(__CYGWIN__)
return static_cast<std::size_t>(result);
}

std::size_t recv(void* buffer, const std::size_t length, const std::int64_t timeout)
{
select(SelectType::read, timeout);
#if defined(_WIN32) || defined(__CYGWIN__)
auto result = ::recv(endpoint, reinterpret_cast<char*>(buffer),
static_cast<int>(length), 0);

while (result == -1 && WSAGetLastError() == WSAEINTR)
result = ::recv(endpoint, reinterpret_cast<char*>(buffer),
static_cast<int>(length), 0);

if (result == -1)
throw std::system_error{WSAGetLastError(), std::system_category(), "Failed to read data"};
#else
auto result = ::recv(endpoint, reinterpret_cast<char*>(buffer),
length, noSignal);

while (result == -1 && errno == EINTR)
result = ::recv(endpoint, reinterpret_cast<char*>(buffer),
length, noSignal);

if (result == -1)
throw std::system_error{errno, std::system_category(), "Failed to read data"};
#endif // defined(_WIN32) || defined(__CYGWIN__)
return static_cast<std::size_t>(result);
}

private:
enum class SelectType
{
read,
write
};

void select(const SelectType type, const std::int64_t timeout)
{
fd_set descriptorSet;
FD_ZERO(&descriptorSet);
FD_SET(endpoint, &descriptorSet);

#if defined(_WIN32) || defined(__CYGWIN__)
TIMEVAL selectTimeout{
static_cast<LONG>(timeout / 1000),
static_cast<LONG>((timeout % 1000) * 1000)
};
auto count = ::select(0,
(type == SelectType::read) ? &descriptorSet : nullptr,
(type == SelectType::write) ? &descriptorSet : nullptr,
nullptr,
(timeout >= 0) ? &selectTimeout : nullptr);

while (count == -1 && WSAGetLastError() == WSAEINTR)
count = ::select(0,
(type == SelectType::read) ? &descriptorSet : nullptr,
(type == SelectType::write) ? &descriptorSet : nullptr,
nullptr,
(timeout >= 0) ? &selectTimeout : nullptr);

if (count == -1)
throw std::system_error{WSAGetLastError(), std::system_category(), "Failed to select socket"};
else if (count == 0)
throw ResponseError{"Request timed out"};
#else
timeval selectTimeout{
static_cast<time_t>(timeout / 1000),
static_cast<suseconds_t>((timeout % 1000) * 1000)
};
auto count = ::select(endpoint + 1,
(type == SelectType::read) ? &descriptorSet : nullptr,
(type == SelectType::write) ? &descriptorSet : nullptr,
nullptr,
(timeout >= 0) ? &selectTimeout : nullptr);

while (count == -1 && errno == EINTR)
count = ::select(endpoint + 1,
(type == SelectType::read) ? &descriptorSet : nullptr,
(type == SelectType::write) ? &descriptorSet : nullptr,
nullptr,
(timeout >= 0) ? &selectTimeout : nullptr);

if (count == -1)
throw std::system_error{errno, std::system_category(), "Failed to select socket"};
else if (count == 0)
throw ResponseError{"Request timed out"};
#endif // defined(_WIN32) || defined(__CYGWIN__)
}

void close() noexcept
{
#if defined(_WIN32) || defined(__CYGWIN__)
closesocket(endpoint);
#else
::close(endpoint);
#endif // defined(_WIN32) || defined(__CYGWIN__)
}

#if defined(__unix__) && !defined(__APPLE__) && !defined(__CYGWIN__)
static constexpr int noSignal = MSG_NOSIGNAL;
#else
static constexpr int noSignal = 0;
#endif // defined(__unix__) && !defined(__APPLE__)

Type endpoint = invalid;
};

// RFC 7230, 3.2.3. WhiteSpace
template <typename C>
constexpr bool isWhiteSpaceChar(const C c) noexcept
{
return c == 0x20 || c == 0x09; // space or tab
};

// RFC 5234, Appendix B.1. Core Rules
template <typename C>
constexpr bool isDigitChar(const C c) noexcept
{
return c >= 0x30 && c <= 0x39; // 0 - 9
}

// RFC 5234, Appendix B.1. Core Rules
template <typename C>
constexpr bool isAlphaChar(const C c) noexcept
{
return
(c >= 0x61 && c <= 0x7A) || // a - z
(c >= 0x41 && c <= 0x5A); // A - Z
}

// RFC 7230, 3.2.6. Field Value Components
template <typename C>
constexpr bool isTokenChar(const C c) noexcept
{
return c == 0x21 || // !
c == 0x23 || // #
c == 0x24 || // $
c == 0x25 || // %
c == 0x26 || // &
c == 0x27 || // '
c == 0x2A || // *
c == 0x2B || // +
c == 0x2D || // -
c == 0x2E || // .
c == 0x5E || // ^
c == 0x5F || // _
c == 0x60 || // `
c == 0x7C || // |
c == 0x7E || // ~
isDigitChar(c) ||
isAlphaChar(c);
};

// RFC 5234, Appendix B.1. Core Rules
template <typename C>
constexpr bool isVisibleChar(const C c) noexcept
{
return c >= 0x21 && c <= 0x7E;
}

// RFC 7230, Appendix B. Collected ABNF
template <typename C>
constexpr bool isObsoleteTextChar(const C c) noexcept
{
return static_cast<unsigned char>(c) >= 0x80 &&
static_cast<unsigned char>(c) <= 0xFF;
}

template <class Iterator>
Iterator skipWhiteSpaces(const Iterator begin, const Iterator end)
{
auto i = begin;
for (i = begin; i != end; ++i)
if (!isWhiteSpaceChar(*i))
break;

return i;
}

// RFC 5234, Appendix B.1. Core Rules
template <typename T, typename C, typename std::enable_if<std::is_unsigned<T>::value>::type* = nullptr>
constexpr T digitToUint(const C c)
{
// DIGIT
return (c >= 0x30 && c <= 0x39) ? static_cast<T>(c - 0x30) : // 0 - 9
throw ResponseError{"Invalid digit"};
}

// RFC 5234, Appendix B.1. Core Rules
template <typename T, typename C, typename std::enable_if<std::is_unsigned<T>::value>::type* = nullptr>
constexpr T hexDigitToUint(const C c)
{
// HEXDIG
return (c >= 0x30 && c <= 0x39) ? static_cast<T>(c - 0x30) : // 0 - 9
(c >= 0x41 && c <= 0x46) ? static_cast<T>(c - 0x41) + T(10) : // A - Z
(c >= 0x61 && c <= 0x66) ? static_cast<T>(c - 0x61) + T(10) : // a - z, some services send lower-case hex digits
throw ResponseError{"Invalid hex digit"};
}

// RFC 3986, 3. Syntax Components
template <class Iterator>
Uri parseUri(const Iterator begin, const Iterator end)
{
Uri result;

// RFC 3986, 3.1. Scheme
auto i = begin;
if (i == end || !isAlphaChar(*begin))
throw RequestError{"Invalid scheme"};

result.scheme.push_back(*i++);

for (; i != end && (isAlphaChar(*i) || isDigitChar(*i) || *i == '+' || *i == '-' || *i == '.'); ++i)
result.scheme.push_back(*i);

if (i == end || *i++ != ':')
throw RequestError{"Invalid scheme"};
if (i == end || *i++ != '/')
throw RequestError{"Invalid scheme"};
if (i == end || *i++ != '/')
throw RequestError{"Invalid scheme"};

// RFC 3986, 3.2. Authority
std::string authority = std::string(i, end);

// RFC 3986, 3.5. Fragment
const auto fragmentPosition = authority.find('#');
if (fragmentPosition != std::string::npos)
{
result.fragment = authority.substr(fragmentPosition + 1);
authority.resize(fragmentPosition); // remove the fragment part
}

// RFC 3986, 3.4. Query
const auto queryPosition = authority.find('?');
if (queryPosition != std::string::npos)
{
result.query = authority.substr(queryPosition + 1);
authority.resize(queryPosition); // remove the query part
}

// RFC 3986, 3.3. Path
const auto pathPosition = authority.find('/');
if (pathPosition != std::string::npos)
{
// RFC 3986, 3.3. Path
result.path = authority.substr(pathPosition);
authority.resize(pathPosition);
}
else
result.path = "/";

// RFC 3986, 3.2.1. User Information
std::string userinfo;
const auto hostPosition = authority.find('@');
if (hostPosition != std::string::npos)
{
userinfo = authority.substr(0, hostPosition);

const auto passwordPosition = userinfo.find(':');
if (passwordPosition != std::string::npos)
{
result.user = userinfo.substr(0, passwordPosition);
result.password = userinfo.substr(passwordPosition + 1);
}
else
result.user = userinfo;

result.host = authority.substr(hostPosition + 1);
}
else
result.host = authority;

// RFC 3986, 3.2.2. Host
const auto portPosition = result.host.find(':');
if (portPosition != std::string::npos)
{
// RFC 3986, 3.2.3. Port
result.port = result.host.substr(portPosition + 1);
result.host.resize(portPosition);
}

return result;
}

// RFC 7230, 2.6. Protocol Versioning
template <class Iterator>
std::pair<Iterator, HttpVersion> parseHttpVersion(const Iterator begin, const Iterator end)
{
auto i = begin;

if (i == end || *i++ != 'H')
throw ResponseError{"Invalid HTTP version"};
if (i == end || *i++ != 'T')
throw ResponseError{"Invalid HTTP version"};
if (i == end || *i++ != 'T')
throw ResponseError{"Invalid HTTP version"};
if (i == end || *i++ != 'P')
throw ResponseError{"Invalid HTTP version"};
if (i == end || *i++ != '/')
throw ResponseError{"Invalid HTTP version"};

if (i == end)
throw ResponseError{"Invalid HTTP version"};

const auto majorVersion = digitToUint<std::uint16_t>(*i++);

if (i == end || *i++ != '.')
throw ResponseError{"Invalid HTTP version"};

if (i == end)
throw ResponseError{"Invalid HTTP version"};

const auto minorVersion = digitToUint<std::uint16_t>(*i++);

return {i, HttpVersion{majorVersion, minorVersion}};
}

// RFC 7230, 3.1.2. Status Line
template <class Iterator>
std::pair<Iterator, std::uint16_t> parseStatusCode(const Iterator begin, const Iterator end)
{
std::uint16_t result = 0;

auto i = begin;
while (i != end && isDigitChar(*i))
result = static_cast<std::uint16_t>(result * 10U) + digitToUint<std::uint16_t>(*i++);

if (std::distance(begin, i) != 3)
throw ResponseError{"Invalid status code"};

return {i, result};
}

// RFC 7230, 3.1.2. Status Line
template <class Iterator>
std::pair<Iterator, std::string> parseReasonPhrase(const Iterator begin, const Iterator end)
{
std::string result;

auto i = begin;
for (; i != end && (isWhiteSpaceChar(*i) || isVisibleChar(*i) || isObsoleteTextChar(*i)); ++i)
result.push_back(static_cast<char>(*i));

return {i, std::move(result)};
}

// RFC 7230, 3.2.6. Field Value Components
template <class Iterator>
std::pair<Iterator, std::string> parseToken(const Iterator begin, const Iterator end)
{
std::string result;

auto i = begin;
for (; i != end && isTokenChar(*i); ++i)
result.push_back(static_cast<char>(*i));

if (result.empty())
throw ResponseError{"Invalid token"};

return {i, std::move(result)};
}

// RFC 7230, 3.2. Header Fields
template <class Iterator>
std::pair<Iterator, std::string> parseFieldValue(const Iterator begin, const Iterator end)
{
std::string result;

auto i = begin;
for (; i != end && (isWhiteSpaceChar(*i) || isVisibleChar(*i) || isObsoleteTextChar(*i)); ++i)
result.push_back(static_cast<char>(*i));

// trim white spaces
result.erase(std::find_if(result.rbegin(), result.rend(), [](const char c) noexcept {
return !isWhiteSpaceChar(c);
}).base(), result.end());

return {i, std::move(result)};
}

// RFC 7230, 3.2. Header Fields
template <class Iterator>
std::pair<Iterator, std::string> parseFieldContent(const Iterator begin, const Iterator end)
{
std::string result;

auto i = begin;

for (;;)
{
const auto fieldValueResult = parseFieldValue(i, end);
i = fieldValueResult.first;
result += fieldValueResult.second;

// Handle obsolete fold as per RFC 7230, 3.2.4. Field Parsing
// Obsolete folding is known as linear white space (LWS) in RFC 2616, 2.2 Basic Rules
auto obsoleteFoldIterator = i;
if (obsoleteFoldIterator == end || *obsoleteFoldIterator++ != '\r')
break;

if (obsoleteFoldIterator == end || *obsoleteFoldIterator++ != '\n')
break;

if (obsoleteFoldIterator == end || !isWhiteSpaceChar(*obsoleteFoldIterator++))
break;

result.push_back(' ');
i = obsoleteFoldIterator;
}

return {i, std::move(result)};
}

// RFC 7230, 3.2. Header Fields
template <class Iterator>
std::pair<Iterator, HeaderField> parseHeaderField(const Iterator begin, const Iterator end)
{
auto tokenResult = parseToken(begin, end);
auto i = tokenResult.first;
auto fieldName = std::move(tokenResult.second);

if (i == end || *i++ != ':')
throw ResponseError{"Invalid header"};

i = skipWhiteSpaces(i, end);

auto valueResult = parseFieldContent(i, end);
i = valueResult.first;
auto fieldValue = std::move(valueResult.second);

if (i == end || *i++ != '\r')
throw ResponseError{"Invalid header"};

if (i == end || *i++ != '\n')
throw ResponseError{"Invalid header"};

return {i, {std::move(fieldName), std::move(fieldValue)}};
}

// RFC 7230, 3.1.2. Status Line
template <class Iterator>
std::pair<Iterator, Status> parseStatusLine(const Iterator begin, const Iterator end)
{
const auto httpVersionResult = parseHttpVersion(begin, end);
auto i = httpVersionResult.first;

if (i == end || *i++ != ' ')
throw ResponseError{"Invalid status line"};

const auto statusCodeResult = parseStatusCode(i, end);
i = statusCodeResult.first;

if (i == end || *i++ != ' ')
throw ResponseError{"Invalid status line"};

auto reasonPhraseResult = parseReasonPhrase(i, end);
i = reasonPhraseResult.first;

if (i == end || *i++ != '\r')
throw ResponseError{"Invalid status line"};

if (i == end || *i++ != '\n')
throw ResponseError{"Invalid status line"};

return {i, Status{
httpVersionResult.second,
statusCodeResult.second,
std::move(reasonPhraseResult.second)
}};
}

// RFC 7230, 4.1. Chunked Transfer Coding
template <typename T, class Iterator, typename std::enable_if<std::is_unsigned<T>::value>::type* = nullptr>
T stringToUint(const Iterator begin, const Iterator end)
{
T result = 0;
for (auto i = begin; i != end; ++i)
result = T(10U) * result + digitToUint<T>(*i);

return result;
}

template <typename T, class Iterator, typename std::enable_if<std::is_unsigned<T>::value>::type* = nullptr>
T hexStringToUint(const Iterator begin, const Iterator end)
{
T result = 0;
for (auto i = begin; i != end; ++i)
result = T(16U) * result + hexDigitToUint<T>(*i);

return result;
}

// RFC 7230, 3.1.1. Request Line
inline std::string encodeRequestLine(const std::string& method, const std::string& target)
{
return method + " " + target + " HTTP/1.1\r\n";
}

// RFC 7230, 3.2. Header Fields
inline std::string encodeHeaderFields(const HeaderFields& headerFields)
{
std::string result;
for (const auto& headerField : headerFields)
{
if (headerField.first.empty())
throw RequestError{"Invalid header field name"};

for (const auto c : headerField.first)
if (!isTokenChar(c))
throw RequestError{"Invalid header field name"};

for (const auto c : headerField.second)
if (!isWhiteSpaceChar(c) && !isVisibleChar(c) && !isObsoleteTextChar(c))
throw RequestError{"Invalid header field value"};

result += headerField.first + ": " + headerField.second + "\r\n";
}

return result;
}

// RFC 4648, 4. Base 64 Encoding
template <class Iterator>
std::string encodeBase64(const Iterator begin, const Iterator end)
{
constexpr std::array<char, 64> chars{
'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M',
'N', 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z',
'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm',
'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z',
'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '+', '/'
};

std::string result;
std::size_t c = 0;
std::array<std::uint8_t, 3> charArray;

for (auto i = begin; i != end; ++i)
{
charArray[c++] = static_cast<std::uint8_t>(*i);
if (c == 3)
{
result += chars[static_cast<std::uint8_t>((charArray[0] & 0xFC) >> 2)];
result += chars[static_cast<std::uint8_t>(((charArray[0] & 0x03) << 4) + ((charArray[1] & 0xF0) >> 4))];
result += chars[static_cast<std::uint8_t>(((charArray[1] & 0x0F) << 2) + ((charArray[2] & 0xC0) >> 6))];
result += chars[static_cast<std::uint8_t>(charArray[2] & 0x3f)];
c = 0;
}
}

if (c)
{
result += chars[static_cast<std::uint8_t>((charArray[0] & 0xFC) >> 2)];

if (c == 1)
result += chars[static_cast<std::uint8_t>((charArray[0] & 0x03) << 4)];
else // c == 2
{
result += chars[static_cast<std::uint8_t>(((charArray[0] & 0x03) << 4) + ((charArray[1] & 0xF0) >> 4))];
result += chars[static_cast<std::uint8_t>((charArray[1] & 0x0F) << 2)];
}

while (++c < 4) result += '='; // padding
}

return result;
}

inline std::vector<std::uint8_t> encodeHtml(const Uri& uri,
const std::string& method,
const std::vector<uint8_t>& body,
HeaderFields headerFields)
{
if (uri.scheme != "http")
throw RequestError{"Only HTTP scheme is supported"};

// RFC 7230, 5.3. Request Target
const std::string requestTarget = uri.path + (uri.query.empty() ? "" : '?' + uri.query);

// RFC 7230, 5.4. Host
headerFields.push_back({"Host", uri.host});

// RFC 7230, 3.3.2. Content-Length
headerFields.push_back({"Content-Length", std::to_string(body.size())});

// RFC 7617, 2. The 'Basic' Authentication Scheme
if (!uri.user.empty() || !uri.password.empty())
{
std::string userinfo = uri.user + ':' + uri.password;
headerFields.push_back({"Authorization", "Basic " + encodeBase64(userinfo.begin(), userinfo.end())});
}

const auto headerData = encodeRequestLine(method, requestTarget) +
encodeHeaderFields(headerFields) +
"\r\n";

std::vector<uint8_t> result(headerData.begin(), headerData.end());
result.insert(result.end(), body.begin(), body.end());

return result;
}
}

class Request final
{
public:
explicit Request(const std::string& uriString,
const InternetProtocol protocol = InternetProtocol::V4):
internetProtocol{protocol},
uri{parseUri(uriString.begin(), uriString.end())}
{
}

Response send(const std::string& method = "GET",
const std::string& body = "",
const HeaderFields& headerFields = {},
const std::chrono::milliseconds timeout = std::chrono::milliseconds{-1})
{
return send(method,
std::vector<uint8_t>(body.begin(), body.end()),
headerFields,
timeout);
}

Response send(const std::string& method,
const std::vector<uint8_t>& body,
const HeaderFields& headerFields = {},
const std::chrono::milliseconds timeout = std::chrono::milliseconds{-1})
{
const auto stopTime = std::chrono::steady_clock::now() + timeout;

if (uri.scheme != "http")
throw RequestError{"Only HTTP scheme is supported"};

addrinfo hints = {};
hints.ai_family = getAddressFamily(internetProtocol);
hints.ai_socktype = SOCK_STREAM;

const char* port = uri.port.empty() ? "80" : uri.port.c_str();

addrinfo* info;
if (getaddrinfo(uri.host.c_str(), port, &hints, &info) != 0)
throw std::system_error{getLastError(), std::system_category(), "Failed to get address info of " + uri.host};

const std::unique_ptr<addrinfo, decltype(&freeaddrinfo)> addressInfo{info, freeaddrinfo};

const auto requestData = encodeHtml(uri, method, body, headerFields);

Socket socket{internetProtocol};

const auto getRemainingMilliseconds = [](const std::chrono::steady_clock::time_point time) noexcept -> std::int64_t {
const auto now = std::chrono::steady_clock::now();
const auto remainingTime = std::chrono::duration_cast<std::chrono::milliseconds>(time - now);
return (remainingTime.count() > 0) ? remainingTime.count() : 0;
};

// take the first address from the list
socket.connect(addressInfo->ai_addr, static_cast<socklen_t>(addressInfo->ai_addrlen),
(timeout.count() >= 0) ? getRemainingMilliseconds(stopTime) : -1);

auto remaining = requestData.size();
auto sendData = requestData.data();

// send the request
while (remaining > 0)
{
const auto size = socket.send(sendData, remaining,
(timeout.count() >= 0) ? getRemainingMilliseconds(stopTime) : -1);
remaining -= size;
sendData += size;
}

std::array<std::uint8_t, 4096> tempBuffer;
constexpr std::array<std::uint8_t, 2> crlf = {'\r', '\n'};
constexpr std::array<std::uint8_t, 4> headerEnd = {'\r', '\n', '\r', '\n'};
Response response;
std::vector<std::uint8_t> responseData;
bool parsingBody = false;
bool contentLengthReceived = false;
std::size_t contentLength = 0U;
bool chunkedResponse = false;
std::size_t expectedChunkSize = 0U;
bool removeCrlfAfterChunk = false;

// read the response
for (;;)
{
const auto size = socket.recv(tempBuffer.data(), tempBuffer.size(),
(timeout.count() >= 0) ? getRemainingMilliseconds(stopTime) : -1);
if (size == 0) // disconnected
return response;

responseData.insert(responseData.end(), tempBuffer.begin(), tempBuffer.begin() + size);

if (!parsingBody)
{
// RFC 7230, 3. Message Format
// Empty line indicates the end of the header section (RFC 7230, 2.1. Client/Server Messaging)
const auto endIterator = std::search(responseData.cbegin(), responseData.cend(),
headerEnd.cbegin(), headerEnd.cend());
if (endIterator == responseData.cend()) break; // two consecutive CRLFs not found

const auto headerBeginIterator = responseData.cbegin();
const auto headerEndIterator = endIterator + 2;

auto statusLineResult = parseStatusLine(headerBeginIterator, headerEndIterator);
auto i = statusLineResult.first;

response.status = std::move(statusLineResult.second);

for (;;)
{
auto headerFieldResult = parseHeaderField(i, headerEndIterator);
i = headerFieldResult.first;

auto fieldName = std::move(headerFieldResult.second.first);
const auto toLower = [](const char c) noexcept {
return (c >= 'A' && c <= 'Z') ? c - ('A' - 'a') : c;
};
std::transform(fieldName.begin(), fieldName.end(), fieldName.begin(), toLower);

auto fieldValue = std::move(headerFieldResult.second.second);

if (fieldName == "transfer-encoding")
{
// RFC 7230, 3.3.1. Transfer-Encoding
if (fieldValue == "chunked")
chunkedResponse = true;
else
throw ResponseError{"Unsupported transfer encoding: " + fieldValue};
}
else if (fieldName == "content-length")
{
// RFC 7230, 3.3.2. Content-Length
contentLength = stringToUint<std::size_t>(fieldValue.cbegin(), fieldValue.cend());
contentLengthReceived = true;
response.body.reserve(contentLength);
}

response.headerFields.push_back({std::move(fieldName), std::move(fieldValue)});

if (i == headerEndIterator)
break;
}

responseData.erase(responseData.cbegin(), headerEndIterator + 2);
parsingBody = true;
}

if (parsingBody)
{
// Content-Length must be ignored if Transfer-Encoding is received (RFC 7230, 3.2. Content-Length)
if (chunkedResponse)
{
// RFC 7230, 4.1. Chunked Transfer Coding
for (;;)
{
if (expectedChunkSize > 0)
{
const auto toWrite = (std::min)(expectedChunkSize, responseData.size());
response.body.insert(response.body.end(), responseData.begin(),
responseData.begin() + static_cast<std::ptrdiff_t>(toWrite));
responseData.erase(responseData.begin(),
responseData.begin() + static_cast<std::ptrdiff_t>(toWrite));
expectedChunkSize -= toWrite;

if (expectedChunkSize == 0) removeCrlfAfterChunk = true;
if (responseData.empty()) break;
}
else
{
if (removeCrlfAfterChunk)
{
if (responseData.size() < 2) break;

if (!std::equal(crlf.begin(), crlf.end(), responseData.begin()))
throw ResponseError{"Invalid chunk"};

removeCrlfAfterChunk = false;
responseData.erase(responseData.begin(), responseData.begin() + 2);
}

const auto i = std::search(responseData.begin(), responseData.end(),
crlf.begin(), crlf.end());

if (i == responseData.end()) break;

expectedChunkSize = detail::hexStringToUint<std::size_t>(responseData.begin(), i);
responseData.erase(responseData.begin(), i + 2);

if (expectedChunkSize == 0)
return response;
}
}
}
else
{
response.body.insert(response.body.end(), responseData.begin(), responseData.end());
responseData.clear();

// got the whole content
if (contentLengthReceived && response.body.size() >= contentLength)
return response;
}
}
}

return response;
}

private:
#if defined(_WIN32) || defined(__CYGWIN__)
WinSock winSock;
#endif // defined(_WIN32) || defined(__CYGWIN__)
InternetProtocol internetProtocol;
Uri uri;
};
}

#endif // HTTPREQUEST_HPP

参考资料:

网络编程(Win/Linux):理解select函数并实现IO复用服务器端 - 戈小戈 - 博客园

Socket通信——C++服务器端和Java客户端_xcy6666的博客-CSDN博客

C++和java通过Socket批量发送和接收文件(C++客户端发送,java服务端接收)_wengtengfan的博客-CSDN博客_socket 批量发送

java 发送字节流图片,c++接收二进制流_51CTO博客_c++读取二进制文件

C++和java通过Socket批量发送和接收文件(C++客户端发送,java服务端接收)_wengtengfan的博客-CSDN博客_socket 批量发送

Java客户端上传图片(文件)到c++服务器 - c++语言程序开发技术文章_c++编程 - 红黑联盟

https://github.com/katiejyoung/file-transfer-system/blob/master/FTClient.java

file-transfer-system/ftserver.c at master · katiejyoung/file-transfer-system · GitHub

Socket通信——C++服务器端和Java客户端_xcy6666的博客-CSDN博客


java:

Java实现socket通信详解(UDP/TCP)c/s模式_寒风凋零的博客-CSDN博客

使用Java完成Socket通信_牛言牛语的博客-CSDN博客_java socket通信

目录

[TOC]

安装(Windows)

1、下载 : https://codeload.github.com/open-source-parsers/jsoncpp/zip/refs/tags/1.9.3

2、VSCode 打开项目,直接Cmake编译项目

3、得到libjsoncpp.a文件

4、新建项目就可以使用json了

项目配置

VS 项目配置g++

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
{
"label": "build demo_json",
"type": "shell",
"command": "g++",
"args": [
"-g",
"${workspaceFolder}/demo_json.cpp",
// "-I", "D:/Tools/cplus_relate/eigen-3.4.0/Eigen/",
"-I", "${workspaceFolder}/include/",
"-L", "${workspaceFolder}/libs/",
"-l", "jsoncpp",
"-o", "main.exe",
],
"group": {
"kind": "build",
"isDefault": true
},
"dependsOn":[
"clear",
],
},

CMake 配置

使用

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
 int ReadJson(const std::string & strValue) 
{
using namespace std;

Json::Reader reader;
Json::Value value;

if (reader.parse(strValue, value))
{
//解析json中的对象
string out = value["name"].asString();
cout << "name : " << out << endl;
cout << "number : " << value["number"].asInt() << endl;
cout << "value : " << value["value"].asBool() << endl;
cout << "no such num : " << value["haha"].asInt() << endl;
cout << "no such str : " << value["hehe"].asString() << endl;

//解析数组对象
const Json::Value arrayNum = value["arrnum"];
for (unsigned int i = 0; i < arrayNum.size(); i++)
{
cout << "arrnum[" << i << "] = " << arrayNum[i];
}
//解析对象数组对象
Json::Value arrayObj = value["array"];
cout << "array size = " << arrayObj.size() << endl;
for(unsigned int i = 0; i < arrayObj.size(); i++)
{
cout << arrayObj[i];
}
//从对象数组中找到想要的对象
for(unsigned int i = 0; i < arrayObj.size(); i++)
{
if (arrayObj[i].isMember("string"))
{
out = arrayObj[i]["string"].asString();
std::cout << "string : " << out << std::endl;
}
}
}

return 0;
}

std::string writeJson()
{
using namespace std;

Json::Value root;
Json::Value arrayObj;
Json::Value item;
Json::Value iNum;

item["string"] = "this is a string";
item["number"] = 999;
item["aaaaaa"] = "bbbbbb";
arrayObj.append(item);

//直接对jsoncpp对象以数字索引作为下标进行赋值,则自动作为数组
iNum[1] = 1;
iNum[2] = 2;
iNum[3] = 3;
iNum[4] = 4;
iNum[5] = 5;
iNum[6] = 6;

//增加对象数组
root["array"] = arrayObj;
//增加字符串
root["name"] = "json";
//增加数字
root["number"] = 666;
//增加布尔变量
root["value"] = true;
//增加数字数组
root["arrnum"] = iNum;

root.toStyledString();
string out = root.toStyledString();

return out;
}
// ref:: http://t.zoukankan.com/fengbohello-p-4066254.html

配置

VScode 配置 svn_LDG1998的博客-CSDN博客_vscode配置svn

memccpy

1
2
3
4
5
6
void *memccpy(void * restrict dest, const void * restrict src, int c, size_t count);

dest - pointer to the object to copy to
src - pointer to the object to copy from
c - terminating character, converted to unsigned char at first
count - number of characters to copy

这里容易出现理解偏差的是参数 c,“terminating character, converted to unsigned char at first” 也就是参数会转化为unsigned char 也就是需要传入的是单个字符,当检测到传入的src 中字符与”c”匹配时,停止copy,退出.

参考资料:

https://en.cppreference.com/w/c/string/byte/memccpy

目标

1.理解针孔相机的模型、内参与径向畸变参数。

2.理解一个空间点如何投影到相机成像平面。

3.掌握 OpenCV 的图像存储与表达方式。

4.学会基本的摄像头标定方法。

在以相机为主的视觉SLAM中,观测主要是指相机成像的过程。

三维世界中的一个物体反射或发出的光线,穿过相机光心后,投影在相机的成像平面上。相机的感光器件接收到光线后,产生测量值,就得到了像素,形成了我们见到的照片。

1、相机模型

相机将三维世界中的坐标点(单位为米)映射到二维图像平面(单位为像素)的过程能够用一个几何模型进行描述,称为针孔模型,它描述了一束光线通过针孔之后,在针孔背面投影成像的关系。同时,由于相机镜头上的透镜的存在,使得光线投影到成像平面的过程中会产生畸变。因此,我们使用针孔和畸变两个模型来描述整个投影过程。这两个模型能够把外部的三维点投影到相机内部成像平面,构成相机的内参数(Intrinsics)。

1.1 针孔相机模型

初中物理的蜡烛投影实验:在一个暗箱的前方放着一支点燃的蜡烛,蜡烛的光透过暗箱上的一个小孔投影在暗箱的后方平面上,并在这个平面上形成一个倒立的蜡烛图像。小孔模型能够把三维世界中的蜡烛投影到一个二维成像平面。同理,可以用这个简单的模型来解释相机的成像过程。对这个简单的针孔模型进行几何建模。设 O − x − y − z 为相机坐标系,z 轴指向相机前方,x 向右,y 向下。O为摄像机的光心,也是针孔模型中的针孔。现实世界的空间点P,经过小孔O投影之后,落在物理成像平面 O′ − x′ − y′ 上,成像点为 P′。设 P 的坐标为 [X,Y,Z]T,P′ 为 [X′,Y′,Z′]T,设物理成像平面到小孔的距离为f(焦距)。那么,根据三角形相似关系,有:

其中负号表示成的像是倒立的。不过,实际相机得到的图像并不是倒像,可以等价地把成像平面对称地放到相机前方,和三维空间点一起放在摄像机坐标系的同一侧,如图所示。

把公式中的负号去掉,X′,Y′ 放到等式左侧,整理得:

这描述了点 P和它的像之间的空间关系。不过,在相机中最终获得的是一个个的像素,这还需要在成像平面上对像进行采样和量化。为了描述传感器将感受到的光线转换成图像像素的过程,设在物理成像平面上固定着一个像素平面 o − u − v,在像素平面有P′的像素坐标:[u,v]T。

像素坐标系通常的定义方式是:原点o′位于图像的左上角,u 轴向右与 x 轴平行,v 轴向下与 y 轴平行。像素坐标系与成像平面之间,相差了一个缩放和一个原点的平移。设像素坐标在 u 轴上缩放了 α 倍,在 v 上缩放了 β 倍。同时,原点平移了 [cx,cy]T。那么,P′ 的坐标与像素坐标[u,v]T 的关系为:

代入式

把 αf 合并成 fx,把 βf 合并成 fy,得:

其中,f 的单位为米,α,β 的单位为像素/米,所以 fx,fy 和 cx,cy 的单位为像素。写成矩阵形式:

K矩阵称为相机的内参数矩阵(Camera Intrinsics)。通常相机的内参在出厂之后是固定的,不会在使用过程中发生变化。但有时需要自己确定相机的内参,也就是所谓的标定。

(单目棋盘格张正友标定法[25]Z. Zhang, “Flexible camera calibration by viewing a plane from unknown orientations,” in Computer Vision, 1999. The Proceedings of the Seventh IEEE International Conference on, vol. 1, pp. 666–673, Ieee, 1999.)

前面内参公式中的P是在相机坐标系下的坐标。由于相机在运动,所以P是相机的世界坐标(记为Pw)根据相机的当前位姿变换到相机坐标系下的结果。相机的位姿由它的旋转矩阵R和平移向量t来描述。那么有:

后一个式子隐含了一次齐次坐标到非齐次坐标的转换。它描述了P的世界坐标到像素坐标的投影关系。相机的位姿R,t称为相机的外参数(Camera Extrinsics) 。 相比于不变的内参,外参会随着相机运动发生改变,同时也是 SLAM 中待估计的目标,代表着机器人的轨迹。 式子表明,可以把一个世界坐标点先转换到相机坐标系,再除掉它最后一维(Z)的数值(即该点距离相机成像平面的深度),这相当于把最后一维进行归一化处理,得到点 P 在相机归一化平面上的投影:

归一化坐标可看成相机前方z=1处的平面上的一个点,这个 z = 1 平面也称为归一化平面。归一化坐标再左乘内参就得到了像素坐标,所以可以把像素坐标 [u,v]T 看成对归一化平面上的点进行量化测量的结果。从这个模型中可以看出,对相机坐标同时乘以任意非零常数,归一化坐标都是一样的,这说明点的深度在投影过程中被丢失了,所以单目视觉中没法得到像素点的深度值。

**注:**1. 相机输出的图像并不是倒像,但相机自身会翻转这张图像,所以实际得到的是正像,也就是对称的成像平面上的像。尽管从物理原理来说,小孔成像应该是倒像。

2.在机器人或自动驾驶车辆中,外参也可以解释成相机坐标系到机器人本体坐标系之间的变换。

1.2 畸变

为了获得好的成像效果,在相机的前方加了透镜。透镜的加入对成像过程中光线的传播会产生新的影响:一是透镜自身的形状对光线传播的影响,引起的畸变(Distortion,也叫失真)称为径向畸变。在针孔模型中,一条直线投影到像素平面上还是一条直线。可是,在实际拍摄的照片中,摄像机的透镜往往使得真实环境中的一条直线在图片中变成了曲线 。越靠近图像的边缘,这种现象越明显。由于实际加工制作的透镜往往是中心对称的,这使得不规则的畸变通常径向对称。它们主要分为两大类:桶形畸变和枕形畸变。

桶形畸变是由于图像放大率随着与光轴之间的距离增加而减小,而枕形畸变则恰好相反。在这两种畸变中,穿过图像中心和光轴有交点的直线还能保持形状不变。

二是在机械组装过程中,透镜和成像平面不可能完全平行,这也会使得光线穿过透镜投影到成像面时的位置发生变化,这引入切向畸变。

用数学形式对两者进行描述。省略具体过程,

对于相机坐标系中的一点P,能够通过 5 个畸变系数找到这个点在像素平面上的正确位置:

  1. 将三维空间点投影到归一化图像平面。设它的归一化坐标为 [x,y]T。
  2. 对归一化平面上的点计算径向畸变和切向畸变。
  3. 将畸变后的点通过内参数矩阵投影到像素平面,得到该点在图像上的正确位置。

在实际应用中,可以灵活选择纠正模型,比如只选择 k1,p1,p2 这 3 项等。

实际的图像系统中,学者们提出了很多其他的模型,比如相机的仿射模型和透视模型等,同时也存在很多其他类型的畸变。视觉 SLAM 中一般都使用普通的摄像头,针孔模型及径向畸变和切向畸变模型已经足够。 当一个图像去畸变之后,我们就可以直接用针孔模型建立投影关系,不用考虑畸变了。

小结单目相机的成像过程:

  1. 首先,世界坐标系下有一个固定的点 P,世界坐标为Pw。
  2. 由于相机在运动,它的运动由 R,t 或变换矩阵T∈SE(3) 描述。P 的相机坐标为 P˜c =RPw + t。
  3. 这时的 P˜c 的分量为 X,Y,Z,把它们投影到归一化平面 Z = 1 上,得到 P 的归一化坐标:Pc = [X/Z,Y /Z,1]T 。
  4. 有畸变时,根据畸变参数计算Pc 发生畸变后的坐标。
  5. 最后,P 的归一化坐标经过内参后,对应到它的像素坐标:Puv = KPc。 一共有四种坐标:世界坐标、相机坐标、归一化坐标和像素坐标。

1.3 双目相机模型

对于单目相机而言,仅根据一个像素,我们无法确定这个空间点的具体位置。这是因为,从相机光心到归一化平面连线上的所有点,都可以投影至该像素上(相当于没有了Z轴维度)。只有当P的深度确定时(比如通过双目或 RGB-D 相机),我们才能确切地知道它的空间位置。如图所示。

测量像素距离(或深度)的方式有很多种,比如人眼可以根据左右眼看到的景物差异(视差)来判断物体离我们的距离。双目相机的原理一样:通过同步采集左右相机的图像,计算图像间视差,来估计每一个像素的深度。

双目相机一般由左眼相机和右眼相机两个水平放置的相机组成。在左右双目相机中,我们可以把两个相机都看作针孔相机。它们是水平放置的,意味着两个相机的光圈中心都位于 x 轴上。两者之间的距离称为双目相机的基线(Baseline,记作 b),是双目相机的重要参数。

考虑一个空间点 P,它在左眼相机和右眼相机各成一像,记作 PL,PR。由于相机基线的存在,这两个成像位置是不同的。理想情况下,由于左右相机只在 x 轴上有位移,因此 P 的像也只在 x 轴(对应图像的u轴)上有差异。记它的左侧坐标为 uL,右侧坐标为 uR,几何关系如图所示。

根据 △PPLPR 和 △POLOR 的相似关系,整理得:

双目相机的成像模型:OL,OR 为左右光圈中心,方框为成像平面,f 为焦距。uL 和 uR 为成像平面的坐标。注意,按照图中坐标定义,uR 应该是负数,所以图中标出的距离为 −uR

其中 d 定义为左右图的横坐标之差,称为视差(Disparity)。根据视差,我们可以估计一个像素与相机之间的距离。视差与距离成反比:视差越大,距离越近。同时,由于视差最小为一个像素,于是双目的深度存在一个理论上的最大值,由 fb 确定。可以看到,当基线越长时,双目能测到的最大距离就会越远。类似人眼在看非常远的物体时(如很远的飞机),通常不能准确判断它的距离。

视差 d 的计算比较困难,需要确切地知道左眼图像某个像素出现在右眼图像的哪一个位置(即对应关系)。当想计算每个像素的深度时,其计算量与精度都将成为问题,而且只有在图像纹理变化丰富的地方才能计算视差。由于计算量的原因,双目深度估计仍需要使用 GPU 或FPGA 来实时计算。

1.4 RGB-D相机模型

RGB-D 相机是主动测量每个像素的深度。目前的 RGB-D 相机按原理可分为两大类:

  1. 红外结构光(Structured Light): Kinect 1 代、Project Tango 1 代、Intel RealSense 等。
  2. 通过飞行时间法(Time-of-flight,ToF):Kinect 2 代和一些现有的 ToF 传感器等。

无论是哪种类型,RGB-D 相机都需要向探测目标发射一束光线(通常是红外光)。在结构光原理中,相机根据返回的结构光图案,计算物体与自身之间的距离。而在 ToF 原理中,相机向目标发射脉冲光,然后根据发送到返回之间的光束飞行时间,确定物体与自身之间的距离。ToF原理的相机和激光雷达十分相似,只不过激光雷达是通过逐点扫描来获取这个物体的距离,而ToF相机则可以获得整个图像的像素深度。

在测量深度之后,RGB-D 相机通常按照生产时的相机摆放位置,自己完成深度与彩色图像素之间的配对,输出一一对应的彩色图和深度图。可以在同一个图像位置,读取到色彩信息和距离信息,计算像素的 3D 相机坐标,生成点云(Point Cloud)。对 RGB-D 数据,既可以在图像层面进行处理,也可在点云层面处理。

RGB-D 相机能够实时地测量每个像素点的距离。但用红外光进行深度值测量的 RGB-D 相机,容易受到日光或其他传感器发射的红外光干扰,因此不能在室外使用。在没有调制的情况下,同时使用多个 RGB-D 相机时也会相互干扰。对于透射材质的物体,因为接收不到反射光,所以无法测量这些点的位置。

2、图像

1
2
3
4
5
st=>start: 现实世界
e=>end: 照片
op1=>operation: 相机-拍照|past

st(right)->op1(right)->e

相机把三维世界中的信息转换成了一张由像素组成的照片,存储在计算机中,作为后续处理的数据来源。在数学中,图像可以用一个矩阵来描述;而在计算机中,它们占据一段连续的磁盘或内存空间,可以用二维数组来表示。

最简单的图像——灰度图:每个像素位置 (x,y) 对应一个灰度值 I,一张宽度为 w、高度为 h 的图像,数学上可以记为一个函数:

$$
I(x,y) : R^2 \to R
$$

其中 (x,y) 是像素的坐标。然而,计算机并不能表达实数空间,所以需要对下标和图像读数在某个范围内进行量化(类似于模拟到数字的概念)。在常见的灰度图中,用 0~255 的整数(一个 unsigned char或1 个字节)来表达图像的灰度读数。那么,一张宽度为 640 像素、高度为 480 像素分辨率的灰度图就可以表示为:

1
unsigned char image[480][640] //二维数组表达图像

在程序中,图像以二维数组形式存储。它的第一个下标是指数组的行,而第二个下标则是列。在图像中,数组的行数对应图像的高度,而列数对应图像的宽度。

当访问某一个像素时,需要指明它所处的坐标。像素坐标系原点位于图像的左上角,X 轴向右,Y 轴向下(也就是u,v* 坐标)。如果还有第三个轴—Z 轴,根据右手法则,Z 轴向前。这种定义方式是与相机坐标系一致的。图像的宽度或列数,对应着 X 轴;而图像的行数或高度,则对应着它的 Y 轴。

根据这种定义方式,访问一个位于 x,y 处的像素,那么在程序中应该是:

1
unsigned char pixel = image[y][x];  //访问图像像素

它对应着灰度值 I(x,y) 的读数。

在 RGB-D 相机的深度图中,记录了各个像素与相机之间的距离。这个距离通常是以毫米为单位,而 RGB-D 相机的量程通常在十几米左右,超过了 255。这时会采用 16 位整数(unsigned short)来记录深度图的信息,也就是位于 0~65535 的值。换算成米的话,最大可以表示 65 米,足够 RGB-D 相机使用。

彩色图像的表示则需要通道(channel)的概念。在计算机中,用红色、绿色和蓝色这三种颜色的组合来表达任意一种色彩。于是对于每一个像素,就要记录其 R、G、B 三个数值,每一个数值就称为一个通道。最常见的彩色图像有三个通道,每个通道都由 8 位整数表示。在这种规定下,一个像素占据 24 位空间。通道的数量、顺序都是可以自由定义的。在 OpenCV 的彩色图像中,通道的默认顺序是 B、G、R。也就是说,当得到一个 24 位的像素时,前 8 位表示蓝色数值,中间 8 位为绿色,最后 8 位为红色。如果还想表达图像的透明度,就使用 R、G、B、A 四个通道。

3 实践:计算机中的图像(OpenCV)

3.1 OpenCV 的基础使用方法

OpenCV提供了大量的开源图像算法,是计算机视觉中使用极广的图像处理算法库。

在Ubuntu下,有两种安装方式:

  1. 从源代码安装,指从OpenCV网站下载所有的OpenCV源代码,并在机器上编译安装,以便使用。好处是可以选择的版本比较丰富,而且能看到源代码,不过需要编译。还可以调整一些编译选项,匹配编程环境(例如,需不需要GPU加速等),还可以使用一些额外的功能。 源代码安装OpenCV 目前维护了两个主要版本,分为 OpenCV2.4系列和 OpenCV3系列。
  2. 只安装库文件,指通过Ubuntu来安装由Ubuntu社区人员已经编译好的库文件,这样无须编译。

源代码安装,安装依赖项:

1
sudo apt−get install build−essential libgtk2.0−dev libvtk5−dev libjpeg−dev libtiff4−dev libjasper−dev libopenexr−dev libtbb−dev

OpenCV 的依赖项很多,缺少某些编译项会影响它的部分功能,但可能不会用上。OpenCV 会在 cmake 阶段检查依赖项是否会安装,并调整自己的功能。如果电脑上有GPU并且安装了相关依赖项,OpenCV也会把GPU加速打开。

安装:

1
2
3
cmake ..
make -j8
sudo make install

安装后,OpenCV 默认存储在/usr/local 目录下

操作 OpenCV 图像

slambook/ch5/imageBasics/imageBasics.cpp

在该例程中操作有:图像读取、显示、像素遍历、复制、赋值等。编译该程序时,需要在CMakeLists.txt中添加 OpenCV的头文件,然后把程序链接到库文件上,还使用了C++11标准(如 nullptr 和 chrono)。

编译运行:

报错:

1
2
CMakeFiles/joinMap.dir/joinMap.cpp.o:在函数‘fmt::v7::detail::compile_parse_context<char, fmt::v7::detail::error_handler>::char_type const* fmt::v7::detail::parse_format_specs<Eigen::Transpose<Eigen::Matrix<double, 4, 1, 0, 4, 1> >, fmt::v7::detail::compile_parse_context<char, fmt::v7::detail::error_handler> >(fmt::v7::detail::compile_parse_context<char, fmt::v7::detail::error_handler>&)’中:
joinMap.cpp:(.text._ZN3fmt2v76detail18parse_format_specsIN5Eigen9TransposeINS3_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEENS1_21compile_parse_contextIcNS1_13error_handlerEEEEEPKNT0_9char_typeERSB_[_ZN3fmt2v76detail18parse_format_specsIN5Eigen9TransposeINS3_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEENS1_21compile_parse_contextIcNS1_13error_handlerEEEEEPKNT0_9char_typeERSB_]+0x247):对‘fmt::v7::detail::error_handler::on_error(char const*)’未定义的引用

需要将之前安装的fmt库链接joinMap.cpp,rgbd文件夹中的cmakelists如下:

1
2
3
4
5
6
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
find_package(Pangolin REQUIRED)
find_package(FMT REQUIRED)
add_executable(joinMap joinMap.cpp)
target_link_libraries(joinMap fmt::fmt ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

在图像中,鼠标点击图像中的每个点都能在左下角得到UV坐标值和RGB三通道值。

函数解析如下:

  1. cv::imread:函数读取图像,并把图像和基本信息显示出来。
  2. OpenCV 提供了迭代器,可以通过迭代器遍历图像的像素。cv::Mat::data 提供了指向图像数据开头的指针,可以直接通过该指针自行计算偏移量,然后得到像素的实际内存位置。
  3. 复制图像中直接赋值是浅拷贝,并不会拷贝数据,而clone方法是深拷贝,会拷贝数据,这在图像存取中会经常用到。
  4. 在编程过程中碰到图像的旋转、插值等操作,自行查阅函数对应的文档,以了解它们的原理与使用方式。

注:1. cv::Mat 亦是矩阵类,除了表示图像之外,我们也可以用它来存储位姿等矩阵数据,但一般还是使用eigen,更快一些。

  1. cmake默认编译的是debug模式,如果使用release模式会快很多。

3.2 图像去畸变

OpenCV 提供了去畸变函数 cv::Undistort(),这个例程从公式出发计算了畸变前后的图像坐标(代码中有内参数据)。

slambook/ch5/imageBasics/undistortImage.cpp

可以看到去畸变前后图像差别还是蛮大的。

4 实践:3D 视觉

4.1 双目视觉(slam2)

在stereo文件夹中,有左右目的图像和对应代码。其中代码计算图像对应的视差图,然后再计算各像素在相机坐标系下的坐标,它们共同构成点云。

slambook/ch5/stereoVision/stereoVision.cpp

运行如下:(下面的第二个图片是视差图)

例程中调用了OpenCV实现的SGBM算法(Semi-global Batch Matching)[26] H. Hirschmuller, “Stereo processing by semiglobal matching and mutual information,” IEEE Transactions on pattern analysis and machine intelligence, vol. 30, no. 2, pp. 328–341, 2008.

计算左右图像的视差,然后通过双目相机的几何模型把它变换到相机的3D空间中。SGBM 使用了来自网络的经典参数配置,主要调整了最大和最小视差。视差数据结合相机的内参、基线,即能确定各点在三维空间中的位置。感兴趣可以阅读相关的参考文献[27, 28]。

[27] D. Scharstein and R. Szeliski, “A taxonomy and evaluation of dense two-frame stereo correspondence algorithms,” International journal of computer vision, vol. 47, no. 1-3, pp. 7–42, 2002.

[28] S. M. Seitz, B. Curless, J. Diebel, D. Scharstein, and R. Szeliski, “A comparison and evaluation of multi-view stereo reconstruction algorithms,” in null, pp. 519–528, IEEE, 2006.

4.2 RGB-D 视觉

RGB-D相机能通过物理方法获得像素深度信息。如果已知相机的内外参,可以计算任何一个像素在世界坐标系下的位置,从而建立一张点云地图。

位于 slambook/ch5/rgbd 文件夹中有5对图像。在 color/下有 1.png 到 5.png 共 5 张 RGB 图,而在 depth/下有 5 张对应的深度图。同时,pose.txt 文件给出了5张图像的相机外参位姿。位姿记录的形式为平移向量加旋转四元数: [x, y, z, qx, qy, qz, qw], 其中 qw 是四元数的实部。

这一段程序,完成了两件事:(1). 根据内参计算一对 RGB-D图像对应的点云;(2). 根据各张图的相机位姿(也就是外参),把点云加起来,组成地图。

slambook/ch5/rgbd/jointMap.cpp

运行程序如下:

习题

  1. 寻找一部相机,标定它的内参。可能会用到标定板, 或者棋盘格。

    参考答案# 视觉SLAM十四讲(第二版)第5讲习题解答

  2. 相机内参的物理意义。如果一部相机的分辨率变为原来的两倍而其他地方不变,它的内参如何变化?

  3. 搜索特殊相机(鱼眼或全景相机)的标定方法。它们与普通的针孔模型有何不同?

  4. 调研全局快门相机(global shutter)和卷帘快门相机(rolling shutter)的异同。它们在SLAM中有何优缺点?

  5. RGB-D 相机是如何标定的?以 Kinect 为例,需要标定哪些参数?(参照https://github.com/code-iai/iai_kinect2

  6. 除了示例程序演示的遍历图像的方式,还能举出哪些遍历图像的方法?

  7. 阅读 OpenCV 官方教程,学习它的基本用法。

习题有代码学习和工程应用的知识,后面实际开发中会很有帮助。

[TOC]

1、李群李代数基础

2、指数与对数映射

3、李代数与对数映射

4、Sophus的基本使用方法

1、简介

Sophus 库是Strasdat 维护的 。Sophus 库支持SO(3) 和 SE(3),此外还含有二维运动 SO(2),SE(2) 以及相似变换 Sim(3) 的内容。它是直接在 Eigen 基础上开发的,不需要安装额外的依赖库。可以直接从 GitHub 上获取 Sophus,在代码目录 slambook/3rdparty 下也提供了 Sophus 源代码。

1
2
Eigen::Matrix3d和Sophus::Matrix3d
Eigen::Vector3d和Sophus::Vector3d

此外,为了方便说明SE(4)和se(4),Sophus库还typedef了Vector4d、Matrix4d、Vector6d和Matrix6d等,即:

1
2
3
4
Sophus::Vector4d
Sophus::Matrix4d
Sophus::Vector6d
Sophus::Matrix6d

2、安装

1
2
3
4
5
6
7
git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff
mkdir build
cd build
cmake ..
make

使用

李代数so(3):Sophus::Vector3d //因为so(3)仅仅只是一个普通的3维向量

李代数se(3):Sophus::Vector6d //因为se(3)仅仅只是一个普通的6维向量

SO3构造函数

1
2
3
4
5
SO3 ();
SO3 (const SO3 & other);
explicit SO3 (const Matrix3d & _R);
explicit SO3 (const Quaterniond & unit_quaternion);
SO3 (double rot_x, double rot_y, double rot_z);

SE3的构造函数

1
2
3
4
5
SE3 ();
SE3 (const SO3 & so3,const Vector3d & translation);
SE3 (const Matrix3d & rotation_matrix,const Vector3d & translation);
SE3 (const Quaterniond & unit_quaternion,const Vector3d & translation_);
SE3 (const SE3 & other);

输出

尽管SO3对应于矩阵群,但是SO3在使用cout时是以so3形式输出的,输出的是一个3维向量

SE3在使用cout输出时输出的是一个6维向量,其中前3维为对应的so3的值,后3维为实际的平移向量t

se3在使用cout输出时输出的也是一个6维向量,但是其前3维为平移值ρ

(注意此时的ρ与SE3输出的t是不同的,t=Jρ,其中J是雅克比矩阵),后3维为其对应的so3.

SO3,so3,SE3,se3初始化和相互转化关系

1、转换关系图

image-20220708021937910

关于旋转矩阵,旋转向量和四元数的初始化和相互转换关系可以参考另一篇博文:http://blog.csdn.net/u011092188/article/details/77430988

参考资料

【SOPHUS库 学习笔记 1】 SOPHUS的安装与使用

# 一文详解四元数、欧拉角、旋转矩阵、轴角如何相互转换 wexin

依赖库

cmake

eigen

额外的:

slam1 pcl

slam2 pangolin库

Pangolin(百度云盘–计算机科学–SLAM)

依赖安装:

1
2
3
sudo apt-get install libglew-dev
sudo apt-get install cmake
sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev
1
2
3
4
mkdir build
cd build
cmake -DCPP11_NO_BOOST=1 ..
make -j 4

OpenCV

OpenCV: cv::StereoSGBM Class Reference

1
2
sudo apt-get install libopencv-dev
pkg-config --modversion opencv

ORB_SLAM2安装Pangolin ubuntu16.04 - CSDN