初入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=⎣ ⎡fx00sfy0x0y01⎦ ⎤(1−1)
其中fx，fy为焦距f为dx，dy的乘积，dx代表u轴上的一个像素点的实际距离是多少，dy代表v轴上的一个像素点的实际距离是多少（可能有误，注意判别）。这里的话其实MATLAB标定已经给你算好了，你就不用管了。其中s是坐标倾斜参数，一般来说都是0。
1.2 外参矩阵
外餐矩阵包括一个旋转矩阵 R 3 x 3 R_{3x3} R3x3,平移向量 T 3 x 1 T_{3x1} T3x1
2 坐标计算
当图片进行校正后，就可以进行坐标计算了，校正我们也略过，到处都有。
2.1 世界坐标——>像素坐标
已知世界坐标中的坐标为 ( X w , Y w , Z w ) (X_w,Y_w,Z_w) (Xw,Yw,Zw)，由旋转和平移矩阵可得摄像机坐标系和世界坐标系的关系为：
[ 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） ⎣ ⎡XcYcZc1⎦ ⎤=[R3×30Tt3×11]⎣ ⎡XwYwZw1⎦ ⎤（2−1）
然后将摄像机坐标系转移到像素坐标系
Z c [ u v 1 ] = K [ X c Y c Z c 1 ] ( 2 − 2 ) 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) Zc⎣ ⎡uv1⎦ ⎤=K⎣ ⎡XcYcZc1⎦ ⎤(2−2)
其中 【 u v 1 】 T 【u v 1】^T 【uv1】T为像素坐标， 【 X c , Y c , Z c 1 】 T 【X_c,Y_c,Z_c 1】^T 【Xc,Yc,Zc1】T是摄像机坐标系的坐标， K K K是摄像机内参矩阵。
通过联立式（2-1）（2-2）可以得到
Z c [ u v 1 ] = [ f x s x 0 0 f y y 0 0 0 1 ] [ R 3 × 3 t 3 × 1 0 T 1 ] [ X w Y w Z w 1 ] ( 2 − 3 ) 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) Zc⎣ ⎡uv1⎦ ⎤=⎣ ⎡fx00sfy0x0y01⎦ ⎤[R3×30Tt3×11]⎣ ⎡XwYwZw1⎦ ⎤(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⎣ ⎡ulvl1⎦ ⎤=Mleft ⋅⎣ ⎡XYZ1⎦ ⎤(2−4)
Z r i g h t [ u r v r 1 ] = M right ⋅ [ X Y Z 1 ] ( 2 − 5 ) 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) Zright⎣ ⎡urvr1⎦ ⎤=Mright⋅⎣ ⎡XYZ1⎦ ⎤(2−5)
其中，
M = [ m 11 m 12 m 13 m 14 m 21 m 22 m 23 m 24 m 31 m 32 m 33 m 34 ] = [ f x s x 0 0 f y y 0 0 0 1 ] [ R 00 R 01 R 02 T 00 R 10 R 11 R 12 T 01 R 20 R 21 R 22 T 02 ] （ 2 − 6 ） 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） M=⎣ ⎡m11m21m31m12m22m32m13m23m33m14m24m34⎦ ⎤=⎣ ⎡fx00sfy0x0y01⎦ ⎤⎣ ⎡R00R10R20R01R11R21R02R12R22T00T01T02⎦ ⎤（2−6）
通过式（2-6）代入（2-4），（2-5）可得：
Z l e f t [ u l v l 1 ] = [ 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 ] [ X Y Z 1 ] ( 2 − 7 ) 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) Zleft⎣ ⎡ulvl1⎦ ⎤=⎣ ⎡m11lm21lm31lm12lm22lm32lm13lm23lm33lm14lm24lm34l⎦ ⎤⎣ ⎡XYZ1⎦ ⎤(2−7)
Z r i g h t [ u r v r 1 ] = [ 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 ] [ X Y Z 1 ] ( 2 − 8 ) 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) Zright⎣ ⎡urvr1⎦ ⎤=⎣ ⎡m11rm21rm31rm12rm22rm32rm13rm23rm33rm14rm24rm34r⎦ ⎤⎣ ⎡XYZ1⎦ ⎤(2−8)
展开公式（2-7）可得：
m 11 l X + m 12 l Y + m 13 l Z + m 14 l = Z l e f t u l − ① m 21 l X + m 22 l Y + m 23 l Z + m 24 l = Z l e f t v l − ② m 31 l X + m 32 l Y + m 33 l Z + m 34 l = Z l e f t − ③ 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} -③ m11lX+m12lY+m13lZ+m14l=Zleftul−①m21lX+m22lY+m23lZ+m24l=Zleftvl−②m31lX+m32lY+m33lZ+m34l=Zleft−③
①/③，②/③化简得：
( u l m 31 l − m 11 l ) X + ( u l m 32 l − m 12 l ) Y + ( u l m 33 l − m 13 l ) Z = m 14 l − u l m 34 l ( v l m 31 l − m 21 l ) X + ( v l m 32 l − m 22 l ) Y + ( v l m 33 l − m 23 l ) Z = m 24 l − v l m 34 l \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} (ulm31l−m11l)X+(ulm32l−m12l)Y+(ulm33l−m13l)Z=m14l−ulm34l(vlm31l−m21l)X+(vlm32l−m22l)Y+(vlm33l−m23l)Z=m24l−vlm34l
同理展开公式（2-8）可得：
m 11 r X + m 12 r Y + m 13 r Z + m 14 r = Z r i g h t u r − ① m 21 r X + m 22 r Y + m 23 r Z + m 24 r = Z r i g h t v r − ② m 31 r X + m 32 r Y + m 33 r Z + m 34 r = Z r i g h t − ③ 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} -③ m11rX+m12rY+m13rZ+m14r=Zrightur−①m21rX+m22rY+m23rZ+m24r=Zrightvr−②m31rX+m32rY+m33rZ+m34r=Zright−③
①/③，②/③化简得：
( u r m 31 r − m 11 r ) X + ( u r m 32 r − m 12 r ) Y + ( u r m 33 r − m 13 r ) Z = m 14 r − u r m 34 r ( v r m 31 r − m 21 r ) X + ( v r m 32 r − m 22 r ) Y + ( v r m 33 r − m 23 r ) Z = m 24 r − v r m 34 r \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} (urm31r−m11r)X+(urm32r−m12r)Y+(urm33r−m13r)Z=m14r−urm34r(vrm31r−m21r)X+(vrm32r−m22r)Y+(vrm33r−m23r)Z=m24r−vrm34r
联立得：
( u l m 31 l − m 11 l ) X + ( u l m 32 l − m 12 l ) Y + ( u l m 33 l − m 13 l ) Z = m 14 l − u l m 34 l ( v l m 31 l − m 21 l ) X + ( v l m 32 l − m 22 l ) Y + ( v l m 33 l − m 23 l ) Z = m 24 l − v l m 34 l ( u r m 31 r − m 11 r ) X + ( u r m 32 r − m 12 r ) Y + ( u r m 33 r − m 13 r ) Z = m 14 r − u r m 34 r ( v r m 31 r − m 21 r ) X + ( v r m 32 r − m 22 r ) Y + ( v r m 33 r − m 23 r ) Z = m 24 r − v r m 34 r \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} (ulm31l−m11l)X+(ulm32l−m12l)Y+(ulm33l−m13l)Z=m14l−ulm34l(vlm31l−m21l)X+(vlm32l−m22l)Y+(vlm33l−m23l)Z=m24l−vlm34l(urm31r−m11r)X+(urm32r−m12r)Y+(urm33r−m13r)Z=m14r−urm34r(vrm31r−m21r)X+(vrm32r−m22r)Y+(vrm33r−m23r)Z=m24r−vrm34r
四个方程，三个未知数，什么要出场了？那肯定又是最小二乘法嘛.——>初入SLAM(2)——用最小二乘法求亚像素坐标(不懂的可以跳转回去看看。)
简略来说，对于 A x = B Ax=B Ax=B的方程，我们可以通过下述公式求解 x x x：
x = ( A T A ) − 1 A T B x=(A^TA)^{-1}A^TB x=(ATA)−1ATB
在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