最近のブログは本当に不十分です、古いブログはまだ適切な知識を説明しています（参考）
opencv 両眼視下での空間座標計算
Python 双眼カメラによる 3 次元座標計算 (opencv 独自の画像を使用)
1. 行く
カメラ マトリックスは内部パラメーター マトリックスと外部パラメーター マトリックスで構成されており、MATLAB ツールボックスを通じて調整できます。ブログはあちこちにあるので、ここでは省略します。
1.1 内部参照マトリックス
K = [ fxsx 0 0 fyy 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{配列}\right](1-1)K=⎣
⎡f×00sfはい0バツ0y01⎦
⎤( 1−1 )
ここで、 fx と fy は焦点距離です。 f は dx と dy の積です。 dx は u 軸上のピクセルの実際の距離を表し、 dy は v 軸上のピクセルの実際の距離を表します (間違っています、差別に注意してください）。実際、ここでは MATLAB キャリブレーションがすでに計算されているため、心配する必要はありません。ここで、s は座標傾斜パラメータであり、通常は 0 です。
1.2 外部パラメータマトリックス
ダイニング行列には回転行列R 3 x 3 R_{3x3}が含まれます。R3 × 3、並進ベクトルT 3 x 1 T_{3x1}T3 × 1
2 座標計算
画像が修正された後、座標計算を実行できるようになり、どこにでもある修正をスキップします。
2.1 世界座標 --> ピクセル座標
既知のワールド座標の座標は( X w , Y w , Z w ) (X_w,Y_w,Z_w)です。( Xｗ、Yｗ、Zｗ)、カメラ座標系とワールド座標系の間の関係は、回転および平行移動行列から取得できます:
[ 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)⎣
⎡バツcYcZc1⎦
⎤=[R3 × 30Tt3 × 11】⎣
⎡バツｗYｗZｗ1⎦
⎤（2−1 )
次に、カメラ座標系をピクセル座標系に転送します
Z c [ uv 1 ] = K [ X c Y c Z c 1 ] ( 2 − 2 ) Z_{c}\left[\begin{array}{c} u \\ v \\ 1 \end{配列}\right]=K\left[\begin{配列}{c} X_{c} \\ Y_{c} \\ Z_{c} \\ 1 \end{配列 }\right](2-2)Zc⎣
⎡あなたv1⎦
⎤=K⎣
⎡バツcYcZc1⎦
⎤( 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【×c、Yc、Zc1 】Tはカメラ座標系の座標です、KKk是摄像机内パラメータ阵
〚联立弈2-1）許可
T 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 {配列}\right]\left[ \begin{配列}{cc}R_{3\times3}&t_{3\times1}\\0^{T}&1\end{配列}\right]\left[\begin {配列}{c } X_{w}\\Y_{w}\\Z_{w}\\1\end{配列}\right](2-3)Zc⎣
⎡あなたv1⎦
⎤=⎣
⎡f×00sfはい0バツ0y01⎦
⎤[R3 × 30Tt3 × 11】⎣
⎡バツｗYｗZｗ1⎦
⎤( 2−3 )
2.2 ピクセル座標 --> 世界座標
式 (2-3) から、左右のカメラについて
Z left [ ulvl 1 ] = M left ⋅ [ XYZ 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{配列}\right](2-4)Z左_ _ _⎣
⎡あなた私v私1⎦
⎤=M左 ⋅⎣
⎡バツYZ1⎦
⎤( 2−4 )
Z right [ urvr 1 ] = M right ⋅ [ XYZ 1 ] ( 2 − 5 ) Z_{right}\left[\begin{array}{c} u_{r} \\ v_{r} \\ 1 \ end{配列}\right]=M_{\text {right}} \cdot\left[\begin{配列}{c} X \\ Y \\ Z \\ 1 \end{配列}\right](2- 5)Zそうですね_ _ _ _⎣
⎡あなたrvr1⎦
⎤=Mそうだよ⋅⎣
⎡バツYZ1⎦
⎤( 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 ] = [ fxsx 0 0 fyy 0 0 0 1 ] [ R 00 R 01 R 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_{1 } &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{配列}\right] (2-6)M=⎣
⎡メートル11メートル21メートル31メートル12メートル22メートル32メートル13メートル23メートル33メートル14メートル24メートル34⎦
⎤=⎣
⎡f×00sfはい0バツ0y01⎦
⎤⎣
⎡R00R10R20R01R11R21R02R12R22T00T01T02⎦
⎤（2−6 ）
式（2-6）代入（2-4）、（2-5）により得られるもの：
Z left [ ulvl 1 ] = [ m 11 lm 12 lm 13 lm 14 lm 21 lm 22 lm 23 lm 24 lm 31 lm 32 lm 33 lm 34 l ] [ XYZ 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{配列}\right](2-7)Z左_ _ _⎣
⎡あなた私v私1⎦
⎤=⎣
⎡メートル11私メートル21私メートル31私メートル12私メートル22私メートル32私メートル13私メートル23私メートル33私メートル14私メートル24私メートル34私⎦
⎤⎣
⎡バツYZ1⎦
⎤( 2−7 )
Z 右 [ urvr 1 ] = [ m 11 rm 12 rm 13 rm 14 rm 21 rm 22 rm 23 rm 24 rm 31 rm 32 rm 33 rm 34 r ] [ XYZ 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{配列}\right]\left[\begin{配列}{c} X \\ Y \\ Z \\ 1 \end{配列}\right](2-8)Zそうですね_ _ _ _⎣
⎡あなたrvr1⎦
⎤=⎣
⎡メートル11rメートル21rメートル31rメートル12rメートル22rメートル32rメートル13rメートル23rメートル33rメートル14rメートル24rメートル34r⎦
⎤⎣
⎡バツYZ1⎦
⎤( 2−8 )
展开公式（2-7）入手可能：
m 11 l X + m 12 l Y + m 13 l Z + m 14 l = Z leftul − ① m 21 l X + m 22 l Y + m 23 l Z + m 24 l = Z leftvl − ② m 31 l X + m 32 l Y + m 33 l Z + m 34 l = Z left − ③ m_{11}^{l} X+m_{12}^{l}Y +m_{13}^{l}Z+m_{14}^{l}=Z_{左} u_l - ①\\ m_{21}^{l} X+m_{22}^{l}Y+m_ {23}^{l}Z+m_{24}^{l}=Z_{左} v_l -②\\ m_{31}^{l} X+m_{32}^{l}Y+m_{33 }^{l}Z+m_{34}^{l}=Z_{左} -③メートル11私バツ+メートル12私Y+メートル13私Z+メートル14私=Z左_ _ _あなた私−①メートル21私バツ+メートル22私Y+メートル23私Z+メートル24私=Z左_ _ _v私−②メートル31私バツ+メートル32私Y+メートル33私Z+メートル34私=Z左_ _ _−③
①/③、②/③化简得：
( ulm 31 l − m 11 l ) X + ( ulm 32 l − m 12 l ) Y + ( ulm 33 l − m 13 l ) Z = m 14 l − ulm 34 l ( vlm 31 l − m 21 l ) X + ( vlm 32 l − m 22 l ) Y + ( vlm 33 l − m 23 l ) Z = m 24 l − vlm 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{配列}(あなた私メートル31私−メートル11私)バツ+(あなた私メートル32私−メートル12私)Y+(あなた私メートル33私−メートル13私)Z=メートル14私−あなた私メートル34私( v私メートル31私−メートル21私)バツ+( v私メートル32私−メートル22私)Y+( v私メートル33私−メートル23私)Z=メートル24私−v私メートル34私
同理展开公式（2-8）入手可能：
m 11 r X + m 12 r Y + m 13 r Z + m 14 r = Z rightur − ① m 21 r X + m 22 r Y + m 23 r Z + m 24 r = Z rightvr − ② m 31 r X + m 32 r Y + m 33 r Z + m 34 r = Z right − ③ m_{11}^{r} X+m_{12}^{r}Y +m_{13}^{r}Z+m_{14}^{r}=Z_{右} u_r - ①\\ m_{21}^{r} X+m_{22}^{r}Y+m_ {23}^{r}Z+m_{24}^{r}=Z_{右} v_r -②\\ m_{31}^{r} X+m_{32}^{r}Y+m_{33 }^{r}Z+m_{34}^{r}=Z_{右} -③メートル11rバツ+メートル12rY+メートル13rZ+メートル14r=Zそうですね_ _ _ _あなたr−①メートル21rバツ+メートル22rY+メートル23rZ+メートル24r=Zそうですね_ _ _ _vr−②メートル31rバツ+メートル32rY+メートル33rZ+メートル34r=Zそうですね_ _ _ _−③
①/③、②/③化简得：
( urm 31 r − m 11 r ) X + ( urm 32 r − m 12 r ) Y + ( urm 33 r − m 13 r ) Z = m 14 r − urm 34 r ( vrm 31 r − m 21 r ) X + ( vrm 32 r − m 22 r ) Y + ( vrm 33 r − m 23 r ) Z = m 24 r − vrm 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{配列}(あなたrメートル31r−メートル11r)バツ+(あなたrメートル32r−メートル12r)Y+(あなたrメートル33r−メートル13r)Z=メートル14r−あなたrメートル34r( vrメートル31r−メートル21r)バツ+( vrメートル32r−メートル22r)Y+( vrメートル33r−メートル23r)Z=メートル24r−vrメートル34r
リアン・ライド：
( ulm 31 l − m 11 l ) X + ( ulm 32 l − m 12 l ) Y + ( ulm 33 l − m 13 l ) Z = m 14 l − ulm 34 l (vlm 31 l − m 21 l ) X + ( vlm 32 l − m 22 l ) Y + ( vlm 33 l − m 23 l ) Z = m 24 l − vlm 34 l ( urm 31 r − m 11 r ) X + ( urm 32 r − m 12 r ) Y + ( urm 33 r − m 13 r ) Z = m 14 r − urm 34 r ( vrm 31 r − m 21 r ) X + ( vrm 32 r − m 22 r ) Y + ( vrm 33 r − m 23 r ) Z = m 24 r − vrm 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 }(あなた私メートル31私−メートル11私)バツ+(あなた私メートル32私−メートル12私)Y+(あなた私メートル33私−メートル13私)Z=メートル14私−あなた私メートル34私( v私メートル31私−メートル21私)バツ+( v私メートル32私−メートル22私)Y+( v私メートル33私−メートル23私)Z=メートル24私−v私メートル34私(あなたrメートル31r−メートル11r)バツ+(あなたrメートル32r−メートル12r)Y+(あなたrメートル33r−メートル13r)Z=メートル14r−あなたrメートル34r( vrメートル31r−メートル21r)バツ+( vrメートル32r−メートル22r)Y+( vrメートル33r−メートル23r)Z=メートル24r−vrメートル34r
4 つの方程式、3 つの未知数、何が起こるでしょうか? ——> SLAM への入力 (2)—— 最小二乗法を使用してサブピクセル座標を見つけます(理解できない場合は、戻って確認してください)
。短い、A x = B Ax =Bの場合あ×=Bの方程式については、次の式でxxを解くことができます。x：
x = ( ATA ) − 1 ATB x=(A^TA)^{-1}A^TBバツ=( ATA）_− 1A _T B が
Opencv にある場合は、cv2.solve(A,B,cv2.DECOMP_SVD) で解決できます。
コードはPython 双眼カメラから来ており、3 次元座標を計算します (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