Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def _read_depth_image(self):
""" Reads a depth image from the device """
# read raw uint16 buffer
im_arr = self._depth_stream.read_frame()
raw_buf = im_arr.get_buffer_as_uint16()
buf_array = np.array([raw_buf[i] for i in range(PrimesenseSensor.DEPTH_IM_WIDTH * PrimesenseSensor.DEPTH_IM_HEIGHT)])
# convert to image in meters
depth_image = buf_array.reshape(PrimesenseSensor.DEPTH_IM_HEIGHT,
PrimesenseSensor.DEPTH_IM_WIDTH)
depth_image = depth_image * MM_TO_METERS # convert to meters
if self._flip_images:
depth_image = np.flipud(depth_image)
else:
depth_image = np.fliplr(depth_image)
return DepthImage(depth_image, frame=self._frame)
def start(self):
""" Start the sensor """
# open device
openni2.initialize(PrimesenseSensor.OPENNI2_PATH)
self._device = openni2.Device.open_any()
# open depth stream
self._depth_stream = self._device.create_depth_stream()
self._depth_stream.configure_mode(PrimesenseSensor.DEPTH_IM_WIDTH,
PrimesenseSensor.DEPTH_IM_HEIGHT,
PrimesenseSensor.FPS,
openni2.PIXEL_FORMAT_DEPTH_1_MM)
self._depth_stream.start()
# open color stream
self._color_stream = self._device.create_color_stream()
self._color_stream.configure_mode(PrimesenseSensor.COLOR_IM_WIDTH,
PrimesenseSensor.COLOR_IM_HEIGHT,
PrimesenseSensor.FPS,
openni2.PIXEL_FORMAT_RGB888)
def color_intrinsics(self):
""":obj:`CameraIntrinsics` : The camera intrinsics for the primesense color camera.
"""
return CameraIntrinsics(self._ir_frame, PrimesenseSensor.FOCAL_X, PrimesenseSensor.FOCAL_Y,
PrimesenseSensor.CENTER_X, PrimesenseSensor.CENTER_Y,
height=PrimesenseSensor.DEPTH_IM_HEIGHT,
width=PrimesenseSensor.DEPTH_IM_WIDTH)
def ir_intrinsics(self):
""":obj:`CameraIntrinsics` : The camera intrinsics for the primesense IR camera.
"""
return CameraIntrinsics(self._ir_frame, PrimesenseSensor.FOCAL_X, PrimesenseSensor.FOCAL_Y,
PrimesenseSensor.CENTER_X, PrimesenseSensor.CENTER_Y,
height=PrimesenseSensor.DEPTH_IM_HEIGHT,
width=PrimesenseSensor.DEPTH_IM_WIDTH)
def _read_color_image(self):
""" Reads a color image from the device """
# read raw buffer
im_arr = self._color_stream.read_frame()
raw_buf = im_arr.get_buffer_as_triplet()
r_array = np.array([raw_buf[i][0] for i in range(PrimesenseSensor.COLOR_IM_WIDTH * PrimesenseSensor.COLOR_IM_HEIGHT)])
g_array = np.array([raw_buf[i][1] for i in range(PrimesenseSensor.COLOR_IM_WIDTH * PrimesenseSensor.COLOR_IM_HEIGHT)])
b_array = np.array([raw_buf[i][2] for i in range(PrimesenseSensor.COLOR_IM_WIDTH * PrimesenseSensor.COLOR_IM_HEIGHT)])
# convert to uint8 image
color_image = np.zeros([PrimesenseSensor.COLOR_IM_HEIGHT, PrimesenseSensor.COLOR_IM_WIDTH, 3])
color_image[:,:,0] = r_array.reshape(PrimesenseSensor.COLOR_IM_HEIGHT,
PrimesenseSensor.COLOR_IM_WIDTH)
color_image[:,:,1] = g_array.reshape(PrimesenseSensor.COLOR_IM_HEIGHT,
PrimesenseSensor.COLOR_IM_WIDTH)
color_image[:,:,2] = b_array.reshape(PrimesenseSensor.COLOR_IM_HEIGHT,
PrimesenseSensor.COLOR_IM_WIDTH)
if self._flip_images:
color_image = np.flipud(color_image.astype(np.uint8))
else:
color_image = np.fliplr(color_image.astype(np.uint8))
return ColorImage(color_image, frame=self._frame)
def ir_intrinsics(self):
""":obj:`CameraIntrinsics` : The camera intrinsics for the primesense IR camera.
"""
return CameraIntrinsics(self._ir_frame, PrimesenseSensor.FOCAL_X, PrimesenseSensor.FOCAL_Y,
PrimesenseSensor.CENTER_X, PrimesenseSensor.CENTER_Y,
height=PrimesenseSensor.DEPTH_IM_HEIGHT,
width=PrimesenseSensor.DEPTH_IM_WIDTH)
openni2.initialize(PrimesenseSensor.OPENNI2_PATH)
self._device = openni2.Device.open_any()
# open depth stream
self._depth_stream = self._device.create_depth_stream()
self._depth_stream.configure_mode(PrimesenseSensor.DEPTH_IM_WIDTH,
PrimesenseSensor.DEPTH_IM_HEIGHT,
PrimesenseSensor.FPS,
openni2.PIXEL_FORMAT_DEPTH_1_MM)
self._depth_stream.start()
# open color stream
self._color_stream = self._device.create_color_stream()
self._color_stream.configure_mode(PrimesenseSensor.COLOR_IM_WIDTH,
PrimesenseSensor.COLOR_IM_HEIGHT,
PrimesenseSensor.FPS,
openni2.PIXEL_FORMAT_RGB888)
self._color_stream.camera.set_auto_white_balance(self._auto_white_balance)
self._color_stream.camera.set_auto_exposure(self._auto_exposure)
self._color_stream.start()
# configure device
if self._registration_mode == PrimesenseRegistrationMode.DEPTH_TO_COLOR:
self._device.set_image_registration_mode(openni2.IMAGE_REGISTRATION_DEPTH_TO_COLOR)
else:
self._device.set_image_registration_mode(openni2.IMAGE_REGISTRATION_OFF)
self._device.set_depth_color_sync_enabled(self._enable_depth_color_sync)
self._running = True
def _read_color_image(self):
""" Reads a color image from the device """
# read raw buffer
im_arr = self._color_stream.read_frame()
raw_buf = im_arr.get_buffer_as_triplet()
r_array = np.array([raw_buf[i][0] for i in range(PrimesenseSensor.COLOR_IM_WIDTH * PrimesenseSensor.COLOR_IM_HEIGHT)])
g_array = np.array([raw_buf[i][1] for i in range(PrimesenseSensor.COLOR_IM_WIDTH * PrimesenseSensor.COLOR_IM_HEIGHT)])
b_array = np.array([raw_buf[i][2] for i in range(PrimesenseSensor.COLOR_IM_WIDTH * PrimesenseSensor.COLOR_IM_HEIGHT)])
# convert to uint8 image
color_image = np.zeros([PrimesenseSensor.COLOR_IM_HEIGHT, PrimesenseSensor.COLOR_IM_WIDTH, 3])
color_image[:,:,0] = r_array.reshape(PrimesenseSensor.COLOR_IM_HEIGHT,
PrimesenseSensor.COLOR_IM_WIDTH)
color_image[:,:,1] = g_array.reshape(PrimesenseSensor.COLOR_IM_HEIGHT,
PrimesenseSensor.COLOR_IM_WIDTH)
color_image[:,:,2] = b_array.reshape(PrimesenseSensor.COLOR_IM_HEIGHT,
PrimesenseSensor.COLOR_IM_WIDTH)
if self._flip_images:
color_image = np.flipud(color_image.astype(np.uint8))
else:
color_image = np.fliplr(color_image.astype(np.uint8))
return ColorImage(color_image, frame=self._frame)
The number of consecutive frames to process.
Returns
-------
:obj:`DepthImage`
The min DepthImage collected from the frames.
"""
depths = []
for _ in range(num_img):
_, depth, _ = self.frames()
depths.append(depth)
return Image.min_images(depths)
class PrimesenseSensor_ROS(PrimesenseSensor):
""" ROS-based version of Primesense RGBD sensor interface
Requires starting the openni2 ROS driver and the two stream_image_buffer (image_buffer.py)
ros services for depth and color images. By default, the class will look for the depth_image buffer
and color_image buffers under "{frame}/depth/stream_image_buffer" and "{frame}/rgb/stream_image_buffer"
respectively (within the current ROS namespace).
This can be changed by passing in depth_image_buffer, color_image_buffer (which change where the program
looks for the buffer services) and depth_absolute, color_absolute (which changes whether the program prepends
the current ROS namespace).
"""
def __init__(self, depth_image_buffer= None, depth_absolute=False, color_image_buffer=None, color_absolute=False,
flip_images=True, frame=None, staleness_limit=10., timeout=10):
import rospy
from rospy import numpy_msg
from perception.srv import ImageBufferResponse