<jupyter_start><jupyter_text>so we should have a grid spacing of 10' in the x direction and 12' in the y direction, divided by 8'1"<jupyter_code>def Rmat(theta):
    S, C = np.sin(theta), np.cos(theta)
    return np.array([
        [C, S],
        [-S, C]
    ])

def RdRmat(theta):
    S, C = np.sin(theta), np.cos(theta)
    return np.array([
        [C, S],
        [-S, C]
    ]), np.array([
        [-S, C],
        [-C, -S]
    ])

xspc = 10/8.1
yspc = 12/8.1
xypts = pts[:, (gray[ceilmask] > 240)]
xypts = np.dot(Rmat(0.0), xypts)
xydist = ((xypts.T + [0.25 + xspc/2, 0.85 + yspc/2]) % [xspc, yspc]) - [xspc/2, yspc/2]
plt.scatter(*xydist.T)
plt.axis('equal')
plt.xlim(-xspc/2, xspc)
#plt.ylim(-yspc/2, yspc)
def mkgrid(xspc, yspc, N, u, v, theta):
    mg = (np.mgrid[:N, :N].reshape(2, -1).T - [(N-1)/2, (N-1)/2]) * [xspc, yspc] - [u, v]
    mg = np.dot(Rmat(theta), mg.T)
    mg = np.vstack([mg, np.ones((1, mg.shape[1]))])
    mg = np.dot(cv2.Rodrigues(np.array([0, (-22.)*np.pi/180., 0]))[0], mg)
    mg = mg[:, mg[2] > 0]
    mg /= mg[2]
    return cv2.fisheye.distortPoints(mg.T[None, :, :2].astype(np.float32), K, dist)

plt.figure(figsize=(12, 10))
plt.imshow(gray*ceilmask > 240, cmap='gray')
plt.plot(*mkgrid(xspc, yspc, 13, 0.25564975,  -0.67761807, 0.00103883)[0].T, '+')
plt.ylim(480, 0)
def moddist(x, q):
    return (x+q/2)%q - q/2

plt.plot(np.arange(-30, 30), moddist(np.arange(-30, 30), 10))
# pixel cost:
# Rxy R.xy
# cost = 1/2 [moddist(Rx - u, xspc)**2,  moddist(Ry - v, yspc)**2]
# dcost/duv = [-(Rx - u),    0   ]
#             [    0,    -(Ry, v)]
# dcost/dtheta = [(Rx - u)*d(Rx)/dtheta, (Ry - v)*d(Ry)/dtheta]

def moddist(x, q):
    return (x+q/2)%q - q/2

def match(gray):
    return pts[:, gray[ceilmask] > 240]

def cost(xy, u, v, theta):
    N = xy.shape[1]
    x = xy[0]
    y = xy[1]
    S = np.sin(theta)
    C = np.cos(theta)
    dRx = x*S - C*y
    dRy = x*C + S*y
    dx = moddist(x*C + y*S - u, xspc)
    dy = moddist(-x*S + y*C - v, yspc)
    S2 = np.sum(dRx)
    S3 = np.sum(dRy)
    JTJ = np.array([[N, 0, S2], [0, N, S3], [S2, S3, np.sum(x**2 + y**2)]])
    JTr = np.array([-np.sum(dx), -np.sum(dy), -np.sum(dx*dRx + dy*dRy)])
    return 0.5*np.sum(dx**2 + dy**2), -np.linalg.solve(JTJ + np.eye(3), JTr)

xy = match(gray)
print(cost(xy, -.248, .145, 0.0126))
%timeit cost(xy, -.248, .145, 0.0126)
#e = 1e-6
#s, *_ = cost(gray, -2.25, 0.7, 0.03)
#s1, *_ = cost(gray, -2.25+e, 0.7, 0.03)
#s2, *_ = cost(gray, -2.25, 0.7+e, 0.03)
#s3, *_ = cost(gray, -2.25, 0.7, 0.03+e)
#print(np.sum(s))
#print(np.sum(Ju, axis=1) + np.sum(Jv, axis=1))
#print(np.sum(s1-s)/e, np.sum(s2-s)/e, np.sum(s3-s)/e)
# 8.0 159701.53572861606
# 8.1 150074.75218936958
# 8.2 143326.9812395357
# 8.3 139387.52407613743
# 8.4 138146.47735365925
# 8.5 139404.41798315747
ceilheight = 8.0
xspc = 10/ceilheight
#xspc = 2/ceilheight
yspc = 12/ceilheight

