Revision history [back]

This is a recap for this question.

I have a camera setup where two cameras are positioned in very different orientations. I managed to rectify the images but the content is transformed out of the image. I believe that the reason for this is that the principal points of the rectified image are set to quite awkward positions after rectification.

I got several questions now:

• Is it save to change the principle points of either projection matrix to center the content of the rectified image?
• How do I get the full projection matrix from world coordinates to rectified image for either camera given:
  • Original rotation matrix of Camera
  • Rectified rotation matrix of Camera (calculated by stereoRectifiy)
  • Intrinsic parameters of Camera
  • Translation vector in Camera Coordinates
  • rectified Projection matrix (calculated by stereoRectifiy)
• Is there a best-practice of how to deal with Camera setups with very different camera orientations?

Code:

import matplotlib.pyplot as plt
import cv2
import numpy as np

# =============================================
# Helper Functions
#     not relevant to understand the problem
# =============================================

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')

def img(Cam, points3d):
    """
    Creates the 'photo' taken from a camera
    """
    W = 2560
    H = 1920
    Size = (W,H)

    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

# =============================================
# Cameras
# =============================================

Cam1 = {
    'pos': np.array(
        [72.5607048220662, 381.44099049969805, 43.382114809969224]),
    'K': np.array([
        [-3765.698429142333, 0.0000000000002, 1240.0306479725434],\
        [0, -3765.6984291423264, 887.4632405702351],\
        [0, 0, 1]]),
    '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.5827337916895, 325.6710837143909, 50.172023537483994]),
    'K': np.array([
        [-3680.6894379194728, 0.000000000006, 1172.8022801685916],\
        [0, -3680.689437919353, 844.1378056931399],\
        [0, 0, 1]]),
    'R': np.array([
        [-0.016444826412680857, 0.7399455721809343, -0.6724657001617901],\
        [0.034691990397870555, -0.6717294370584418, -0.7399838033304401],\
        [-0.9992627449707563, -0.03549807880687472, -0.014623710696333836]]).T
}

# =============================================
# Test rig
# =============================================

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)

points2d_Cam1 = project_3d_to_2d(Cam1, points3d)
points2d_Cam2 = project_3d_to_2d(Cam2, points3d)

# =============================================
# Actual rectification
# =============================================

img1 = img(Cam1, points3d)
img2 = img(Cam2, points3d)
assert(img1.shape == img2.shape)
Size = (img1.shape[1], img1.shape[0])

A1 = Cam1['K'].astype('float')
A2 = Cam2['K'].astype('float')

flags = cv2.CALIB_FIX_INTRINSIC
rms, A1, dist1, A2, dist2, R, T, E, F = cv2.stereoCalibrate(
    np.array([points3d]).astype('float32'),
    np.array([points2d_Cam1]).astype('float32'),
    np.array([points2d_Cam2]).astype('float32'),
    A1,
    0,
    A2,
    0,
    Size,
    flags=flags
)
print("rms:" + str(rms))

R1 = Cam1['R']
R2 = Cam2['R']

alpha = -1
SizeBg = (int(Size[0]), int(Size[1]))

flags = cv2.CALIB_ZERO_DISPARITY
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(
    A1, dist1, A2, dist2, SizeBg, R, T, alpha=alpha, flags=flags)

map1, map2 = cv2.initUndistortRectifyMap(
    A1, dist1, R1, P1, SizeBg, cv2.CV_32FC1)

I1 = cv2.remap(img1.astype('float32'), map1, map2, cv2.INTER_AREA)


map1, map2 = cv2.initUndistortRectifyMap(
    A2, dist1, R2, P2, SizeBg, cv2.CV_32FC1)

I2 = cv2.remap(img2.astype('float32'), map1, map2, cv2.INTER_AREA)    

# =============================================
# plotting
# =============================================

fig = plt.figure(figsize=(16,16))
ax = fig.add_subplot(221)
ax.set_title("Cam1")
ax.imshow(img1)

ay = fig.add_subplot(222)
ay.set_title("Cam2")
ay.imshow(img2)

az1 = fig.add_subplot(223)
az1.set_title("Cam1 (rectified)")
az1.imshow(I1)

az2 = fig.add_subplot(224)
az2.set_title("Cam2 (rectified)")
az2.imshow(I2)

plt.show()