1+ from pyk4a import PyK4A
2+ from matplotlib import pyplot as plt
3+ from pyk4a .calibration import CalibrationType
4+ import open3d as o3d
5+ import numpy as np
6+ def main ():
7+ # Capture a depth image and an rgb image from the camera.
8+
9+ # Load camera with the default config
10+ k4a = PyK4A (
11+ device_id = 1 ,
12+ )
13+ k4a .start ()
14+
15+ # Get the next capture (blocking function)
16+ capture = k4a .get_capture ()
17+ img_color = capture .color
18+ img_depth = capture .depth
19+ capture_pts = capture .depth_point_cloud
20+
21+ # # Display with pyplot
22+ # plt.imshow(img_color[:, :, 2::-1]) # BGRA to RGB
23+ # plt.show()
24+
25+ # get the transform from the rgb frame to the depth frame.
26+ R , t = k4a .calibration .get_extrinsic_parameters (CalibrationType .DEPTH , CalibrationType .COLOR )
27+ print (R , t )
28+
29+ # get the intrinsics from the depth camera.
30+ K = k4a .calibration .get_camera_matrix (CalibrationType .DEPTH )
31+ print (K )
32+
33+ # Backproject the depth image into 3D space.
34+ # This is a 2D array of 3D points.
35+ # Do it manually, without calling the k4a api.
36+ # Do it vectorized.
37+ # Do it with numpy.
38+
39+ # Get a numpy meshgrid of the pixel coordinates.
40+ # This is a 2D array of 2D points.
41+ xx , yy = np .meshgrid (np .arange (img_depth .shape [1 ]), np .arange (img_depth .shape [0 ]))
42+
43+ # Reshape the meshgrid into a 2D array of 2D points.
44+ # This is a 2D array of 2D points.
45+ xx = xx .reshape (- 1 )
46+ yy = yy .reshape (- 1 )
47+
48+ # Get the depth values.
49+ # This is a 1D array of depth values.
50+ zz = img_depth .reshape (- 1 )
51+
52+ # Get the camera intrinsics.
53+ # This is a 2D array of camera intrinsics.
54+ KK = np .array (K ).reshape (3 , 3 )
55+
56+ # Get the inverse of the camera intrinsics.
57+ # This is a 2D array of camera intrinsics.
58+ KK_inv = np .linalg .inv (KK )
59+
60+ # Get the 2D points in the camera frame.
61+ # This is a 2D array of 2D points.
62+ points_2d = np .stack ([xx , yy , np .ones_like (xx )], axis = 0 )
63+
64+ # Get the 3D points in the camera frame.
65+ # This is a 2D array of 3D points.
66+ points_3d = KK_inv @ points_2d
67+
68+ # Scale the 3D points by the depth values.
69+ # This is a 2D array of 3D points.
70+ points_3d = points_3d * zz
71+
72+ # breakpoint()
73+
74+
75+
76+ # # Visualize the 3D point cloud.
77+ # pcd2 = o3d.geometry.PointCloud()
78+ # pcd2.points = o3d.utility.Vector3dVector(points_3d.T)
79+ # pcd2.paint_uniform_color([1, 0.706, 0])
80+ # # o3d.visualization.draw_geometries([pcd3])
81+
82+ # pcd = o3d.geometry.PointCloud()
83+ # pcd.points = o3d.utility.Vector3dVector(capture_pts.reshape(-1, 3))
84+ # pcd.paint_uniform_color([0, 0.651, 0.929])
85+ # o3d.visualization.draw_geometries([pcd, pcd2])
86+
87+ # Visualize color to depth points
88+ color_3d = capture .transformed_color
89+ color = capture .color
90+ plt .subplot (1 , 2 , 1 )
91+ plt .imshow (color [:, :, 2 ::- 1 ])
92+ plt .subplot (1 , 2 , 2 )
93+ plt .imshow (color_3d [:, :, 2 ::- 1 ])
94+ plt .show ()
95+
96+ # color_to_depth = k4a.calibration.color_to_depth_3d()
97+ # pcd3 = o3d.geometry.PointCloud()
98+ # pcd3.points = o3d.utility.Vector3dVector(color_to_depth @ color_3d.T)
99+
100+
101+
102+
103+
104+
105+ if __name__ == "__main__" :
106+ main ()
0 commit comments