b = np.zeros(3)

xy = match(gray)
for i in range(10):
    c, dB = cost(xy, *b)
    print(b, c, dB)
    b += dB
print(b)
f = open("../../logs/cl20190707/cycloid-20190707-162426.rec", "rb")
track = []
gyroz = []
b = np.zeros(3)
totalc = 0
for frame in recordreader.RecordIterator(f):
    gray = frame['yuv420'][:480]
    gyroz.append(frame['carstate'][3][2])
    xy = match(gray)
    for i in range(6):
        c, dB = cost(xy, *b)
        b += dB
    track.append(b.copy())
    totalc += c
f.close()
print(totalc)
track = np.array(track)
plt.figure(figsize=(12,10))
plt.plot(*track[:, :2].T, '.')
mg = (np.mgrid[:8, :4].reshape(2, -1).T - [5, 0]) * [xspc, yspc]
plt.plot(mg[:, 0], mg[:, 1], '+')
plt.plot(*track[0, :2].T, 'x')
plt.axis('equal')
plt.plot(-30*(track[1:, 2] - track[:-1, 2]))
plt.plot(gyroz)
floorheight = 0.2 / ceilheight  # it stands 90mm off the ground, which is almost exactly 0.3 feet (4")
floorlut = frontend.genlut()[0]
fxy = floorlut[:2] / floorlut[2]
#floormask = (floorlut[2] < 0) & (np.sum(fxy**2, axis=0) < 128**2) & (np.sum(origpts**2, axis=2) < 6**2) & (gray > 50)
floormask = (floorlut[2] < 0) & (np.sum(fxy**2, axis=0) < 128**2) & (np.sum(origpts**2, axis=2) < 6**2) & (gray > 50)

plt.figure(figsize=(12,10))
plt.imshow(bgr[:, :, ::-1]*floormask[:, :, None] + 170*(1-floormask[:, :, None]))
fpts = floorheight * fxy[:, floormask]
plt.axis('equal')
plt.scatter(*np.dot(Rmat(0.), fpts), s=1, c=bgr[floormask, ::-1]/255.0)
vidout = cv2.VideoWriter("mapping.mp4", cv2.VideoWriter_fourcc('X', '2', '6', '4'), 30, (640, 360), True)

ceilheight = 8.25
xspc = 10/ceilheight
yspc = 12/ceilheight
#xspc = 2/ceilheight
#yspc = 12/ceilheight
floorheight = 0.23 / ceilheight  # it stands 90mm off the ground, which is almost exactly 0.3 feet (4")

floorlut, origpts = frontend.genlut()
fxy = floorlut[:2] / floorlut[2]
floormask = (floorlut[2] < 0) & (np.sum(fxy**2, axis=0) < 16**2) & (np.sum(origpts**2, axis=2) < 6**2) & (gray > 50)
fpts = floorheight * fxy[:, floormask]

