bulletphysics / bullet3

Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc.
http://bulletphysics.org
Other
12.6k stars 2.87k forks source link

skewed pointcloud from depth image #4655

Open Astik-2002 opened 2 months ago

Astik-2002 commented 2 months ago

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 commented 1 week ago

I am facing same issue, have you solved it.

Astik-2002 commented 1 week ago

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 commented 6 days ago

Thanks for reply, I was denormalizing it. https://github.com/bulletphysics/bullet3/issues/1924 helped me