根据其他2台摄像机的校准输出计算第三台摄像机中的3D点

如何解决根据其他2台摄像机的校准输出计算第三台摄像机中的3D点

我从三个不同的天使那里进行了一次心理治疗会议：

1. 摄像机1专注于患者
2. cam 2专注于治疗师
3. cam 3从侧面同时捕捉

我用Matlab校准工具校准了cam1和cam2，并对带cam1的cam3和带cam2的cam3进行了外部立体声校准，并且获得了所有会话的所有旋转矢量和本征矩阵。

现在，给定2个点（来自cam1 / 2），我感兴趣的是计算“真实世界” cam3中的3D点（我使用校准矩阵从两个相机的每个点绘制的矢量之间的交点，较早发现）。

我用python编写了这段代码以找到3D点：

'''
External calibration output between 2 cameras:
  ---------------------------------------------
  R - Rotation Matrix of camera one relative to camera two
  O - Translation vector of camera one relative to camera two
  
  Anotations:
  ---------
  org - of camera which represents the real world (with origin [0,0])
  rel - of relative camera with translation vector
  d - direction vector of each camera,i.e,np.linalg.inv(K).dot([u,v,1])
      [u,v] - pixel in the image
      K - Calibration matrix (the intrinsicMatrix)
      
  Output:
  ------
  3D point of each camera
'''
def find3DPoints (d_org,O_org,d_rel,R_rel,O_rel):
    dist = 1000
    alpha=0
    beta=0
    X_org=0
    X_rel=0
    
    d_rel = np.matmul(np.linalg.inv(R_rel),d_rel)
    
    while (dist > 10) :
        alpha = np.random.randint(200)
        beta = np.random.randint(200)
        
        #print("alpha: ",alpha)
        #print("beta: ",beta)
        
        X_org = alpha*d_org + O_org
        X_rel = beta*d_rel + O_rel
        
        dist = np.linalg.norm(X_org-X_rel)
        #print("distance: ",dist)
        
    print("distance between 2 closest points: ",dist)
    print("alpha: ",alpha)
    print("beta: ",beta)
    print("X Origin: ",X_org)
    print("X Relative: ",X_rel)
    return X_org,X_rel

我得到的结果令人满意：

distance between 2 closest points:  8.663262014946904
alpha:  140
beta:  81
X Origin:  [-41.49543898  11.50037104 140.        ]
X Relative:  [-41.69268956   3.02469031 141.78214397]

然后，我尝试编写一种更好的解决方案，该解决方案使用线性代数使用最小二乘（Ax = b）查找alpha和beta：

我之所以这样想，是因为：

X = O_org + alpha * d_org

X = O_rel + beta * d_rel

===>

[d_rel，d_org] * [-beta，alpha] = O_rel-O_org

[d_rel，d_org] => 3x2矩阵

O_rel-O_org => 3x1向量

def find3DPointByLeastSquare(d_org,O_rel):
   A=np.array([d_org,d_rel])
   #print("A=",A)

   b=O_rel-O_org
   #b=np.array(O_org)-O_rel
   #print("b=",y)

   # Ax=b
   alpha,beta = np.linalg.lstsq(A.T,b,rcond=None)[0]
   #beta=-beta
   print("alpha: ",alpha)
   print("beta: ",beta)

   X_org = O_org + alpha*d_org
   print("X Origin=",X_org)

   X_rel = O_rel + -beta*d_rel
   print("X Relative=",X_rel)

   return X_org,X_rel

但是我得到的结果与第一个解决方案不同！！！ 我期望得到相同的结果。可能是什么原因？

alpha:  199.83226356812804
beta:  -204.61307546220422
X Origin= [ 27.31677265  16.63486267 199.83226357]
X Relative= [ 67.03447507 -28.86206184 198.19026467]