CUDA Learning

GpuMat

一、简介
GpuMat可以从其命名看出，它是“GPU”版本的Mat，绝大部分接口和Mat相同，功能也类似。

和Mat相比，GpuMat多了两个成员函数upload和download，分别用于把数据从内存上传(通过总线传输)到显存和从显存下载(通过总线传输)到内存。

GpuMat和Mat都有数据指针(指向一块存储区域，其中存放着图像数据)，不过GpuMat的数据指针是指向显存上的某一块区域，而Mat的数据指针是指向内存上的某一块区域。

GpuMat仅支持二维数据；GpuMat::isContinuous() == false，这是为了对齐内存，当一行的数据不是8字节(具体是几字节不知道)的倍数时，为了提高访问速度，这一行数据的末尾会填充空白字节，所以一行的实际内存大小保存在GpuMat的成员step里面。

二、访问GpuMat的每个元素
要访问GpuMat的每一个元素，实现自定义的算法，就得自己重新实现一个Kernel，然后把GpuMat作为参数传进去。但实际上，为了提高程序性能，一般不直接使用GpuMat作为参数，而是使用它的精简版PtrStepSz或者PtrStep代替。

Block数量只能多不能少，否则有的像素访问不到。
观察全局Thread索引是怎么算的。
在Kernel里面一定要判断是否越界。当然，rows和cols分别是threadsPerBlock.x和threadsPerBlock.y的整倍数时，不需要判断。
访问src的一个元素的方法是src(行坐标, 列坐标)。

Kernel

Kernel是在GPU上执行的函数，访问的数据都应该在显存中；函数没有返回值，需用void作为返回类型；语法和C++相同，也能使用C++的一些标准库函数(因为这些库函数有GPU实现，不过函数名字和参数相同而已)。kernel是函数的名字，可以随便改。

线程组织模型

GPU有很多个流处理器，每个流处理器相互独立，可以执行不同的代码；每个流处理器里面还有很多小核心，这些核心在同一时刻执行相同的代码，不过可以通过索引去访问不同的数据。在CUDA的线程模型里面，这些小核心对应的概念叫做Thread，每个Thread都可以计算出一个全局唯一从0开始的索引(索引可以是一维的，可以是二维的，甚至可以时是三维的)。
Grid由许多个Block组成，一个Block由许多Thread组成。

一维：Block的索引为(b_x)，Thread的索引是(t_x)
二维：Block的索引为(b_x, b_y)，Thread的索引是(t_x, t_y)
三维：Block的索引为(b_x, b_y, b_z)，Thread的索引是(t_x, t_y, t_z)

最小二乘法

非线性：最速下降法、牛顿法、高斯牛顿法（GN）、列文伯格-马夸尔特（LM）

线性：QR分解、乔姆斯基分解法求解（Cholesky）、奇异值分解（SVD）

一.最小二乘问题

​ 通常可以表述为,通过搜集到的一些数据(获取得到的样本),对某一个模型进行拟合,并尽可能的使得模型结果和样本达到某种程度上的最佳拟合，在SLAM中亦可以看作为观测值和模型估计值之间的误差；

1.线性最小二乘与非线性最小二乘的关系

线性：直接对目标函数求导，令其等于零，以此求得极值，比较后得到全局最小值。

非线性：由于函数复杂无法直接写出导数形式，无法直接得到全局最小值。退而求其次，从一个初始估计值通过不断迭代寻找局部最小值，不断寻找梯度并下降。

2、梯度下降法

保留一阶：最速下降法。梯度下降法值保留泰勒展开的一阶项（只有雅克比项J），

保留二阶：牛顿法。牛顿法保留到二阶项（有海森矩阵项H）。

二、非线性最小二乘（高斯牛顿、LM）

高斯-牛顿法：将非线性函数f(x)进行一阶泰勒展开，缺点：需要保证H矩阵为正定的，但是在实际中H矩阵很有可能是半正定的或者其他情况

LM法：基于信赖区域理论，是由于高斯-牛顿方法在计算时需要保证矩阵的正定性，于是引入了一个约束，从而保证计算方法更具普适性。