f = open("../../logs/cl20190707/cycloid-20190707-183147.rec", "rb")
#f = open("../../logs/cl20190707/cycloid-20190707-162426.rec", "rb")
track = []
totalc = 0
mapsz = 360
floormapbgr = np.zeros((mapsz, mapsz, 3))
floormapN = np.ones((mapsz, mapsz))
Z = 40
b = np.float32([230/Z, 40/Z, 0])
cmapsz = 50
ceilmapY = np.zeros((cmapsz, cmapsz))
ceilmapN = np.ones((cmapsz, cmapsz))
track = []
for frame in recordreader.RecordIterator(f):
    gray = frame['yuv420'][:480]
    bgr = cv2.cvtColor(frame['yuv420'], cv2.COLOR_YUV2BGR_I420)
    gyroz.append(frame['carstate'][3][2])

    # use wheel odometry for correction
    # b[2] -= frame['carstate'][3][2] / 30.0

    xy = match(gray)
    for i in range(6):
        c, dB = cost(xy, *b)
        b += dB
    track.append(b.copy())
    totalc += c
    
    p = Z*(np.dot(Rmat(b[2]), fpts).T + b[:2])
    mask2 = (p[:, 0] >= 0) & (p[:, 1] >= 0) & (p[:, 0] < mapsz-1) & (p[:, 1] < mapsz-1)
    p = p[mask2]
    pi = p.astype(np.int)
    idxs = pi[:, 1] * mapsz + pi[:, 0]
    
    floormapN[:] += np.bincount(idxs, np.ones(len(idxs)), mapsz*mapsz).reshape((-1, mapsz))
    for i in range(3):
        floormapbgr[:, :, i] += np.bincount(idxs, bgr[floormask, i][mask2], mapsz*mapsz).reshape((-1, mapsz))
    #floormapbgr[pi[:, 1], pi[:, 0], :] = bgr[floormask][mask2]
    fview = (floormapbgr / floormapN[:, :, None]).astype(np.uint8)
    cv2.line(fview,
             (int(Z*b[0]), int(Z*b[1])),
             (int(Z*b[0] - 20*np.cos(b[2])), int(Z*b[1] + 20*np.sin(b[2]))),
             (0, 255, 0),
             1)
    
    p = (-np.dot(Rmat(b[2]), pts).T + b[:2])/xspc
    p[:, 0] += 8.5
    p[:, 1] += 20
    ceilp = p
    mask2 = (p[:, 0] >= 0) & (p[:, 1] >= 0) & (p[:, 0] < cmapsz-1) & (p[:, 1] < cmapsz-1)
    p = p[mask2]
    pi = p.astype(np.int)
    idxs = pi[:, 1] * cmapsz + pi[:, 0]
    
    ceilmapN[:] += np.bincount(idxs, np.ones(len(idxs)), cmapsz*cmapsz).reshape((-1, cmapsz))
    ceilmapY[:, :] += np.bincount(idxs, 255 * (gray[ceilmask][mask2] > 240), cmapsz*cmapsz).reshape((-1, cmapsz))
        
    Knew = np.array([
        [-250, 0, 320],
        [0, 250, 260],
        [0, 0, 1]
    ])
    R=cv2.Rodrigues(np.array([0, (22.-90)*np.pi/180., 0]))[0]
    R = np.dot(np.array([[0, 1, 0], [1, 0, 0], [0, 0, 1]]), R)

    for gp in mkgrid(xspc, yspc, 101, *-b)[0]:
        cv2.circle(bgr, (int(gp[0]), int(gp[1])), 3, (255, 0, 0), 1)
    bgr[~(ceilmask | floormask)] //= 3
    vframe = np.zeros((360, 640, 3), np.uint8)
    vframe[:, :480] = cv2.resize(bgr, (480, 360), cv2.INTER_LINEAR)
    # remaining area is 160x360
    vframe[:, 480:] = fview[:160, :, :].transpose((1, 0, 2))
    vidout.write(vframe)

    cv2.imshow("vframe", vframe)
    #cv2.imshow("ceilmap", cv2.resize(
    #    (ceilmapY / ceilmapN).astype(np.uint8), (cmapsz*16, cmapsz*16), interpolation=cv2.INTER_NEAREST))
    #cv2.imshow("front", cv2.fisheye.undistortImage(bgr, K=K, D=dist, Knew=np.dot(Knew, R), new_size=(640, 360)))
    #cv2.imshow("map", fview[::-1])
    k = cv2.waitKey(1)
    if k == ord('q'):
        break
    
f.close()
del vidout
print(totalc)
cv2.destroyAllWindows()
cv2.waitKey(1)
plt.imshow(floormapbgr / floormapN[:, :, None] / 255.0)

plt.scatter(p[:, 0], p[:, 1])
p = ceilp
#p = (np.dot(Rmat(b[2]), pts).T - b[:2])/xspc
p.shape
plt.scatter(p[mask2, 0], p[mask2, 1], s=1, c=bgr[ceilmask, :][mask2]/255.0)
#plt.axis('equal')
for i in range(19, 22):
    plt.axvline(i)
for i in range(-1, 2):
    plt.axhline(i)

plt.figure(figsize=(12,10))
plt.imshow((ceilmapY / np.sqrt(ceilmapN))[10:42] > 10000)
plt.plot(*(track[0]/xspc + [8.5, 20, 0])[:2], 'o')
# okay, this is the final light map we'll use from now on

lightmap = (((ceilmapY / np.sqrt(ceilmapN)) > 10000).astype(np.int)[10:42])

# now we need to generate a distance function to find the closest point on the grid
# run Dijkstra's algorithm on this to get minimum (x, y) distance (manhattan metric)
# then add the vector distance minus 1, so that:
# if the pixel is white and expected to be white, the distance is zero,
# but if it's white and not expected to be, it's the distance to the nearest white edge.
# we're doing squared x and y distances separately, so we precompute the x, y vector at each point on the map
# if the nearest is in the positive direction, use dist - fpart(u)
# if it's negative, use dist + fpart(u)-1

