图像频率域分析之傅里叶变换

[TOC]

傅里叶变换基础

傅里叶级数

法国数学家傅里叶发现,任何周期函数都可以用正弦函数和余弦函数构成的无穷级数来表示(选择正弦函数与余弦函数作为基函数是因为它们是正交的),即 任何周期信号都可以表示成一系列正弦信号的叠加

  • 三角形式
\[f(t) = \frac{a_0}{2} + \sum_{k=1}^{+\infty} \big[ a_k cos (n \omega t) + b_k sin (n \omega t) \big], \quad \frac{a_0}{2} = \frac{1}{T} \int_{-\frac{T}{2}}^{\frac{T}{2}} f(t) dt\]
  • 复指数形式
\[f(t) = \frac{1}{T} \sum_{n=-\infty}^{+\infty} [ \int_{-\frac{T}{2}}^{\frac{T}{2}} f(\tau)e^{-j\omega_n\tau} d\tau ] e^{j\omega_nt}\]

基波角频率 $\omega = \frac{2\pi}{T}$ , $T$ 为 $f(t)$ 的周期, $j$ 为虚数单位

傅里叶积分

复指数形式

\[f(t) = \frac{1}{2\pi} \int_{-\infty}^{+\infty} [ \int_{-\infty}^{+\infty} f(\tau)e^{-j\omega\tau} d\tau ] e^{j\omega t} d\omega\]

傅里叶变换

一维连续傅里叶变换

正变换

\[F(\omega) = \int_{-\infty}^{+\infty} f(t) e^{-j\omega t} dt\]

逆变换

\[f(t) = \frac{1}{2\pi} \int_{-\infty}^{+\infty} F(\omega) e^{j\omega t} d\omega\]

一维离散傅里叶变换

正变换

\[F(u) = \sum_{x=0}^{M-1} f(x) e^{-j2\pi \frac{ux}{M}}\]

\[F(0) = \sum_{x=0}^{M-1} f(x)\]

反变换

\[f(x) = \frac{1}{M} \sum_{u=0}^{M-1} F(u) e^{j2\pi \frac{ux}{M}}\]

对于反变换式前的系数 $\frac{1}{M}$ ,也可放在正变换中,只要保证正变换与反变换之前的系数乘积为 $\frac{1}{M}$ 即可。

二维离散傅里叶变换

正变换

二维离散傅里叶变换:

\[F[f(x,y)] = F(u,v) = \sum_{x=0}^{M-1} \sum_{y=0}^{N-1} f(x,y) e^{-j2\pi(\frac{ux}{M}+\frac{vy}{N})}\]

当 $(u,v)$ 等于 $(0,0)$ 时,直流分量 为:

\[F(0,0) = \sum_{x=0}^{M-1} \sum_{y=0}^{N-1} f(x,y)\]

幅度谱 为:

\[A(u,v) = |F(u,v)| = \sqrt{Real(u,v)^2 + Image(u,u)^2}\]

功率谱 为:

\[P(u,v) = |F(u,v)|^{2} = Real(u,v)^2 + Image(u,u)^2\]

相位谱 为:

\[\phi(u,v) = arctan \frac{Image(u,v)}{Real(u,v)}\]

通过 幅度谱 和 相位谱,我们也能合成 其傅里叶变换(频谱):

\[\begin{aligned} F(u,v) &= A(u,v)e^{j\phi(u,v)} \\ &= A( cos \phi + jsin \phi ) \quad \text{(省略(u,v),应用 欧拉公式)}\\ &= Acos\phi + jAsin\phi \end{aligned}\]

注意:

  • 上面式子中的 $j$ 为 虚数单位
  • $Real(u,v)$ 为 复数的 实部
  • $Image(u,v)$ 为 复数的 虚部

反变换

\[f(x,y) = F^{-1}(u,v) = \frac{1}{MN} \sum_{u=0}^{M-1} \sum_{v=0}^{N-1} F(u,v) e^{j2\pi(\frac{ux}{M}+\frac{vy}{N})}\]

卷积

\[\int_{-\infty}^{+\infty} f_1(\tau)f_2(t-\tau) d\tau = f_1(t) * f_2(t)\]

卷积定理

函数卷积的傅立叶变换是函数傅立叶变换的乘积

  • 时域卷积定理:时域内的卷积对应频域内的乘积
\[F[f_1(t) * f_2(t)] = F_1(\omega) \cdot F_2(\omega)\]
  • 频域卷积定理:频域内的卷积对应时域内的乘积
\[F[f_1(t) \cdot f_2(t)] = \frac{1}{2\pi} F_1(\omega) * F_2(\omega)\]

数字图像DFT

借用知乎大神Heinrich的一张图,来个感性认识:

image_dft_plot.jpg

空间域和频域

  • 空间域:在图像处理中,时域可以理解为 空间域 或者 图像空间,处理对象为图像像元;
  • 频域:以 空间频率 为自变量描述图像的特征,可以将一幅图像像元值在空间上的变化分解为具有不同振幅、空间频率和相位的简振函数的线性叠加,图像中各种空间频率成分的组成和分布称为 图像频谱

空间域与频域可互相转换,对图像施行 二维离散傅立叶变换 或 小波变换 ,可以将图像由空间域转换到频域;通过 对应的反变换 又可转换回空间域图像,即人可以直接识别的图像。

图像频域滤波

二维数字图像的滤波主要分为 空间域滤波 和 频域滤波:

  • 空间域滤波: 用各种模板直接与图像进行 卷积运算,实现对图像的处理,这种方法直接对图像空间操作,操作简单

  • 频域滤波: 在实现某些图像处理的时候,频域的处理比空间域更简单;对于在空间域上的数字图像,根据 卷积定理 可以通过 傅立叶变换空域卷积滤波 变换为 频域滤波,然后再将频域滤波处理后的图像 反变换 回空间域

