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
