Exemple #1
0
    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))
Exemple #6
0
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()