基本步骤

图像频域滤波步骤为(频谱图中心化):

  • 计算 原始图像 $f(x,y)$ 的DFT,得到 频谱 $F(u,v)$
  • 中心化:将频谱 $F(u,v)$ 的零频点移动到频谱图的中心位置
  • 计算 滤波器函数 $H(u,v)$ 与 $F(u,v)$ 的乘积 $G(u,v) = F(u,v) \cdot H(u,v)$
  • 反中心化:将频谱 $G(u,v)$ 的零频点移回到频谱图的左上角位置
  • 计算上一步计算结果的 傅里叶反变换 $g(x,y)$
  • 取 $g(x,y)$ 的 实部 作为最终滤波后的结果图像

上面步骤是对 图像频谱 进行 中心变换;我们也可以先对 原始图像 进行 中心变换,再计算其 频谱图,滤波步骤如下(原始图中心化):

  • 原始图像 $f(x,y)$ 中心变换:$f(x,y) \cdot (-1)^{(x+y)}$
  • 计算上一步计算结果的DFT,得到其 频谱 $F(u,v)$
  • 计算 滤波器函数 $H(u,v)$ 与 $F(u,v)$ 的乘积 $G(u,v) = F(u,v) \cdot H(u,v)$
  • 计算 $G(u,v)$ 的 傅里叶反变换 $g(x,y)$
  • 取 $g(x,y)$ 的 实部
  • 上一步计算结果 乘以 $(-1)^{(x+y)}$ 作为最终滤波后的结果图像

滤波能否取得理想结果的关键取决于上面的 滤波器函数 $H(u,v)$

这时让我想到了《自动控制理论》中的 传递函数 $G(s)$,定义为:初始条件为零的线性定常系统输出的拉普拉斯变换与输入的拉普拉斯变换之比。

下面以 控制论的思想 给出图像频域滤波的示意框图:

image_fft_flow.jpg

图像频率特性分析

频谱图上的每一个像素点都代表一个频率值,幅值由像素点亮度变码而得。对于一幅图像,图像信号的 频率特性 如下:

  • 直流分量 表示预想的平均灰度
  • 低频分量 代表了大面积背景区域和缓慢变化部分
  • 高频分量 代表了它的边缘、细节、跳跃部分以及颗粒噪声
  • 振幅 描述了图像灰度的亮度
  • 相位 决定了图像是什么样子

数字图像的二维离散傅立叶变换所得的结果的频域成分如下图所示,左上角是直流成分,变换结果四个角周围对应于低频成分,中央部分对应于高频部分

image_fft2_figure.png

为了便于观察,常常采取 换位 方法使直流成分出现在窗口的中央(中心化),变换后中心为低频,向外是高频。

在频域,可以很方便的实现 图像的锐化和模糊

  • 截取频率的低频分量,对其作傅立叶反变换,得到的就是模糊后的图像,即 低通滤波
  • 截取频率的高频分量,对其作傅立叶反变换,得到的就是锐化后的图像,即 高通滤波

图像滤波实践

下面,我们以 lena.bmp(点此下载) 图像进行滤波实践。

Python分析

(1)加载图像,并转换为 灰度图
image_gray.png

(2)对其 快速傅里叶变换,并经过 中心变换,得到 频率谱相位谱
image_frequency.png image_frequency_phase.png

(3)分别截取 频谱图低频部分(中间部分)高频分量(四周部分)
image_frequency_lf.png image_frequency_hf.png

(4)对以上处理过的频谱图分别进行 反中心化傅里叶反变换取实部,得到 低通滤波高通滤波 后的图像
image_back_lf.png image_back_hf.png

C++分析

使用 CImgFFTW库 对 lena图像进行傅里叶变换(源代码见文末),结果如下
lena_fftw.jpg

源代码

以上所有代码均存储在我的Github仓库:

参考资料

Read More

图像空间域分析之图像统计特征

[TOC]

我们可以将一幅数字图像视为一个 二维函数$I(x,y)$ ,其中x和y是空间坐标,在x-y平面中的任意空间坐标 $(x,y)$ 上的 幅值 $I_{xy}$ 称为该点 图像的灰度、亮度或强度

\[I_{x,y} = I(x,y);\]

下面以 $I_{xy}$ 作为随机变量,分析二维数字图像的 统计特征。

数学期望

数学期望(Expectation) 就是随机变量 以概率为权数的加权平均值,以 级数 的形式表示为

\[E(X) = \sum_{k=1}^{\infty} x_k p_k\]

一幅数字图像的数学期望为其 灰度平均值,即所有像元灰度值的算数平均值

\[\bar{I} = \frac{1}{M \cdot N} \sum_{x=1}^{M} \sum_{y=1}^{N} I(x,y)\]

方差

方差(variance) 是对随机变量离散程度的度量。

\[D(x) = E[X-E(X)]^2 = E(X^2) - [E(X)]^2\]

称 $\sqrt{D(X)}$ 为随机变量的 标准差(standard deviation),或 均方差(mean square deviation),记为 $\sigma(X)$

二维数字图像的 灰度方差 反应的是图像中各个像素的灰度值与整个图像平均灰度值的离散程度,与图像的 对比度 有关。如果图片对比度小,那方差就小;如果图片对比度很大,那方差就大。

\[var(I) = \frac{1}{M \cdot N} \sum_{x=1}^{M} \sum_{y=1}^{N} \left[ I(x,y) - \bar{I} \right]\]

协方差(矩阵)与相关系数

随机变量X、Y的 协方差(covariance)

