def triangulate(self, frame_1, frame_2, idx1, idx2): # triangulate the points we don't have matches for good_pts4d = np.array([frame_1.pts[i] is None for i in idx1]) # do triangulation in global frame pts4d = triangulate(frame_1.pose, frame_2.pose, frame_1.kps[idx1], frame_2.kps[idx2]) good_pts4d &= np.abs(pts4d[:, 3]) != 0 pts4d /= pts4d[:, 3:] # homogeneous 3-D coords return pts4d, good_pts4d
def process_frame(self, img, pose=None): start_time = time.time() assert img.shape[0:2] == (self.H, self.W) frame = Frame(self.mapp, img, self.K) if frame.id == 0: return f1 = self.mapp.frames[-1] f2 = self.mapp.frames[-2] idx1, idx2, Rt = match_frames(f1, f2) # add new observations if the point is already observed in the previous frame # TODO: consider tradeoff doing this before/after search by projection for i, idx in enumerate(idx2): if f2.pts[idx] is not None and f1.pts[idx1[i]] is None: f2.pts[idx].add_observation(f1, idx1[i]) if frame.id < 5 or True: # get initial positions from fundamental matrix f1.pose = np.dot(Rt, f2.pose) else: # kinematic model (not used) velocity = np.dot(f2.pose, np.linalg.inv(self.mapp.frames[-3].pose)) f1.pose = np.dot(velocity, f2.pose) # pose optimization if pose is None: #print(f1.pose) pose_opt = self.mapp.optimize(local_window=1, fix_points=True) print("Pose: %f" % pose_opt) #print(f1.pose) else: # have ground truth for pose f1.pose = pose sbp_pts_count = 0 # search by projection if len(self.mapp.points) > 0: # project *all* the map points into the current frame map_points = np.array([p.homogeneous() for p in self.mapp.points]) projs = np.dot(np.dot(K, f1.pose[:3]), map_points.T).T projs = projs[:, 0:2] / projs[:, 2:] # only the points that fit in the frame good_pts = (projs[:, 0] > 0) & (projs[:, 0] < self.W) & \ (projs[:, 1] > 0) & (projs[:, 1] < self.H) for i, p in enumerate(self.mapp.points): if not good_pts[i]: # point not visible in frame continue if f1 in p.frames: # we already matched this map point to this frame # TODO: understand this better continue for m_idx in f1.kd.query_ball_point(projs[i], 2): # if point unmatched if f1.pts[m_idx] is None: b_dist = p.orb_distance(f1.des[m_idx]) # if any descriptors within 64 if b_dist < 64.0: p.add_observation(f1, m_idx) sbp_pts_count += 1 break # triangulate the points we don't have matches for good_pts4d = np.array([f1.pts[i] is None for i in idx1]) # do triangulation in global frame pts4d = triangulate(f1.pose, f2.pose, f1.kps[idx1], f2.kps[idx2]) good_pts4d &= np.abs(pts4d[:, 3]) != 0 pts4d /= pts4d[:, 3:] # homogeneous 3-D coords # adding new points to the map from pairwise matches new_pts_count = 0 for i, p in enumerate(pts4d): if not good_pts4d[i]: continue # check parallax is large enough # TODO: learn what parallax means """ r1 = np.dot(f1.pose[:3, :3], add_ones(f1.kps[idx1[i]])) r2 = np.dot(f2.pose[:3, :3], add_ones(f2.kps[idx2[i]])) parallax = r1.dot(r2) / (np.linalg.norm(r1) * np.linalg.norm(r2)) if parallax >= 0.9998: continue """ # check points are in front of both cameras 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.kpus[idx1[i]] pp2 = (pp2[0:2] / pp2[2]) - f2.kpus[idx2[i]] pp1 = np.sum(pp1**2) pp2 = np.sum(pp2**2) if pp1 > 2 or pp2 > 2: continue # add the point color = img[int(round(f1.kpus[idx1[i], 1])), int(round(f1.kpus[idx1[i], 0]))] pt = Point(self.mapp, p[0:3], color) pt.add_observation(f2, idx2[i]) pt.add_observation(f1, idx1[i]) new_pts_count += 1 print("Adding: %d new points, %d search by projection" % (new_pts_count, sbp_pts_count)) # optimize the map if frame.id >= 4 and frame.id % 5 == 0: err = self.mapp.optimize() #verbose=True) print("Optimize: %f units of error" % err) print("Map: %d points, %d frames" % (len(self.mapp.points), len(self.mapp.frames))) print("Time: %.2f ms" % ((time.time() - start_time) * 1000.0)) print(np.linalg.inv(f1.pose))
def process_frame(img): start_time = time.time() img = cv2.resize(img, (W,H)) frame = Frame(mapp, img, K) if frame.id == 0: return f1 = mapp.frames[-1] f2 = mapp.frames[-2] idx1, idx2, Rt = match_frames(f1, f2) if frame.id < 5: # get initial positions from fundamental matrix f1.pose = np.dot(Rt, f2.pose) else: # kinematic model velocity = np.dot(f2.pose, np.linalg.inv(mapp.frames[-3].pose)) f1.pose = np.dot(velocity, f2.pose) # add new observations if the point is already observed in the previous frame # TODO: consider tradeoff doing this before/after search by projection for i,idx in enumerate(idx2): if f2.pts[idx] is not None and f1.pts[idx1[i]] is None: f2.pts[idx].add_observation(f1, idx1[i]) # pose optimization #print(f1.pose) pose_opt = mapp.optimize(local_window=1, fix_points=True) print("Pose: %f" % pose_opt) #print(f1.pose) # search by projection sbp_pts_count = 0 if len(mapp.points) > 0: map_points = np.array([p.homogeneous() for p in mapp.points]) projs = np.dot(np.dot(K, f1.pose[:3]), map_points.T).T projs = projs[:, 0:2] / projs[:, 2:] good_pts = (projs[:, 0] > 0) & (projs[:, 0] < W) & \ (projs[:, 1] > 0) & (projs[:, 1] < H) for i, p in enumerate(mapp.points): if not good_pts[i]: continue q = f1.kd.query_ball_point(projs[i], 5) for m_idx in q: if f1.pts[m_idx] is None: # if any descriptors within 32 for o in p.orb(): o_dist = hamming_distance(o, f1.des[m_idx]) if o_dist < 32.0: p.add_observation(f1, m_idx) sbp_pts_count += 1 break # triangulate the points we don't have matches for good_pts4d = np.array([f1.pts[i] is None for i in idx1]) # reject pts without enough "parallax" (this right?) pts4d = triangulate(f1.pose, f2.pose, f1.kps[idx1], f2.kps[idx2]) good_pts4d &= np.abs(pts4d[:, 3]) > 0.005 # homogeneous 3-D coords pts4d /= pts4d[:, 3:] # locally in front of camera # NOTE: This check is broken and maybe unneeded #pts_tri_local = np.dot(f1.pose, pts4d.T).T #good_pts4d &= pts_tri_local[:, 2] > 0 print("Adding: %d new points, %d search by projection" % (np.sum(good_pts4d), sbp_pts_count)) # adding new points to the map from pairwise matches for i,p in enumerate(pts4d): if not good_pts4d[i]: continue u,v = int(round(f1.kpus[idx1[i],0])), int(round(f1.kpus[idx1[i],1])) pt = Point(mapp, p[0:3], img[v,u]) pt.add_observation(f1, idx1[i]) pt.add_observation(f2, idx2[i]) # 2-D display if disp2d is not None: # paint annotations on the image for i1, i2 in zip(idx1, idx2): u1, v1 = int(round(f1.kpus[i1][0])), int(round(f1.kpus[i1][1])) u2, v2 = int(round(f2.kpus[i2][0])), int(round(f2.kpus[i2][1])) if f1.pts[i1] is not None: if len(f1.pts[i1].frames) >= 5: cv2.circle(img, (u1, v1), color=(0,255,0), radius=3) else: cv2.circle(img, (u1, v1), color=(0,128,0), 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)) disp2d.paint(img) # optimize the map if frame.id >= 4 and frame.id%5 == 0: err = mapp.optimize() print("Optimize: %f units of error" % err) # 3-D display if disp3d is not None: disp3d.paint(mapp) print("Map: %d points, %d frames" % (len(mapp.points), len(mapp.frames))) print("Time: %.2f ms" % ((time.time()-start_time)*1000.0))
def processFrame(self, image): """ :image: TODO :slam_map: TODO :returns: TODO """ start_time = time.time() frame = fm.Frame(image, self.K) self.slam_map.addFrame(frame) if frame.id == 0: return curr_frame = self.slam_map.frames[-1] prev_frame = self.slam_map.frames[-2] relative_pose, match_idx1, match_idx2 = self.matchFrame( prev_frame, curr_frame) # Compose the pose of the frame (this forms our pose estimate for optimize) curr_frame.pose = relative_pose @ prev_frame.pose print("Previous frame pose: \n{}\n".format(prev_frame.pose)) print("Relative pose: \n{}\n".format(relative_pose)) print("Current Pose before optimize: \n{}\n".format( np.linalg.inv(curr_frame.pose))) P1 = prev_frame.pose[0:3, :] P2 = curr_frame.pose[0:3, :] # If a point has been observed in previous frame, add a corresponding observation even in the current frame for i, idx in enumerate(match_idx1): if prev_frame.map_points[idx] is not None and curr_frame.map_points[ match_idx2[i]] is None: prev_frame.map_points[idx].addObservation( curr_frame, match_idx2[i]) # If the map contains points sbp_count = 0 if len(self.slam_map.points) > 0: # Optimize the pose only by keeping the map_points fixed reproj_error = self.slam_map.optimize(local_window=2, fix_points=True) print("Reprojection error after pose optimize: {}".format( reproj_error)) # proj_matches, sbp_count = self.searchByProjection(curr_frame) # Triangulate only those points that were not found by searchByProjection (innovative points) points3d, _ = helper.triangulate(P1, prev_frame.keypoints[match_idx1], P2, curr_frame.keypoints[match_idx2]) remaining_point_mask = np.array( [curr_frame.map_points[i] is None for i in match_idx2]) add_count = 0 for i, point3d in enumerate(points3d): if not remaining_point_mask[i]: continue # Check if 3D point is in front of both frames point4d_homo = np.hstack((point3d, 1)) point1 = prev_frame.pose @ point4d_homo point2 = curr_frame.pose @ point4d_homo if point1[2] < 0 and point2[2] < 0: continue proj_point1, _ = helper.projectPoints(point3d, prev_frame.pose, self.width, self.height) proj_point2, _ = helper.projectPoints(point3d, curr_frame.pose, self.width, self.height) # print("Proj points", proj_point1, proj_point2) # print("Frame keypoints", prev_frame.keypoints[match_idx1[i]], curr_frame.keypoints[match_idx2[i]]) err1 = np.sum( np.square(proj_point1 - prev_frame.keypoints[match_idx1[i]])) err2 = np.sum( np.square(proj_point2 - curr_frame.keypoints[match_idx2[i]])) # print(err1, err2) if err1 > 1 or err2 > 1: continue color = image[ int(np.round(curr_frame.keypoints[match_idx2[i]][1])), int(np.round(curr_frame.keypoints[match_idx2[i]][0]))] point = SlamPoint(points3d[i], color) point.addObservation(prev_frame, match_idx1[i]) point.addObservation(curr_frame, match_idx2[i]) self.slam_map.addPoint(point) add_count += 1 if frame.id > 4 and frame.id % 5 == 0: self.slam_map.optimize() print("Added points in map: {} Search By projection: {}".format( add_count, sbp_count)) print("Map: {} points, {} frames".format(len(self.slam_map.points), len(self.slam_map.frames))) print("Time: {:.2f} ms".format((time.time() - start_time) * 1000.0)) # wait = input("Enter to continue") return frame.drawFrame(prev_frame.keypoints_un[match_idx1], curr_frame.keypoints_un[match_idx2])