예제 #1
0
def main():
  import argparse
  parser = argparse.ArgumentParser()
  parser.add_argument('--test', action='store_true')
  args = parser.parse_args()

  if args.test:
    import tests
    tests.run_tests()

  cam_t = (0, -.5)
  r_angle = 0
  fov = 75 * np.pi/180.
  cam1d = flatland.Camera1d(cam_t, r_angle, fov, SIZE)
  cam2d = flatland.Camera2d(WORLD_MIN, WORLD_MAX, SIZE)

  empty_sdf = grid.SpatialFunction(WORLD_MIN[0], WORLD_MAX[0], WORLD_MIN[1], WORLD_MAX[1], np.ones((SIZE, SIZE)))
  tracker = Tracker(empty_sdf)

  poly_center = np.array([0., 0.])
  poly_scale = .5
  poly_rot = 0.
  while True:
    poly_pts = np.array([[-0.5, -0.5], [ 0.5, -0.5], [ 0.5,  0.5], [-0.5,  0.5]]).dot(flatland.rotation2d(poly_rot).T)*poly_scale + poly_center[None,:]

    #poly = flatland.Polygon([[.2, .2], [0,1], [1,1], [1,.5]])
    poly = flatland.Polygon(poly_pts)
    polylist = [poly]

    image1d, depth1d = cam1d.render(polylist)
    # print 'depth1d', depth1d

    depth_min, depth_max = 0, 1
    depth1d_normalized = (np.clip(depth1d, depth_min, depth_max) - depth_min)/(depth_max - depth_min)
    depth1d_image = np.array([[.5, 0, 0]])*depth1d_normalized[:,None] + np.array([[1., 1., 1.]])*(1. - depth1d_normalized[:,None])
    depth1d_image[np.logical_not(np.isfinite(depth1d))] = (0, 0, 0)

    observed_XYs = cam1d.unproject(depth1d)
    filtered_obs_XYs = np.array([p for p in observed_XYs if np.isfinite(p).all()])

    obs_render_list = [flatland.Point(p, c) for (p, c) in zip(observed_XYs, depth1d_image) if np.isfinite(p).all()]
    # print 'obs world', filtered_obs_XYs
    camera_poly_list = [flatland.make_camera_poly(cam1d.t, cam1d.r_angle, fov)]
    image2d = cam2d.render(polylist + obs_render_list + camera_poly_list)

    flatland.show_1d_image([image1d, depth1d_image], "image1d+depth1d")
    flatland.show_2d_image(image2d, "image2d")
    cv2.moveWindow("image2d", 0, 0)
    key = cv2.waitKey() & 255
    print "key", key

    # linux
    if key == 81:
        cam1d.t[0] += .1
    elif key == 82:
        cam1d.t[1] += .1
    elif key == 84:
        cam1d.t[1] -= .1
    elif key == 83:
        cam1d.t[0] -= .1
    elif key == ord('['):
        cam1d.r_angle -= .1
    elif key == ord(']'):
        cam1d.r_angle += .1

    elif key == ord('q'):
        break

    if key == ord('a'):
        poly_center[0] += .01
    elif key == ord('w'):
        poly_center[1] += .01
    elif key == ord('s'):
        poly_center[1] -= .01
    elif key == ord('d'):
        poly_center[0] -= .01
    elif key == ord('-'):
      poly_rot -= .1
    elif key == ord('='):
      poly_rot += .1

    elif key == ord('c'):
      tracker = Tracker(empty_sdf)
      tracker.plot()
      print 'zeroed out sdf and control'

    elif key == ord('i'):
      # initialization
      # compute sdf of starting state as initialization
      image2d = cam2d.render(polylist)
      init_state_edge = np.ones((SIZE, SIZE), dtype=int)
      is_edge = image2d[:,:,0] > .5
      init_state_edge[is_edge] = 0
      init_sdf_img = distance_transform_edt(init_state_edge) * PIXEL_SIDE
      # negate inside the boundary
      orig_filling = [p.filled for p in polylist]
      for p in polylist: p.filled = True
      image2d_filled = cam2d.render(polylist)
      for orig, p in zip(orig_filling, polylist): p.filled = orig
      init_sdf_img[image2d_filled[:,:,0] > .5] *= -1
      init_sdf = grid.SpatialFunction.FromImage(WORLD_MIN[0], WORLD_MAX[0], WORLD_MIN[1], WORLD_MAX[1], init_sdf_img)

      tracker = Tracker(init_sdf)
      tracker.observe(filtered_obs_XYs)
      tracker.plot()

    elif key == ord('o'):
      obs = filtered_obs_XYs
      # hack: ensure obs occurs only on grid

      obs_inds = np.c_[empty_sdf.to_grid_inds(obs[:,0], obs[:,1])].round()
      print 'grid inds', obs_inds
      obs = np.c_[empty_sdf.to_world_xys(obs_inds[:,0], obs_inds[:,1])]

      # print 'orig obs', obs
      # render2d = flatland.Render2d(cam2d.bl, cam2d.tr, cam2d.width)
      # xys = obs.dot(render2d.P[:2,:2].T) + render2d.P[:2,2]
      # ixys = xys.astype(int)
      # pts = []
      # for ix, iy in ixys:
      #   if 0 <= iy < render2d.image.shape[0] and 0 <= ix < render2d.image.shape[1]:
      #     pts.append([ix,iy])
      # print 'orig pts', pts
      # pts = np.array(pts)
      # obs = pts
      # Pinv = np.linalg.inv(render2d.P)
      # obs = np.array(pts).dot(Pinv[:2,:2].T) + Pinv[:2,2]
      # print 'rounded obs', obs
      # print 'rounded obs inds', empty_sdf.to_grid_inds(obs[:,0], obs[:,1])

      tracker.observe(obs)
      tracker.plot()
      print 'observed.'

    elif key == ord(' '):
      tracker.optimize_sqp()
      tracker.plot()