\[\begin{aligned} cov(X,Y) &= E \{ [X-E(X)][Y-E(Y)] \} = E[XY] - E(X)E(Y) \\ &= \sum_{i} \sum_{j} [x_i-E(X)] [y_j-E(Y)] p_{ij} \end{aligned}\]

协方差矩阵 可表示为

\[\Sigma = \begin{bmatrix} \sigma_{11} & \sigma_{12} & \ldots & \sigma_{1n} \\ \sigma_{21} & \sigma_{22} & \ldots & \sigma_{2n} \\ \vdots & \vdots & \ddots & \vdots \\ \sigma_{n1} & \sigma_{n2} & \ldots & \sigma_{nn} \\ \end{bmatrix}\]

其中,$\sigma_{ij} = cov(X_i, X_j)$

随机变量X、Y的 相关系数(correlation coefficient)标准协方差(standard covariance)

\[\rho_{XY} = \frac{ cov(X,Y) }{ \sqrt{D(X)} \sqrt{D(Y)} }\]

数字图像 $I_A$ 和 $I_B$ 的协方差为

\[cov(I_A, I_B) = \frac{1}{M \cdot N} \sum_{x=1}^{M} \sum_{y=1}^{N} [ I_A(x,y) - \bar{I_A} ] [ I_B(x,y) - \bar{I_B} ]\]

图像的相关系数表征的是两个不同波段图像的所含信息的重叠程度,相关系数越大,重叠度越高,反之越低。

\[\rho_{I_A I_B} = \frac {\sum_{x=1}^{M} \sum_{y=1}^{N} [ I^A_{xy} - \bar{I_A} ] [ I^B_{xy} - \bar{I_B} ]} { \bigg(\sum_{x=1}^{M} \sum_{y=1}^{N} [ I^A_{xy} - \bar{I_A} ]\bigg)^{1/2} \bigg(\sum_{x=1}^{M} \sum_{y=1}^{N} [ I^B_{xy} - \bar{I_A} ]\bigg)^{1/2} }\]

下面计算 lena图像 灰度图高斯模糊图 的相关系数
image_gray.pngimage_gauss.png

img = Image.open('lena.bmp').convert('L')
im = np.asarray(img)
im_blur = ndimage.gaussian_filter(im, 4)
a = np.corrcoef(im.flatten(), im_blur.flatten())
print a

计算得到其相关系数为 0.95346

  • $X$ 的 $k$阶原点矩,简称 $k$阶矩: $E[X^k]$

  • $X$ 的 $k$阶中心矩: $E[X-E(X)]^k$

  • $X$ 和 $Y$ 的 $k+l$阶混合矩: $E [ X^k Y^l ]$

  • $X$ 和 $Y$ 的 $k+l$阶混合中心矩: $E { [X-E(X)]^k [Y-E(Y)]^l }$

显然,$X$ 的数学期望 $E(X)$ 是 $X$ 的一阶原点矩,方差 $D(X)$ 是 $X$的二阶中心矩,协方差 $cov(X,Y)$ 是 $X$ 和 $Y$ 的 1+1阶混合中心矩

图像矩

图像的矩(Image Moments)主要表征了图像区域的几何特征,又称为 几何矩

Raw Moments

二维灰度图像 $I$ 的矩定义为

\[M_{pq} = \sum_{x} \sum_{y} x^p y^q I(x,y), \quad p,q \in \{ 0,1,2, \ldots \}\]

零阶矩

\[M_{00} = \sum_{x} \sum_{y} I(x,y)\]
  • 当图像为二值图时,$M_{00}$ 就是这个图像上白色区域的总和;因此,$M_{00}$ 可以用来求二值图像(轮廓、连通域)的面积

一阶矩

\[M_{10} = \sum_{x} \sum_{y} x \cdot I(x,y)\] \[M_{01} = \sum_{x} \sum_{y} y \cdot I(x,y)\]
  • 当图像为二值图时,$I$ 只有 0 和 1 两个值,$M_{10}$ 就是图像上所有白色区域 x坐标值 的累加
  • 一阶矩可以用来求图像的 质心(Centroid),这种方法对噪声不太敏感:
\[(x_c, y_c) = \bigg( \frac{M_{10}}{M_{00}}, \frac{M_{01}}{M_{00}} \bigg)\]
  • 一阶矩还可以用来求 图像块几何中心的方向,即几何中心 $O$ 与 质心 $C$ 连接的 方向向量$\vec{OC}$ 的方向(计算中 $x,y$ 均以几何中心 $O$ 为原点):
\[\theta = arctan( \frac{M_{01}}{M_{10}} )\]

通过 取以关键点kp为几何中心的图像块 计算其 一阶矩 进而计算 该点方向,示例代码如下:

int m01 = 0;
int m10 = 0;
for(int y=-half_patch_size; y<half_patch_size; ++y){
  for(int x=-half_patch_size; x<half_patch_size; ++x){
    m01 += y * image.at<uchar>(kp.pt.y+y, kp.pt.x+x);
    m10 += x * image.at<uchar>(kp.pt.y+y, kp.pt.x+x);
  }
}
kp.angle = std::atan2(m01, m10)/CV_PI*180.0;

二阶矩

\[M_{20} = \sum_{x} \sum_{y} x^2 \cdot I(x,y)\] \[M_{02} = \sum_{x} \sum_{y} y^2 \cdot I(x,y)\] \[M_{11} = \sum_{x} \sum_{y} x \cdot y \cdot I(x,y)\]

Central Moments

\[\mu_{pq} = \sum_{x} \sum_{y} (x-x_c)^p (y-y_c)^q I(x,y), \quad p,q \in \{ 0,1,2, \ldots \}\]

利用二阶中心矩可以求图像的方向,图像的 协方差矩阵

