Distortion in Point Cloud from RGBD images

[DaniCarias]

Hello, I'm trying to create the ground truth (in Point Cloud) of the Carla map from RGBD images using Open3d. I needed 3 cameras to get the 360º because Carla has this issue #612 in FOVs bigger than 180º. I'm getting three Point Clouds from three RGBD images with FOV of 120º each (120+120+120 = 360), with different rotations (steps of 120º), and then I concatenate the three Point Clouds to create a 360º Point Cloud. First I eliminate the distortion in the RGB and Depth images, putting 0 in "lens_k", and 0 in "lens_kcube", based on issue, so the output image does not have any distortion but at the same time have the distortion close the sensor source.

One Point Cloud from one RGBD:

Distortion on the curve to the camera source:

Distortion on the curve (up) to the camera source:

Distortion on the curve (up) to the camera source, in 360º Point Cloud:

I would appreciate some help resolving this distortion to create a method to get the ground truth.

[DaniCarias]

Hi @MarcelPiNacy , Yes, I spawn RGB and Depth cameras in the same spot with rotation, the code:

# Depth & RGB - 1
        camera_transform = carla.Transform(carla.Location(x=0.9, z=2.5))
        camera_depth1 = spawn_cameras('sensor.camera.depth', world, blueprint_library, vehicle, IMG_WIDTH, IMG_HEIGHT, camera_transform)
        camera_rgb1 = spawn_cameras('sensor.camera.rgb', world, blueprint_library, vehicle, IMG_WIDTH, IMG_HEIGHT, camera_transform)
        actor_list.append([camera_depth1, camera_rgb1])
        print(f"Camera Depth1: {camera_depth1} | Camera RGB1: {camera_rgb1}")

# Depth & RGB - 2
        rotation120 = carla.Rotation(yaw=120.0)
        camera_transform = carla.Transform(carla.Location(x=0.9, z=2.5), rotation120)
        camera_depth2 = spawn_cameras('sensor.camera.depth', world, blueprint_library, vehicle, IMG_WIDTH, IMG_HEIGHT, camera_transform)
        camera_rgb2 = spawn_cameras('sensor.camera.rgb', world, blueprint_library, vehicle, IMG_WIDTH, IMG_HEIGHT, camera_transform)
        actor_list.append([camera_depth2, camera_rgb2])
        print(f"Camera Depth2: {camera_depth2} | Camera RGB2: {camera_rgb2}")

# Depth & RGB - 3
        rotation240 = carla.Rotation(yaw=240.0)
        camera_transform = carla.Transform(carla.Location(x=0.9, z=2.5), rotation240)
        camera_depth3 = spawn_cameras('sensor.camera.depth', world, blueprint_library, vehicle, IMG_WIDTH, IMG_HEIGHT, camera_transform)
        camera_rgb3 = spawn_cameras('sensor.camera.rgb', world, blueprint_library, vehicle, IMG_WIDTH, IMG_HEIGHT, camera_transform)
        actor_list.append([camera_depth3, camera_rgb3])
        print(f"Camera Depth3: {camera_depth3} | Camera RGB2: {camera_rgb3}")

The spawn_cameras function:

def spawn_cameras(camera, world, blueprint_library, vehicle, img_width, img_height, camera_transform):
    camera_bp = blueprint_library.find(camera)

    camera_bp.set_attribute('image_size_x', f"{img_width}")
    camera_bp.set_attribute('image_size_y', f"{img_height}")

    camera_bp.set_attribute('fov', f"{120}")
    camera_bp.set_attribute('lens_k', f"{0}")               # Remove distortion
    camera_bp.set_attribute('lens_kcube', f"{0}")           # Remove distortion
    camera_bp.set_attribute('lens_circle_falloff', f"{0}")

    camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle)
    return camera

Thanks for your attention, Daniel

[DaniCarias]

I forgot to mention that the intrinsic matrix is based on this issue for transforming into a point cloud.

[DaniCarias]

Hi, sorry for the delay but thanks, I'll give you feedback on it.

[DaniCarias]

Hi, Are these Perspective Matrixes, from the link of UE, intrinsic matrixes?

Can you tell me where the documentation for the "PerspectiveMatrix.h" and their params is?

Because I'm using the create_point_cloud_from_rgbd_image method from the Open3d library and only accept the intrinsic camera matrix like this: Like I'm using from this issue:

focal_length = imgSizeX / (2 * mt.tan(fov * mt.pi / 360))
centerX = imgSizeX / 2
centerY = imgSizeY / 2

intrinsic_matrix = [[focal_length, 0, centerX],
                                  [0, focal_length, centerY],
                                  [0, 0, 1]]

Using this way I get the point cloud right but only with distortion on the center (source of the RGBD image, where the camera is). So I think the intrinsic matrix I'm using is right... But I don't understand why the PCL has the distortion only in the center.

[PabloVD]

Hello @DaniCarias , Are you using the same values for img_width and img_height? In your last response you made use of focal_length for fx and fy, which should be different if the width and the height have different values.

[DaniCarias]

Hi @PabloVD, Thanks too for helping me... yes you are right, I'm using different values for img_width (1280 px) and img_height (720 px)...

I just changed the code to:

focal_lengthX = imgSizeX / (2 * mt.tan(fov * mt.pi / 360))
focal_lengthY = imgSizeY / (2 * mt.tan(fov * mt.pi / 360))
centerX = imgSizeX / 2
centerY = imgSizeY / 2

intrinsic_matrix = [[focal_lengthX, 0, centerX],
                    [0, focal_lengthY, centerY],
                    [0, 0, 1]]

But I get the same point clouds with the same distortion.

[PabloVD]

I'm not sure what the open3d routine is doing under the hood, but (as is also stated here) you could convert a 2D point p2d = [u,v,1] to the 3d world position asP = ( inv(K) * p2d ) * depth, where K is the intrinsic matrix. Have you tried directly doing that to ensure that it is not a problem on how the open3d routine deals with the intrinsic matrix?

[DaniCarias]

Yes I don't know what open3d routine is doing under the hood too, but I remade the code based on that issue. If I made any mistake please tell me.

# Intrinsics of the camera
focal_lengthX = imgSizeX / (2 * mt.tan(fov * mt.pi / 360))
focal_lengthY = imgSizeY / (2 * mt.tan(fov * mt.pi / 360))
centerX = imgSizeX / 2
centerY = imgSizeY / 2

intrinsic_matrix = [[focal_lengthX, 0, centerX],
                    [0, focal_lengthY, centerY],
                    [0, 0, 1]]

intrinsic_matrix_inv = np.linalg.inv(intrinsic_matrix)

depth_img = cv2.imread(depth_path_0, cv2.IMREAD_UNCHANGED)

points_3d = []

for v in range(imgSizeY):
    for u in range(imgSizeX):
        # Depth of the point
        depth = depth_img[v, u]

        p2d = np.array([u, v, 1])

        # Get the 3D point
        P = np.dot(intrinsic_matrix_inv, p2d) * depth
        points_3d.append(P)

# Create a point cloud from array
point_cloud = o3d.geometry.PointCloud()
point_cloud.points = o3d.utility.Vector3dVector(np.array(points_3d))

# Visualize
o3d.visualization.draw_geometries([point_cloud])

But I get the same distortion:

So I assume that the problem is not the open3d method.

[DaniCarias]

I tried with a FOV of 60º with 6 RGBD images to make the 360º and I got this result:

The distortion is less but exists... My goal is to get the ground truth of the map, is someone with some idea how to get it in another way?