初入SLAM（6）——双目视觉标定后得到三维空间坐标

近期的博客真是水的不行，还是老博客知识讲解到位（Reference）

opencv双目视觉下空间坐标计算
Python双目相机计算三维坐标（使用opencv自带图片）

1. GO

摄像机矩阵由内参矩阵跟外参矩阵构成，具体得到可以通过MATLAB工具箱标定得到，到处都是博客，在此就略过了。

1.1 内参矩阵

K = [ f x s x 0 0 f y y 0 0 0 1 ] ( 1 − 1 ) K=\left[

$$\begin{array}{ccc} f_{x} & s & x_{0} \\ 0 & f_{y} & y_{0} \\ 0 & 0 & 1 \end{array}$$ \right](1-1) K=⎣ ⎡​fx​00​sfy​0​x0​y0​1​⎦ ⎤​(1−1)
其中fx，fy为焦距f为dx，dy的乘积，dx代表u轴上的一个像素点的实际距离是多少，dy代表v轴上的一个像素点的实际距离是多少（可能有误，注意判别）。这里的话其实MATLAB标定已经给你算好了，你就不用管了。其中s是坐标倾斜参数，一般来说都是0。

1.2 外参矩阵

外餐矩阵包括一个旋转矩阵$R_{3x3}$,平移向量$T_{3x1}$

2 坐标计算

当图片进行校正后，就可以进行坐标计算了，校正我们也略过，到处都有。

2.1 世界坐标——>像素坐标

已知世界坐标中的坐标为$(X_w,Y_w,Z_w)$，由旋转和平移矩阵可得摄像机坐标系和世界坐标系的关系为：
[ X c Y c Z c 1 ] = [ R 3 × 3 t 3 × 1 0 T 1 ] [ X w Y w Z w 1 ] （ 2 − 1 ） \left[

$$\begin{array}{c} X_{c} \\ Y_{c} \\ Z_{c} \\ 1 \end{array}$$ \right]=\left[ $$\begin{array}{cc} R_{3 \times 3} & t_{3 \times 1} \\ 0^{T} & 1 \end{array}$$ \right]\left[ $$\begin{array}{c} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{array}$$ \right]（2-1） ⎣ ⎡​Xc​Yc​Zc​1​⎦ ⎤​=[R3×3​0T​t3×1​1​]⎣ ⎡​Xw​Yw​Zw​1​⎦ ⎤​（2−1）
然后将摄像机坐标系转移到像素坐标系
$$Z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right](2-2)$$
其中$【uv1{】}^T$为像素坐标，$【X_c,Y_c,Z_{c}1{】}^T$是摄像机坐标系的坐标，$K$是摄像机内参矩阵。
通过联立式（2-1）（2-2）可以得到
$$Z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& s& x_0\\ 0& f_y& y_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_{3\times 3}& t_{3\times 1}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right](2-3)$$

2.2 像素坐标——>世界坐标