\[cov[I(x,y)] = \begin{bmatrix} \mu_{20}' & \mu_{11}' \\ \mu_{11}' & \mu_{02}' \end{bmatrix}\]

求得图像方向为

\[\theta = \frac{1}{2} arctan(\frac{2\mu_{11}'}{\mu_{20}'-\mu_{02}'})\]

其中

\[\mu_{20}' = \frac{\mu_{20}}{\mu_{00}} = \frac{M_{20}}{M_{00}} - x_c^2\] \[\mu_{02}' = \frac{\mu_{02}}{\mu_{00}} = \frac{M_{02}}{M_{00}} - y_c^2\] \[\mu_{11}' = \frac{\mu_{11}}{\mu_{00}} = \frac{M_{11}}{M_{00}} - x_c y_c\]

参考资料

Read More

机器人学之3D欧式变换理论与实践

[TOC]

理论基础

三维空间中的变换主要分为如下几种:

  • 射影变换
  • 仿射变换
  • 相似变换
  • 欧式变换

其性质如下图所示:
3d_transform.png

本文主要介绍欧式变换。

欧式变换

\[\mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4}\] \[\mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R}^T & -\mathbf{R}^T \cdot \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4}\]

Translate by $-C$ (align origins), Rotate to align axes:

\[\begin{aligned} P_c &= \mathbf{T} \cdot P_w \\ &= \mathbf{R} \cdot (P_w - C) \\ &= \mathbf{R} \cdot P_w - \mathbf{R} \cdot C \\ &= \mathbf{R} \cdot P_w + \mathbf{t} \end{aligned}\]

旋转

旋转矩阵

\[\mathbf{R} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \in \mathbb{R}^{3 \times 3}, \quad s.t. \quad \mathbf{RR}^T = \mathbf{I}, det(\mathbf{R}) = 1\]

旋转向量(轴角)

\[\boldsymbol{\phi} = \alpha\mathbf{a} = log(\mathbf{R})^{\vee} \in \mathbb{R}^3\]
  • 旋转轴:矩阵 $\mathbf{R}$ 特征值1对应的特征向量(单位矢量) \(\mathbf{a} = \frac{\boldsymbol{\phi}}{||\boldsymbol{\phi}||} \in \mathbb{R}^3\)

  • 旋转角 \(\alpha = ||\boldsymbol{\phi}|| = arccos(\frac{tr(\mathbf{R})-1}{2}) \in \mathbb{R}\)

罗德里格斯公式(Rodrigues’ rotation formula): \(\mathbf{R} = cos\alpha \mathbf{I} + (1-cos\alpha) \mathbf{aa}^T + sin\alpha \mathbf{a}^{\wedge}\)

单位四元数

2D旋转单位复数 可用来表示2D旋转。

\[z = a + b\vec{i} = r ( cos\theta + sin\theta\vec{i} ) = e^{\theta \vec{i}}, r = ||z||=1\]

3D旋转单位四元数 才可表示3D旋转,四元数是复数的扩充,在表示旋转前需要进行 归一化

\[\mathbf{q} = \begin{bmatrix} \boldsymbol\varepsilon \\ \eta \end{bmatrix} \quad s.t. \quad ||\mathbf{q}||_2 = 1\]

where

\[\eta = cos\frac{\alpha}{2}, \quad \boldsymbol\varepsilon = \mathbf{a} sin\frac{\alpha}{2} = \begin{bmatrix} a_1sin \frac{\alpha}{2} \\ a_2sin \frac{\alpha}{2} \\ a_3sin \frac{\alpha}{2} \end{bmatrix} = \begin{bmatrix} \varepsilon_1 \\ \varepsilon_2 \\ \varepsilon_3 \end{bmatrix}\]

and

\[||\mathbf{a}||_2 = 1, \quad \eta^2 + \varepsilon_1^2 + \varepsilon_2^2 + \varepsilon_3^2 = 1\]

\[\mathbf{q} = \begin{bmatrix} \mathbf{a} sin\frac{\alpha}{2} \\ cos\frac{\alpha}{2}\end{bmatrix}\]

当 $\alpha$ 很小时,可以近似表达为

\[\mathbf{q} \approx \begin{bmatrix} \mathbf{a} \frac{\alpha}{2} \\ 1 \end{bmatrix} = \begin{bmatrix} \frac{\boldsymbol{\phi}}{2} \\ 1 \end{bmatrix}\]

四元数可以在 保证效率 的同时,减小矩阵1/4的内存占有量,同时又能 避免欧拉角的万向锁问题

欧拉角

旋转矩阵可以可以分解为绕各自轴对应旋转矩阵的乘积:

\[\mathbf{R} = \mathbf{R}_1 \mathbf{R}_2 \mathbf{R}_3\]

根据绕轴的不同,欧拉角共分为两大类,共12种,如下图(基于 右手系)所示:
euler_angles_12.png

以上不同旋转轴合成的旋转矩阵,每一种都可以看成 同一旋转矩阵的两种不同物理变换

  • 固定轴 旋转
  • 动轴 旋转

$Z_1Y_2X_3$ 进行为例,旋转矩阵表示为 $\mathbf{R} = \mathbf{R}_z \mathbf{R}_y \mathbf{R}_x$,说明:

  • 固定轴 旋转:以初始坐标系作为固定坐标系,分别先后绕固定坐标系的X、Y、Z轴 旋转;
  • 动轴 旋转:先绕 初始Z轴 旋转,再绕 变换后的Y轴 旋转,最后绕 变换后的X轴 旋转

即 绕 固定坐标轴的XYZ绕运动坐标轴的ZYX 的旋转矩阵是一样的。

我们经常用的欧拉角一般就是 $Z_1Y_2X_3$ 轴序的 yaw-pitch-roll,如下图所示:
rpy_plane.png

对应的旋转矩阵为

\[\mathbf{R} = \mathbf{R}_z \mathbf{R}_y \mathbf{R}_x = \mathbf{R}(\theta_{yaw}) \mathbf{R}(\theta_{pitch}) \mathbf{R}(\theta_{roll})\]

其逆矩阵为:

\[\begin{aligned} \mathbf{R}^{-1} &= (\mathbf{R}_z \mathbf{R}_y \mathbf{R}_x)^{-1} \\ &= \mathbf{R}_x^{-1} \mathbf{R}_y^{-1} \mathbf{R}_z^{-1} \\ &= \mathbf{R}(-\theta_{roll}) \mathbf{R}(-\theta_{pitch}) \mathbf{R}(-\theta_{yaw}) \end{aligned}\]

上面 $\mathbf{R}_x \mathbf{R}_y \mathbf{R}_z$ 以 Cosine Matrix 的形式表示为(右手系):

\[\mathbf{R}_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta) \end{bmatrix}\] \[\mathbf{R}_y(\theta) = \begin{bmatrix} cos(\theta) & 0 & sin(\theta) \\ 0 & 1 & 0 \\ -sin(\theta) & 0 & cos(\theta) \end{bmatrix}\] \[\mathbf{R}_z(\theta) = \begin{bmatrix} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{bmatrix}\]