예제 #2
0
def main():
    import argparse
    parser = argparse.ArgumentParser()
    parser.add_argument('--test', action='store_true')
    args = parser.parse_args()

    if args.test:
        import tests
        tests.run_tests()

    cam_t = (0, -.5)
    r_angle = 0
    fov = 75 * np.pi / 180.
    cam1d = flatland.Camera1d(cam_t, r_angle, fov, SIZE)
    cam2d = flatland.Camera2d(WORLD_MIN, WORLD_MAX, SIZE)

    tracker = Tracker(np.ones((SIZE, SIZE)))

    poly_center = np.array([0., 0.])
    poly_scale = .5
    poly_rot = 0.
    while True:
        poly_pts = np.array([[-0.5, -0.5], [0.5, -0.5], [0.5, 0.5], [
            -0.5, 0.5
        ]]).dot(flatland.rotation2d(poly_rot).T) * poly_scale + poly_center[
            None, :]

        #poly = flatland.Polygon([[.2, .2], [0,1], [1,1], [1,.5]])
        poly = flatland.Polygon(poly_pts)
        polylist = [poly]

        image1d, depth1d = cam1d.render(polylist)
        # print 'depth1d', depth1d

        depth_min, depth_max = 0, 1
        depth1d_normalized = (np.clip(depth1d, depth_min, depth_max) -
                              depth_min) / (depth_max - depth_min)
        depth1d_image = np.array(
            [[.5, 0, 0]]) * depth1d_normalized[:, None] + np.array(
                [[1., 1., 1.]]) * (1. - depth1d_normalized[:, None])
        depth1d_image[np.logical_not(np.isfinite(depth1d))] = (0, 0, 0)

        # observed_XYs = cam1d.unproject(depth1d)
        # filtered_obs_XYs = np.array([p for p in observed_XYs if np.isfinite(p).all()])
        # obs_render_list = [flatland.Point(p, c) for (p, c) in zip(observed_XYs, depth1d_image) if np.isfinite(p).all()]
        # print 'obs world', filtered_obs_XYs
        obs_render_list = []
        camera_poly_list = [
            flatland.make_camera_poly(cam1d.t, cam1d.r_angle, fov)
        ]
        image2d = cam2d.render(polylist + obs_render_list + camera_poly_list)
        image2d_poly = cam2d.render(polylist)
        #import IPython; IPython.embed()

        drawn_inds = np.nonzero((abs(image2d_poly) > .00001).astype(int).sum(
            axis=2))  # HACK: assumes not drawn is filled with 0
        image2d[drawn_inds] = [0, 1, 0]
        drawn_pts = np.transpose(drawn_inds)

        P = flatland.Render2d(cam2d.bl, cam2d.tr, cam2d.width).P
        observed_XYs = np.array(
            [p for p in cam1d.unproject(depth1d) if np.isfinite(p).all()])
        observed_px = np.round(observed_XYs.dot(P[:2, :2].T) +
                               P[:2, 2]).astype(int)
        # from scipy.spatial import KDTree
        # kdt = KDTree(drawn_pts)
        # print observed_px
        # print kdt.query(observed_px)
        # observed_drawn_pts = drawn_pts[kdt.query(observed_px)[1]]
        #image2d[observed_drawn_pts[:,1],observed_drawn_pts[:,0]] = [1,0,0]
        image2d[observed_px[:, 1], observed_px[:, 0]] = [0, 0, 1]
        Pinv = np.linalg.inv(P)
        filtered_obs_XYs = observed_px.dot(Pinv[:2, :2].T) + Pinv[:2, 2]

        flatland.show_1d_image([image1d, depth1d_image], "image1d+depth1d")
        flatland.show_2d_image(image2d, "image2d")
        cv2.moveWindow("image2d", 0, 0)
        key = cv2.waitKey() & 255
        print "key", key

        # linux
        if key == 81:
            cam1d.t[0] += .1
        elif key == 82:
            cam1d.t[1] += .1
        elif key == 84:
            cam1d.t[1] -= .1
        elif key == 83:
            cam1d.t[0] -= .1
        elif key == ord('['):
            cam1d.r_angle -= .1
        elif key == ord(']'):
            cam1d.r_angle += .1

        elif key == ord('q'):
            break

        if key == ord('a'):
            poly_center[0] += .01
        elif key == ord('w'):
            poly_center[1] += .01
        elif key == ord('s'):
            poly_center[1] -= .01
        elif key == ord('d'):
            poly_center[0] -= .01
        elif key == ord('-'):
            poly_rot -= .1
        elif key == ord('='):
            poly_rot += .1

