Color Mismatch and Depth Inconsistency in Point Cloud to RGB Image Conversion Using Open3D

Question

I am working with Open3D to convert point cloud data into an RGB image. However, I'm encountering issues where the colors in the output image do not match those in the input point cloud. Additionally, points that are supposed to be in the background are appearing in the foreground. Here is the relevant part of my code:

import open3d as o3d
import numpy as np
import cv2

class Projector:
    def __init__(self, cloud) -> None:
        self.cloud = cloud
        self.points = np.asarray(cloud.points)
        self.colors = np.asarray(cloud.colors)
        self.n = len(self.points)

    def project_to_rgbd(self,
                        width,
                        height,
                        intrinsic,
                        extrinsic,
                        depth_scale,
                        depth_max):
        depth = np.zeros((height, width, 1), dtype=np.uint16)
        color = np.zeros((height, width, 3), dtype=np.uint8)

        for i in range(0, self.n):
            point4d = np.append(self.points[i], 1)
            new_point4d = np.matmul(extrinsic, point4d)
            point3d = new_point4d[:-1]
            zc = point3d[2]
            new_point3d = np.matmul(intrinsic, point3d)
            new_point3d = new_point3d / new_point3d[2]
            u = int(round(new_point3d[0]))
            v = int(round(new_point3d[1]))

            if u < 0 or u > width - 1 or v < 0 or v > height - 1 or zc <= 0 or zc > depth_max:
                continue

            d = zc * depth_scale
            if depth[v, u] == 0 or depth[v, u] > d:
                depth[v, u] = d
                color[v, u, :] = (self.colors[i] * 255).astype(np.uint8)

        im_color = o3d.geometry.Image(color)
        im_depth = o3d.geometry.Image(depth)
        rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
            im_color, im_depth, depth_scale=1000.0, depth_trunc=depth_max, convert_rgb_to_intensity=False)
        return rgbd

pcd_path = "/home/shirono/Poe/data/1944301031BF721300_1721279605_z20.pcd"
pcd = o3d.io.read_point_cloud(pcd_path)

scene = o3d.visualization.Visualizer()
scene.create_window()
scene.add_geometry(pcd)
scene.update_renderer()
scene.poll_events()
scene.run()
view_control = scene.get_view_control()
cam = view_control.convert_to_pinhole_camera_parameters()
scene.destroy_window()

p = Projector(pcd)
rgbd = p.project_to_rgbd(cam.intrinsic.width,
                         cam.intrinsic.height,
                         cam.intrinsic.intrinsic_matrix,
                         cam.extrinsic,
                         1000,
                         10)

def save_rgb_from_rgbd(rgbd_image, save_path):
    rgb_image = np.asarray(rgbd_image.color)
    rgb_image = (rgb_image * 255).astype(np.uint8)
    rgb_image = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR)
    cv2.imwrite(save_path, rgb_image)

save_path = "output_rgb_image.png"
save_rgb_from_rgbd(rgbd, save_path)

We thought the conversion from RGB to BGR was wrong and tried but could not improve it.