Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

skewed pointcloud from depth image #4655

Open
Astik-2002 opened this issue Aug 27, 2024 · 3 comments
Open

skewed pointcloud from depth image #4655

Astik-2002 opened this issue Aug 27, 2024 · 3 comments

Comments

@Astik-2002
Copy link

Astik-2002 commented Aug 27, 2024

Hello. I'm trying to generate a point cloud from depth image generated in gym_pybullet_drone simulation. The generated point cloud is skewed, and so far I've been unable to fix it. I'm calculating the intrinsic parameters of the camera and generating the pcd via this code

def _pcd_generation(self,depth_image):

        if depth_image.dtype != np.float32:
            depth_image = depth_image.astype(np.float32)
        
        depth_image[depth_image == 1.0] = 0.0
        
        width=self.VID_WIDTH
        height=self.VID_HEIGHT
        aspect = width/height
        fov = 90    
        
        fx = width / (2 * np.tan(np.radians(fov / 2)))
        fy = height / (2 * np.tan(np.radians(fov / 2)))

        print(fx, fy)

        intrinsic = o3d.camera.PinholeCameraIntrinsic(
            width=width,
            height=height,
            fx=fx, fy=fy,
            cx=self.VID_WIDTH/2,
            cy=self.VID_HEIGHT/2
        )
        # Extract drone state
        drone_state = self._getDroneStateVector(0)
        drone_position = drone_state[:3]  # The first three elements are the position
        drone_orientation_quat = drone_state[3:7]  # The next four elements are the quaternion

        # Convert quaternion to rotation matrix
        rotation_matrix = R.from_quat(drone_orientation_quat).as_matrix()
        axis_adjustment = np.array([[0, -1, 0, 0],
                                [0, 0, -1, 0],
                                [1, 0, 0, 0],
                                [0, 0, 0, 1]])
        

        # Create the extrinsic transformation matrix
        extrinsic = np.eye(4)
        extrinsic[:3, :3] = rotation_matrix
        extrinsic[:3, 3] = drone_position
        extrinsic = axis_adjustment @ extrinsic

        depth_o3d = o3d.geometry.Image(depth_image)
        pcd = o3d.geometry.PointCloud.create_from_depth_image(depth_o3d, intrinsic, extrinsic)
        return pcd

I have manually set the fov = 90 in p.computeProjectionMatrixFOV functions. the generated pcd looks like this (video in zipped folder)
Uploading pcd.zip…
Screenshot from 2024-08-27 15-41-51
I'll be glad for any help

I'm using this simulator https://github.com/utiasDSL/gym-pybullet-drones which is based on pybullet

@codepk37
Copy link

I am facing same issue, have you solved it.

@Astik-2002
Copy link
Author

Astik-2002 commented Oct 23, 2024

I am facing same issue, have you solved it.

Yes. The depth values are normalised in the image. You have to de-normalize them and then convert them to pcd using correct intrinsic parameters

The new depth values are

depth_mm = (2*nearVal*farVal)/(farVal + nearVal - depth_image*(farVal-nearVal))

Here depth_image is a numpy array

@codepk37
Copy link

Thanks for reply, I was denormalizing it.
#1924 helped me

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants