def get_pose_rpy(self): """Return the current pose as a tuple of location and orientation as [roll, pitch, yaw]""" if self._keep_pose_state: orientation_rpy = [math_utils.degrees_to_radians(v) for v in self._orientation_rpy] return self._location.copy(), orientation_rpy response = self._request_get([b"location", b"orientation_rpy"]) location = response[b"location"] orientation_rpy = response[b"orientation_rpy"] # Convert to radians orientation_rpy = [math_utils.degrees_to_radians(v) for v in orientation_rpy] return location, orientation_rpy
def __init__(self, address="tcp://localhost:22222", image_scale_factor=1.0, max_depth_distance=np.finfo(np.float).max, max_depth_viewing_angle=math_utils.degrees_to_radians(90.), max_request_trials=3, request_timeout=0.5, location_tolerance=1e-3, orientation_tolerance=1e-3, connect=True, connect_wait_time=0.5, connect_timeout=0.5, keep_pose_state=False): super(MeshRendererZMQClient, self).__init__(max_depth_distance, max_depth_viewing_angle) self._serializer = serialization.MsgPackSerializer() self._width = None self._height = None self._image_scale_factor = image_scale_factor self._max_request_trials = max_request_trials self._request_timeout = request_timeout self._connect_wait_time = connect_wait_time self._connect_timeout = connect_timeout self._location_tolerance = location_tolerance self._orientation_tolerance = orientation_tolerance self._request_trials = 0 self._conn = zmq_utils.Connection(address, zmq_utils.zmq.REQ) if keep_pose_state: self._location = np.array([0, 0, 0], dtype=np.float) self._orientation_rpy = np.array([0, 0, 0], dtype=np.float) self._keep_pose_state = keep_pose_state if connect: self.connect()
def __init__(self, max_depth_distance=np.finfo(np.float).max, max_depth_viewing_angle=math_utils.degrees_to_radians(90.), image_scale_factor=1.0): self._max_depth_distance = max_depth_distance self._max_depth_viewing_dot_prod = np.cos(max_depth_viewing_angle) self._image_scale_factor = image_scale_factor self._ray_direction_image = None
def get_orientation_rpy(self): """Return the current orientation in radians as [roll, pitch, yaw]""" if self._keep_pose_state: orientation_rpy = self._orientation_rpy else: response = self._request_get([b"orientation_rpy"]) orientation_rpy = response[b"orientation_rpy"] # Convert to radians orientation_rpy = [math_utils.degrees_to_radians(v) for v in orientation_rpy] roll, pitch, yaw = orientation_rpy if pitch <= -np.pi: pitch += 2 * np.pi elif pitch > np.pi: pitch -= 2 * np.pi orientation_rpy = np.array([roll, pitch, yaw]) return orientation_rpy
def handle_request_msg(self, request_dump, raise_exception=False): try: request = self._serializer.loads(request_dump) request_name = request.get(b"_request", None) response = None if request_name == b"ping": logger.info("Received 'ping' request") response = {b"ping": b"pong"} elif request_name == b"set": logger.debug("Received 'set' request") projection_transform_changed = False view_transform_changed = False for name, value in request.items(): if name.startswith(b"_"): continue if name == b"width": # TODO logger.warn( "Changing of width and height is not implemented yet" ) pass elif name == b"height": # TODO logger.warn( "Changing of width and height is not implemented yet" ) pass elif name == b"horz_fov": horz_fov = value focal_length = camera_utils.fov_to_focal_length( math_utils.degrees_to_radians(horz_fov), self._framebuffer.width) vert_fov = math_utils.radians_to_degrees( camera_utils.focal_length_to_fov( focal_length, self._framebuffer.height)) logger.info( "Setting horizontal FOV: {:.4f}, vertical FOV: {:.4f}" .format(horz_fov, vert_fov)) self._projection_transform.fov = vert_fov projection_transform_changed = True elif name == b"location": world_location = request[b"location"] self._view_transform.location = -np.dot( self._world_to_opengl_mat[:3, :3], world_location) view_transform_changed = True elif name == b"orientation_rpy": # TODO: There should also be a minus sign here self._view_transform.orientation_rpy = request[ b"orientation_rpy"] view_transform_changed = True elif name == b"input_enabled": self._input_enabled = value elif name == b"window_visible": self._window_visible_callbacks(value) elif name == b"window_active": self._window_active_callbacks(value) elif name == b"_request": pass else: logger.error( "Unknown field in 'get' request: {}".format(name)) if projection_transform_changed: self._projection_transform_callbacks( self._projection_transform) if view_transform_changed: self._view_transform_callbacks(self._view_transform) response = {} elif request_name == b"get": logger.debug("Received 'get' request") response = {} for name in request.get(b"names", []): if name == b"width": response[b"width"] = self._framebuffer.width elif name == b"height": response[b"height"] = self._framebuffer.height elif name == b"horz_fov": vert_fov = self._projection_transform.fov focal_length = camera_utils.fov_to_focal_length( math_utils.degrees_to_radians(vert_fov), self._framebuffer.height) horz_fov = math_utils.radians_to_degrees( camera_utils.focal_length_to_fov( focal_length, self._framebuffer.width)) response[b"horz_fov"] = horz_fov elif name == b"location": # Minus sign because we move the world and not the camera world_location = -np.dot( self._opengl_to_world_mat[:3, :3], self._view_transform.location) response[b"location"] = world_location elif name == b"orientation_rpy": # TODO: There should also be a minus sign here response[ b"orientation_rpy"] = self._view_transform.orientation_rpy elif request_name == b"render_images": logger.debug("Received 'render_images' request") response = {} requested_images = [ b"rgb_image", b"depth_image", b"normal_image" ] projection_transform_changed = False view_transform_changed = False tmp_projection_transform = self._projection_transform tmp_view_transform = self._view_transform for name, value in request.items(): if name == b"location": world_location = value self._view_transform.location = -np.dot( self._world_to_opengl_mat[:3, :3], world_location) view_transform_changed = True elif name == b"orientation_rpy": # TODO: There should also be a minus sign here self._view_transform.orientation_rpy = value view_transform_changed = True elif name == b"use_trackball" and value: if self._trackball is None: logger.warn( "Client asked for trackball viewpoint but no trackball was registered." ) else: tmp_view_transform = self._trackball.transform elif name == b"fov": tmp_projection_transform = tmp_projection_transform.copy( ) horz_fov = value focal_length = camera_utils.fov_to_focal_length( math_utils.degrees_to_radians(horz_fov), self._framebuffer.width) vert_fov = math_utils.radians_to_degrees( camera_utils.focal_length_to_fov( focal_length, self._framebuffer.height)) logger.info( "Setting horizontal FOV: {:.4f}, vertical FOV: {:.4f}" .format(horz_fov, vert_fov)) tmp_projection_transform.fov = vert_fov projection_transform_changed = True elif name == b"requested_images": requested_images = value elif name == b"_request": pass else: logger.error( "Unknown field in 'render_images' request: {}". format(name)) if projection_transform_changed: self._projection_transform_callbacks( self._projection_transform) if view_transform_changed: self._view_transform_callbacks(self._view_transform) logger.debug("Requested images: {}".format(requested_images)) with self._framebuffer.activate(clear=True): self._drawer.draw(tmp_projection_transform.matrix, tmp_view_transform.matrix, self._framebuffer.width, self._framebuffer.height) if b"rgb_image" in requested_images: rgb_pixels = self._framebuffer.read_rgba_pixels() rgb_pixels = np.flip(rgb_pixels, 0) response[b"rgb_image"] = rgb_pixels if b"depth_image" in requested_images: depth_pixels = self._framebuffer.read_rgba_pixels( color_index=1) depth_pixels = depth_pixels[:, :, 0] depth_pixels = np.flip(depth_pixels, 0) response[b"depth_image"] = depth_pixels if b"normal_image" in requested_images: normal_pixels = self._framebuffer.read_rgba_pixels( color_index=2) normal_pixels = normal_pixels[:, :, :3] normal_pixels = np.flip(normal_pixels, 0) response[b"normal_image"] = normal_pixels else: logger.warn("WARNING: Invalid request: {}".format(request)) if response is not None: response_dump = self._serializer.dumps(response) # logger.debug("Sending response") return response_dump except Exception as exc: logger.exception("Exception when handling request") if raise_exception: raise
def run(args): # Command line arguments address = args.address show_window = args.show_window poll_timeout = args.poll_timeout width = args.width height = args.height mesh_filename = args.mesh_filename use_msgpack_for_mesh = args.use_msgpack_for_mesh window_width = args.window_width if window_width is None: window_width = width window_height = args.window_height if window_height is None: window_height = height window_pos_x = args.window_pos_x window_pos_y = args.window_pos_y model_scale = args.model_scale model_rpy = [args.model_roll, args.model_pitch, args.model_yaw] depth_scale = args.depth_scale yaw_speed = args.yaw_speed pitch_speed = args.pitch_speed # Compute vertical field of view (needed for glm.perspective) horz_fov = args.horz_fov focal_length = camera_utils.fov_to_focal_length( math_utils.degrees_to_radians(horz_fov), width) vert_fov = math_utils.radians_to_degrees( camera_utils.focal_length_to_fov(focal_length, height)) logger.info("Horizontal FOV: {:.4f}, vertical FOV: {:.4f}".format( horz_fov, vert_fov)) window_focal_length = camera_utils.fov_to_focal_length( math_utils.degrees_to_radians(horz_fov), window_width) window_vert_fov = math_utils.radians_to_degrees( camera_utils.focal_length_to_fov(window_focal_length, window_height)) logger.info("Window vertical FOV: {:.4f}".format(window_vert_fov)) # Fixed parameters initial_distance = 10 # initial camera distance # Glumpy initialization # glumpy.app.use("glfw") # glumpy.app.use("glfw", 'ES', 3, 3) glumpy_config = glumpy.app.configuration.get_default() glumpy_config.double_buffer = True glumpy_config.samples = 0 glumpy_config.api = "ES" glumpy_config.depth_size = 32 glumpy_config.major_version = 3 glumpy_config.minor_version = 1 glumpy_config.profile = "core" # if show_window: window = Window(width=window_width, height=window_height, fov=window_vert_fov, znear=0.5, zfar=1000, config=glumpy_config, visible=show_window, title=b"mesh_renderer_zmq") trackball = Trackball(pitch=0, yaw=0) window.glumpy_window.attach(trackball) app = Application() logger.info("Glumpy backend: {}".format(glumpy.app.__backend__)) # Process initial events if window_pos_x is not None or window_pos_y is not None: if window_pos_x is None: window_pos_x, _ = window.glumpy_window.get_position() if window_pos_y is None: _, window_pos_y = window.glumpy_window.get_position() window.glumpy_window.set_position(window_pos_x, window_pos_y) # Offscreen surface initialization framebuffer = Framebuffer(width, height, num_color_attachments=3, color_dtypes=[np.ubyte, np.float32, np.float32]) fb_color_drawer = FramebufferDrawer(framebuffer, color_index=0, draw_mode=FramebufferDrawer.BLIT) fb_depth_drawer = FramebufferDrawer(framebuffer, color_index=1, draw_mode=FramebufferDrawer.DRAW_QUAD) fb_depth_drawer.set_color_scale(depth_scale) fb_normal_drawer = FramebufferDrawer(framebuffer, color_index=2, draw_mode=FramebufferDrawer.DRAW_QUAD) fb_normal_drawer.set_color_scale(0.5) fb_normal_drawer.set_color_offset(0.5) # Offscreen surface initialization window_framebuffer = Framebuffer( window_width, window_height, num_color_attachments=3, color_dtypes=[np.ubyte, np.float32, np.float32]) window_fb_color_drawer = FramebufferDrawer( window_framebuffer, color_index=0, draw_mode=FramebufferDrawer.BLIT) window_fb_depth_drawer = FramebufferDrawer( window_framebuffer, color_index=1, draw_mode=FramebufferDrawer.DRAW_QUAD) window_fb_depth_drawer.set_color_scale(depth_scale) window_fb_normal_drawer = FramebufferDrawer( window_framebuffer, color_index=2, draw_mode=FramebufferDrawer.DRAW_QUAD) window_fb_normal_drawer.set_color_scale(0.5) window_fb_normal_drawer.set_color_offset(0.5) # Add framebuffer draw for normals window.add_drawer(window_fb_normal_drawer) # Transformation from world coordinates (z up, x foward, y left) to opengl coordinates (z backward, x right, y up) world_to_opengl_mat = np.eye(4, dtype=np.float32) world_to_opengl_mat = glm.xrotate(world_to_opengl_mat, -90) world_to_opengl_mat = glm.zrotate(world_to_opengl_mat, 90) opengl_to_world_mat = np.linalg.inv(world_to_opengl_mat) # Mesh loading if mesh_filename is not None: mesh = None if use_msgpack_for_mesh: msgpack_mesh_filename = mesh_filename + ".msgpack" if os.path.isfile(msgpack_mesh_filename): logger.info("Loading mesh from msgpack file {}".format( msgpack_mesh_filename)) mesh = SimpleMesh.read_from_msgpack(msgpack_mesh_filename) if mesh is None: logger.info("Loading mesh from file {}".format(mesh_filename)) mesh = SimpleMesh.read_from_file(mesh_filename) if use_msgpack_for_mesh: logger.info("Saving mesh to msgpack file {}".format( msgpack_mesh_filename)) mesh.write_to_msgpack(msgpack_mesh_filename) else: mesh = CubeMesh() logger.info("Setting uniform color") mesh.set_colors_uniform([0.5, 0.5, 0.5, 1]) # logger.info("Computing mesh colors with z-colormap") # mesh.set_colors_with_coordinate_colormap(min_coord=0, max_coord=10) # Drawer initialization # cube_mesh = CubeMesh() # cube_drawer = MeshDrawer(cube_mesh) # cube_drawer.depth_scale = 0.1 # cube_drawer.normal_scale = 0.1 # mesh = SimpleMesh.read_from_ply(sys.argv[1]) # mesh.write_to_pickle("mesh.pickle") # mesh = SimpleMesh.read_from_pickle("mesh2.pickle") # mesh = Mesh.from_file(filename) # mesh_drawer = MeshDrawer(mesh) mesh_drawer = MeshDrawer(mesh) mesh_drawer.transform.scale = model_scale mesh_drawer.transform.orientation_rpy = model_rpy # Assume mesh is in world coordinate system # mesh_drawer.model = np.matmul(np.transpose(world_to_opengl_mat), mesh_drawer.model) # mesh_drawer.color_scale = color_scale # mesh_drawer.normal_scale = normal_scale # mesh_drawer.depth_scale = depth_scale # glm.scale(mesh_drawer.model, 0.01) # cube_drawer = CubeDrawer() # Server initialization logger.info("Starting ZMQ server on address {}".format(address)) server_conn = zmq_utils.Connection(address, zmq_utils.zmq.REP) server_conn.bind() renderer_service = renderer_zmq_service.RendererZMQService( framebuffer, mesh_drawer, vert_fov, initial_distance, trackball, world_to_opengl_mat) global input_enabled global override_renderer_service_transform global window_enabled input_enabled = args.input_enabled window_enabled = window.visible override_renderer_service_transform = False def window_visible_callback(visible): if visible: logger.info("Showing window") window.show() else: logger.info("Hiding window") window.hide() def window_active_callback(active): global window_enabled if active: logger.info("Activating window") else: logger.info("Deactivating window") window_enabled = active renderer_service.window_visible_callbacks.register(window_visible_callback) renderer_service.window_active_callbacks.register(window_active_callback) # Some keyboard input handling @window.glumpy_window.event def on_key_press(symbol, modifiers): try: character = chr(symbol).lower() except ValueError as err: character = "" logger.debug("Key pressed: {}, modifiers: {}, character: {}".format( symbol, modifiers, character)) if character == 'n': logger.info("Showing normals") window.clear_drawers() window.add_drawer(window_fb_normal_drawer) elif character == 'c': logger.info("Showing colors") window.clear_drawers() window.add_drawer(window_fb_color_drawer) elif character == 'd': logger.info("Showing depth") window.clear_drawers() window.add_drawer(window_fb_depth_drawer) elif character == 'i': global input_enabled if input_enabled: logger.info("Disabling input") input_enabled = False else: logger.info("Enabling input") input_enabled = True elif character == 'a': global window_enabled if window_enabled: logger.info("Disabling window") window_enabled = False else: logger.info("Enabling window") window_enabled = True elif character == 'o': global override_renderer_service_transform if override_renderer_service_transform: logger.info("Not overriding renderer service transform input") override_renderer_service_transform = False else: logger.info("Overriding renderer service transform input") override_renderer_service_transform = True elif character == 'p': logger.info("Current location: {}".format(trackball.location)) logger.info("Current world location: {}".format( np.dot(opengl_to_world_mat[:3, :3], trackball.camera_location))) logger.info("Current orientation_rpy: {}".format( trackball.orientation_rpy)) timer = utils.RateTimer(reset_interval=500) while True: dt = app.clock.tick() # Handle network requests request_dump = server_conn.recv(timeout=poll_timeout * 1000) if request_dump is not None: response_dump = renderer_service.handle_request_msg( request_dump, raise_exception=True) if response_dump is not None: server_conn.send(response_dump) # Perform any drawer processing mesh_drawer.tick(dt) # Animate model if desired if yaw_speed != 0: mesh_drawer.transform.yaw += yaw_speed * dt if pitch_speed != 0: mesh_drawer.transform.pitch += pitch_speed * dt if override_renderer_service_transform: renderer_service.view_transform = trackball.transform.copy() if window.visible and window_enabled: if input_enabled and renderer_service.input_enabled: view_transform = trackball.transform else: view_transform = renderer_service.view_transform with window_framebuffer.activate(clear=True): mesh_drawer.draw(window.projection, view_transform.matrix, window.width, window.height) # For debugging # color_pixels = framebuffer.read_rgba_pixels() # depth_pixels = framebuffer.read_rgba_pixels(color_index=1) # import cv2 # cv2.imshow("color", color_pixels) # cv2.imshow("depth", depth_pixels[:, :, 0]) # cv2.waitKey(50) window.override_view(trackball.view) app.process(dt) timer.update_and_print_rate(print_interval=100)
def get_horizontal_field_of_view(self): """Return the horizontal field of view of the camera""" horz_fov_degrees = self.get_horizontal_field_of_view_degrees() # Convert to radians horz_fov = math_utils.degrees_to_radians(horz_fov_degrees) return horz_fov