Esempio n. 1
0
def get_tag_detections(im, cache_tag):
    #
    # Because of a bug in the tag detector, it doesn't seem
    # to detect tags larger than a certain size. To work-around
    # this limitation, we detect tags on two different image
    # scales and use the one with more detections
    #
    assert len(im.shape) == 2

    from skimage.transform import rescale as imrescale
    im4 = imrescale(im, 1./4)

    from skimage.util import img_as_ubyte
    im = img_as_ubyte(im)
    im4 = img_as_ubyte(im4)

    from apriltag import AprilTagDetector, AprilTagDetection
    detections1 = AprilTagDetector().detect(im)
    detections4 = AprilTagDetector().detect(im4)
    for d in detections4:
        d.c[0] *= 4.
        d.c[1] *= 4.

    # note that everything other than the tag center is wrong
    # in detections4

    if len(detections4) > len(detections1):
        return detections4
    else:
        return detections1
Esempio n. 2
0
def process(filename):
    #
    # Conventions:
    # a_i, b_i
    #    are variables in image space, units are pixels
    # a_w, b_w
    #    are variables in world space, units are meters
    #
    print '\n========================================'
    print '  File: ' + filename
    print '========================================\n'

    im = imread(filename)
    im = rgb2gray(im)
    im = (im * 255.).astype(np.uint8)

    tag_mosaic = TagMosaic(0.0254)
    detections = AprilTagDetector().detect(im)
    print '  %d tags detected.' % len(detections)

    #
    # Sort detections by distance to center
    #
    c_i = np.array([im.shape[1], im.shape[0]]) / 2.
    dist = lambda p_i: np.linalg.norm(p_i - c_i)
    closer_to_center = lambda d1, d2: int(dist(d1.c) - dist(d2.c))
    detections.sort(cmp=closer_to_center)

    mosaic_pos = lambda det: tag_mosaic.get_position_meters(det.id)

    det_i = np.array([ d.c for d in detections ])
    det_w = np.array([ mosaic_pos(d) for d in detections ])

    #
    # To learn a weighted local homography, we find the weighting
    # function parameters that minimize reprojection error across
    # leave-one-out validation folds of the data. Since the
    # homography is local at the center, we only use 9 nearest
    # detections to the center
    #
    det_i9 = det_i[:9]
    det_w9 = det_w[:9]

    def local_homography_loocv_error(theta, args):
        src, tgt = args
        errs = [ local_homography_error(theta, src[t_ix], tgt[t_ix], src[v_ix], tgt[v_ix])
                    for t_ix, v_ix in LeaveOneOut(len(src)) ]
        return np.mean(errs)

    def learn_homography_i2w():
        result = minimize( local_homography_loocv_error,
                    x0=[ 50, 1, 1e-3 ],
                    args=[ det_i9, det_w9 ],
                    method='Powell',
                    options={'ftol': 1e-3} )

        print '\nHomography: i->w'
        print '------------------'
        print '  params:', result.x
        print '    rmse: %.6f' % sqrt(result.fun)
        print '\n  Optimization detail:'
        print '  ' + str(result).replace('\n', '\n      ')

        H = create_local_homography_object(*result.x)
        for i, w in zip(det_i9, det_w9):
            H.add_correspondence(i, w)

        return H

    def learn_homography_w2i():
        result = minimize( local_homography_loocv_error,
                    x0=[ 0.0254, 1, 1e-3 ],
                    method='Powell',
                    args=[ det_w9, det_i9 ],
                    options={'ftol': 1e-3} )

        print '\nHomography: w->i'
        print '------------------'
        print '  params:', result.x
        print '    rmse: %.6f' % sqrt(result.fun)
        print '\n  Optimization detail:'
        print '  ' + str(result).replace('\n', '\n      ')

        H = create_local_homography_object(*result.x)
        for w, i in zip(det_w9, det_i9):
            H.add_correspondence(w, i)

        return H

    #
    # We assume that the distortion is zero at the center of
    # the image and we are interesting in the word to image
    # homography at the center of the image. However, we don't
    # know the center of the image in world coordinates.
    # So we follow a procedure as explained below:
    #
    # First, we learn a homography from image to world
    # Next, we find where the image center `c_i` maps to in
    # world coordinates (`c_w`). Finally, we find the local
    # homography `LH0` from world to image at `c_w`
    #
    H_iw = learn_homography_i2w()
    c_i = np.array([im.shape[1], im.shape[0]]) / 2.
    c_w = H_iw.map(c_i)[:2]
    H_wi = learn_homography_w2i()
    LH0 = H_wi.get_homography_at(c_w)

    print '\nHomography at center'
    print '----------------------'
    print '      c_w =', c_w
    print '      c_i =', c_i
    print 'LH0 * c_w =', H_wi.map(c_w)

    #
    # Obtain distortion estimate
    #       detected + undistortion = mapped
    #  (or) undistortion = mapped - detected
    #
    def homogeneous_coords(arr):
        return np.hstack([ arr, np.ones((len(arr), 1)) ])

    mapped_i = LH0.dot(homogeneous_coords(det_w).T).T
    mapped_i = np.array([ p / p[2] for p in mapped_i ])
    mapped_i = mapped_i[:,:2]

    undistortion = mapped_i - det_i # image + undistortion = mapped
    max_distortion = np.max([np.linalg.norm(u) for u in undistortion])
    print '\nMaximum distortion is %.2f pixels' % max_distortion

    #
    # Fit non-parametric model to the observations
    #
    model = GPModel(det_i, undistortion)

    print '\nGP Hyper-parameters'
    print '---------------------'
    print '  x: ', model._gp_x._covf.theta
    print '        log-likelihood: %.4f' % model._gp_x.model_evidence()
    print '  y: ', model._gp_y._covf.theta
    print '        log-likelihood: %.4f' % model._gp_y.model_evidence()
    print ''
    print '  Optimization detail:'
    print '  [ x ]'
    print '  ' + str(model._gp_x.fit_result).replace('\n', '\n      ')
    print '  [ y ]'
    print '  ' + str(model._gp_y.fit_result).replace('\n', '\n      ')

    #
    # Visualization
    #
    from skimage.filters import scharr
    from matplotlib import pyplot as plt

    plt.style.use('ggplot')
    plt.figure(figsize=(16,10))

    #__1__
    plt.subplot(221)
    plt.title(filename.split('/')[-1])
    plt.imshow(im, cmap='gray')
    plt.plot(det_i[:,0], det_i[:,1], 'o')
    for i, d in enumerate(detections):
        plt.text(d.c[0], d.c[1], str(i),
            fontsize=8, color='white', bbox=dict(facecolor='maroon', alpha=0.75))
    plt.grid()
    plt.axis('equal')

    #__2__
    plt.subplot(222)
    plt.title('Non-linear Homography')

    if False:
        plt.imshow(1.-scharr(im), cmap='bone', interpolation='gaussian')

        from collections import defaultdict
        row_groups = defaultdict(list)
        col_groups = defaultdict(list)

        for d in detections:
            row, col = tag_mosaic.get_row(d.id), tag_mosaic.get_col(d.id)
            row_groups[row] += [ d.id ]
            col_groups[col] += [ d.id ]

        for k, v in row_groups.iteritems():
            a = tag_mosaic.get_position_meters(np.min(v))
            b = tag_mosaic.get_position_meters(np.max(v))
            x_coords = np.linspace(a[0], b[0], 100)
            points = np.array([ H_wi.map([x, a[1]]) for x in x_coords ])
            plt.plot(points[:,0], points[:,1], '-',color='#CF4457', linewidth=2)

        for k, v in col_groups.iteritems():
            a = tag_mosaic.get_position_meters(np.min(v))
            b = tag_mosaic.get_position_meters(np.max(v))
            y_coords = np.linspace(a[1], b[1], 100)
            points = np.array([ H_wi.map([a[0], y]) for y in y_coords ])
            plt.plot(points[:,0], points[:,1], '-',color='#CF4457', linewidth=2)

        plt.plot(det_i[:,0], det_i[:,1], 'kx')
        plt.grid()
        plt.axis('equal')

    #__3__
    plt.subplot(223)
    plt.title('Qualitative Estimates')

    for i, d in enumerate(detections):
        plt.text(d.c[0], d.c[1], str(i), fontsize=8, color='#999999')

    X, Y = det_i[:,0], det_i[:,1]
    U, V = undistortion[:,0], undistortion[:,1]
    plt.quiver(X, Y, U, -V, units='dots')
    # plt.quiver(X, Y, U, -V, angles='xy', scale_units='xy', scale=1) # plot exact
    plt.text( 0.5, 0, 'max observed distortion: %.2f px' % max_distortion, color='#CF4457', fontsize=10,
        horizontalalignment='center', verticalalignment='bottom', transform=plt.gca().transAxes)
    plt.gca().invert_yaxis()
    plt.axis('equal')

    #__4__
    plt.subplot(224)
    plt.title('Qualitative Undistortion')
    H, W = im.shape
    grid = np.array([[x, y] for y in xrange(0, H, 80) for x in xrange(0, W, 80)])
    predicted = model.predict(grid)
    X, Y = grid[:,0], grid[:,1]
    U, V = predicted[:,0], predicted[:,1]
    plt.quiver(X, Y, U, -V, units='dots')
    #plt.quiver(X, Y, U, -V, angles='xy', scale_units='xy', scale=1)) # plot exact
    plt.gca().invert_yaxis()
    plt.axis('equal')

    plt.tight_layout()
    plt.gcf().patch.set_facecolor('#dddddd')
    #plt.show()
    plt.savefig(filename + '.svg', bbox_inches='tight')
