Example #1
0
def correction2modelmat(R_aligned, x, y, z, r):
    import expmap
    R = expmap.axis2rot(np.array([0, -r * np.pi / 2, 0]))

    R_correct = R_aligned.copy()
    R_correct[:3, :] = np.dot(R, R_correct[:3, :])
    R_correct[:3, 3] += [x * config.LW, 0, z * config.LW]
    return R_correct
Example #2
0
def correction2modelmat(R_aligned, x, y, z, r):
    import expmap

    R = expmap.axis2rot(np.array([0, -r * np.pi / 2, 0]))

    R_correct = R_aligned.copy()
    R_correct[:3, :] = np.dot(R, R_correct[:3, :])
    R_correct[:3, 3] += [x * config.LW, 0, z * config.LW]
    return R_correct
Example #3
0
def backproject_depth(im, depth, corners):
    # Use 'extrinsic rectification' to find plane equation
    global obj, cam, distcc, rvec, tvec, bp
    obj = np.mgrid[:6, :8, :1].astype('f').reshape(3, -1) * 0.0254
    f = 583.0
    cx = 321
    cy = 249
    distcc = np.zeros((4, 1), 'f')
    rvec = np.zeros((3, 1), 'f')
    tvec = np.zeros((3, 1), 'f')
    cam = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]])
    cv.FindExtrinsicCameraParams2(obj, corners, cam, distcc, rvec, tvec)

    # Back project to see how it went
    bp = np.array(corners)
    cv.ProjectPoints2(obj, rvec, tvec, cam, distcc, bp)

    global pts1
    # Show the points in a point cloud, using rvec and tvec
    RT = np.eye(4).astype('f')
    RT[:3, :3] = expmap.axis2rot(rvec.squeeze())
    RT[:3, 3] = tvec.squeeze()
    #RT = np.linalg.inv(RT)
    pts1 = np.dot(RT[:3, :3], obj) + RT[:3, 3].reshape(3, 1)
    pts1[1, :] *= -1
    pts1[2, :] *= -1
    rgb1 = np.zeros((pts1.shape[1], 4), 'f')
    rgb1[:, :] = [0, 0, 0, 1]

    # Also show the points in the region, using the calib image
    global mask, pts2
    bounds = corners[[0, 7, 5 * 8 + 7, 5 * 8], :]
    polys = (tuple([tuple(x) for x in tuple(bounds)]), )
    mask = np.zeros((480, 640), 'f')
    cv.FillPoly(mask, polys, [1, 0, 0])
    mask = (mask > 0) & (depth < 2047)
    v, u = np.mgrid[:480, :640].astype('f')
    pts2 = np.vstack(project(depth[mask].astype('f'), u[mask], v[mask]))
    rgb2 = np.zeros((pts2.shape[1], 4), 'f')
    rgb2[:, :] = [0, 1, 0, 1]

    if np.any(np.isnan(pts2.mean(1))): return

    window.update_points(
        np.hstack((pts1, pts2)).transpose(), np.vstack((rgb1, rgb2)))
    window.lookat = pts1.mean(1)
    window.Refresh()

    return
def backproject_depth(im, depth, corners):
  # Use 'extrinsic rectification' to find plane equation
  global obj, cam, distcc, rvec, tvec,bp
  obj = np.mgrid[:6,:8,:1].astype('f').reshape(3,-1)*0.0254
  f = 583.0
  cx = 321
  cy = 249
  distcc = np.zeros((4,1),'f')
  rvec = np.zeros((3,1),'f')
  tvec = np.zeros((3,1),'f')
  cam = np.array([[f,0,cx],[0,f,cy],[0,0,1]])
  cv.FindExtrinsicCameraParams2(obj, corners, cam, distcc, rvec,tvec)

  # Back project to see how it went
  bp = np.array(corners)
  cv.ProjectPoints2(obj, rvec, tvec, cam, distcc, bp)

  global pts1
  # Show the points in a point cloud, using rvec and tvec
  RT = np.eye(4).astype('f')
  RT[:3,:3] = expmap.axis2rot(rvec.squeeze())
  RT[:3,3] = tvec.squeeze()
  #RT = np.linalg.inv(RT)
  pts1 = np.dot(RT[:3,:3], obj) + RT[:3,3].reshape(3,1)
  pts1[1,:] *= -1
  pts1[2,:] *= -1
  rgb1 = np.zeros((pts1.shape[1],4),'f')
  rgb1[:,:] = [0,0,0,1]

  # Also show the points in the region, using the calib image 
  global mask,pts2
  bounds = corners[[0,7,5*8+7,5*8],:]
  polys = (tuple([tuple(x) for x in tuple(bounds)]),)
  mask = np.zeros((480,640),'f')
  cv.FillPoly(mask, polys, [1,0,0])
  mask = (mask>0)&(depth<2047)
  v,u = np.mgrid[:480,:640].astype('f')
  pts2 = np.vstack(project(depth[mask].astype('f'),u[mask],v[mask]))
  rgb2 = np.zeros((pts2.shape[1],4),'f')
  rgb2[:,:] = [0,1,0,1]

  if np.any(np.isnan(pts2.mean(1))): return

  window.update_points(np.hstack((pts1,pts2)).transpose(),np.vstack((rgb1,rgb2)))
  window.lookat = pts1.mean(1)
  window.Refresh()
  
  return
