我有两个英特尔 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,以及像素位置 (uleft, vleft)、(uright, vright),可以得到相机坐标 (Xleft, Yleft, Zleft) 和 (Xright, Yright, Zright)。
问题在于:
在我的假设中,R*(Xleft, Yleft, Zleft) + T = (Xright, Yright, Zright),但结果并不是这样的。
以下是我的代码:
以下是我的尝试方法:
1. 使用 cv.calibrateCamera() 获取内参数矩阵:Mleft 和 Mright 2. 使用 cv.stereoCalibrate() 获取旋转矩阵 R 和变换向量 T 3. 同时拍摄两张照片 Pleft 和 Pright 4. (uleft, vleft) 和 (uright, vright) 分别表示两张照片中相同物体的像素位置。根据内参数矩阵 Mleft、Mright、深度 Dleft、Dright,以及像素位置 (uleft, vleft)、(uright, vright),可以得到相机坐标 (Xleft, Yleft, Zleft) 和 (Xright, Yright, Zright)。
问题在于:
在我的假设中,R*(Xleft, Yleft, Zleft) + T = (Xright, Yright, Zright),但结果并不是这样的。
以下是我的代码:
import numpy as np
import h5py
import cv2
cameraMatLeft = np.array([
[
1286.540148375528,
0.0,
929.5596785987797
],
[
0.0,
1272.8889372475517,
586.0340979684613
],
[
0.0,
0.0,
1.0
]
])
cameraMatRight = np.array([
[
1294.8590822926074,
0.0,
976.7466553094133
],
[
0.0,
1283.5006893318534,
520.6437123281569
],
[
0.0,
0.0,
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)
我做错了什么吗?请帮我解决一下,非常感谢您的任何帮助。