@devilogic 2020-05-07T12:11:27.000000Z 字数 2269 阅读 741

MyMath数学库(二) - 支撑工具

我的无人车之路 mymath

$\quad$ 支撑工具库提供了一些基本通用的数学运算函数，为其他的工具做底层支持。

2.1 支撑工具函数表

函数	说明
`sqr`	计算`x`的平方。
`inner_prod`	计算两个向量的内积。
`inner_prod`(三端点版本)	计算两个向量的内积，两个向量同一个起点。
`cross_prod`	计算两个向量的外积。
`cross_prod`(三端点版本)	计算两个向量的外积。两个向量同一个起点。
`wrap_angle`	将角度缩放到 $2\pi$ 内的同样的角度内,如果角度为负则求出它相同的正值。（弧度制）
`normalize_angle`	将角度标准化到 $[-\pi, \pi)$ 内。
`angle_diff`	计算两角度之间的差距，并正则化到 $[0, \pi)$ 之间。
`sigmoid`	sigmoid函数。
`square`(模板函数)	计算两个模板类型值的平方。
`clamp`(模板函数)	如果参数value小于参数bound1则返回参数bound1，如果大于参数bound2则返回参数bound2,否则返回参数value。
`rotate_axis`	将轴1中的点 $(x_0，y_0)$ 转换为轴2中的点 $(x_1，y_1)$ ，其中从轴1到轴2的逆时针旋转角度为 $\theta$ 。
`rfu_to_flu`	转换RFU到FLU。
`flu_to_rfu`	转换FLU到RFU。
`l2_norm`	计算 $n$ 维的向量的L2范数。
`random_int`	生成随机整数。
`random_double`	生成随机浮点数。
`gaussian`	生成高斯分布随机数。

2.2 向量点乘与叉乘的意义

$\quad$ 上述函数表中提供了两个在图像变换中常用的向量操作，一个是点乘，一个是叉乘。都提供了两个版本，一个是两个独立的向量。另外一个版本是两个向量拥有同一个起点，终点不同。

2.2.1 点乘

$\quad$ 向量的点乘,也叫向量的内积、数量积，对两个向量执行点乘运算，就是对这两个向量对应位一一相乘之后求和的操作，点乘的结果是一个标量。
$\quad$ 对于向量 $a$ 与向量 $b$ ：

$a = [a_1,a_2,\ldots,a_n] \quad b = [b_1,b_2,\ldots,b_n]$

$a$ 和

$b$ 的点积公式为：

$a \bullet b = a_1b_1 + a_2b_2 + \ldots + a_nb_n$
这里需要一维向量

$a$ 和向量

$b$ 的行列数相同。

$\quad$ 再探讨一下点乘的几何意义，点乘的几何意义是可以用来表征或计算两个向量之间的夹角，以及在 $b$ 向量在 $a$ 向量方向上的投影，有公式：

$a \bullet b = |a||b|\cos \theta$
那么这里就可以得到两个向量的夹角

$\theta$ ：

$\theta = arc\cos{\frac{a \bullet b}{|a||b|}}$
根据这个公式就可以计算向量

$a$ 和向量

$b$ 之间的夹角。从而就可以进一步判断这两个向量是否是同一方向，是否正交(也就是垂直)等方向关系，具体对应关系为：

$a \bullet b \gt 0$ 方向基本相同，夹角在 $0^{\circ}$ 到 $90^{\circ}$ 之间。
$a \bullet b = 0$ 正交，相互垂直。
$a \bullet b \lt 0$ 方向基本相反，夹角在 $90^{\circ}$ 到 $180^{\circ}$ 之间。

2.2.2 叉乘

$\quad$ 两个向量的叉乘，又叫向量积、外积、叉积，叉乘的运算结果是一个向量而不是一个标量。并且两个向量的叉积与这两个向量组成的坐标平面垂直。
$\quad$ 对于向量 $a$ 与向量 $b$ ：

$a = (x_1,y_1,z_1) \quad b = (x_2,y_2,z_2)$

$\quad$

$a$ 与

$b$ 的叉乘公式为：

$a \times b = \begin{vmatrix} i & j & k \\ x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \end{vmatrix} = (y_1z_2 - y_2z_1)i-(x_1z_2-x_2z_1)j+(x_1y_2-x_2y_1)k$
其中：

$i=(1,0,0) \quad j=(0,1,0) \quad k=(0,0,1)$
根据

$i,j,k$ 间的关系，有：

$a \times b=(y_1z_2 - y_2z_1, -(x_1z_2-x_2z_1), x_1y_2-x_2y_1)$

$\quad$ 在三维几何中，向量a和向量b的叉乘结果是一个向量，更为熟知的叫法是法向量，该向量垂直于 $a$ 和 $b$ 向量构成的平面。在3D图像学中，叉乘的概念非常有用，可以通过两个向量的叉乘，生成第三个垂直于 $a$ ， $b$ 的法向量，从而构建 $(X,Y,Z)$ 坐标系。
$\quad$ 在二维空间中，叉乘还有另外一个几何意义就是： $a \times b$ 等于由向量 $a$ 和向量 $b$ 构成的平行四边形的面积。

2.3 坐标系探讨

全球地理坐标系。
局部坐标系, 东-北-天坐标（ENU）。X : 东边, Y : 北边, Z : 天。
车身坐标系, 右-前-天坐标（RFU）。X : 面向车前方(横向), Y : 车辆前进方向, Z : 与地面垂直，指向车顶方向。一般用于感知模块。
车身坐标系, 前-左-天坐标（FLU）。X : 车辆前进方向, Y : 面向车前方(横向), Z : 与地面垂直，指向车顶方向。该坐标系常用于实时相对地图模块。将RFU坐标系向左旋转 $90$ 度即可得到FLU坐标系。
Frenet坐标系,详细参考此文。

$\quad$ 工具库中提供了RFU与FLU的相互转换。

inline std::pair<double, double> rfu_to_flu(const double x, const double y) {
  return std::make_pair(y, -x);
}
inline std::pair<double, double> flu_to_rfu(const double x, const double y) {
  return std::make_pair(-y, x);
}