LM引入了阻尼因子来调节算法的特性

该因子的作用有：

当阻尼因子大于0，能保证系数矩阵正定，从而确保迭代的下降方向
当阻尼因子很大时，退化为梯度下降法；
当阻尼因子很小时，退化为高斯牛顿法，从而使得接近解时快速收敛；

Distances and bearings between points on an ellipsoidal-model earth

Vincenty’s solution for the distance between points on an ellipsoidal earth model is accurate to within 0.5 mm distance

We can use live examples at https://www.movable-type.co.uk/scripts/latlong-vincenty.html

You can git the Matlab Version at git@github.com:Jason-Yoo/Vincenty-solutions.git

  首先将起点P1和终点P2的纬度转换为弧度记为φ1，φ2，并计算经度差：

$$\Delta L=(\pi / 180)\left(L_{2}-L_{1}\right)$$

  对P1、P2点点纬度进行归化处理:

$$\begin{array}{l}U_{1}=\arctan \left[(1-f) \tan \phi_{1}\right] \\ U_{2}=\arctan \left[(1-f) \tan \phi_{2}\right]\end{array}$$

  设初始条件$ \lambda_0 = \Delta L$，迭代公式：

$$\begin{aligned} \lambda_{k+1}=& \Delta L+(1-C) f \sin \alpha\{\sigma+C \sin \sigma X\\ &\left.\left[\cos 2 \sigma_{m}+C \cos \sigma\left(-1+2 \cos ^{2} 2 \sigma_{m}\right)\right]\right\} \end{aligned}$$

  当$\varepsilon = 10^{-12}$,迭代终止：

$$\left|\lambda_{\mathrm{k}+1}-\lambda_{\mathrm{k}}\right|<\varepsilon$$

  式中 $\alpha$ 为大椭圆航线在赤道的方位角；$\sigma$ 为起点与终点间的球面角距；$\sigma_m$ 为大椭圆航线与赤道的交点到大椭圆航线中点点球面角距。

  最后分别计算距离$s$、起点和终点各自的方位角度𝜶1、𝜶2，𝜶1是起点经线按顺时针方向到大椭圆航线夹角，𝜶2是终点经线按顺时针方向到大椭圆航线夹脚；

$$u^{2}=\cos ^{2} \alpha \cdot\left(a^{2}-b^{2}\right) / b^{2}$$ $$\begin{aligned} A=& 1+\frac{u^{2}}{16384}\left\{4096+u^{2} \cdot\left[-768+u^{2}\left(320-175 u^{2}\right)\right]\right\} \end{aligned}$$ $$B=u^{2} / 1024 \cdot\left\{256+u^{2} \cdot\left[-128+u^{2} \cdot\left(74-47 \cdot u^{2}\right)\right]\right\}$$ $$\begin{aligned} \Delta \sigma &=B \cdot \sin \sigma \cdot\left\{\cos 2 \sigma_{\mathrm{m}}+B / 4 \cdot\left[\cos \sigma \cdot\left(-1+2 \cdot \cos ^{2} 2 \sigma_{\mathrm{m}}\right)\right.\right.\\-&\left.\left.B / 6 \cdot \cos 2 \sigma_{\mathrm{m}} \cdot\left(-3+4 \cdot \sin ^{2} \sigma\right) \cdot\left(-3+4 \cdot \cos ^{2} 2 \sigma_{\mathrm{m}}\right)\right]\right\} \end{aligned}$$ $$s=b A(\sigma-\Delta \sigma)$$ $$\alpha_{1}=\operatorname{atan}\left(\cos U_{2} \cdot \sin \lambda / \cos U_{1} \cdot \sin U_{2}-\sin U_{1} \cdot \cos U_{2} \cdot \cos \lambda\right)$$ $$\alpha_{2}=\operatorname{atan}\left(\cos U_{1} \cdot \sin \lambda /-\sin U_{1} \cdot \cos U_{2}+\cos U_{1} \cdot \sin U_{2} \cdot \cos \lambda\right)$$

  式中：$a=6378137.0m,b≈6 356752.314245m$ 分别为WGS-84椭球的长半轴和短半轴。