旋转转换

平移

\[\mathbf{t} = \begin{bmatrix} x & y & z \end{bmatrix}^T \in \mathbb{R}^3\]

李群和李代数

特殊正交群 $SO(3)$

\[SO(3) = \Bigg\{ \mathbf{R} \in \mathbb{R}^{3 \times 3} \Bigg| \mathbf{RR}^T = \mathbf{I}, det(\mathbf{R}) = 1 \Bigg\}\]

李代数 $\mathfrak{so}(3)$

\[\mathfrak{so}(3) = \Bigg\{ \boldsymbol{\Phi} = \boldsymbol{\phi}^{\wedge} \in \mathbb{R}^{3 \times 3} \Bigg| \boldsymbol{\phi} \in \mathbb{R}^3 \Bigg\}\]

where

\[\boldsymbol{\phi}^{\wedge} = \begin{bmatrix} \phi_1 \\ \phi_2 \\ \phi_3 \end{bmatrix}^{\wedge} = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix} \in \mathbb{R}^{3 \times 3}\]

指数映射:$\mathbf{R} = exp(\boldsymbol{\phi}^{\wedge}) {\approx} \mathbf{I} + \boldsymbol{\phi}^{\wedge}$ (first-order approximation)

对数映射:$\boldsymbol{\phi} = log(\mathbf{R})^{\vee}$

特殊欧式群 $SE(3)$

\[SE(3) = \Bigg\{ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \Bigg| \mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^{3} \Bigg\}\]

李代数 $\mathfrak{se}(3)$

\[\mathfrak{se}(3) = \Bigg\{ \boldsymbol{\Xi} = \boldsymbol{\xi}^{\wedge} \in \mathbb{R}^{4 \times 4} \Bigg| \boldsymbol{\xi} \in \mathbb{R}^6 \Bigg\}\]

where

\[\boldsymbol{\xi}^{\wedge} = \begin{bmatrix} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{bmatrix}^{\wedge} = \begin{bmatrix} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T, & 0 \end{bmatrix} \in \mathbb{R}^{4 \times 4}, \quad \boldsymbol{\rho},\boldsymbol{\phi} \in \mathbb{R}^3\]

指数映射:$\mathbf{T} = exp(\boldsymbol{\xi}^{\wedge})$
对数映射:$\boldsymbol{\xi} = log(\mathbf{T})^{\vee}$

坐标系手性

坐标系的手性主要分为 右手系左手系,主要通过以下两种方法区分(右手系):

  • 3 finger method
    right_handed_3fingers.png
  • Curling method
    right_handed_curling.png

另外,不同的几何编程库所基于的坐标系的手性会有所不同

  • Eigen: 右手系
  • OpenGL: 右手系
  • Unity3D: 左手系
  • ROS tf: 右手系

注意事项

区分 点的变换 和 坐标系本身的变换

\[P_a = \mathbf{T}_{AB} \cdot P_b\]

指的是 将某点在B坐标系中的坐标表示变换为其在A坐标系中的坐标表示,实质是同一点在不同坐标系下的不同坐标表示,即 点的变换;若将A和B坐标系假设为刚体,则B坐标系变换到A坐标系(坐标系本身的变换)的变换矩阵为 $\mathbf{T}_{AB}^{-1}$。

  • 使用传感器(Camera-IMU)标定工具(例如Kalibr)标定出的外参指的是 点的变换
  • ROS中 static_transform_publisher 则是 坐标系本身的变换
    static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
    

在分析多个坐标系的姿态变换时,要注意根据点的变换或者坐标系的变换确定矩阵左乘还是右乘:

  • 点的变换:矩阵相乘 从右到左,即 矩阵左乘
  • 坐标系的变换:矩阵相乘 从左到右,即 矩阵右乘

区分 绕定轴旋转 和 绕动轴旋转

注意 右手系 和 左手系

注意 同一刚体中不同坐标系姿态变换的相互表示

以带有IMU的相机模组为例,已知 IMU(坐标系)本身的姿态变换 $\mathbf{T}^{B}$ 和 同一模组中Camera到IMU(Body)的坐标系变换 $\mathbf{T}_{BC}$,则 该Camera(坐标系)本身的姿态变换为:

\[{}_C\mathbf{T} = \mathbf{T}_{BC} \cdot \mathbf{T}^{B} \cdot \mathbf{T}_{BC}^{-1}\]

