问题描述
我有两个 intel realsense 相机:CameraLeft 和 CameraRight。我试图找到这两个相机之间的旋转矩阵和变换向量,所以我可以在两个相机之间转换坐标系。
这是我尝试过的:
1.我使用 cv.calibrateCamera() 得到内在矩阵:Mleft 和 Mright
2.我使用cv.stereoCalibrate()得到旋转矩阵R和变换向量T
3.我同时拍了两张照片,分别是Pleft和Pright。
4.(uleft,vleft) 和 (uright,vright) 表示同一个对象两张图片。具有固有矩阵 Mleft 、 Mright 和深度 Dleft、Dright 和 (u left,vleft),(uright,vright) ,我得到相机坐标 (Xleft,Yleft,Zleft) 和 (Xright,Yright,Z右).
问题来了:
在我的假设中,R*(Xleft,Zleft) + T = (Xright,Zright)。但结果并非如此。
这是我的代码:
import numpy as np
import h5py
import cv2
cameraMatLeft = np.array([
[
1286.540148375528,0.0,929.5596785987797
],[
0.0,1272.8889372475517,586.0340979684613
],1.0
]
])
cameraMatRight = np.array([
[
1294.8590822926074,976.7466553094133
],1283.5006893318534,520.6437123281569
],1.0
]
])
R = np.array([
[
0.736828762715704,0.1290536263233139,0.6636479005976342
],[
-0.09833992557804119,0.9916309806151367,-0.08364961040894248
],[
-0.6688891040166153,-0.0036276462155228617,0.7433533525254223
]
])
T = np.array([
[
-190.9527690494799
],[
11.868938400892926
],[
71.571368261639625
]
])
# two pixel point
rightPoint = (1107,568)
leftPoint = (1840,697)
fLeft = h5py.File('C:\\SMIIP\\camera\\depth_pics\\leftDepth0.h5','r')
fRight = h5py.File('C:\\SMIIP\\camera\\depth_pics\\rightDepth0.h5','r')
d_left = fLeft['depth'][leftPoint[1],leftPoint[0]]
d_right = fRight['depth'][rightPoint[1],rightPoint[0]]
#print(d_left)
#print(d_right)
leftInv = np.linalg.inv(cameraMatLeft)
RightInv = np.linalg.inv(cameraMatRight)
leftPixPoint = np.array([d_left*leftPoint[0],d_left*leftPoint[1],d_left])
rightPixPoint = np.array([d_right*rightPoint[0],d_right*rightPoint[1],d_right])
#compute the camera coordinate
leftResult = np.matmul(leftInv,leftPixPoint)
rightResult = np.matmul(RightInv,rightPixPoint)
leftResult = leftResult.reshape(3,1)
rightResult = rightResult.reshape(3,1)
leftRotated = np.matmul(R,leftResult) + T
rightRotated = np.matmul(R,rightResult) + T
print(leftResult,rightResult)
#print(leftRotated,rightRotated)
我做错了吗?请帮帮我。任何帮助将不胜感激。
解决方法
我认为您所做的并非完全错误,但我不确定您用于将 left_cam 坐标转换为 right_cam 坐标的公式。 从我在这个参考资料中看到的:Here.
所以从他们所说的 Pr=R(Pl-T) 。 不确定这会解决您的情况,但希望我能有所帮助。