def estimate_ground_plane(reconstruction, config): """Estimate ground plane orientation. It assumes cameras are all at a similar height and uses the align_orientation_prior option to enforce cameras to look horizontally or vertically. """ orientation_type = config['align_orientation_prior'] onplane, verticals = [], [] if orientation_type == 'plane_based': for shot in reconstruction.shots.values(): R = shot.pose.get_rotation_matrix() x, y, z = get_horizontal_and_vertical_directions( R, shot.metadata.orientation) verticals.append(-z) ground_points = np.array( [point.coordinates for point in reconstruction.points.values()]) ground_points -= ground_points.mean(axis=0) plane = multiview.fit_plane(ground_points, onplane, verticals) return plane for shot in reconstruction.shots.values(): R = shot.pose.get_rotation_matrix() x, y, z = get_horizontal_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) ground_points = [] for shot in reconstruction.shots.values(): ground_points.append(shot.pose.get_origin()) ground_points = np.array(ground_points) ground_points -= ground_points.mean(axis=0) plane = multiview.fit_plane(ground_points, onplane, verticals) return plane
def estimate_ground_plane(reconstruction, config): """Estimate ground plane orientation. It assumes cameras are all at a similar height and uses the align_orientation_prior option to enforce cameras to look horizontally or vertically. """ orientation_type = config['align_orientation_prior'] onplane, verticals = [], [] # added to realign the reconstruction if orientation_type == 'plane_based': pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector( np.array([ point.coordinates for point in reconstruction.points.values() ])) plane_model, _ = pcd.segment_plane(distance_threshold=1, ransac_n=3, num_iterations=1000) # we assume the ground is flat enough that the normal is # the up vector. the normal has to be in the direction of the # "ground_points" ground_points = [] for shot in reconstruction.shots.values(): ground_points.append(shot.pose.get_origin()) ones_col = np.ones((len(ground_points), 1)) ground_points = np.array(ground_points) ground_points = np.hstack((ground_points, ones_col)) signs = np.sum(np.sign(ground_points @ plane_model.reshape((4, 1)))) if signs > 0: return plane_model else: return -plane_model for shot in reconstruction.shots.values(): R = shot.pose.get_rotation_matrix() x, y, z = get_horizontal_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) ground_points = [] for shot in reconstruction.shots.values(): ground_points.append(shot.pose.get_origin()) ground_points = np.array(ground_points) ground_points -= ground_points.mean(axis=0) plane = multiview.fit_plane(ground_points, onplane, verticals) return plane
def transform_reconstruction(reconstruction, ref_shots_dict): """ transform recon based on two reference positions :param reconstruction: :param ref_shots_dict: :return: """ X, Xp = [], [] onplane, verticals = [], [] for shot_id in ref_shots_dict: X.append(reconstruction.shots[shot_id].pose.get_origin()) Xp.append(ref_shots_dict[shot_id]) R = reconstruction.shots[shot_id].pose.get_rotation_matrix() onplane.append(R[0,:]) onplane.append(R[2,:]) verticals.append(R[1,:]) 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 pdr predictions T = tf.affine_matrix_from_points(X.T[:2], Xp.T[:2], shear=False) s = np.linalg.det(T[:2, :2]) ** 0.5 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 ]) # Align points. for point in reconstruction.points.values(): p = s * A.dot(point.coordinates) + b point.coordinates = p.tolist() # Align cameras. for shot in reconstruction.shots.values(): R = shot.pose.get_rotation_matrix() t = np.array(shot.pose.translation) Rp = R.dot(A.T) tp = -Rp.dot(b) + s * t try: shot.pose.set_rotation_matrix(Rp) shot.pose.translation = list(tp) except: logger.debug("unable to transform reconstruction!")
def align_reconstruction_orientation_prior(reconstruction, config): X, Xp = [], [] orientation_type = config.get('align_orientation_prior', 'horizontal') onplane, verticals = [], [] for shot in reconstruction['shots'].values(): X.append(optical_center(shot)) Xp.append(shot['gps_position']) R = cv2.Rodrigues(np.array(shot['rotation']))[0] x, y, z = get_horitzontal_and_vertical_directions( R, shot['exif_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 apply_similarity(reconstruction, s, A, b)
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
def get_sab_2d(filtered_shots, config): X, Xp = [], [] orientation_type = config['align_orientation_prior'] onplane, verticals = [], [] for shot in filtered_shots: X.append(shot.pose.get_origin()) Xp.append(shot.metadata.gps_position) R = shot.pose.get_rotation_matrix() x, y, z = get_horizontal_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 T = tf.affine_matrix_from_points(X.T[:2], Xp.T[:2], shear=False) s = np.linalg.det(T[:2, :2])**0.5 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
def estimate_ground_plane(reconstruction: types.Reconstruction, config: Dict[str, Any]) -> Optional[np.ndarray]: """Estimate ground plane orientation. It assumes cameras are all at a similar height and uses the align_orientation_prior option to enforce cameras to look horizontally or vertically. """ orientation_type = config["align_orientation_prior"] onplane, verticals = [], [] for shot in reconstruction.shots.values(): R = shot.pose.get_rotation_matrix() x, y, z = get_horizontal_and_vertical_directions( R, shot.metadata.orientation.value) 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) ground_points = [] for shot in reconstruction.shots.values(): ground_points.append(shot.pose.get_origin()) ground_points = np.array(ground_points) ground_points -= ground_points.mean(axis=0) try: plane = multiview.fit_plane(ground_points, np.array(onplane), np.array(verticals)) except ValueError: return None return plane
def align_reconstruction_orientation_prior_similarity(reconstruction, config): """Align with GPS data assuming particular a camera orientation. In some cases, using 3D-3D matches directly fails to find proper orientation of the world. That happends mainly when all cameras lie close to a straigh line. In such cases, we can impose a particular orientation of the cameras to improve the orientation of the alignment. The config parameter `align_orientation_prior` can be used to specify such orientation. Accepted values are: - no_roll: assumes horizon is horizontal on the images - horizontal: assumes cameras are looking towards the horizon - vertical: assumes cameras are looking down towards the ground """ 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_horizontal_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. # Set the arbitrary scale proportional to the number of cameras. s = len(X) / max(1e-8, X.std(axis=0).max()) 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])**0.5 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
def align_reconstruction_orientation_prior_similarity(reconstruction, config): """Align with GPS data assuming particular a camera orientation. In some cases, using 3D-3D matches directly fails to find proper orientation of the world. That happends mainly when all cameras lie close to a straigh line. In such cases, we can impose a particular orientation of the cameras to improve the orientation of the alignment. The config parameter `align_orientation_prior` can be used to specify such orientation. Accepted values are: - no_roll: assumes horizon is horizontal on the images - horizontal: assumes cameras are looking towards the horizon - vertical: assumes cameras are looking down towards the ground """ X, Xp = [], [] orientation_type = config['align_orientation_prior'] 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_horizontal_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. # Set the arbitrary scale proportional to the number of cameras. s = len(X) / max(1e-8, X.std(axis=0).max()) 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])**0.5 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