class Day11: MOVE = { 0: Point(0, 1), 1: Point(1, 0), 2: Point(0, -1), 3: Point(-1, 0) } def __init__(self, filename, debug=0): self.intcode = Intcode.from_file(filename, debug=debug) self.process = self.intcode.create_process() self.data = defaultdict(int) self.current = Point(0, 0) self.dir = 0 def run(self): self.data[Point(0, 0)] = 1 while not self.process.is_terminated(): self.process.set_input(self.data[self.current]) col = self.process.run_to_next_output() lr = self.process.run_to_next_output() self.data[self.current] = col if lr == 0: self.dir = (self.dir-1) % 4 elif lr == 1: self.dir = (self.dir+1) % 4 else: print("bad output") self.current = self.current.add(self.MOVE[self.dir]) def print_data(self): xmin, xmax, ymin, ymax = 0, 0, 0, 0 for p in self.data.keys(): if p.x < xmin: xmin = p.x if p.x > xmax: xmax = p.x if p.y < ymin: ymin = p.y if p.y > ymax: ymax = p.y for y in range(ymax, ymin-1, -1): for x in range(xmin, xmax+1): p = Point(x, y) if self.data[p] == 1: print('#', end='') else: print('.', end='') print()
def process_frame(mapp, img, K, Kinv, plot2d, plot3d): t1 = time.time() # first resize: frame = Frame(mapp, K, Kinv, img) if frame.id == 0: return # current frame f1 = mapp.frames[-1] # previous frame f2 = mapp.frames[-2] print(f1.id) # once we have two frames: # step1: match two images to get point indices and Rt # these indices whose points correspond have passed RANSAC filter idx1, idx2, pose_matrix, _ = match_frame(f1, f2) f1.pose = np.dot(pose_matrix, f2.pose) # step2: # add new observation if the point is already observed in the previous frame for i, idx in enumerate(idx2): if f2.key_pts[idx] is not None and f1.key_pts[idx1[i]] is None: f2.key_pts[idx].add(f1, idx1[i]) # idx1 and idx2 correspond to the same list of points but in two adjacent frames # because key points have the same length as raw points: # idx1 is the index of the points that have match for # we want to triangulate the key points that we don't have match for: good_pts = np.array([f1.key_pts[ii] is None for ii in idx1]) # using normalized coordinates to compute triangulation # this will return the key points' homogeneous coordinates: pts_homo = triangulate(f1, f2, normalize(f1.Kinv, f1.raw_pts[idx1]), normalize(f2.Kinv, f2.raw_pts[idx2])) good_pts &= np.abs(pts_homo[:, 3]) != 0 pts_homo /= pts_homo[:, 3:] # for a qualified point to be added to the map, it has to pass the following tests: new_pts_count = 0 for i, p in enumerate(pts_homo): if not good_pts[i]: continue # check if the points are in front of the camera pl1 = np.dot(f1.pose, p) pl2 = np.dot(f2.pose, p) if pl1[2] < 0 or pl2[2] < 0: continue # reproject: pp1 = np.dot(K, pl1[:3]) pp2 = np.dot(K, pl2[:3]) # check reprojection error: pp1 = (pp1[0:2] / pp1[2]) - f1.raw_pts[idx1[i]] pp2 = (pp2[0:2] / pp2[2]) - f2.raw_pts[idx2[i]] pp1 = np.sum(pp1**2) pp2 = np.sum(pp2**2) if pp1 > 2 or pp2 > 2: continue # using the colored pixel in the frame at the point location to color the point color = img[int(round(f1.raw_pts[idx1[i], 1])), int(round(f1.raw_pts[idx1[i], 0]))] pt = Point(mapp, p[0:3], color) pt.add(f1, idx1[i]) pt.add(f2, idx2[i]) new_pts_count += 1 # by these two filters, adding the qualified points print("New points added: %d" % new_pts_count) # now display the result (in both 2D and 3D mapping): if plot2d is not None: # paint annotations on the image for i1, i2 in zip(idx1, idx2): u1, v1 = int(round(f1.raw_pts[i1][0])), int( round(f1.raw_pts[i1][1])) u2, v2 = int(round(f2.raw_pts[i2][0])), int( round(f2.raw_pts[i2][1])) # this is for plotting the matching line: if f1.key_pts[i1] is not None: if len(f1.key_pts[i1].frames) >= 5: cv2.circle(img, (u1, v1), color=(0, 255, 0), radius=3) else: cv2.circle(img, (u1, v1), color=(0, 0, 255), radius=3) else: cv2.circle(img, (u1, v1), color=(0, 0, 0), radius=3) cv2.line(img, (u1, v1), (u2, v2), color=(255, 0, 0)) # font = cv2.FONT_HERSHEY_SIMPLEX # cv2.putText(img, 'FPS: %.2F' % (1/(time.time()-t1)), (10, 500), font, 1, (0, 0, 255), 2, cv2.LINE_AA) plot2d.draw(img) # Now doing Bundle Adjustment for every 10 frames: if frame.id >= 9 and frame.id % 10 == 0: err = mapp.optimize() print("Optimizing the frame: %f units of error" % err) # now display the 3D map: if plot3d is not None: plot3d.draw(mapp) print('FPS: %.1f' % (1 / (time.time() - t1)))