通过式（2-3），我们可知，对于左右相机，存在
Z l e f t [ u l v l 1 ] = M left  ⋅ [ X Y Z 1 ] ( 2 − 4 ) Z_{left}\left[

$$\begin{array}{c} u_{l} \\ v_{l} \\ 1 \end{array}$$ \right]=M_{\text {left }} \cdot\left[ $$\begin{array}{c} X \\ Y \\ Z \\ 1 \end{array}$$ \right](2-4) Zleft​⎣ ⎡​ul​vl​1​⎦ ⎤​=Mleft ​⋅⎣ ⎡​XYZ1​⎦ ⎤​(2−4)
$$Z_{right}\left[\begin{array}{c}{u}_r\\ v_r\\ 1\end{array}\right]=M_{\text{right}}\cdot \left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right](2-5)$$
其中，
$$M=\left[\begin{array}{llll}{m}_{11}& m_{12}& m_{13}& m_{14}\\ m_{21}& m_{22}& m_{23}& m_{24}\\ m_{31}& m_{32}& m_{33}& m_{34}\end{array}\right]=\left[\begin{array}{ccc}{f}_x& s& x_0\\ 0& f_y& y_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{llll}{R}_{00}& R_{01}& R_{02}& T_{00}\\ R_{10}& R_{11}& R_{12}& T_{01}\\ R_{20}& R_{21}& R_{22}& T_{02}\end{array}\right]（2-6）$$
通过式（2-6）代入（2-4），（2-5）可得：
$$Z_{left}\left[\begin{array}{c}{u}_l\\ v_l\\ 1\end{array}\right]=\left[\begin{array}{llll}{m}_{11}^l& m_{12}^l& m_{13}^l& m_{14}^l\\ m_{21}^l& m_{22}^l& m_{23}^l& m_{24}^l\\ m_{31}^l& m_{32}^l& m_{33}^l& m_{34}^l\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right](2-7)$$
$$Z_{right}\left[\begin{array}{c}{u}_r\\ v_r\\ 1\end{array}\right]=\left[\begin{array}{llll}{m}_{11}^r& m_{12}^r& m_{13}^r& m_{14}^r\\ m_{21}^r& m_{22}^r& m_{23}^r& m_{24}^r\\ m_{31}^r& m_{32}^r& m_{33}^r& m_{34}^r\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right](2-8)$$
展开公式（2-7）可得：
$$\begin{gathered} m_{11}^{l}X+m_{12}^{l}Y+m_{13}^{l}Z+m_{14}^l=Z_{left}{u}_l-① \\ {m}_{21}^{l}X+m_{22}^{l}Y+m_{23}^{l}Z+m_{24}^l=Z_{left}{v}_l-② \\ {m}_{31}^{l}X+m_{32}^{l}Y+m_{33}^{l}Z+m_{34}^l=Z_{left}-③ \end{gathered}$$
①/③，②/③化简得：
$$\begin{array}{l}\left(u_l{m}_{31}^l-m_{11}^l\right)X+\left(u_l{m}_{32}^l-m_{12}^l\right)Y+\left(u_l{m}_{33}^l-m_{13}^l\right)Z=m_{14}^l-u_l{m}_{34}^l\\ \left(v_l{m}_{31}^l-m_{21}^l\right)X+\left(v_l{m}_{32}^l-m_{22}^l\right)Y+\left(v_l{m}_{33}^l-m_{23}^l\right)Z=m_{24}^l-v_l{m}_{34}^l\end{array}$$
同理展开公式（2-8）可得：
$$\begin{gathered} m_{11}^{r}X+m_{12}^{r}Y+m_{13}^{r}Z+m_{14}^r=Z_{right}{u}_r-① \\ {m}_{21}^{r}X+m_{22}^{r}Y+m_{23}^{r}Z+m_{24}^r=Z_{right}{v}_r-② \\ {m}_{31}^{r}X+m_{32}^{r}Y+m_{33}^{r}Z+m_{34}^r=Z_{right}-③ \end{gathered}$$
①/③，②/③化简得：
$$\begin{array}{l}\left(u_r{m}_{31}^r-m_{11}^r\right)X+\left(u_r{m}_{32}^r-m_{12}^r\right)Y+\left(u_r{m}_{33}^r-m_{13}^r\right)Z=m_{14}^r-u_r{m}_{34}^r\\ \left(v_r{m}_{31}^r-m_{21}^r\right)X+\left(v_r{m}_{32}^r-m_{22}^r\right)Y+\left(v_r{m}_{33}^r-m_{23}^r\right)Z=m_{24}^r-v_r{m}_{34}^r\end{array}$$
联立得：
$$\begin{gathered} \left(u_l{m}_{31}^l-m_{11}^l\right)X+\left(u_l{m}_{32}^l-m_{12}^l\right)Y+\left(u_l{m}_{33}^l-m_{13}^l\right)Z=m_{14}^l-u_l{m}_{34}^l \\ \left(v_l{m}_{31}^l-m_{21}^l\right)X+\left(v_l{m}_{32}^l-m_{22}^l\right)Y+\left(v_l{m}_{33}^l-m_{23}^l\right)Z=m_{24}^l-v_l{m}_{34}^l \\ \left(u_r{m}_{31}^r-m_{11}^r\right)X+\left(u_r{m}_{32}^r-m_{12}^r\right)Y+\left(u_r{m}_{33}^r-m_{13}^r\right)Z=m_{14}^r-u_r{m}_{34}^r \\ \left(v_r{m}_{31}^r-m_{21}^r\right)X+\left(v_r{m}_{32}^r-m_{22}^r\right)Y+\left(v_r{m}_{33}^r-m_{23}^r\right)Z=m_{24}^r-v_r{m}_{34}^r \end{gathered}$$
四个方程，三个未知数，什么要出场了？那肯定又是最小二乘法嘛.——>初入SLAM(2)——用最小二乘法求亚像素坐标(不懂的可以跳转回去看看。)
简略来说，对于$Ax=B$的方程，我们可以通过下述公式求解$x$：
$$x=(A^{T}A{)}^{-1}{A}^{T}B$$
在Opencv中的话，可以通过cv2.solve(A,B,cv2.DECOMP_SVD)求解。
代码来自Python双目相机计算三维坐标（使用opencv自带图片）