Example #5
0
def nearest(R_previous, R_aligned):
    # Find the nearest rotation
    import expmap
    M = [expmap.axis2rot(np.array([0,-i*np.pi/2,0])) for i in range(4)]
    rs = [np.dot(m, R_aligned[:3,:3]) for m in M]
    global dots
    dots = [np.dot(r[0,:3], R_previous[0,:3]) for r in rs]
    rot = np.argmax(dots)
    R_correct = R_aligned.copy()
    R_correct[:3,:3] = rs[rot]
    R_correct[:3,3] = np.dot(M[rot], R_correct[:3,3])

    # Find the nearest translation
    bx = int(np.round((R_previous[0,3]-R_correct[0,3])/config.LW))
    bz = int(np.round((R_previous[2,3]-R_correct[2,3])/config.LW))

    R_correct[0,3] += config.LW * bx
    R_correct[2,3] += config.LW * bz
    return R_correct, (bx,bz,rot)
Example #6
0
def nearest(R_previous, R_aligned):
    # Find the nearest rotation
    import expmap
    M = [expmap.axis2rot(np.array([0,-i*np.pi/2,0])) for i in range(4)]
    rs = [np.dot(m, R_aligned[:3,:3]) for m in M]
    global dots
    dots = [np.dot(r[0,:3], R_previous[0,:3]) for r in rs]
    rot = np.argmax(dots)
    R_correct = R_aligned.copy()
    R_correct[:3,:3] = rs[rot]
    R_correct[:3,3] = np.dot(M[rot], R_correct[:3,3])

    # Find the nearest translation
    bx = int(np.round((R_previous[0,3]-R_correct[0,3])/config.LW))
    bz = int(np.round((R_previous[2,3]-R_correct[2,3])/config.LW))

    R_correct[0,3] += config.LW * bx
    R_correct[2,3] += config.LW * bz
    return R_correct, (bx,bz,rot)
Example #7
0
	def err(x):
		rot = np.array(rotmat)
		rot[:3,:3] = expmap.axis2rot(x)
		draw(rot)
		return depthcost(d1, depthim)
Example #8
0
	except: pass
	window = pyglet.window.Window(width=width, height=height)
	
getcontext()

rotmat = np.array([
[ 0.84804809, -0.20705596,  0.48779327,  0.        ],
[ 0.        ,  0.92050487,  0.39073113,  0.        ],
[-0.52991927, -0.33135879,  0.78063238,  0.        ],
[ 0.        ,  0.        , -0.80000001,  1.        ]]).transpose()

r0 = expmap.rot2axis(rotmat[:3,:3])
#r1 = r0 + np.array([0.3,0.3,0.3])
r1 = r0 + (np.random.rand(3)*2-1)*0.3

rm0 = np.array(rotmat); rotmat[:3,:3] = expmap.axis2rot(r0)
rm1 = np.array(rm0); rm1[:3,:3] = expmap.axis2rot(r1)

def draw(rotmat):
	clearcolor = [0,0,0,0]
	glClearColor(*clearcolor)
	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT)
	glEnable(GL_DEPTH_TEST)
	glMatrixMode(GL_PROJECTION)
	glLoadIdentity()
	gluPerspective(60, 1, 0.3,20)
	glMatrixMode(GL_MODELVIEW)
	glLoadMatrixf(rotmat.transpose())
	lego.draw_pieces(tasks.taskconfig.model)
	global depthim
	depthim = glReadPixels(0, 0, width, height, GL_DEPTH_COMPONENT, GL_FLOAT)