#   elif key == ord('c'):
#     tracker = Tracker(empty_sdf)
#     tracker.plot()
#     print 'zeroed out sdf and control'

        elif key == ord('i'):
            # initialization
            # compute sdf of starting state as initialization
            image2d = cam2d.render(polylist)
            init_state_edge = np.ones((SIZE, SIZE), dtype=int)
            is_edge = image2d[:, :, 0] > .5
            init_state_edge[is_edge] = 0
            init_sdf_img = distance_transform_edt(init_state_edge) * np.sqrt(
                sqp_problem.Config.PIXEL_AREA)
            # negate inside the boundary
            orig_filling = [p.filled for p in polylist]
            for p in polylist:
                p.filled = True
            image2d_filled = cam2d.render(polylist)
            for orig, p in zip(orig_filling, polylist):
                p.filled = orig
            init_sdf_img[image2d_filled[:, :, 0] > .5] *= -1
            init_sdf = init_sdf_img
            #init_sdf = sqp_problem.make_interp(init_sdf_img)#grid.SpatialFunction.FromImage(WORLD_MIN[0], WORLD_MAX[0], WORLD_MIN[1], WORLD_MAX[1], init_sdf_img)

            tracker = Tracker(init_sdf)  #smooth_by_edt(init_sdf))
            tracker.observe(filtered_obs_XYs)
            tracker.plot()

        elif key == ord('o'):
            tracker.observe(filtered_obs_XYs)
            tracker.plot()
            print 'observed.'

        elif key == ord(' '):
            tracker.optimize_sqp()
            tracker.plot()
