def align_reconstruction_orientation_prior_similarity(reconstruction, config):
X, Xp = [], []
orientation_type = config.get('align_orientation_prior', 'horizontal')
onplane, verticals = [], []
for shot in reconstruction.shots.values():
X.append(shot.pose.get_origin())
Xp.append(shot.metadata.gps_position)
R = shot.pose.get_rotation_matrix()
x, y, z = get_horitzontal_and_vertical_directions(R, shot.metadata.orientation)
if orientation_type == 'no_roll':
onplane.append(x)
verticals.append(-y)
elif orientation_type == 'horizontal':
onplane.append(x)
onplane.append(z)
verticals.append(-y)
elif orientation_type == 'vertical':
onplane.append(x)
onplane.append(y)
verticals.append(-z)
X = np.array(X)
Xp = np.array(Xp)
# Estimate ground plane.
p = multiview.fit_plane(X - X.mean(axis=0), onplane, verticals)
Rplane = multiview.plane_horizontalling_rotation(p)
X = Rplane.dot(X.T).T
# Estimate 2d similarity to align to GPS
if (len(X) < 2 or
X.std(axis=0).max() < 1e-8 or # All points are the same.
Xp.std(axis=0).max() < 0.01): # All GPS points are the same.
s = len(X) / max(1e-8, X.std(axis=0).max()) # Set the arbitrary scale proportional to the number of cameras.
A = Rplane
b = Xp.mean(axis=0) - X.mean(axis=0)
else:
T = tf.affine_matrix_from_points(X.T[:2], Xp.T[:2], shear=False)
s = np.linalg.det(T[:2,:2])**(1./2)
A = np.eye(3)
A[:2,:2] = T[:2,:2] / s
A = A.dot(Rplane)
b = np.array([T[0,2],
T[1,2],
Xp[:,2].mean() - s * X[:,2].mean()]) # vertical alignment
return s, A, b