因为上面的变换都是 坐标系的变换,所以矩阵相乘 从左到右,即 矩阵右乘

pointcloud_imu.jpg

编程库实践

下面通过示例代码对自己使用过的库进行介绍。

Eigen

Eigen is a C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms.

Eigen::Matrix3d m3_r_z = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
Eigen::Quaterniond q_r_z(m3_r_z);

Eigen::Vector3f v3_translation(x, y, z);

Eigen::Quaternion<double> q(w, qx, qy, qz);
Eigen::Matrix3f m3_rotation = q.matrix();

Eigen::Matrix4f m4_transform = Eigen::Matrix4f::Identity();
m4_transform.block<3,1>(0,3) = v3_translation;
m4_transform.block<3,3>(0,0) = m3_rotation;

TooN

Tom’s Object-oriented numerics library, is a set of C++ header files which provide basic linear algebra facilities

Array2SE3:

#include <TooN/TooN.h>
#include <TooN/se3.h>

/**
 * @brief transform array to TooN::SE3
 * @param array array of 3x4 row-major matrix of RT
 * @param se3 TooN::SE3 object
 */
void Tools::Array2SE3(const float *array, SE3<> &se3)
{
    Matrix<3,3> m3Rotation;
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            m3Rotation[i][j] = array[i*4+j];
        }
    }
    SO3<> so3 = SO3<>(m3Rotation);

    Vector<3> v3Translation;
    v3Translation[0] = array[ 3];
    v3Translation[1] = array[ 7];
    v3Translation[2] = array[11];

    se3.get_rotation()    = so3;
    se3.get_translation() = v3Translation;
}

Sophus

C++ implementation of Lie Groups using Eigen commonly used for 2d and 3d geometric problems (i.e. for Computer Vision or Robotics applications)

#include <iostream>
#include <sophus/se3.hpp>

Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
Eigen::Quaterniond q(R);

Eigen::Vector3d t(1,0,0);

Sophus::SE3 SE3_Rt(R, t);
Sophus::SE3 SE3_qt(q, t);

typedef Eigen::Matrix<double,6,1> Vector6d;

Vector6d se3 = SE3_Rt.log();

std::cout << "se3 hat = " << std::endl
          << Sophus::SE3::hat(se3) << std::endl;
std::cout <<"se3 hat vee = " << std::endl
          << Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose() << std::endl;

Vector6d update_se3;
update_se3.setZero();
update_se3(0,0) = 1e-4d;
Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3) * SE3_Rt;
std::cout << "SE3 updated = " << std::endl
          << SE3_updated.matrix() << std::endl;

ROS tf & tf2

tf is a package that lets the user keep track of multiple coordinate frames over time. tf maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.

#include <Eigen/Geometry>
#include <tf_conversions/tf_eigen.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <geometry_msgs/TransformStamped.h>

tf::Transform transform;
transform.setOrigin( tf::Vector3(x, y, z) );
tf::Quaternion q;
q.setRPY(r, p, y);
transform.setRotation(q);

geometry_msgs::Quaternion q_msg;

Eigen::Vector3d v3_r;
tf2::Matrix3x3(tf2::Quaternion(q_msg.x, q_msg.y, q_msg.z, quaternion_imu_.w))
.getRPY(v3_r[0], v3_r[1], v3_r[2]);

tf2::Quaternion q_tf2;
q_tf2.setRPY(v3_r[0], v3_r[1], v3_r[2]);
q_tf2.normalize();

geometry_msgs::TransformStamped tf_stamped;
tf_stamped.transform.rotation.x = q.x();
tf_stamped.transform.rotation.y = q.y();
tf_stamped.transform.rotation.z = q.z();
tf_stamped.transform.rotation.w = q.w();
Read More

YUV(NV21)图像数据到RGB颜色空间的转换

[TOC]

本文主要介绍YUV_NV21颜色空间到RGB(BGR in OpenCV)颜色空间的转换,并给出示例代码,另附YUV图像查看工具。

NV21(YUV420)介绍

NV12和NV21属于YUV420格式(每2x2四个Y,共用一组uv),是一种two-plane模式,即Y和UV分为两个Plane,但是UV(CbCr)为交错存储,而不是分为三个plane。先存储所有的Y,然后UV交错存储:NV12先U后V,NV21先V后U。

YUV420sp示例格式如下:

在这里插入图片描述

YUV_NV21转BGR代码

YUV_NV21颜色空间到RGB(BGR in OpenCV)颜色空间的转换示例代码如下:

const int width  = 1280;
const int height = 800;

std::ifstream file_in;
file_in.open("../image_yuv_nv21_1280_800_01.raw", std::ios::binary);
std::filebuf *p_filebuf = file_in.rdbuf();
size_t size = p_filebuf->pubseekoff(0, std::ios::end, std::ios::in);
p_filebuf->pubseekpos(0, std::ios::in);

char *buf_src = new char[size];
p_filebuf->sgetn(buf_src, size);

cv::Mat mat_src = cv::Mat(height*1.5, width, CV_8UC1, buf_src);
cv::Mat mat_dst = cv::Mat(height, width, CV_8UC3);

cv::cvtColor(mat_src, mat_dst, cv::COLOR_YUV2BGR_NV21);

转换出的(正确)效果如下:

在这里插入图片描述

接下来,我自己按如下逻辑实现该算法,替换掉 OpenCV的 cv::cvtColor(mat_src, mat_dst, cv::COLOR_YUV2BGR_NV21)

  • 从内存中读取出每个像素的YUV,即 YUV420 –> YUV444
  • 根据 YUV –> RGB 公式,计算出RGB值
  • 按 BGR888 的内存分布格式,将RGB值写入内存传给 cv::Mat
  • 保存图片到本地显示

