Alignment of depth and color image on an oak-d camera

Question

I'm trying to create an RGB-D dataset and I'm having some troubles in aligning the color and depth images from the oak-d camera. The depth image is aligned with the right camera of the stereo rig and the objective is to align it with the color image, which the camera is central to the stereo rig. For that, I would like to use the calibration parameters. I have the intrinsic matrices (K1, K2) for both cameras and the matrix (D) that transforms from one camera referential to another. D is a 4x4 homogeneous matrix that contains the rotation and translation matrices between the two cameras. I already performed the calibration using only the rotation matrix, using the formula: H = K2 * R_2_1 * inv(K1). Although I'm having problems in considering the translation matrix too, which I find extremely necessary in my case. Could anyone provide me the formula for the final transformation matrix, or some other method so I can perform the alignment between the color and depth image, having in account both the rotation and translation matrices between the two cameras?

Here's the code that I used to perform the alignment:

import cv2
import numpy as np
from google.colab.patches import cv2_imshow

# Intrinsic parameters of RGB camera
K2 = np.array([[553.0378462082966, 0, 355.449626693058],
              [0, 555.6255112161116, 197.16318449210289],
              [0, 0, 1]])

# Intrinsic parameters of depth camera
K1 = np.array([[426.6990467952769, 0, 319.5763569688426],
              [0, 428.2569124831735, 203.66385041327385],
              [0, 0, 1]])

# Distortion coefficients of RGB camera
distortion_coeffs = np.array([-0.01015686833772626, 0.018782266649409993, 0.0015606228293145892, -0.00019459459264923275])

#-0.07501121652962993/2
# Homogeneous transformation matrix
D = np.array([[0.9999971496369501, 0.00012642174287522421, -0.0023842683365914922, -0.07501121652962993/2],
            [-0.00012887613805504113, 0.9999994619796904, -0.0010292867693778803, -4.5329260960026137e-05],
            [0.002384136929579421, 0.0010295911108322208, 0.9999966279109286, -0.0005594943499687595],
            [0, 0, 0, 1]])

# Load depth and color images
depth = cv2.imread("/content/drive/MyDrive/RGBD_images/img_00001_dep.png", cv2.IMREAD_GRAYSCALE)
color = cv2.imread("/content/drive/MyDrive/RGBD_images/img_00001_rgb.jpg")

# Undistort the color image
height, width = color.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(K2, distortion_coeffs, (width, height), 0, (width, height))
color_undistorted = cv2.undistort(color, K2, distortion_coeffs, None, newcameramtx)

# Resize color image to the same size as depth image
resized_depth = cv2.resize(depth, (color_undistorted.shape[1], color_undistorted.shape[0]))

# Compute the transformation matrix H
R = D[:3, :3]
t = D[:3, 3].reshape(-1, 1)
H = np.dot(K2, np.dot(R, np.linalg.inv(K1)))

# Warp the depth image to align with the color image
aligned_depth = cv2.warpPerspective(resized_depth, H, (color.shape[1], color.shape[0]))

# Step 5: Project the transformed depth image onto the color image
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(aligned_depth, alpha=0.03), cv2.COLORMAP_JET)
aligned_image = cv2.addWeighted(color_undistorted, 0.5, depth_colormap, 0.5, 0)


# Display the aligned depth and color images
cv2_imshow(color_undistorted)
cv2_imshow(resized_depth)
cv2_imshow(aligned_image)

# Save the image
cv2.imwrite("/content/drive/MyDrive/selected_RGBD_images/aligned_depth_0000.jpg", aligned_image)