1.未知焦距位姿测量—PNP问题

关键词：Focal Length Estimation 、 Camera Pose Estimation、 PNP Problem

方法一：通过成像模型和相机模型，对相机内参数矩阵利用点之间的距离关系等构件多项式代数方程，通过隐变量消去法或Grobner基，吴方法的方式进行求解；

位姿估计的最小问题：

​ 所谓最小问题,是指使用最少的匹配点对来估计对极几何、相机标定参数以及畸变参数等，但是最小问题的求解一般需进行复杂的代数多项式求解，最初的算法都选择忽略几何限制直接使用更多的点对进行求解。随着 Grobner 基的应用， 复杂多项式方程组的求解变得可行，推动了基于最小问题相对姿态估计的快速发展。

基于单目视觉的堆叠箱体工件空间位姿求解研究 —浙江大学 刘年福

（1）特征点空间坐标测量模型

P4P问题的求解，顶点平面坐标的精确度直接影响目标平面空间位姿求解的准确度，建议采用边缘拟合直线的交点坐标来替代工件的四个顶点；

（2）工件图像边长与实际边长的匹配

预先假定工件某个表面为已知面，将该表面４个顶点的二维平面坐标代入特征点空间坐标测量模型求解出其三维空间坐标，利用空间坐标求解出相应边长的实际值，并和假定已知面的边长比较，若边长一致，证明所假定的已知面为真实平面由于工件堆叠引起的旋转，实际长边在图像中的成像可能变成短边，实际短边在成像中可能变成长边，针对这一误判问题，对尝试法进行了优化，引入校验平面的概念，结合工件自身约束，设定了校验条件，保证了尝试的正确性，最终实现了工件图像边长与实际边长的正确匹配。

（3）待抓取平面的空间位姿求解

箱体工件空间位姿求解的最终目的就是根据工件长宽高等尺寸信息和工件顶点在成像平面上的二维坐标计算出待抓取平面的位姿参数目标平面即最终的待抓取平面。

（4）堆叠箱体工件边缘分割

各箱体表面灰度接近，多箱体在成像中存在粘连；

先通过图像预处理算法，然后用canny算子进行边缘提取，工件与工件粘连部分的边缘分割采用SVM分类算法，先以若干粘连工件图像中的粘连边界作为分类器的训练样本，得出学习模型之后再带入新的待分割工件图像，从中提取出粘连的工件边缘，最后建议采用边缘拟合直线的交点坐标来替代工件的四个顶点；

其他关注点：

将用于位姿求解的工件面定义为特征平面，一般取视野里面积最大的表面；

待抓取的工件表面定义为目标平面，目标平面与特征平面垂直，且共用两个特征点；

约束条件：

1.由于是矩形，任意三个特征点组成的三角形面积都相等

2.V=A*h/3

3.引入校验的平面，通过法向量之间叉乘为0，确定唯一的关系

为了解决的问题：

假设某个时刻相机的位姿是T，它观察到一个在世界坐标系中的一个空间点p，并在相机上产生了一个观测数据z，那么

z = Tp + noise

noise是观测噪声。那么观测误差就是

e = z - Tp

假设我们总共有N个这样的三维点p和观测值z，那么我们的目标就是寻找一个最佳的位姿T，使得整体误差最小化，也就是

$$\min _{\boldsymbol{T}} J(\boldsymbol{T})=\sum_{i=1}^{N}\left\|\boldsymbol{z}_{i}-\boldsymbol{T} \boldsymbol{p}_{i}\right\|_{2}^{2}\tag{1}$$

求解此问题，也就是求目标函数J对于变换矩阵T的导数，而T所在的的SE(3)空间，对加法计算并不封闭，也就是说任意两个变换矩阵相加后并不是一个变换矩阵，这主要是因为旋转矩阵对加法是不封闭造成的，它是有如下约束：

$$R^{T} R=I, \operatorname{det}(R)=1$$

李代数就是解决旋转矩阵对加法不封闭的问题。我们把大写SE(3)空间的T映射为一种叫做李代数的东西，映射后的李代数我们叫做小se(3)好了。它是由向量组成的，我们知道向量是对加法封闭的。这样我们就可以通过对李代数求导来间接的对变换矩阵求导了。