实现代码如下:

void yuv_nv21_to_rgb(unsigned char rgb[], char yuv[], int width, int height) {

    int total = width * height;
    char Y, U, V;
    unsigned char R, G, B;
    int index = 0;

    for (int h = 0; h < height; h++) {
        for (int w = 0; w < width; w++) {

            Y = yuv[h * width + w];
            if ((w & 1) == 0)
                V = yuv[total + (h >> 1) * width + w];
            if ((w & 1) == 1)
                U = yuv[total + (h >> 1) * width + w - 1];

            // OpenCV YCrCb --> RGB
            //B = Y + 1.773*(U-128);
            //G = Y - 0.714*(V-128) - 0.344*(U-128);
            //R = Y + 1.403*(V-128);

            // YUV-->RGB for HDTV(BT.601)
            //B = Y + 2.03211*(U-128);
            //G = Y - 0.39465*(U-128) - 0.5806*(V-128);
            //R = Y + 1.13983*(V-128);

            // YUV-->RGB for HDTV(BT.709)
            //B = Y + 2.12798*(U-128);
            //G = Y - 0.21482*(U-128) - 0.38059*(V-128);
            //R = Y + 1.28033*(V-128);

            // YCbCr-->RGB
            B = 1.164*(Y-16) + 2.018*(U-128);
            G = 1.164*(Y-16) - 0.813*(U-128) - 0.391*(V-128);
            R = 1.164*(Y-16) + 1.596*(V-128);

            if (R < 0) R = 0; else if (R > 255) R = 255;
            if (G < 0) G = 0; else if (G > 255) G = 255;
            if (B < 0) B = 0; else if (B > 255) B = 255;

            rgb[index++] = B;
            rgb[index++] = G;
            rgb[index++] = R;
        }
    }
}

该代码中试了几种 YUV –> RGB 的算法,效果最好的即上面使用(未注释)的代码,转换结果如下:

在这里插入图片描述

欢迎各位同学指正,找出效果不好的原因,并解决问题,谢谢~

  • 在StackOverflow回答的问题:Converting YUV into BGR or RGB in OpenCV
  • 整个工程可参见我的Github工程:https://github.com/chenguang055/cgocv_app/tree/master/image_convertor ,可在此提交 Issue
  • 本代码 测试yuv raw图像 可在此下载:https://download.csdn.net/download/u011178262/10791506

YUV图像 查看工具

Read More

Kalibr 之 Camera-IMU 标定 (总结)

[TOC]

Overview

ethz-asl/kalibr is a toolbox that solves the following calibration problems:

  • Multiple camera calibration: intrinsic and extrinsic calibration of a camera-systems with non-globally shared overlapping fields of view
  • Camera-IMU calibration: spatial and temporal calibration of an IMU w.r.t a camera-system
  • Rolling Shutter Camera calibration: full intrinsic calibration (projection, distortion and shutter parameters) of rolling shutter cameras

本文以 单目+IMU双目+IMU 为例,讲解使用 Kalibr工具 标定 Camera-IMU,其中使用的摄像头分别为 Realsense ZR300MYNT-EYE S系列摄像头

注意:本文用于学习kalibr标定过程,文中结果仅供参考。

1. 标定 Camera

采集 images

注意: 采集图像时,帧率控制在4帧左右

  • 单目

    rosbag record /camera/fisheye/image_raw -O images.bag
    
  • 双目

    rosbag record /stereo/left/image_raw /stereo/right/image_raw -O images.bag
    

标定 Camera

  • 单目

    kalibr_calibrate_cameras \
        --target april_6x6_24x24mm.yaml \
        --bag images.bag --bag-from-to 5 20 \
        --models pinhole-fov \
        --topics /camera/fisheye/image_raw
    
  • 双目

    kalibr_calibrate_cameras \
        --target april_6x6_24x24mm.yaml \
        --bag images.bag --bag-from-to 5 30 \
        --models pinhole-radtan pinhole-radtan \
        --topics /stereo/left/image_raw /stereo/right/image_raw
    

标定评估

重投影误差在 0.1~0.2 以内,标定结果较好,如下所示。

Other Camera Calib Tools

  • ROS camera_calibration
  • PTAM Calibration ( ATAN / FOV camera model )
  • OCamCalib toolbox

输出 cam_chain.yaml

  • 单目

    sample file output:

    cam_overlaps: []
      camera_model: pinhole
      distortion_coeffs: [0.9183540411447179]
      distortion_model: fov
      intrinsics: [252.40344712951838, 253.29272771389083, 310.9288373770512, 227.37425906476517]
      resolution: [640, 480]
      rostopic: /camera/fisheye/image_raw
    
  • 双目

    sample file output:

    cam0:
      cam_overlaps: [1]
      camera_model: pinhole
      distortion_coeffs: [0.962084349711143]
      distortion_model: fov
      intrinsics: [334.23991339518517, 333.6035571693483, 368.20264278064553, 252.393048692916]
      resolution: [752, 480]
      rostopic: /stereo/left/image_raw
    cam1:
      T_cn_cnm1:
      - [0.9999904159643447, 0.0026734233431591698, -0.003467100673890538, -0.1172292375035688]
      - [-0.002666210133778015, 0.999994275307285, 0.002083428947247444, 0.0001658846059485747]
      - [0.003472650713385957, -0.002074164960638575, 0.9999918192349059, -0.0002328222935304919]
      - [0.0, 0.0, 0.0, 1.0]
      cam_overlaps: [0]
      camera_model: pinhole
      distortion_coeffs: [0.9617138563016285]
      distortion_model: fov
      intrinsics: [330.66005261900216, 330.07191301082963, 371.03802575515203, 231.03601204806853]
      resolution: [752, 480]
      rostopic: /stereo/right/image_raw
    

