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
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
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
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)
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)
def err(x): rot = np.array(rotmat) rot[:3,:3] = expmap.axis2rot(x) draw(rot) return depthcost(d1, depthim)
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)