def _fill_ros_marker_msg_with_pose(self, marker_msg, pose): marker_msg.pose.position = ros_utils.point_numpy_to_ros( pose.location()) orientation_quat = math_utils.convert_rpy_to_quat( pose.orientation_rpy()) marker_msg.pose.orientation = ros_utils.quaternion_numpy_to_ros( orientation_quat)
def perform_insert_point_cloud_rpy(self, location, orientation_rpy, point_cloud, simulate=False): orientation_quat = math_utils.convert_rpy_to_quat(orientation_rpy) return self.perform_insert_point_cloud_quat(location, orientation_quat, point_cloud, simulate)
def get_view_direction_world(self): """Return camera viewing direction in world frame. Assuming x-axis points forward, y-axis left and z-axis up. """ orientation_rpy = self.get_orientation_rpy() quat = math_utils.convert_rpy_to_quat(orientation_rpy) x_axis = [1, 0, 0] view_direction = math_utils.rotate_vector_with_quaternion(quat, x_axis) return view_direction
def _pose_to_ros_pose_msg(self, pose, time=None): pose_msg = PoseStamped() pose_msg.header.frame_id = self._world_frame if time is None: pose_msg.header.stamp = rospy.Time.now() pose_msg.pose.position = ros_utils.point_numpy_to_ros(pose.location()) orientation_quat = math_utils.convert_rpy_to_quat( pose.orientation_rpy()) pose_msg.pose.orientation = ros_utils.quaternion_numpy_to_ros( orientation_quat) return pose_msg
def perform_query_subvolume_rpy(self, center, orientation_rpy, level, size_x, size_y, size_z, axis_mode=0): orientation_quat = math_utils.convert_rpy_to_quat(orientation_rpy) return self.perform_query_subvolume_quat(center, orientation_quat, level, size_x, size_y, size_z, axis_mode)
def get_depth_point_cloud_world_rpy(self, location, orientation_rpy, filter=True): """Return point cloud (from depth image) in world coordinate frame""" points = self.get_depth_point_cloud_rpy(location, orientation_rpy, filter=filter) world_quat = math_utils.convert_rpy_to_quat(orientation_rpy) world_trans = location world_rot_mat = transformations.quaternion_matrix(world_quat)[:3, :3] points_world = points.dot(world_rot_mat.T) + world_trans return points_world
def perform_raycast_camera_rpy(self, location, orientation_rpy, width, height, focal_length, ignore_unknown_voxels=False, max_range=-1): orientation_quat = math_utils.convert_rpy_to_quat(orientation_rpy) return self.perform_raycast_camera_quat(location, orientation_quat, width, height, focal_length, ignore_unknown_voxels, max_range)
def perform_insert_depth_map_rpy(self, location, orientation_rpy, depth_image, intrinsics, downsample_to_grid=False, simulate=False, max_depth=-1): orientation_quat = math_utils.convert_rpy_to_quat(orientation_rpy) return self.perform_insert_depth_map_quat(location, orientation_quat, depth_image, intrinsics, downsample_to_grid, simulate, max_depth)
def get_ray_direction_image_world(self, width, height, focal_length, orientation_rpy=None): """Return camera ray for given width, height and focal length in world frame. Assuming x-axis points forward, y-axis left and z-axis up. """ ray_directions = self.get_ray_direction_image(width, height, focal_length) # Transform ray directions into world-frame if orientation_rpy is None: orientation_rpy = self.get_orientation_rpy() orientation_quat = math_utils.convert_rpy_to_quat(orientation_rpy) rot_mat = transformations.quaternion_matrix(orientation_quat)[:3, :3] ray_directions_world = ray_directions.dot(rot_mat.T) return ray_directions_world
def get_depth_image(self): location, orientation_rpy = self._base_engine.get_pose_rpy() quat = math_utils.convert_rpy_to_quat(orientation_rpy) stereo_offset = np.array([0, -self._stereo_baseline, 0]) stereo_offset = math_utils.rotate_vector_with_quaternion( quat, stereo_offset) # print(stereo_offset) pose1 = (location, orientation_rpy) pose2 = (location + stereo_offset, orientation_rpy) self._base_engine.set_pose_rpy(pose1, wait_until_set=True) rgb_image1 = self._base_engine.get_rgb_image() rgb_image1 = cv2.cvtColor(rgb_image1, cv2.COLOR_RGB2GRAY) rgb_image1 = (255 * rgb_image1).astype(np.uint8) self._base_engine.set_pose_rpy(pose2, wait_until_set=True) rgb_image2 = self._base_engine.get_rgb_image() # rgb_image2 = environment.base.get_engine().get_normal_image() rgb_image2 = (255 * rgb_image2).astype(np.uint8) rgb_image2 = cv2.cvtColor(rgb_image2, cv2.COLOR_RGB2GRAY) disp_img = self._compute_disparity_image(rgb_image1, rgb_image2) print("Minimum disparity: {}".format(np.min(disp_img))) print("Maximum disparity: {}".format(np.max(disp_img))) # disp_img[:50, :] = 0.5 depth_img = self._compute_depth_image(disp_img) # disparity_img = self._stereo_matcher.compute(rgb_image1, rgb_image2) # # OpenCV disparity image is represented as a fixed point disparity map with 4 fractional bits # disparity_img = disparity_img.astype(np.float32) / 16.0 # print("Minimum disparity: {}".format(np.min(disparity_img))) # print("Maximum disparity: {}".format(np.max(disparity_img))) # # print("focal_length: {}".format(self._focal_length)) # depth_img = self._stereo_baseline * self._focal_length / disparity_img # depth_img[:, :self._num_disparities] = 0 # depth_img[disparity_img <= 0] = np.finfo(np.float32).max # # depth_img[disparity_img == 0] = 10.0 self._base_engine.set_pose_rpy(pose1, wait_until_set=True) gt_depth_img = self._base_engine.get_depth_image() # depth_img[depth_img <= self._min_depth] = -1 # depth_img[:, :self._num_disparities] = -1\ # # Overwrite depth for far away pixels (problem with sky and block matching) # mask = gt_depth_img > 100 # depth_img[mask] = gt_depth_img[mask] # depth_img[:, :self._num_disparities] = 0 # depth_img[depth_img > 10] = 10.0 # gt_depth_img[gt_depth_img > 10] = 10.0 # error_img = np.abs(depth_img - gt_depth_img) # print(np.max(error_img)) depth_img_copy = depth_img.copy() depth_img_copy[depth_img_copy > 10] = 10.0 gt_depth_img_copy = gt_depth_img.copy() gt_depth_img_copy[gt_depth_img_copy > 10] = 10.0 cv2.imwrite("disp_img_{}.png".format(self._stereo_method), disp_img.astype(np.uint8)) cv2.imwrite("depth_img_{}.png".format(self._stereo_method), 255 * depth_img_copy / 10.) cv2.imwrite("gt_depth_img_{}.png".format(self._stereo_method), 255 * gt_depth_img_copy / 10.) # cv2.imwrite("gt_depth_img.png", 255 * gt_depth_img / 10.) # cv2.imwrite("error_img.png", 255 * error_img / 10.) cv2.imwrite("rgb_image1_{}.png".format(self._stereo_method), rgb_image1) cv2.imwrite("rgb_image2_{}.png".format(self._stereo_method), rgb_image2) # from matplotlib import pyplot as plt # plt.figure() # plt.hist(disparity_img.flatten()) # plt.figure() # plt.hist(depth_img.flatten()) # plt.show() # return None # dsize = (64, 64) dsize = (self._width, self._height) # depth_img = gt_depth_img depth_img = cv2.resize(depth_img, dsize=dsize, interpolation=cv2.INTER_NEAREST) return depth_img
def get_vector_from_pose(self, pose): location = pose.location() quat = math_utils.convert_rpy_to_quat(pose.orientation_rpy()) pose_vector = np.concatenate([location, quat]) return pose_vector
def _get_transform_msg_rpy(self, location, orientation_rpy): quat = math_utils.convert_rpy_to_quat(orientation_rpy) return self._get_transform_msg_quat(location, quat)