2. 标定 IMU

  • imu_utils: A ROS package tool to analyze the IMU performance, C++ version of Allan Variance Tool.

采集 IMU 数据

  • collect the data while the IMU is Stationary, with a two hours duration
rosbag record /camera/imu/data_raw -O imu.bag

标定 IMU

rosbag play -r 200 imu.bag
roslaunch imu_utils ZR300.launch

ZR300.launch 文件内容

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/camera/imu/data_raw"/>
        <param name="imu_name" type="string" value= "ZR300"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "80"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

输出 ZR300_imu_param.yaml,sample file output:

%YAML:1.0
---
type: IMU
name: ZR300
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 2.7878706973951564e-03
      gyr_w: 1.6503780396374297e-05
   x-axis:
      gyr_n: 3.2763884944799469e-03
      gyr_w: 1.8012497709865783e-05
   y-axis:
      gyr_n: 2.7204386280639753e-03
      gyr_w: 1.6637042617714669e-05
   z-axis:
      gyr_n: 2.3667849696415461e-03
      gyr_w: 1.4861800861542444e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.5172832889483965e-02
      acc_w: 4.4150867224248972e-04
   x-axis:
      acc_n: 2.4450765767551903e-02
      acc_w: 4.0728821351916671e-04
   y-axis:
      acc_n: 2.1474226370935746e-02
      acc_w: 2.1468705215157706e-04
   z-axis:
      acc_n: 2.9593506529964245e-02
      acc_w: 7.0255075105672530e-04

输出 imu.yaml

根据标定结果修改 imu.yaml,其文件内容为

#Accelerometers
accelerometer_noise_density: 2.52e-02   #Noise density (continuous-time)
accelerometer_random_walk:   4.41e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     2.78e-03   #Noise density (continuous-time)
gyroscope_random_walk:       1.65e-05   #Bias random walk

rostopic:                    /camera/imu/data_raw   #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

3. 标定 Camera-IMU

采集 images & imu 数据

  • 单目 + IMU

    rosbag record /camera/imu/data_raw /camera/fisheye/image_raw -O images_imu.bag
    
  • 双目 + IMU

    rosbag record /camera/imu/data_raw /stereo/left/image_raw /stereo/right/image_raw -O images_imu.bag
    

标定

kalibr_calibrate_imu_camera \
    --target april_6x6_24x24mm.yaml \
    --bag images_imu.bag \
    --bag-from-to 5 45 \
    --cam camchain.yaml \
    --imu imu.yaml \
    --imu-models scale-misalignment \
    --timeoffset-padding 0.1
  • –bag-from-to 5 45: because there are shocks in the dataset (sensor pick-up/lay-down), only the data between 5 to 45 s is used

输出 camchain-imucam.yaml

  • 单目 + IMU

    sample file output:

    cam0:
      T_cam_imu:
      - [0.9996455719455962, 0.02441693761016358, -0.010608659071806014, -0.15423539234968817]
      - [-0.024769907516072436, 0.9990969029165591, -0.03452289478279192, -0.0032297199459559245]
      - [0.00975613505470538, 0.03477343440443987, 0.9993476002315277, 0.150153755143352]
      - [0.0, 0.0, 0.0, 1.0]
      cam_overlaps: []
      camera_model: pinhole
      distortion_coeffs: [0.9183540411447179]
      distortion_model: fov
      intrinsics: [252.40344712951838, 253.29272771389083, 310.9288373770512, 227.37425906476517]
      resolution: [640, 480]
      rostopic: /camera/fisheye/image_raw
      timeshift_cam_imu: 0.7904787918609288
    
  • 双目 + IMU

    sample file output:

    cam0:
      T_cam_imu:
      - [0.0008247496568674628, 0.9999961104998093, -0.002664352314491823, 0.043041669055924436]
      - [-0.9999929524133787, 0.0008149826348758382, -0.003664822898610003, 0.003376471075594937]
      - [-0.0036626372434111396, 0.0026673560986662063, 0.9999897350972485, -0.021104195227740437]
      - [0.0, 0.0, 0.0, 1.0]
      cam_overlaps: [1]
      camera_model: pinhole
      distortion_coeffs: [0.962084349711143]
      distortion_model: fov
      intrinsics: [334.23991339518517, 333.6035571693483, 368.20264278064553, 252.393048692916]
      resolution: [752, 480]
      rostopic: /stereo/left/image_raw
      timeshift_cam_imu: 0.00019201226395901445
    cam1:
      T_cam_imu:
      - [-0.001835964017484093, 0.999979457302906, -0.00614118948676923, -0.07410578385444819]
      - [-0.9999970575613598, -0.001845664547293735, -0.001574290634432294, 0.003383609126826685]
      - [-0.001585592869970595, 0.0061382810757381065, 0.9999799034984085, -0.021194379548050524]
      - [0.0, 0.0, 0.0, 1.0]
      T_cn_cnm1:
      - [0.9999904159643451, 0.00267342334315917, -0.003467100673890538, -0.1172292375035688]
      - [-0.0026662101337780156, 0.9999942753072855, 0.0020834289472474446, 0.0001658846059485747]
      - [0.003472650713385957, -0.0020741649606385755, 0.9999918192349063, -0.0002328222935304919]
      - [0.0, 0.0, 0.0, 1.0]
      cam_overlaps: [0]
      camera_model: pinhole
      distortion_coeffs: [0.9617138563016285]
      distortion_model: fov
      intrinsics: [330.66005261900216, 330.07191301082963, 371.03802575515203, 231.03601204806853]
      resolution: [752, 480]
      rostopic: /stereo/right/image_raw
      timeshift_cam_imu: 0.0001648708557824339
    
Read More

^