# ... or something like that?
u = np.linspace(0.1, 4.9, 100)
uf = u - u.astype(np.int)
dist = (5 - u).astype(np.int) + 1
plt.plot(u, dist)
plt.plot(u, dist - uf)
dist = (u).astype(np.int)
plt.show()
plt.plot(u, dist)
plt.plot(u, dist + uf - 1)
# dijkstra's algorithm on lightmap
ldist = np.zeros((lightmap.shape[0], lightmap.shape[1], 2))
ldist[lightmap != 1] = 1000
while np.any(ldist == 1000):
#for i in range(100):
    cost = np.sum(np.abs(ldist)**2, axis=2)
    # now propagate cost in four directions:
    left = cost[:, 1:] < cost[:, :-1]
    ldist[:, :-1][left] = ldist[:, 1:][left] + [1, 0]
    
    right = cost[:, :-1] < cost[:, 1:]
    ldist[:, 1:][right] = ldist[:, :-1][right] + [-1, 0]
    
    up = cost[1:] < cost[:-1]
    ldist[:-1, :][up] = ldist[1:, :][up] + [0, 1]
    
    down = cost[:-1] < cost[1:]
    ldist[1:, :][down] = ldist[:-1, :][down] + [0, -1]

#plt.imshow(cost[:, :, 0] == cost[:, :, 0])
plt.imshow(np.clip(ldist[:, :, [0, 0, 1]] + 5*(cost == 0)[:, :, None], -10, 10) / 20 + 0.5)
plt.imshow(cost == 0)
with open("../../logs/cl20190707/cycloid-20190707-162426.rec", "rb") as f:
    for frame in recordreader.RecordIterator(f):
        bgr = cv2.cvtColor(frame['yuv420'], cv2.COLOR_YUV2BGR_I420)
        break
def undistortMap():
    CAM_TILT = np.array([0, 22. * np.pi / 180., 0])
    
    global K, dist
    K = np.load("../../tools/camcal/camera_matrix.npy")
    dist = np.load("../../tools/camcal/dist_coeffs.npy")
    K[:2] /= 4.05
    fx, fy = np.diag(K)[:2]
    cx, cy = K[:2, 2]
    uv = np.mgrid[:480, :640][[1, 0]].transpose(1, 2, 0).astype(np.float32)
    R = cv2.Rodrigues(CAM_TILT)[0]
    R = np.dot(cv2.Rodrigues(np.array([0.015, 0, 0]))[0], R)
    origpts = cv2.fisheye.undistortPoints(uv, K=K, D=dist)
    pts = np.stack([origpts[:, :, 0], origpts[:, :, 1], np.ones((480, 640))])
    return np.dot(R, pts.transpose(1, 0, 2)), origpts

pts, _ = undistortMap()

horizonang = np.tan(5*np.pi/180)
horizonmask = np.abs(pts[2]) <= horizonang
#plt.imshow(horizonmask[:, :, None]*bgr[:, :, ::-1])

Knew = np.array([
    [-250/3, 0, 320],
    [0, 250*5, 260],
    [0, 0, 1]
])
R=cv2.Rodrigues(np.array([0, (22.-90)*np.pi/180., 0]))[0]
R = np.dot(np.array([[0, 1, 0], [1, 0, 0], [0, 0, 1]]), R)
#plt.figure(figsize=(12, 6))
#plt.imshow(cv2.fisheye.undistortImage(horizonmask[:, :, None]*bgr, K=K, D=dist, Knew=np.dot(Knew, R), new_size=(640, 360))[:, :, ::-1])

with open("../../logs/cl20190707/cycloid-20190707-162426.rec", "rb") as f:
    for frame in recordreader.RecordIterator(f):
        bgr = cv2.cvtColor(frame['yuv420'], cv2.COLOR_YUV2BGR_I420)
        im = cv2.fisheye.undistortImage(horizonmask[:, :, None]*bgr, K=K, D=dist, Knew=np.dot(Knew, R), new_size=(640, 360))
        #im = bgr * horizonmask[:, :, None]
        cv2.imshow("im", im)
        k = cv2.waitKey(1)
        if k == ord('q'):
            break
cv2.destroyAllWindows()
cv2.waitKey(1)<jupyter_output><empty_output>
StarcoderdataJupyterStructuredCleanDedup