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))
print 'Poses:', len(poses_checker), len(poses_tool)
print 'Hand Eye Calibration Solution'
tlc = HandEyeCalibration()
T2C = tlc.solve(poses_tool, poses_checker)
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
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))
print 'Poses:', len(poses_checker), len(poses_tool)
print 'Hand Eye Calibration Solution'
tlc = HandEyeCalibration()
T2C = tlc.solve(poses_tool, poses_checker)
