def _add_point(graph, reconstruction, point_id, observations): graph.add_node(point_id, bipartite=1) for shot_id in observations: graph.add_edge(shot_id, point_id) point = types.Point() point.id = point_id reconstruction.add_point(point)
def triangulate_robust(self, track, reproj_threshold, min_ray_angle_degrees): """Triangulate track in a RANSAC way and add point to reconstruction.""" os, bs, ids = [], [], [] for shot_id, obs in self.tracks_manager.get_track_observations(track).items(): if shot_id in self.reconstruction.shots: shot = self.reconstruction.shots[shot_id] os.append(self._shot_origin(shot)) b = shot.camera.pixel_bearing(np.array(obs.point)) r = self._shot_rotation_inverse(shot) bs.append(r.dot(b)) ids.append(shot_id) if len(ids) < 2: return best_inliers = [] best_point = types.Point() best_point.id = track combinatiom_tried = set() ransac_tries = 11 # 0.99 proba, 60% inliers all_combinations = list(combinations(range(len(ids)), 2)) thresholds = len(os) * [reproj_threshold] for i in range(ransac_tries): random_id = int(np.random.rand()*(len(all_combinations)-1)) if random_id in combinatiom_tried: continue i, j = all_combinations[random_id] combinatiom_tried.add(random_id) os_t = [os[i], os[j]] bs_t = [bs[i], bs[j]] e, X = pygeometry.triangulate_bearings_midpoint( os_t, bs_t, thresholds, np.radians(min_ray_angle_degrees)) if X is not None: reprojected_bs = X-os reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis] inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < reproj_threshold if sum(inliers) > sum(best_inliers): best_inliers = inliers best_point.coordinates = X.tolist() pout = 0.99 inliers_ratio = float(sum(best_inliers))/len(ids) if inliers_ratio == 1.0: break optimal_iter = math.log(1.0-pout)/math.log(1.0-inliers_ratio*inliers_ratio) if optimal_iter <= ransac_tries: break if len(best_inliers) > 1: self.reconstruction.add_point(best_point) for i, succeed in enumerate(best_inliers): if succeed: self._add_track_to_graph_inlier(track, ids[i])
def test_shot_neighborhood_linear_graph(): graph = nx.Graph() reconstruction = types.Reconstruction() graph.add_node('im0', bipartite=0) shot = types.Shot() shot.id = 'im0' reconstruction.add_shot(shot) for i in range(1, 4): shot = types.Shot() shot.id = 'im' + str(i) point = types.Point() point.id = str(i) prev_shot_id = 'im' + str(i - 1) reconstruction.add_shot(shot) reconstruction.add_point(point) graph.add_node(shot.id, bipartite=0) graph.add_node(point.id, bipartite=1) graph.add_edge(shot.id, point.id) graph.add_edge(prev_shot_id, point.id) interior, boundary = opensfm.reconstruction.shot_neighborhood( graph, reconstruction, 'im2', 1) assert interior == set(['im2']) assert boundary == set(['im1', 'im3']) interior, boundary = opensfm.reconstruction.shot_neighborhood( graph, reconstruction, 'im2', 2) assert interior == set(['im1', 'im2', 'im3']) assert boundary == set(['im0']) interior, boundary = opensfm.reconstruction.shot_neighborhood( graph, reconstruction, 'im2', 3) assert interior == set(['im0', 'im1', 'im2', 'im3']) assert boundary == set()
def triangulate(self, track, reproj_threshold, min_ray_angle_degrees, return_reason=False): """Triangulate a track and add point to reconstruction.""" os, bs = [], [] for shot_id in self.graph[track]: # This will not add in new image, it will only triangulate the shots that are included if shot_id in self.reconstruction.shots: # The formed track # one track, and the subset of the images in reconstruction right now shot = self.reconstruction.shots[shot_id] os.append(self._shot_origin(shot)) x = self.graph[track][shot_id]['feature'] b = shot.camera.pixel_bearing(np.array(x)) r = self._shot_rotation_inverse(shot) bs.append(r.dot(b)) if len(os) >= 2: # error and triangulated 3D point e, X = csfm.triangulate_bearings_midpoint( os, bs, reproj_threshold, np.radians(min_ray_angle_degrees)) if X is not None: point = types.Point() point.id = track point.coordinates = X.tolist() self.reconstruction.add_point(point) else: e = 4 if return_reason: return e '''
def add_points_to_reconstruction(points, color, reconstruction): shift = len(reconstruction.points) for i in range(points.shape[0]): point = types.Point() point.id = str(shift+i) point.coordinates = points[i, :] point.color = color reconstruction.add_point(point)
def copy_cluster_points(cluster, tracks, points, noise): for shot in cluster.shots: for point in tracks[shot]: base = points[point] copy = types.Point() copy.id = base.id copy.coordinates = base.coordinates+np.random.rand()*noise cluster.add_point(copy) return cluster
def point_from_json(key, obj): """ Read a point from a json object """ point = types.Point() point.id = key point.color = obj["color"] point.coordinates = obj["coordinates"] return point
def copy_cluster_points(cluster, tracks_manager, points, noise): for shot in cluster.shots: for point in tracks_manager.get_shot_observations(shot): base = points[point] copy = types.Point() copy.id = base.id copy.coordinates = base.coordinates+np.random.rand()*noise cluster.add_point(copy) return cluster
def point_from_json(key, obj): """ Read a point from a json object """ point = types.Point() point.id = key point.color = obj["color"] point.coordinates = obj["coordinates"] if "reprojection_error" in obj: point.reprojection_error = obj["reprojection_error"] return point
def synthetic_reconstruction(): cube_dataset = data_generation.CubeDataset(10, 100, 0.001, 0.3) synthetic_reconstruction = types.Reconstruction() for shot in cube_dataset.shots.values(): synthetic_reconstruction.add_shot(shot) for camera in cube_dataset.cameras.values(): synthetic_reconstruction.add_camera(camera) for point_id, point in cube_dataset.points.items(): point_type = types.Point() point_type.coordinates = point point_type.id = point_id synthetic_reconstruction.add_point(point_type) return synthetic_reconstruction, cube_dataset.tracks
def triangulate_dlt(self, track, reproj_threshold, min_ray_angle_degrees): """Triangulate track using DLT and add point to reconstruction.""" Rts, bs = [], [] for shot_id in self.graph[track]: if shot_id in self.reconstruction.shots: shot = self.reconstruction.shots[shot_id] Rts.append(self._shot_Rt(shot)) x = self.graph[track][shot_id]['feature'] b = shot.camera.pixel_bearing(np.array(x)) bs.append(b) if len(Rts) >= 2: e, X = csfm.triangulate_bearings_dlt( Rts, bs, reproj_threshold, np.radians(min_ray_angle_degrees)) if X is not None: point = types.Point() point.id = track point.coordinates = X.tolist() self.reconstruction.add_point(point)
def test_shot_neighborhood_complete_graph(): graph = nx.Graph() reconstruction = types.Reconstruction() point = types.Point() point.id = '1' reconstruction.add_point(point) graph.add_node(point.id, bipartite=1) for i in range(4): shot = types.Shot() shot.id = 'im' + str(i) reconstruction.add_shot(shot) graph.add_node(shot.id, bipartite=0) graph.add_edge(shot.id, point.id) interior, boundary = opensfm.reconstruction.shot_neighborhood( graph, reconstruction, 'im2', 2) assert interior == set(['im0', 'im1', 'im2', 'im3']) assert boundary == set()
def triangulate(self, track, reproj_threshold, min_ray_angle_degrees): """Triangulate track and add point to reconstruction.""" os, bs = [], [] for shot_id in self.graph[track]: if shot_id in self.reconstruction.shots: shot = self.reconstruction.shots[shot_id] os.append(self._shot_origin(shot)) x = self.graph[track][shot_id]['feature'] b = shot.camera.pixel_bearing(np.array(x)) r = self._shot_rotation_inverse(shot) bs.append(r.dot(b)) if len(os) >= 2: e, X = csfm.triangulate_bearings_midpoint( os, bs, reproj_threshold, np.radians(min_ray_angle_degrees)) if X is not None: point = types.Point() point.id = track point.coordinates = X.tolist() self.reconstruction.add_point(point)
def triangulate_dlt(self, track, reproj_threshold, min_ray_angle_degrees): """Triangulate track using DLT and add point to reconstruction.""" Rts, bs, ids = [], [], [] for shot_id, obs in self.tracks_manager.get_track_observations(track).items(): if shot_id in self.reconstruction.shots: shot = self.reconstruction.shots[shot_id] Rts.append(self._shot_Rt(shot)) b = shot.camera.pixel_bearing(np.array(obs.point)) bs.append(b) ids.append(shot_id) if len(Rts) >= 2: e, X = pygeometry.triangulate_bearings_dlt( Rts, bs, reproj_threshold, np.radians(min_ray_angle_degrees)) if X is not None: point = types.Point() point.id = track point.coordinates = X.tolist() self.reconstruction.add_point(point) for shot_id in ids: self._add_track_to_graph_inlier(track, shot_id)
def __init__(self, num_cameras, num_points, noise): self.cameras = {} for i in range(num_cameras): camera = types.PerspectiveCamera() camera.id = 'camera' + str(i) camera.focal = 0.9 camera.k1 = -0.1 camera.k2 = 0.01 camera.height = 600 camera.width = 800 self.cameras[camera.id] = camera self.shots = {} r = 2.0 for i in range(num_cameras): phi = np.random.rand() * math.pi theta = np.random.rand() * 2.0 * math.pi x = r * np.sin(theta) * np.cos(phi) y = r * np.sin(theta) * np.sin(phi) z = r * np.cos(theta) position = [x, y, z] alpha = np.random.rand() lookat = [0.0, 0, 0] up = [alpha * 0.2, alpha * 0.2, 1.0] shot = types.Shot() shot.id = 'shot' + str(i) shot.camera = self.cameras['camera' + str(i)] shot.pose = camera_pose(position, lookat, up) self.shots[shot.id] = shot points = np.random.rand(num_points, 3) - [0.5, 0.5, 0.5] self.points = {} for i, p in enumerate(points): pt = types.Point() pt.id = 'point' + str(i) pt.coordinates = p pt.color = [100, 100, 20] self.points[pt.id] = pt
def triangulate(self, track, reproj_threshold, min_ray_angle_degrees): """Triangulate track and add point to reconstruction.""" os, bs, ids = [], [], [] for shot_id, obs in self.tracks_manager.get_track_observations(track).items(): if shot_id in self.reconstruction.shots: shot = self.reconstruction.shots[shot_id] os.append(self._shot_origin(shot)) b = shot.camera.pixel_bearing(np.array(obs.point)) r = self._shot_rotation_inverse(shot) bs.append(r.dot(b)) ids.append(shot_id) if len(os) >= 2: thresholds = len(os) * [reproj_threshold] e, X = pygeometry.triangulate_bearings_midpoint( os, bs, thresholds, np.radians(min_ray_angle_degrees)) if X is not None: point = types.Point() point.id = track point.coordinates = X.tolist() self.reconstruction.add_point(point) for shot_id in ids: self._add_track_to_graph_inlier(track, shot_id)
def triangulate_track(track, graph, reconstruction, Rt_by_id, reproj_threshold, min_ray_angle_degrees): min_ray_angle = np.radians(min_ray_angle_degrees) Rts, bs = [], [] for shot_id in graph[track]: if shot_id in reconstruction.shots: shot = reconstruction.shots[shot_id] if shot_id not in Rt_by_id: Rt_by_id[shot_id] = shot.pose.get_Rt() Rts.append(Rt_by_id[shot_id]) x = graph[track][shot_id]['feature'] b = shot.camera.pixel_bearing(np.array(x)) bs.append(b) if len(Rts) >= 2: e, X = csfm.triangulate_bearings(Rts, bs, reproj_threshold, min_ray_angle) if X is not None: point = types.Point() point.id = track point.coordinates = X.tolist() reconstruction.add_point(point)
def import_bundler(data_path, bundle_file, list_file, track_file, reconstruction_file=None): """ Reconstruction and tracks graph from Bundler's output """ # Init OpenSfM working folder. mkdir_p(data_path) # Copy image list. list_dir = os.path.dirname(list_file) with open(list_file, 'rb') as fin: lines = fin.read().splitlines() ordered_shots = [] image_list = [] for line in lines: image_path = os.path.join(list_dir, line.split()[0]) rel_to_data = os.path.relpath(image_path, data_path) image_list.append(rel_to_data) ordered_shots.append(os.path.basename(image_path)) with open(os.path.join(data_path, 'image_list.txt'), 'w') as fout: fout.write('\n'.join(image_list) + '\n') # Check for bundle_file if not bundle_file or not os.path.isfile(bundle_file): return None with open(bundle_file, 'rb') as fin: lines = fin.readlines() offset = 1 if '#' in lines[0] else 0 # header num_shot, num_point = map(int, lines[offset].split(' ')) offset += 1 # initialization reconstruction = types.Reconstruction() # cameras for i in xrange(num_shot): # Creating a model for each shot. shot_key = ordered_shots[i] focal, k1, k2 = map(float, lines[offset].rstrip('\n').split(' ')) if focal > 0: im = imread(os.path.join(data_path, image_list[i])) height, width = im.shape[0:2] camera = types.PerspectiveCamera() camera.id = 'camera_' + str(i) camera.width = width camera.height = height camera.focal = focal / max(width, height) camera.k1 = k1 camera.k2 = k2 reconstruction.add_camera(camera) # Shots rline = [] for k in xrange(3): rline += lines[offset + 1 + k].rstrip('\n').split(' ') R = ' '.join(rline) t = lines[offset + 4].rstrip('\n').split(' ') R = np.array(map(float, R.split())).reshape(3, 3) t = np.array(map(float, t)) R[1], R[2] = -R[1], -R[2] # Reverse y and z t[1], t[2] = -t[1], -t[2] shot = types.Shot() shot.id = shot_key shot.camera = camera shot.pose = types.Pose() shot.pose.set_rotation_matrix(R) shot.pose.translation = t reconstruction.add_shot(shot) else: print 'ignore failed image', shot_key offset += 5 # tracks track_lines = [] for i in xrange(num_point): coordinates = lines[offset].rstrip('\n').split(' ') color = lines[offset + 1].rstrip('\n').split(' ') point = types.Point() point.id = i point.coordinates = map(float, coordinates) point.color = map(int, color) reconstruction.add_point(point) view_line = lines[offset + 2].rstrip('\n').split(' ') num_view, view_list = int(view_line[0]), view_line[1:] for k in xrange(num_view): shot_key = ordered_shots[int(view_list[4 * k])] if shot_key in reconstruction.shots: camera = reconstruction.shots[shot_key].camera scale = max(camera.width, camera.height) v = '{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}'.format( shot_key, i, view_list[4 * k + 1], float(view_list[4 * k + 2]) / scale, -float(view_list[4 * k + 3]) / scale, point.color[0], point.color[1], point.color[2] ) track_lines.append(v) offset += 3 # save track file with open(track_file, 'wb') as fout: fout.writelines('\n'.join(track_lines)) # save reconstruction if reconstruction_file is not None: with open(reconstruction_file, 'wb') as fout: obj = reconstructions_to_json([reconstruction]) json_dump(obj, fout) return reconstruction