Esempio n. 3
0
def main():
    im = imread(sys.argv[1])
    im = img_as_ubyte(im)

    tagdetector = AprilTagDetector()
    print "\n".join([str((d.id, d.c)) for d in tagdetector.detect(im)])
Esempio n. 4
0
def process(filename, options):
    #
    # Conventions:
    # a_i, b_i
    #    are variables in image space, units are pixels
    # a_w, b_w
    #    are variables in world space, units are meters
    #
    print('\n========================================')
    print('  File: ' + filename)
    print('========================================\n')

    from skimage.io import imread
    im = imread(filename)

    from skimage.color import rgb2gray
    im = rgb2gray(im)

    im = img_as_ubyte(im)

    from apriltag import AprilTagDetector
    detections = AprilTagDetector().detect(im)
    print('  %d tags detected.' % len(detections))

    #
    # Sort detections by distance to center
    #
    c_i = np.array([im.shape[1], im.shape[0]]) / 2.
    dist = lambda p_i: np.linalg.norm(p_i - c_i)
    closer_to_center = lambda d1, d2: int(dist(d1.c) - dist(d2.c))
    import functools
    detections.sort(key=functools.cmp_to_key(closer_to_center))

    from apriltag.tag_mosaic import Tag36h11Mosaic
    tag_mosaic = Tag36h11Mosaic(0.0254)
    mosaic_pos = lambda det: tag_mosaic.get_position_meters(det.id)

    det_i = np.array([d.c for d in detections])
    det_w = np.array([mosaic_pos(d) for d in detections])

    #
    # To learn a weighted local homography, we find the weighting
    # function parameters that minimize reprojection error across
    # leave-one-out validation folds of the data. Since the
    # homography is local at the center, we only use 36 nearest
    # detections to the center
    #
    det_i36 = det_i[:36]
    det_w36 = det_w[:36]

    from sklearn.model_selection import LeaveOneOut
    from scipy.optimize import minimize

    def local_homography_loocv_error(theta, args):
        src, tgt = args
        errs = [
            local_homography_error(theta, src[t_ix], tgt[t_ix], src[v_ix],
                                   tgt[v_ix])
            for t_ix, v_ix in LeaveOneOut().split(range(len(src)))
        ]
        return np.mean(errs)

    def learn_homography_i2w():
        result = minimize(local_homography_loocv_error,
                          x0=[50, 1, 1e-3],
                          args=[det_i36, det_w36],
                          method='Powell',
                          options={'ftol': 1e-3})

        print('\nHomography: i->w')
        print('------------------')
        print('  params:', result.x)
        print('    rmse: %.6f' % sqrt(result.fun))
        print('\n  Optimization detail:')
        print('  ' + str(result).replace('\n', '\n      '))

        H = create_local_homography_object(*result.x)
        for i, w in zip(det_i36, det_w36):
            H.add_correspondence(i, w)

        return H

    def learn_homography_w2i():
        result = minimize(local_homography_loocv_error,
                          x0=[0.0254, 1, 1e-3],
                          method='Powell',
                          args=[det_w36, det_i36],
                          options={'ftol': 1e-3})

        print('\nHomography: w->i')
        print('------------------')
        print('  params:', result.x)
        print('    rmse: %.6f' % sqrt(result.fun))
        print('\n  Optimization detail:')
        print('  ' + str(result).replace('\n', '\n      '))

        H = create_local_homography_object(*result.x)
        for w, i in zip(det_w36, det_i36):
            H.add_correspondence(w, i)

        return H

    #
    # We assume that the distortion is zero at the center of
    # the image and we are interested in the world to image
    # homography at the center of the image. However, we don't
    # know the center of the image in world coordinates.
    # So we follow a procedure as explained below:
    #
    # First, we learn a homography from image to world
    # Next, we find where the image center `c_i` maps to in
    # world coordinates (`c_w`). Finally, we find the local
    # homography `LH0` from world to image at `c_w`
    #
    H_iw = learn_homography_i2w()
    c_i = np.array([im.shape[1], im.shape[0]]) / 2.
    c_w = H_iw.map(c_i)[:2]
    H_wi = learn_homography_w2i()
    LH0 = H_wi.get_homography_at(c_w)

    print('\nHomography at center')
    print('----------------------')
    print('      c_w =', c_w)
    print('      c_i =', c_i)
    print('LH0 * c_w =', H_wi.map(c_w))

    #
    # Obtain distortion estimate
    #       mapped + distortion = detected
    #  (or) distortion = detected - mapped
    #
    def homogeneous_coords(arr):
        return np.hstack([arr, np.ones((len(arr), 1))])

    subset_step = int(options.subset_step)
    if subset_step == 1:
        print('\nUsing all observed correspondences for training model ...')
    else:
        print(
            '\nUsing every %d-th observed correspondence for training model ...'
            % subset_step)

    det_i = det_i[::subset_step]
    det_w = det_w[::subset_step]

    mapped_i = LH0.dot(homogeneous_coords(det_w).T).T
    mapped_i = np.array([p / p[2] for p in mapped_i])
    mapped_i = mapped_i[:, :2]

    distortion = det_i - mapped_i  # image + distortion = mapped
    max_distortion = np.max([np.linalg.norm(u) for u in distortion])
    print('\nMaximum observed distortion is %.2f pixels' % max_distortion)

    #
    # Fit non-parametric model to the observations
    #
    model = GPModel(mapped_i, distortion)

    print('\nGP Hyper-parameters')
    print('---------------------')
    print('  x: ', model._gp_x._covf.theta)
    print('        log-likelihood: %.4f' % model._gp_x.model_evidence())
    print('  y: ', model._gp_y._covf.theta)
    print('        log-likelihood: %.4f' % model._gp_y.model_evidence())
    print('')
    print('  Optimization detail:')
    print('  [ x ]')
    print('  ' + str(model._gp_x.fit_result).replace('\n', '\n      '))
    print('  [ y ]')
    print('  ' + str(model._gp_y.fit_result).replace('\n', '\n      '))

    #
    # Use the non-parametric model to compute the source map
    # we compute the map in blocks to reduce our memory requirements
    #
    print('\nComputing source map')
    print('----------------------')
    H, W = im.shape
    center = np.array([W / 2., H / 2.])

    def scale_coords(xy):
        return (xy - center) * float(options.output_scale) + center

    source_map = np.zeros((2, H, W))

    B = 50  # Block size is 50x50
    for sy in range(0, H, B):
        sys.stdout.write('  ')

        for sx in range(0, W, B):
            ty, tx = min(sy + B, H), min(sx + B, W)
            dst_coords = np.array([[x, y] for y in range(sy, ty)
                                   for x in range(sx, tx)])
            dst_coords = scale_coords(dst_coords)
            src_coords = dst_coords + model.predict(dst_coords)
            source_map[1, sy:ty, sx:tx] = src_coords[:, 0].reshape(
                (ty - sy, tx - sx))
            source_map[0, sy:ty, sx:tx] = src_coords[:, 1].reshape(
                (ty - sy, tx - sx))

            sys.stdout.write('. ')
            sys.stdout.flush()
        sys.stdout.write('\n')

    #
    # output undistortion table if requested
    #
    if not options.no_write_table:
        table_filename = filename + '.table'
        print('\nWriting undistortion table to %s' % table_filename)

        import struct
        with open(table_filename, 'wb') as f:
            f.write(struct.pack('<II', H, W))
            for y in range(H):
                for x in range(W):
                    f.write(struct.pack('<dd', *source_map[:, y, x]))

    #
    # undistorted image if requested
    #
    if not options.no_write_image:
        import os.path
        render_filename = os.path.splitext(filename)[0] + '.fixed.png'
        print('\nRendering undistorted image: %s' % render_filename)

        from skimage.transform import warp
        im_fixed = img_as_ubyte(warp(im, source_map))

        from skimage.io import imsave
        imsave(render_filename, im_fixed)
Esempio n. 5
0
def main():
    im = imread(sys.argv[1])
    im = img_as_ubyte(im)

    tagdetector = AprilTagDetector()
    print "\n".join([ str((d.id, d.c)) for d in tagdetector.detect(im) ])