**Remark:** The coordinate system is right-handed, the cameras look into *y*-direction (rather then *z*).
I got a set of fixed, calibrated cameras which I want to use to get a disparity map of the scene. To do so I want to rectify the given images for a pair of cameras. However, the resulting rectified image is black and the roi`s returned by **stereoRectify** are either *(0,0,0,0)* or some very strange and small region of the image.
I suspect that my cameras are too far apart from each other and that their orientation differs too much. Is this indeed a problem or might there be some other issue?
My current workflow is as follows:
1. Gather 2d-positions on the images of known 3d points
2. **stereoCalibrate** the camera pair, retrieve their relative rotation (**R**) and translation (**T**)
3. Use **R** and **T** for **stereoRectify**
4. undistort the '*left* and *right* image with **initUndistortRectifyMap** and **remap**
The full code to reproduce the result is posted below.
#%matplotlib inline
import matplotlib.pyplot as plt
import cv2
import numpy as np
# ============================
# Cameras
# ============================
Cam1 = {
'pos': np.array(
[72.5, 381.4, 43.3]),
'K': np.array([
[-3765.698429142333, 0.0000000000002, 1240.0306479725434],\
[0, -3765.6984291423264, 887.4632405702351],\
[0, 0, 1]]).astype('float'),
'R': np.array([
[0.9999370140766937, -0.011183065596097623, 0.0009523251859989448],\
[0.001403670192465846, 0.04042146114315272, -0.999181732813928],\
[0.011135420484979138, 0.9991201351804128, 0.04043461249593852]
]).T
}
Cam2 = {
'pos': np.array(
[315, 325, 50]),
'K': np.array([
[-3680, 0.000000000006, 1172],\
[0, -3680, 844],\
[0, 0, 1]]).astype('float'),
'R': np.array([
[-0.016444826412680857, 0.7399455721809343, -0.6724657001617901],\
[0.034691990397870555, -0.6717294370584418, -0.7399838033304401],\
[-0.9992627449707563, -0.03549807880687472, -0.014623710696333836]]
).T
}
W = 2560
H = 1920
Size = (W,H)
# ============================
# Data
# ============================
calib_A = np.array([20.0, 90.0, 50.0]) # Light-Green
calib_B = np.array([130.0, 90.0, 50.0]) # White
calib_C = np.array([ # Red
(10.0, 90.0, 10.0),
(75.0, 90.0, 10.0),
(140.0, 90.0, 10.0),
(140.0, 90.0, 90.0),
(75.0, 90.0, 90.0),
(10.0, 90.0, 90.0)
])
calib_D = np.array([ # Green
(20.0, 16.0, 20.0),
(75.0, 16.0, 20.0),
(130.0, 16.0, 20.0),
(130.0, 16.0, 80.0),
(70.0, 16.0, 80.0),
(20.0, 16.0, 80.0)
])
points3d = [calib_A, calib_B]
points3d.extend(calib_C)
points3d.extend(calib_D)
# ============================
# Various helper functions
# ============================
def img(Cam, points3d):
"""
Creates the 'photo' taken from a camera
"""
points2d = project_3d_to_2d(Cam, points3d)
I = np.zeros((H,W,3), "int8")
for i, p in enumerate(points2d):
color = (0, 50, 0)
if i == 1:
color = (255, 255, 255)
elif i > 1 and i < 8:
color = (255, 0, 0)
elif i >= 8:
color = (0, 255, 0)
center = (int(p[0]), int(p[1]))
cv2.circle(I, center, 32, color, -1)
return I
def project_3d_to_2d(Cam, points3d):
"""
This is a 'dummy' function to create the image for
the rectification/stereo-calibration.
"""
R = Cam['R']
pos = Cam['pos']
K = Cam['K'].astype('float32')
# pos to tvec
tvec = np.expand_dims(
np.linalg.inv(-np.transpose(R)) @ pos,
axis=1
).astype('float64')
# rot to rvec
rvec = cv2.Rodrigues(R)[0].astype('float64')
points2d, _ = cv2.projectPoints(
np.array(points3d), rvec, tvec, K, 0)
return np.ndarray.squeeze(points2d).astype('float32')
points2d = project_3d_to_2d(Cam2, points3d)
# ============================
# Step-by-step execution
# ============================
# (1) get 2d points and images of Cameras with 3d-setup
points2d_1, points2d_2 = project_3d_to_2d(Cam1, points3d), \
project_3d_to_2d(Cam2, points3d)
img1, img2 = img(Cam1, points3d), img(Cam2, points3d)
K_1, K_2 = Cam1['K'], Cam2['K']
# (2) stereo-calibrate the cameras
flags = cv2.CALIB_FIX_INTRINSIC
points2d_1 = np.expand_dims(points2d_1, axis=0)
points2d_2 = np.expand_dims(points2d_1, axis=0)
retval, K_1, dist1, K_2, dist2, R, T, E, F = cv2.stereoCalibrate(
np.array([points3d]).astype('float32'),
points2d_1, points2d_2,
K_1, 0, K_2, 0,
Size,
flags=flags
)
# (3) stereo-rectify
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(
K_1, dist1, K_2, dist2, Size, R, T)
print("roi1: " + str(roi1)) # this yields (0,0,0,0)
print("roi2: " + str(roi2)) # this yields (0,0,0,0)
# (4) undistort images
mapx1, mapy1 = cv2.initUndistortRectifyMap(
K_1, dist1, R1, P1, Size, cv2.CV_32FC1
)
img1_r = cv2.remap(img1, mapx1, mapy1, cv2.INTER_NEAREST)
mapx2, mapy2 = cv2.initUndistortRectifyMap(
K_2, dist2, R2, P2, Size, cv2.CV_32FC1
)
img2_r = cv2.remap(img2, mapx2, mapy2, cv2.INTER_NEAREST)
# ============================
# Plot
# ============================
fig = plt.figure(figsize=(16,12))
ax = fig.add_subplot(221)
ax.set_title("Cam1")
ax.imshow(img1)
ax = fig.add_subplot(222)
ax.set_title("Cam2")
ax.imshow(img2)
ax = fig.add_subplot(223)
ax.set_title("Cam1 rectified")
ax.imshow(img1_r)
ax = fig.add_subplot(224)
ax.set_title("Cam2 rectified")
ax.imshow(img2_r)
plt.show()
↧