예제 #3
0
def main():
  import argparse
  parser = argparse.ArgumentParser()
  parser.add_argument('--test', action='store_true')
  args = parser.parse_args()

  if args.test:
    import tests
    tests.run_tests()

  cam_t = (0, -.5)
  r_angle = 0
  fov = 75 * np.pi/180.
  cam1d = flatland.Camera1d(cam_t, r_angle, fov, SIZE)
  cam2d = flatland.Camera2d(WORLD_MIN, WORLD_MAX, SIZE)

  tracker = Tracker(np.ones((SIZE, SIZE)))

  poly_center = np.array([0., 0.])
  poly_scale = .5
  poly_rot = 0.
  while True:
    poly_pts = np.array([[-0.5, -0.5], [ 0.5, -0.5], [ 0.5,  0.5], [-0.5,  0.5]]).dot(flatland.rotation2d(poly_rot).T)*poly_scale + poly_center[None,:]

    #poly = flatland.Polygon([[.2, .2], [0,1], [1,1], [1,.5]])
    poly = flatland.Polygon(poly_pts)
    polylist = [poly]

    image1d, depth1d = cam1d.render(polylist)
    # print 'depth1d', depth1d

    depth_min, depth_max = 0, 1
    depth1d_normalized = (np.clip(depth1d, depth_min, depth_max) - depth_min)/(depth_max - depth_min)
    depth1d_image = np.array([[.5, 0, 0]])*depth1d_normalized[:,None] + np.array([[1., 1., 1.]])*(1. - depth1d_normalized[:,None])
    depth1d_image[np.logical_not(np.isfinite(depth1d))] = (0, 0, 0)

    # observed_XYs = cam1d.unproject(depth1d)
    # filtered_obs_XYs = np.array([p for p in observed_XYs if np.isfinite(p).all()])
    # obs_render_list = [flatland.Point(p, c) for (p, c) in zip(observed_XYs, depth1d_image) if np.isfinite(p).all()]
    # print 'obs world', filtered_obs_XYs
    obs_render_list = []
    camera_poly_list = [flatland.make_camera_poly(cam1d.t, cam1d.r_angle, fov)]
    image2d = cam2d.render(polylist + obs_render_list + camera_poly_list)
    image2d_poly = cam2d.render(polylist)
    #import IPython; IPython.embed()

    drawn_inds = np.nonzero((abs(image2d_poly) > .00001).astype(int).sum(axis=2)) # HACK: assumes not drawn is filled with 0
    image2d[drawn_inds] = [0,1,0]
    drawn_pts = np.transpose(drawn_inds)

    P = flatland.Render2d(cam2d.bl, cam2d.tr, cam2d.width).P
    observed_XYs = np.array([p for p in cam1d.unproject(depth1d) if np.isfinite(p).all()])
    observed_px = np.round(observed_XYs.dot(P[:2,:2].T) + P[:2,2]).astype(int)
    # from scipy.spatial import KDTree
    # kdt = KDTree(drawn_pts)
    # print observed_px
    # print kdt.query(observed_px)
    # observed_drawn_pts = drawn_pts[kdt.query(observed_px)[1]]
    #image2d[observed_drawn_pts[:,1],observed_drawn_pts[:,0]] = [1,0,0]
    image2d[observed_px[:,1],observed_px[:,0]] = [0,0,1]
    Pinv = np.linalg.inv(P)
    filtered_obs_XYs = observed_px.dot(Pinv[:2,:2].T) + Pinv[:2,2]


    flatland.show_1d_image([image1d, depth1d_image], "image1d+depth1d")
    flatland.show_2d_image(image2d, "image2d")
    cv2.moveWindow("image2d", 0, 0)
    key = cv2.waitKey() & 255
    print "key", key

    # linux
    if key == 81:
        cam1d.t[0] += .1
    elif key == 82:
        cam1d.t[1] += .1
    elif key == 84:
        cam1d.t[1] -= .1
    elif key == 83:
        cam1d.t[0] -= .1
    elif key == ord('['):
        cam1d.r_angle -= .1
    elif key == ord(']'):
        cam1d.r_angle += .1

    elif key == ord('q'):
        break

    if key == ord('a'):
        poly_center[0] += .01
    elif key == ord('w'):
        poly_center[1] += .01
    elif key == ord('s'):
        poly_center[1] -= .01
    elif key == ord('d'):
        poly_center[0] -= .01
    elif key == ord('-'):
      poly_rot -= .1
    elif key == ord('='):
      poly_rot += .1

#   elif key == ord('c'):
#     tracker = Tracker(empty_sdf)
#     tracker.plot()
#     print 'zeroed out sdf and control'

    elif key == ord('i'):
      # initialization
      # compute sdf of starting state as initialization
      image2d = cam2d.render(polylist)
      init_state_edge = np.ones((SIZE, SIZE), dtype=int)
      is_edge = image2d[:,:,0] > .5
      init_state_edge[is_edge] = 0
      init_sdf_img = distance_transform_edt(init_state_edge) * np.sqrt(sqp_problem.Config.PIXEL_AREA)
      # negate inside the boundary
      orig_filling = [p.filled for p in polylist]
      for p in polylist: p.filled = True
      image2d_filled = cam2d.render(polylist)
      for orig, p in zip(orig_filling, polylist): p.filled = orig
      init_sdf_img[image2d_filled[:,:,0] > .5] *= -1
      init_sdf = init_sdf_img
      #init_sdf = sqp_problem.make_interp(init_sdf_img)#grid.SpatialFunction.FromImage(WORLD_MIN[0], WORLD_MAX[0], WORLD_MIN[1], WORLD_MAX[1], init_sdf_img)

      tracker = Tracker(init_sdf)#smooth_by_edt(init_sdf))
      tracker.observe(filtered_obs_XYs)
      tracker.plot()

    elif key == ord('o'):
      tracker.observe(filtered_obs_XYs)
      tracker.plot()
      print 'observed.'

    elif key == ord(' '):
      tracker.optimize_sqp()
      tracker.plot()