def tag_array_callback(tag_array_msg):
    filename = 'known_correspondences.csv'

    # Write to the correspondences in CSV format where the first two columns
    # are the (xi, yi) coordinates in image space and the next two columns
    # are the corresponding (xr, xr) coordinates in robot space.
    with open(filename, 'w') as csvfile:
        writer = csv.DictWriter(csvfile, fieldnames=['xi', 'yi', 'xr', 'yr'], \
                                delimiter=',')
        writer.writeheader()

        for tag in tag_array_msg.tags:
            index = tagDict[tag.id]
            (xr, yr) = get_corresponding_point(index)

            # Compute the centroid of the four corners of the tag
            #xi = 0
            #yi = 0
            #for i in range(4):
            #    xi += tag.image_coordinates[i].x
            #    yi += tag.image_coordinates[i].y
            #xi /= 4
            #yi /= 4
            xi = int(tag.image_coordinates[0].x)
            yi = int(tag.image_coordinates[0].y)

            rowDict = {'xi':xi, \
                       'yi':yi, \
                       'xr':xr, \
                       'yr':yr }
            writer.writerow(rowDict)

    rospy.signal_shutdown("Work complete")
def main():
    image_filename = 'robot_7.png'
    filename_out =   'robot_7_known_correspondences.csv'

    robotCoords = []
    id = 0
    for j in range(common_calib.height_in_targets):
        for i in range(common_calib.width_in_targets):
            robotCoords.append(common_calib.get_corresponding_point(id))
            id += 1
    #print robotCoords

    print '''
    Please click on the centre of the targets (left click performs a click,
    right click does an undo) ordered from left-to-right and top-to-bottom.
    '''
    pickList = common_picking.pick(image_filename, robotCoords)

    # Write to the output file in CSV format where the first two columns
    # are the (x, y) coordinates in image space and the next two columns
    # are the corresponding (Xr, Yr) coordinates in robot space.
    with open(filename_out, 'w') as csvfile:
        writer = csv.DictWriter(csvfile, fieldnames=['xi', 'yi', 'xr', 'yr'], \
                                delimiter=',')
        writer.writeheader()
        for row in range(len(robotCoords)):
            xy = pickList[row]
            robotCoord = robotCoords[row]
            rowDict = {'xi':xy[0], \
                       'yi':xy[1], \
                       'xr':robotCoord[0], \
                       'yr':robotCoord[1] }
            writer.writerow(rowDict)
def main():
    image_filename = 'pixy.png'
    filename_out = 'known_correspondences.csv'

    robotCoords = []
    id = 0
    for j in range(common_calib.height_in_targets):
        for i in range(common_calib.width_in_targets):
            robotCoords.append(common_calib.get_corresponding_point(id))
            id += 1
    #print robotCoords

    print '''
    Please click on the centre of the targets (left click performs a click,
    right click does an undo) ordered from left-to-right and top-to-bottom.
    '''
    pickList = common_picking.pick(image_filename, robotCoords)

    # Write to the output file in CSV format where the first two columns
    # are the (x, y) coordinates in image space and the next two columns
    # are the corresponding (Xr, Yr) coordinates in robot space.
    with open(filename_out, 'w') as csvfile:
        writer = csv.DictWriter(csvfile, fieldnames=['xi', 'yi', 'xr', 'yr'], \
                                delimiter=',')
        writer.writeheader()
        for row in range(len(robotCoords)):
            xy = pickList[row]
            robotCoord = robotCoords[row]
            rowDict = {'xi':xy[0], \
                       'yi':xy[1], \
                       'xr':robotCoord[0], \
                       'yr':robotCoord[1] }
            writer.writerow(rowDict)
def tag_array_callback(tag_array_msg):
    filename = 'known_correspondences.csv'

    # Write to the correspondences in CSV format where the first two columns
    # are the (xi, yi) coordinates in image space and the next two columns
    # are the corresponding (xr, xr) coordinates in robot space.
    with open(filename, 'w') as csvfile:
        writer = csv.DictWriter(csvfile, fieldnames=['xi', 'yi', 'xr', 'yr'], \
                                delimiter=',')
        writer.writeheader()

        for tag in tag_array_msg.tags:
            index = tagDict[tag.id]
            (xr, yr) = get_corresponding_point(index)

            # Compute the centroid of the four corners of the tag
            #xi = 0
            #yi = 0
            #for i in range(4):
            #    xi += tag.image_coordinates[i].x
            #    yi += tag.image_coordinates[i].y
            #xi /= 4
            #yi /= 4
            xi = int(tag.image_coordinates[0].x)
            yi = int(tag.image_coordinates[0].y)

            rowDict = {'xi':xi, \
                       'yi':yi, \
                       'xr':xr, \
                       'yr':yr }
            writer.writerow(rowDict)

    rospy.signal_shutdown("Work complete")