def register(sensor, config): """ Registers a camera to a chessboard. Parameters ---------- sensor : :obj:`perception.RgbdSensor` the sensor to register config : :obj:`autolab_core.YamlConfig` or :obj:`dict` configuration file for registration Returns ------- :obj:`ChessboardRegistrationResult` the result of registration Notes ----- The config must have the parameters specified in the Other Parameters section. Other Parameters ---------------- num_transform_avg : int the number of independent registrations to average together num_images : int the number of images to read for each independent registration corners_x : int the number of chessboard corners in the x-direction corners_y : int the number of chessboard corners in the y-direction color_image_rescale_factor : float amount to rescale the color image for detection (numbers around 4-8 are useful) vis : bool whether or not to visualize the registration """ # read config num_transform_avg = config['num_transform_avg'] num_images = config['num_images'] sx = config['corners_x'] sy = config['corners_y'] point_order = config['point_order'] color_image_rescale_factor = config['color_image_rescale_factor'] flip_normal = config['flip_normal'] y_points_left = False if 'y_points_left' in config.keys() and sx == sy: y_points_left = config['y_points_left'] num_images = 1 vis = config['vis'] # read params from sensor logging.info('Registering camera %s' %(sensor.frame)) ir_intrinsics = sensor.ir_intrinsics # repeat registration multiple times and average results R = np.zeros([3,3]) t = np.zeros([3,1]) points_3d_plane = PointCloud(np.zeros([3, sx*sy]), frame=sensor.ir_frame) k = 0 while k < num_transform_avg: # average a bunch of depth images together depth_ims = None for i in range(num_images): start = time.time() small_color_im, new_depth_im, _ = sensor.frames() end = time.time() logging.info('Frames Runtime: %.3f' %(end-start)) if depth_ims is None: depth_ims = np.zeros([new_depth_im.height, new_depth_im.width, num_images]) depth_ims[:,:,i] = new_depth_im.data med_depth_im = np.median(depth_ims, axis=2) depth_im = DepthImage(med_depth_im, sensor.ir_frame) # find the corner pixels in an upsampled version of the color image big_color_im = small_color_im.resize(color_image_rescale_factor) corner_px = big_color_im.find_chessboard(sx=sx, sy=sy) if vis: plt.figure() plt.imshow(big_color_im.data) for i in range(sx): plt.scatter(corner_px[i,0], corner_px[i,1], s=25, c='b') plt.show() if corner_px is None: logging.error('No chessboard detected! Check camera exposure settings') continue # convert back to original image small_corner_px = corner_px / color_image_rescale_factor if vis: plt.figure() plt.imshow(small_color_im.data) for i in range(sx): plt.scatter(small_corner_px[i,0], small_corner_px[i,1], s=25, c='b') plt.axis('off') plt.show() # project points into 3D camera_intr = sensor.ir_intrinsics points_3d = camera_intr.deproject(depth_im) # get round chessboard ind corner_px_round = np.round(small_corner_px).astype(np.uint16) corner_ind = depth_im.ij_to_linear(corner_px_round[:,0], corner_px_round[:,1]) if corner_ind.shape[0] != sx*sy: logging.warning('Did not find all corners. Discarding...') continue # average 3d points points_3d_plane = (k * points_3d_plane + points_3d[corner_ind]) / (k + 1) logging.info('Registration iteration %d of %d' %(k+1, config['num_transform_avg'])) k += 1 # fit a plane to the chessboard corners X = np.c_[points_3d_plane.x_coords, points_3d_plane.y_coords, np.ones(points_3d_plane.num_points)] y = points_3d_plane.z_coords A = X.T.dot(X) b = X.T.dot(y) w = np.linalg.inv(A).dot(b) n = np.array([w[0], w[1], -1]) n = n / np.linalg.norm(n) if flip_normal: n = -n mean_point_plane = points_3d_plane.mean() # find x-axis of the chessboard coordinates on the fitted plane T_camera_table = RigidTransform(translation = -points_3d_plane.mean().data, from_frame=points_3d_plane.frame, to_frame='table') points_3d_centered = T_camera_table * points_3d_plane # get points along y if point_order == 'row_major': coord_pos_x = int(math.floor(sx*sy/2.0)) coord_neg_x = int(math.ceil(sx*sy/2.0)) points_pos_x = points_3d_centered[coord_pos_x:] points_neg_x = points_3d_centered[:coord_neg_x] x_axis = np.mean(points_pos_x.data, axis=1) - np.mean(points_neg_x.data, axis=1) x_axis = x_axis - np.vdot(x_axis, n)*n x_axis = x_axis / np.linalg.norm(x_axis) y_axis = np.cross(n, x_axis) else: coord_pos_y = int(math.floor(sx*(sy-1)/2.0)) coord_neg_y = int(math.ceil(sx*(sy+1)/2.0)) points_pos_y = points_3d_centered[:coord_pos_y] points_neg_y = points_3d_centered[coord_neg_y:] y_axis = np.mean(points_pos_y.data, axis=1) - np.mean(points_neg_y.data, axis=1) y_axis = y_axis - np.vdot(y_axis, n)*n y_axis = y_axis / np.linalg.norm(y_axis) x_axis = np.cross(-n, y_axis) # produce translation and rotation from plane center and chessboard basis rotation_cb_camera = RigidTransform.rotation_from_axes(x_axis, y_axis, n) translation_cb_camera = mean_point_plane.data T_cb_camera = RigidTransform(rotation=rotation_cb_camera, translation=translation_cb_camera, from_frame='cb', to_frame=sensor.frame) if y_points_left and np.abs(T_cb_camera.y_axis[1]) > 0.1: if T_cb_camera.x_axis[0] > 0: T_cb_camera.rotation = T_cb_camera.rotation.dot(RigidTransform.z_axis_rotation(-np.pi/2).T) else: T_cb_camera.rotation = T_cb_camera.rotation.dot(RigidTransform.z_axis_rotation(np.pi/2).T) T_camera_cb = T_cb_camera.inverse() # optionally display cb corners with detected pose in 3d space if config['debug']: # display image with axes overlayed cb_center_im = camera_intr.project(Point(T_cb_camera.translation, frame=sensor.ir_frame)) cb_x_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.x_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_y_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.y_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_z_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.z_axis * config['scale_amt'], frame=sensor.ir_frame)) x_line = np.array([cb_center_im.data, cb_x_im.data]) y_line = np.array([cb_center_im.data, cb_y_im.data]) z_line = np.array([cb_center_im.data, cb_z_im.data]) plt.figure() plt.imshow(small_color_im.data) plt.scatter(cb_center_im.data[0], cb_center_im.data[1]) plt.plot(x_line[:,0], x_line[:,1], c='r', linewidth=3) plt.plot(y_line[:,0], y_line[:,1], c='g', linewidth=3) plt.plot(z_line[:,0], z_line[:,1], c='b', linewidth=3) plt.axis('off') plt.title('Chessboard frame in camera %s' %(sensor.frame)) plt.show() return ChessboardRegistrationResult(T_camera_cb, points_3d_plane)
def get_camera_chessboard(sensor_frame, sensor_type, device_num, config): # load cfg num_transform_avg = config['num_transform_avg'] num_images = config['num_images'] sx = config['corners_x'] sy = config['corners_y'] color_image_upsample_rate = config['color_image_upsample_rate'] vis = config['vis'] # open sensor if sensor_type.lower() in CameraChessboardRegister._SENSOR_TYPES: sensor = Kinect2Sensor( device_num=device_num, frame=sensor_frame, packet_pipeline_mode=Kinect2PacketPipelineMode.CPU) else: logging.warning( 'Could not register device at %s. Sensor type %s not supported' % (sensor_frame, sensor_type)) logging.info('Registering camera %s' % (sensor_frame)) sensor.start() # repeat registration multiple times and average results R = np.zeros([3, 3]) t = np.zeros([3, 1]) points_3d_plane = PointCloud(np.zeros([3, sx * sy]), frame=sensor.ir_frame) k = 0 while k < num_transform_avg: # average a bunch of depth images together depth_ims = np.zeros([ Kinect2Sensor.DEPTH_IM_HEIGHT, Kinect2Sensor.DEPTH_IM_WIDTH, num_images ]) for i in range(num_images): small_color_im, new_depth_im, _ = sensor.frames() depth_ims[:, :, i] = new_depth_im.data med_depth_im = np.median(depth_ims, axis=2) depth_im = DepthImage(med_depth_im, sensor.ir_frame) # find the corner pixels in an upsampled version of the color image big_color_im = small_color_im.resize(color_image_upsample_rate) corner_px = CameraChessboardRegister._find_chessboard(big_color_im, sx=sx, sy=sy, vis=vis) if corner_px is None: logging.error('No chessboard detected') continue # convert back to original image small_corner_px = corner_px / color_image_upsample_rate if vis: plt.figure() plt.imshow(small_color_im.data) for i in range(sx): plt.scatter(small_corner_px[i, 0], small_corner_px[i, 1], s=25, c='b') plt.axis('off') plt.show() # project points into 3D camera_intr = sensor.ir_intrinsics points_3d = camera_intr.deproject(depth_im) # get round chessboard ind corner_px_round = np.round(small_corner_px).astype(np.uint16) corner_ind = depth_im.ij_to_linear(corner_px_round[:, 0], corner_px_round[:, 1]) if corner_ind.shape[0] != sx * sy: print 'Did not find all corners. Discarding...' continue # average 3d points points_3d_plane = (k * points_3d_plane + points_3d[corner_ind]) / (k + 1) logging.info('Registration iteration %d of %d' % (k + 1, config['num_transform_avg'])) k += 1 # fit a plane to the chessboard corners X = np.c_[points_3d_plane.x_coords, points_3d_plane.y_coords, np.ones(points_3d_plane.num_points)] y = points_3d_plane.z_coords A = X.T.dot(X) b = X.T.dot(y) w = np.linalg.inv(A).dot(b) n = np.array([w[0], w[1], -1]) n = n / np.linalg.norm(n) mean_point_plane = points_3d_plane.mean() # find x-axis of the chessboard coordinates on the fitted plane T_camera_table = RigidTransform( translation=-points_3d_plane.mean().data, from_frame=points_3d_plane.frame, to_frame='table') points_3d_centered = T_camera_table * points_3d_plane # get points along y coord_pos_y = int(math.floor(sx * (sy - 1) / 2.0)) coord_neg_y = int(math.ceil(sx * (sy + 1) / 2.0)) points_pos_y = points_3d_centered[:coord_pos_y] points_neg_y = points_3d_centered[coord_neg_y:] y_axis = np.mean(points_pos_y.data, axis=1) - np.mean( points_neg_y.data, axis=1) y_axis = y_axis - np.vdot(y_axis, n) * n y_axis = y_axis / np.linalg.norm(y_axis) x_axis = np.cross(-n, y_axis) # WARNING! May need symmetry breaking but it appears the points are ordered consistently # produce translation and rotation from plane center and chessboard basis rotation_cb_camera = rotation_from_axes(x_axis, y_axis, n) translation_cb_camera = mean_point_plane.data T_cb_camera = RigidTransform(rotation=rotation_cb_camera, translation=translation_cb_camera, from_frame='cb', to_frame=sensor.frame) T_camera_cb = T_cb_camera.inverse() cb_points_cam = points_3d[corner_ind] # optionally display cb corners with detected pose in 3d space if config['debug']: # display image with axes overlayed cb_center_im = camera_intr.project( Point(T_cb_camera.translation, frame=sensor.ir_frame)) cb_x_im = camera_intr.project( Point(T_cb_camera.translation + T_cb_camera.x_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_y_im = camera_intr.project( Point(T_cb_camera.translation + T_cb_camera.y_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_z_im = camera_intr.project( Point(T_cb_camera.translation + T_cb_camera.z_axis * config['scale_amt'], frame=sensor.ir_frame)) x_line = np.array([cb_center_im.data, cb_x_im.data]) y_line = np.array([cb_center_im.data, cb_y_im.data]) z_line = np.array([cb_center_im.data, cb_z_im.data]) plt.figure() plt.imshow(small_color_im.data) plt.scatter(cb_center_im.data[0], cb_center_im.data[1]) plt.plot(x_line[:, 0], x_line[:, 1], c='r', linewidth=3) plt.plot(y_line[:, 0], y_line[:, 1], c='g', linewidth=3) plt.plot(z_line[:, 0], z_line[:, 1], c='b', linewidth=3) plt.axis('off') plt.title('Chessboard frame in camera %s' % (sensor.frame)) plt.show() return T_camera_cb, cb_points_cam, points_3d_plane