# -*-coding:utf-8-*-
import cv2
import numpy as np
from numpy.linalg import *

# 左/右相机内参数、旋转、平移矩阵
leftIntrinsic = np.array([[772.649318661082, 0, 666.888963662870],
                          [0, 762.103316826583, 389.509591934574],
                          [0, 0, 1]])
leftRotation = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
leftTranslation = np.array([[0],
                            [0],
                            [0]])
rightIntrinsic = np.array([[772.604738100536, 0, 661.915975602235],
                           [0, 761.832668789731, 384.687323993015],
                           [0, 0, 1]])
rightRotation = np.array([[0.999974190753182, 0.00150152859000334, -0.00702589776563414],
                          [-0.00150386192724381, 0.999998815788740, -0.000326833936454379],
                          [0.00702539869498713, 0.000337391481271093, 0.999975264664164]])
rightTranslation = np.array([[-59.5572247428607],
                             [0.952587213826756],
                             [-4.00796520517714]])

# 函数参数为左右相片同名点的像素坐标，获取方式后面介绍
# lx，ly为左相机某点像素坐标，rx，ry为右相机对应点像素坐标
def uvToXYZ(lx, ly, rx, ry):
    mLeft = np.hstack([leftRotation, leftTranslation])
    mLeftM = np.dot(leftIntrinsic, mLeft)
    mRight = np.hstack([rightRotation, rightTranslation])
    mRightM = np.dot(rightIntrinsic, mRight)
    A = np.zeros(shape=(4, 3))
    for i in range(0, 3):
        A[0][i] = lx * mLeftM[2, i] - mLeftM[0][i]
    for i in range(0, 3):
        A[1][i] = ly * mLeftM[2][i] - mLeftM[1][i]
    for i in range(0, 3):
        A[2][i] = rx * mRightM[2][i] - mRightM[0][i]
    for i in range(0, 3):
        A[3][i] = ry * mRightM[2][i] - mRightM[1][i]
    B = np.zeros(shape=(4, 1))
    for i in range(0, 2):
        B[i][0] = mLeftM[i][3] - lx * mLeftM[2][3]
    for i in range(2, 4):
        B[i][0] = mRightM[i - 2][3] - rx * mRightM[2][3]
    XYZ = np.zeros(shape=(3, 1))
    # 根据大佬的方法，采用最小二乘法求其空间坐标
    cv2.solve(A, B, XYZ, cv2.DECOMP_SVD)
    print(XYZ)
    return XYZ
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