def build_samples(self, sample_part):
for point in read_points(sample_part):
self.sample.append(point)
self.sample_count += 1
if self.sample_count == 6:
if self.mode == 'cnn':
self.cnn_predict()
if self.mode == 'crafted':
self.crafted_predict()
self.sample = []
self.sample_count = 0
def save_depth_img(self, msg, img_num): img = np.empty((480,640), dtype=np.float32) pts = [] # print msg.fields for p in point_cloud.read_points(msg): pts.append(p[2]) img = np.reshape(pts, (480, 640)) max_px = np.nanmax(img) min_px = np.nanmin(img) scale = int(255 / (max_px-min_px)) filename = self.output_dir + 'img_depth_' + str(img_num) + '.jpg' cv.SaveImage(filename, img*scale)
def save_depth_img(self, msg, img_num):
img = np.empty((480, 640), dtype=np.float32)
pts = []
# print msg.fields
for p in point_cloud.read_points(msg):
pts.append(p[2])
img = np.reshape(pts, (480, 640))
max_px = np.nanmax(img)
min_px = np.nanmin(img)
scale = int(255 / (max_px - min_px))
filename = self.output_dir + 'img_depth_' + str(img_num) + '.jpg'
cv.SaveImage(filename, img * scale)
def save_color_img(self, msg, img_num):
img = np.empty((480,640), dtype=np.float32)
pts = []
# print msg.fields
for p in point_cloud.read_points(msg):
# print len(p)
# pts.append(p[3])
x=1
print 0
print p[3]
x = struct.pack('f', p[3])
print x
img = np.reshape(pts, (480, 640))
# max_px = np.nanmax(img)
# min_px = np.nanmin(img)
# scale = int(255 / (max_px-min_px))
filename = self.output_dir + 'img_color_' + str(img_num) + '.jpg'
cv.SaveImage(filename, np.uint8(img))
def save_color_img(self, msg, img_num):
img = np.empty((480, 640), dtype=np.float32)
pts = []
# print msg.fields
for p in point_cloud.read_points(msg):
# print len(p)
# pts.append(p[3])
x = 1
print 0
print p[3]
x = struct.pack('f', p[3])
print x
img = np.reshape(pts, (480, 640))
# max_px = np.nanmax(img)
# min_px = np.nanmin(img)
# scale = int(255 / (max_px-min_px))
filename = self.output_dir + 'img_color_' + str(img_num) + '.jpg'
cv.SaveImage(filename, np.uint8(img))
def main():
if len(sys.argv) < 3:
print 'grab_cbs_auto cb_config.yaml output_bag.bag'
return
rospy.init_node("grab_cbs")
f = file(sys.argv[1], 'r')
cb_config = yaml.safe_load(f.read())
print cb_config
f.close()
is_kinect = rospy.get_param("/grab_cbs/is_kinect", True)
# load cb stuff
chessboard = ChessboardInfo()
chessboard.n_cols = cb_config['cols'] # 6
chessboard.n_rows = cb_config['rows'] # 7
chessboard.dim = cb_config['dim'] # 0.0258
calib = Calibrator([chessboard])
bridge = CvBridge()
l = DataListener(is_kinect, bridge, calib)
tf_list = tf.TransformListener()
cb_knowns = []
for j in range(chessboard.n_cols):
for i in range(chessboard.n_rows):
cb_knowns.append((chessboard.dim*i, chessboard.dim*j, 0.0))
bag = rosbag.Bag(sys.argv[2], 'w')
i = 0
while not rospy.is_shutdown():
if raw_input("Press enter to take CB, type 'q' to quit: ") == "q":
break
tries = 0
while not rospy.is_shutdown() and tries < 3:
corners = l.wait_for_new(5.)
if corners is None:
print "No corners detected"
tries += 1
continue
corners_2d = np.uint32(np.round(corners)).tolist()
corners_3d = []
if is_kinect:
for x,y,z in read_points(l.cur_pc, field_names=['x', 'y', 'z'], uvs=corners_2d):
corners_3d.append((x,y,z))
frame_id = l.cur_pc.header
else:
obj_pts = cv.fromarray(np.array(cb_knowns))
img_pts = cv.fromarray(np.array(corners))
K = cv.fromarray(np.reshape(l.cam_info.K,(3,3)))
D = cv.fromarray(np.array([l.cam_info.D]))
R_vec = cv.fromarray(np.zeros((3,1)))
t = cv.fromarray(np.zeros((3,1)))
cv.FindExtrinsicCameraParams2(obj_pts, img_pts, K, D, R_vec, t)
R_mat = cv.fromarray(np.zeros((3,3)))
cv.Rodrigues2(R_vec, R_mat)
T = PoseConv.to_homo_mat(np.mat(np.asarray(t)).T.A.tolist(),
np.mat(np.asarray(R_mat)).A.tolist())
cb_knowns_mat = np.vstack((np.mat(cb_knowns).T, np.mat(np.ones((1, len(cb_knowns))))))
corners_3d = np.array((T * cb_knowns_mat)[:3,:].T)
frame_id = l.cur_img.header
print corners_3d
if np.any(np.isnan(corners_3d)):
print "Pointcloud malformed"
tries += 1
continue
now = rospy.Time.now()
corners_pc = create_cloud_xyz32(frame_id, corners_3d)
try:
pose = tf_list.lookupTransform('/base_link', '/ee_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "TF screwed up..."
continue
bag.write("/pose", PoseConv.to_pose_stamped_msg('/base_link', pose), now)
bag.write("/pc", corners_pc, now)
print "Wrote pose/CB to bag file"
break
i += 1
bag.close()