我们SLAM目的就是优化求解相机的这个最佳的位姿T（变换矩阵），优化方法一般都采用迭代优化的方法，每次迭代都更新一个位姿的增量delta，使得目标函数最小。这个delta就是通过误差函数对T微分得到的。也就是说我们需要对变换矩阵T求微分（导数），

李群

群（group）就是一种集合加上一种运算的代数结构，满足封闭性，结合律，幺元（单位矩阵I）和逆。李群的定义是指连续光滑的群，比如我们前面说的旋转矩阵群SO(3)，你想象你拿个杯子就可以在空间中以某个支点连续的旋转它，所以SO(3)它就是李群。如果你一般旋转一边移动它，也是连续的或者说光滑的运动，所以变换矩阵群SE(3)也是李群。

李代数

李代数对应李群的正切空间，它描述了李群局部的导数。

对于某个时刻的R(t)（李群空间），存在一个三维向量φ=（φ1，φ2，φ3）（李代数空间），用来描述R在t时刻的局部的导数。

$$\dot{\mathbf{R}}(t)=\phi(t)^{\wedge} \mathbf{R}(t)=\left[\begin{array}{ccc}0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0\end{array}\right] \mathbf{R}(t)$$

我们发现旋转矩阵的微分是一个反对称(也叫斜对称)矩阵左乘它本身，反对称矩阵其实是将三维向量和三维矩阵建立对应关系。它是这样定义的：如果一个3 X 3的矩阵A满足如下式子,也就是说反对称矩阵对角线元素都为0。

$$A^{T}=-A$$

对于反对称矩阵A，只有三个自由度:

$$A=\left[ \begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array} \right]$$

则定义一个三维向量$a=\left[ a_{1}\; ,\; a_{2}\; ,\; a_{3}\; \right]^{T}$，则可以用一个上三角符号来定义如下对应关系：

$$a^{\wedge }=A$$

这样就可以顺利理解

$$\dot{\mathbf{R}}(t)=\phi(t)^{\wedge} \mathbf{R}(t)=\left[\begin{array}{ccc}0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0\end{array}\right] \mathbf{R}(t)$$

指数映射关系

李代数小so(3)是三维向量φ的集合，每个向量φi的反对称矩阵都可以表达李群(大SO(3))上旋转矩阵R的导数，而R和φ是一个指数映射关系。我们的目的就是用对李代数求导来间接的对变换矩阵求导了，也就是说，李群空间的任意一个旋转矩阵R都可以用李代数空间的一个向量的反对称矩阵指数来近似。

我们最终得到下面式子，它的前提是R在原点附近的一阶泰勒展开，我们看到这个向量φ=（φ1，φ2，φ3）反应了R的导数性质，故称它在SO(3)上的原点 φ0 附近的正切空间上。这个φ正是李群大SO(3)对应的李代数小so(3)。

$$R\; \left( t \right)=\exp \left( \phi _{0}^{\wedge }t \right)$$

经过对指数e的泰勒展开，以及反对称矩阵的性质，我们可以得到如下结果：

$$\exp (\theta \mathbf{a})=\cos \theta \mathbf{I}+(1-\cos \theta) \mathbf{a} \mathbf{a}^{T}+\sin \theta \mathbf{a}^{\wedge}$$

其中：三维向量 φ = θa，a是一个长度为1的方向向量，其实有点像罗德里格斯公式 ，罗德里格斯公式（Rodriguez formula）是计算机视觉中的一大经典公式，在描述相机位姿的过程中很常用，表示从旋转向量到旋转矩阵的转换过程，公式如下

$$R=I+\sin (\theta) K+(1-\cos (\theta)) K^{2}$$

其中：$k=\left[ k_{x}\; k_{y}\; k_{z} \right]^{T}$是旋转轴（两个自由度），theta是旋转角度（一个自由度）；

综上所述，我们得到如下映射关系

李代数求导分两种：一种是用李代数表示位姿，然后根据李代数加法来对李代数求导，但结果中有复杂的雅克比公式，不是很方便。一般都用第二种，就是对李群进行左乘或者右乘微小的扰动，然后对该扰动求导。

