D415 Pointcloud Rendering extremely slow

Hi,

I am working with both Panda3D and a D415 depth camera, so I'm not totally sure who to ask. I am capturing and rendering a pointcloud from the depth camera onto a 3D grid made in Panda3D. However, during the run time, the pointcloud is extremely slow to render and update if the depth camera moves -- and it is also interfering with moving the Panda3D camera object itself (its controls are also extremely slow to update). Is there something I am doing wrong when getting the frames?

frames = self.pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()

if not depth_frame or not color_frame:
     return task.cont

# Calculate point cloud
self.pc.map_to(color_frame)

points = self.pc.calculate(depth_frame)
vtx = np.asanyarray(points.get_vertices())
tex_coords = np.asanyarray(points.get_texture_coordinates())

# Convert color data for P3D compatibility
color = np.asanyarray(color_frame.get_data())

# Clear previous vertices and colors
self.vdata.unclean_set_num_rows(0)

# Adding vertices and colors to the vertex data structure
vertex = GeomVertexWriter(self.vdata, 'vertex')
color_writer = GeomVertexWriter(self.vdata, 'color')
scale_factor = 30.0

for i in range(len(vtx)):
     # Scale the vertices
     vertex.addData3(vtx[i][0] * scale_factor, vtx[i][1] * scale_factor, vtx[i][2] * scale_factor)

     u, v = tex_coords[i][0], tex_coords[i][1]

     # Need min to not go out of bounds
     u = min(u * color.shape[1], color.shape[1] - 1)
     v = min(v * color.shape[0], color.shape[0] - 1)

     color_value = color[int(v), int(u)]

     # Format: (color_value[red], color_value[green], color_value[blue], A)
     color_writer.addData4(color_value[0]/255.0, color_value[1]/255.0, color_value[2]/255.0, 1.0)

# Update the primitive with the new number of vertices
self.points_prim.clearVertices()
self.points_prim.addConsecutiveVertices(0, len(vtx))

self.nodePath.set_hpr(0, -70, 0)

D415 Pointcloud Rendering extremely slow

Comments

Didn't find what you were looking for?