最近实习在读浙大最近的NeuralRecon，里面涉及到一些3D点云处理的数学方法和3DVison的深度学习Tricks，主要包括TSDF算法，特征点提取的SIFT、ORB两种算法，及稀疏卷积等，以及SLAM(simultaneous localization and mapping)基础，从《视觉SLAM十四讲》（高翔等）学习基础。双目算法现在已经比较成熟，目前自己工作只涉及单目相机。

SLAM框架

视觉里程计（Visual Odometry）

目前项目的硬件设备由单目相机获取信息，进而进行姿态估计、深度估计等计算。
简单来说，VO是由相邻两张图片间像素的位置关系估计相机的位置，

坐标变换 旋转矩阵

求相机坐标系（o）到世界坐标系（w）下的旋转矩阵$R^o_w$，进行欧式变换可以将o下的向量$p_o$ 转换到w下向量$p_w$
求出刚体旋转矩阵$R^o_w$，那么w下向量$p_w$左乘R就可以转化到$p_o$:
$p_o = R^o_w \cdot\ p_w$
同理如果两个坐标系下的旋转矩阵可以得到
$$m = $$
eg. 下面是由w到o1 和o2两个旋转矩阵传递得到的o1 -> o2的旋转矩阵
设两个相机坐标系下o1,o2对应的三个点a b c，d e f，各获得两个向量 $m1,n1$,$m2,n2$
分别构建出该点集合所在的坐标系方程，求解得到世界坐标到o1 , o2的旋转坐标$R_1,R_2$， 则有：世界坐标系向量$m2 = R_1^T \cdot R_2 \cdot m1$

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
def RigidBody_Transform(p=np.zeros((3,3),dtype=float), q=np.zeros((3,3),dtype=float)):
    # 世界坐标到 O1的旋转矩阵 ，x = x / ||x|| 单位化
    x = (p[1,:] - p[0,:]) / np.linalg.norm(p[1,:] - p[0,:]) 
    y = (p[2,:] - p[1,:]) - np.inner(np.inner((p[2,:] - p[1,:]),x), x)
    y = y / np.linalg.norm(y) # y_bar = y / ||y|| # 单位化第二行
    print("x",x)
    print("y",y)
    z = np.cross(x, y) #叉乘
    print("z",z)
    rotate_matrix_w2o1 = np.array([x, y, z])
    # 世界坐标到  O2的转转矩阵
    x_new = (q[1,:] - q[0,:]) / np.linalg.norm(q[1,:] - q[0,:]) 
    y_new = (q[2,:] - q[1,:]) - np.inner(np.inner((q[2,:] - q[1,:]),x_new), x_new)
    y_new = y_new / np.linalg.norm(y_new)
    # 这里要注意，叉积（cross product）和 外积（outer product）不一样
    # ps:国内教材讲的是叉积和外积一样
    # np.cross算叉积 ，np.outer算外积
    z_new = np.cross(x_new, y_new) #这要计算的是叉积（只有三维空间有意义，就是右手系的那个）
    rotate_matrix_w2o2 = np.array([x_new, y_new, z_new])
    return rotate_matrix_w2o1,rotate_matrix_w2o2, (rotate_matrix_w2o1.T * rotate_matrix_w2o2)

#p = np.array([(-47.34,-18.71,-155.02), (-73.64,-29.82,-210.88), (-64.88,-36.77,-216.15)])
#p = np.array([(-4.34,-36.71,51), (-30,25.5,-4), (-21,18,-10.15)])
#q = np.array([(-40,25.5,6), (-30,25.5,-4), (15.01,55.19,22.818)])
p = np.array([(0,1,0), (0,0,0), (0,0,1)])
q = np.array([(0,-1,0), (0,0,0), (0,0,-1)])
m1,m2,rotate_matrix = RigidBody_Transform(p, q)
print("rotate_matrix is:\n")
print(rotate_matrix)

旋转向量

上面的旋转矩阵表示具有局限性，原因是求出的矩阵必须是正交阵，优化时比较困难，并且计算量比较大，需要进行矩阵运算，一次运算需要9次浮点乘法，所以又提出一个用旋转角和旋转轴表示一个旋转向量的描述旋转的方法。同时，旋转向量也可以转换成旋转矩阵:
由罗德里格斯公式(Rodrigus’ Formula)，n_r 即n^，表示向量n到n对应的反对称矩阵的转换符，计算如下：

$a \times b=\begin{Vmatrix} e_1&e_2&e_3\\a_1&a_2&a_3\\b_1&b_2&b_3 \end{Vmatrix}=\begin{bmatrix} a_2b_3-a_3b_2\\a_3b_1-a_1b_3\\a_1b_2-a_2b_1 \end{bmatrix}=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\ -a_2&a_1&0\end{bmatrix} \cdot b = a$^$b$
上面的$a$^ 表示其对应的反对称矩阵
$R = cos \theta I + (1-cos\theta) n \cdot n^T + sin\theta n$^
求转角$\theta$，可以：
$tr(R)= \cos\theta tr(I) +(1-\cos\theta)tr(n \cdot n^T) +\sin\theta tr(n$^$)$
$\quad\quad =3\cos\theta +(1-\cos\theta) = 1+2\cos\theta$
求出$\theta = \arccos \frac{tr(R)-1}{2}$

回环检测（Loop Closure Detection）

判断镜头是否到达过先前位置，和后端（优化）解决因里程计每次计算相邻两张图片的位置关系，每次前后误差叠加出现的漂移问题，简而言之就是校正。

后端优化（非线性）

接受VO获得的相机位姿、回环检测

建图

还没整理好

3D点云模型

相机模型

相机模型得到的相机内参(camera_intrinsic_perview)一般为一张图得到一个
通过两种模型针孔模型（PINHOLE，还有放射模型RADIAL），畸变模型两种实现内参计算