camera_calibration = CameraCalibration(grid_size=grid_size, square_size=square_size) laser_calibration = LaserCalibration(grid_size=grid_size, square_size=square_size, profile=laser_profile) laser_calibration.find_calibration_3d(images) laser_calibration.save_parameters(os.path.join(path, 'config', config_file)) filenames = sorted(glob.glob('../data/pose*.txt')) ks = [int(filename[-8:-4]) for filename in filenames] poses_checker, poses_tool = [], [] poses_ichecker, poses_itool = [], [] for k in ks: print 'Frame: %i' % k img = read_image('../data/frame%04i.png' % k) grid = camera_calibration.find_chessboard(img) pose_checker = None if grid is not None: pose_checker = laser_calibration.get_chessboard_pose(grid) pose_checker = calc.pose_to_matrix((pose_checker[0], pose_checker[1])) img = camera_calibration.draw_chessboard(img, grid) show_images([img]) with open('../data/pose%04i.txt' % k, 'r') as f: pose = eval(f.read()) quatpose_tool0 = (np.array(pose[0]), np.array(pose[1])) pose_tool0 = calc.quatpose_to_matrix(*quatpose_tool0) if pose_checker is not None: poses_checker.append(pose_checker) poses_tool.append(pose_tool0) poses_ichecker.append(calc.matrix_invert(pose_checker)) poses_itool.append(calc.matrix_invert(pose_tool0))
class ImageViewer(): def __init__(self): rospy.init_node('viewer', anonymous=True) image_topic = rospy.get_param('~image', '/camera/image') # Pattern parameters pattern_rows = rospy.get_param('~pattern_rows', 7) pattern_cols = rospy.get_param('~pattern_cols', 8) pattern_size = rospy.get_param('~pattern_size', 10) config_file = rospy.get_param('~config', 'profile3d.yaml') rospy.Subscriber(image_topic, Image, self.callback, queue_size=10) rospy.on_shutdown(self.on_shutdown_hook) self.counter = 0 self.bridge = CvBridge() self.listener = tf.TransformListener() self.square_size = pattern_size self.grid_size = (pattern_cols-1, pattern_rows-1) self.laser_profile = Profile(axis=1, thr=180, method='pcog') self.camera_calibration = CameraCalibration(grid_size=self.grid_size, square_size=self.square_size) cv2.namedWindow('viewer') cv2.cv.SetMouseCallback('viewer', self.on_mouse, '') rospy.spin() def on_shutdown_hook(self): cv2.destroyWindow('viewer') #TODO: Fix changes in the Calibration module. #calibration = LaserCalibration(grid_size=self.grid_size, square_size=self.square_size, profile=self.laser_profile) #print os.path.join(path, 'data', 'frame*.png') #calibration.find_calibration_3d(os.path.join(path, 'data', 'frame*.png')) #calibration.save_parameters(os.path.join(path, 'config', config_file)) def on_mouse(self, event, x, y, flags, params): if event == cv2.cv.CV_EVENT_RBUTTONDOWN: self.counter += 1 filename = os.path.join(path, 'data', 'frame%04i.png' %self.counter) cv2.imwrite(filename, self.frame) rospy.loginfo(filename) try: #base_link self.listener.waitForTransform("/base_link", "/tool0", self.stamp, rospy.Duration(1.0)) transform = self.listener.lookupTransform("/base_link", "/tool0", self.stamp) #(trans, rot) filename = os.path.join(path, 'data', 'pose%04i.txt' %self.counter) with open(filename, 'w') as f: f.write(str(transform)) rospy.loginfo(transform) except: rospy.loginfo('The transformation is not accesible.') def callback(self, data): try: self.stamp = data.header.stamp self.frame = self.bridge.imgmsg_to_cv2(data) #TODO: Change to work with gray images if data.encoding == 'mono8': self.frame = cv2.cvtColor(self.frame, cv2.COLOR_GRAY2BGR) #TODO: Show chessboard and line detection. frame = cv2.resize(self.frame, (self.frame.shape[1]/2, self.frame.shape[0]/2)) grid = self.camera_calibration.find_chessboard(frame) if grid is not None: self.camera_calibration.draw_chessboard(frame, grid) cv2.imshow("viewer", frame) cv2.waitKey(1) except CvBridgeError, e: print e
class ImageViewer(): def __init__(self): rospy.init_node('viewer', anonymous=True) image_topic = rospy.get_param('~image', '/ueye/image') # Pattern parameters pattern_rows = rospy.get_param('~pattern_rows', 7) pattern_cols = rospy.get_param('~pattern_cols', 8) pattern_size = rospy.get_param('~pattern_size', 10) config_file = rospy.get_param('~config', 'profile3d.yaml') rospy.Subscriber(image_topic, Image, self.callback, queue_size=10) rospy.on_shutdown(self.on_shutdown_hook) self.counter = 0 self.bridge = CvBridge() self.listener = tf.TransformListener() self.square_size = pattern_size self.grid_size = (pattern_cols - 1, pattern_rows - 1) self.laser_profile = Profile(axis=1, thr=180, method='pcog') self.camera_calibration = CameraCalibration( grid_size=self.grid_size, square_size=self.square_size) cv2.namedWindow('viewer') cv2.cv.SetMouseCallback('viewer', self.on_mouse, '') rospy.spin() def on_shutdown_hook(self): cv2.destroyWindow('viewer') #TODO: Fix changes in the Calibration module. #calibration = LaserCalibration(grid_size=self.grid_size, square_size=self.square_size, profile=self.laser_profile) #print os.path.join(path, 'data', 'frame*.png') #calibration.find_calibration_3d(os.path.join(path, 'data', 'frame*.png')) #calibration.save_parameters(os.path.join(path, 'config', config_file)) def on_mouse(self, event, x, y, flags, params): if event == cv2.cv.CV_EVENT_RBUTTONDOWN: self.counter += 1 filename = os.path.join(path, 'data', 'frame%04i.png' % self.counter) cv2.imwrite(filename, self.frame) rospy.loginfo(filename) try: #base_link self.listener.waitForTransform("/base_link", "/tool0", self.stamp, rospy.Duration(1.0)) transform = self.listener.lookupTransform( "/base_link", "/tool0", self.stamp) #(trans, rot) filename = os.path.join(path, 'data', 'pose%04i.txt' % self.counter) with open(filename, 'w') as f: f.write(str(transform)) rospy.loginfo(transform) except: rospy.loginfo('The transformation is not accesible.') def callback(self, data): try: self.stamp = data.header.stamp self.frame = self.bridge.imgmsg_to_cv2(data) #TODO: Change to work with gray images if data.encoding == 'mono8': self.frame = cv2.cvtColor(self.frame, cv2.COLOR_GRAY2BGR) #TODO: Show chessboard and line detection. frame = cv2.resize( self.frame, (self.frame.shape[1] / 2, self.frame.shape[0] / 2)) grid = self.camera_calibration.find_chessboard(frame) if grid is not None: self.camera_calibration.draw_chessboard(frame, grid) cv2.imshow("viewer", frame) cv2.waitKey(1) except CvBridgeError, e: print e
camera_calibration = CameraCalibration(grid_size=grid_size, square_size=square_size) laser_calibration = LaserCalibration(grid_size=grid_size, square_size=square_size, profile=laser_profile) laser_calibration.find_calibration_3d(images) laser_calibration.save_parameters(os.path.join(path, 'config', config_file)) filenames = sorted(glob.glob('../data/pose*.txt')) ks = [int(filename[-8:-4]) for filename in filenames] poses_checker, poses_tool = [], [] poses_ichecker, poses_itool = [], [] for k in ks: print 'Frame: %i' % k img = read_image('../data/frame%04i.png' % k) grid = camera_calibration.find_chessboard(img) pose_checker = None if grid is not None: pose_checker = laser_calibration.get_chessboard_pose(grid) pose_checker = calc.pose_to_matrix((pose_checker[0], pose_checker[1])) img = camera_calibration.draw_chessboard(img, grid) show_images([img]) with open('../data/pose%04i.txt' % k, 'r') as f: pose = eval(f.read()) quatpose_tool0 = (np.array(pose[0]), np.array(pose[1])) pose_tool0 = calc.quatpose_to_matrix(*quatpose_tool0) if pose_checker is not None: poses_checker.append(pose_checker) poses_tool.append(pose_tool0) poses_ichecker.append(calc.matrix_invert(pose_checker)) poses_itool.append(calc.matrix_invert(pose_tool0))