机器人手眼标定Ax=xB（求解相机和机械臂末端坐标系位姿关系）

eye in hand（机械臂底座与相机相对位姿固定）

代表从机器人末端（gripper/robot）到机器人基座（end/base）的齐次变换矩阵。

为相机到目标的变换矩阵， 代表了两个末端达到的位置。

根据机器人基座与目标/标定板之间位置相对固定的关系建立以下等式：

常用标定方法：Tsai-Lenz标定

eye to hand（机械臂底座与标定板相对位姿固定）

常用标定方法：

九点标定—直接建立相机和机械臂末端的坐标变换关系

方法：让机械手的末端去走这就9个点得到在机器人坐标系中的坐标，同时还要用相机识别9个点得到像素坐标。这样就得到了9组对应的坐标。

手眼标定具体步骤：

1. 进行相机内参标定（张正友标定法）；
2. 使用eye-to-hand或者eye-in-hand的方式将相机、标定板固定好，启动机器人调整机械臂末端位置姿态，并将对应的照片、机器人末端位姿记录下来；
3. 利用相机内参计算得到照片中相机与标定板之间的坐标转换关系；
4. 利用“两步法”求解基本方程。先从基本方程中求解R,再求解T，其中特征点世界坐标为A组数据，末端坐标为B组数据；

“两步法”手眼标定
一般用“两步法”求解基本方程，即先从基本方程上式求解出$R_{cg}$，再代入下式求解出$T_{cg}$。在TSAI文献中引入旋转轴-旋转角系统来描述旋转运动来进行求解该方程组，具体的公式推导可以查看原始文献，这里只归纳计算步骤，不明白的地方可阅读文献，计算步骤如下：

Step1：利用罗德里格斯变换将旋转矩阵转换为旋转向量

Step2：向量归一化

Step3：修正的罗德里格斯参数表示姿态变化

Step4：计算初始旋转向量$P_{cg}^{‘}$

Step5：计算旋转向量$P_{cg}$

Step6：计算旋转矩阵$R_{cg}$

Step7：计算平移向量$T_{cg}$

备注：

计算两组数据的变换矩阵实际上为3D-3D的位姿估计问题，可用迭代最近点（Iterative Closest Point, ICP）求解，实现方法有两种：

1.利用线性代数的求解（主要是 SVD）（建议采用该方法）
2.利用非线性优化方式的求解（类似于 Bundle Adjustment）

一种单目视觉位姿测量系统的误差分析方法

​ 相邻特征点在图像平面所成像点的间距主要受测量距离和特征点实际间距的影响。测量距离越大，像素点之间代表的实际距离就越大，测量误差就越大。

​ 位姿测量误差的本质是回归到像素点代表的实际距离的大小，近距离的时候图像提取误差大一点也不会影响太多，因为像素间代表的实际距离很小，一旦距离变远，意味着单个像素点代表的距离就增大了，所以提取误差就显得尤为重要。

基于点特征的位姿测量系统鲁棒性分析 郝颖明 朱枫

证明了由图像坐标检测误差引起的位姿误差随摄像机焦比的增大而减小。 位姿求解误差随着目标模型中特征点间距离的增大而减小, 随着测量距离的增大而增大等结论。

横滚角对P3P位姿测量方法鲁棒性的影响分析 朱枫

证明了当横滚角为 0度 或 ±90度时，测量位姿的鲁棒性较好，而当横滚角为 ±45度或 ±135度时，测量位姿的鲁棒性最差。

合作目标姿态对视觉位姿测量精度的影响分析 朱枫

证明了在三个控制点构成等腰三角形的条件下，当等腰三角形的高线与摄像机光轴平行（合作目标构成的三角形所在平面与摄像机像平面垂直）时，测量结果的精度最高。

P3P位姿测量方法的误差 郝颖明 朱枫

证明了在三个控制点构成等腰三角形的条件下，图像坐标的检测误差和像机内参数标定误差对测量位姿误差对影响较大，而目标测量模型的测量误差对位姿求解的影响可以忽略不计。