def test_align_vectors_scaled_weights(): rng = np.random.RandomState(0) c = Rotation.random(random_state=rng) b = rng.normal(size=(5, 3)) a = c.apply(b) est1, rmsd1, cov1 = Rotation.align_vectors(a, b, np.ones(5), True) est2, rmsd2, cov2 = Rotation.align_vectors(a, b, 2 * np.ones(5), True) assert_allclose(est1.as_matrix(), est2.as_matrix()) assert_allclose(np.sqrt(2) * rmsd1, rmsd2) assert_allclose(cov1, cov2)
def get_predicted_distance(coordinates, noise=0): X = (coordinates - coordinates[0]) x_true, y_true, z_true = X.transpose() D = distance_matrix(coordinates, coordinates) D2 = D ** 2 M = np.zeros_like(D2) for i in range(10): for j in range(10): M[i, j] = (D2[0, j] + D2[i, 0] - D2[i, j]) / 2 M[j, i] = M[i, j] noise = noise * np.random.randn(*M.shape) noise = (noise + noise.transpose()) / 2 w, v = np.linalg.eigh(M + noise) size = len(w) nd = coordinates.shape[1] vevs = np.arange(size - nd, size)[::-1] x_v = v[:, vevs] sqrt_s = np.expand_dims(np.sqrt(w[vevs]), 0) x_v = (sqrt_s * x_v) x, y, z = x_v.transpose() R, loss = Rotation.align_vectors(x_v, X) x_new, y_new, z_new = R.apply(x_v, inverse=True).transpose() return x_true, y_true, x_new, y_new, D, D2, loss
def get_predicted(coordinates, noise=.2): X = (coordinates - coordinates[0]) x_true, y_true, z_true = X.transpose() D = distance_matrix(coordinates, coordinates) M = np.inner(X, X) noise = noise * np.random.randn(*M.shape) noise = (noise + noise.transpose()) / 2 w, v = np.linalg.eigh(M + noise) size = len(w) nd = coordinates.shape[1] vevs = np.arange(size - nd, size)[::-1] x_v = v[:, vevs] sqrt_s = np.expand_dims(np.sqrt(w[vevs]), 0) x_v = (sqrt_s * x_v) x, y, z = x_v.transpose() R, loss = Rotation.align_vectors(x_v, X) x_new, y_new, z_new = R.apply(x_v, inverse=True).transpose() return x_true, y_true, x_new, y_new, loss
def test_align_vectors_no_rotation(): x = np.array([[1, 2, 3], [4, 5, 6]]) y = x.copy() r, rmsd = Rotation.align_vectors(x, y) assert_array_almost_equal(r.as_matrix(), np.eye(3)) assert_allclose(rmsd, 0, atol=1e-6)
def extract_planes_and_polygons_from_mesh(tri_mesh, avg_peaks, polylidar_kwargs=dict(alpha=0.0, lmax=0.1, min_triangles=2000, z_thresh=0.1, norm_thresh=0.95, norm_thresh_min=0.95, min_hole_vertices=50, task_threads=4), filter_polygons=True, pl_=None, optimized=False, postprocess=dict(filter=dict(hole_area=dict(min=0.025, max=100.0), hole_vertices=dict(min=6), plane_area=dict(min=0.05)), positive_buffer=0.00, negative_buffer=0.00, simplify=0.0)): if pl_ is not None: pl = pl_ else: pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks) t0 = time.perf_counter() if optimized: all_planes, all_polygons = pl.extract_planes_and_polygons_optimized(tri_mesh, avg_peaks_mat) else: all_planes, all_polygons = pl.extract_planes_and_polygons(tri_mesh, avg_peaks_mat) t1 = time.perf_counter() # tri_set = pl.extract_tri_set(tri_mesh, avg_peaks_mat) # planes_tri_set = [np.argwhere(np.asarray(tri_set) == i) for i in range(1, 2)] # o3d_mesh_painted = paint_planes(o3d_mesh, planes_tri_set) polylidar_time = (t1 - t0) * 1000 # logging.info("Polygon Extraction - Took (ms): %.2f", polylidar_time) all_planes_shapely = [] all_obstacles_shapely = [] time_filter = [] time_geometric_planes = [] # all_poly_lines = [] geometric_planes = [] # all_polygons = [[NORMAL_0_POLY_1, NORMAL_0_POLY_2], [NORMAL_1_POLY_1]] if filter_polygons: vertices = np.asarray(tri_mesh.vertices) for i in range(avg_peaks.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) # Rotating matrix the polygon polygons_for_normal = all_polygons[i] planes = all_planes[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: planes_shapely, obstacles_shapely, planes_indices, filter_time = filter_and_create_polygons( vertices, polygons_for_normal, rm=rm, postprocess=postprocess) t3 = time.perf_counter() geometric_planes_for_normal = [extract_geometric_plane(plane_poly[0], planes[plane_idx], tri_mesh, avg_peak) for ( plane_poly, plane_idx) in zip(planes_shapely, planes_indices)] geometric_planes.append(geometric_planes_for_normal) t4 = time.perf_counter() time_geometric_planes.append((t4 - t3) * 1000) all_planes_shapely.extend(planes_shapely) all_obstacles_shapely.extend(obstacles_shapely) time_filter.append(filter_time) # all_poly_lines.extend(poly_lines) timings = dict(t_polylidar_planepoly=polylidar_time, t_polylidar_filter=np.array(time_filter).mean(), t_geometric_planes=np.array(time_geometric_planes).sum()) # all_planes_shapely, all_obstacles_shapely, all_poly_lines, timings return all_planes_shapely, all_obstacles_shapely, geometric_planes, timings
def test_align_vectors_noise(): rnd = np.random.RandomState(0) n_vectors = 100 rot = Rotation.random(random_state=rnd) vectors = rnd.normal(size=(n_vectors, 3)) result = rot.apply(vectors) # The paper adds noise as independently distributed angular errors sigma = np.deg2rad(1) tolerance = 1.5 * sigma noise = Rotation.from_rotvec(rnd.normal(size=(n_vectors, 3), scale=sigma)) # Attitude errors must preserve norm. Hence apply individual random # rotations to each vector. noisy_result = noise.apply(result) est, rmsd, cov = Rotation.align_vectors(noisy_result, vectors, return_sensitivity=True) # Use rotation compositions to find out closeness error_vector = (rot * est.inv()).as_rotvec() assert_allclose(error_vector[0], 0, atol=tolerance) assert_allclose(error_vector[1], 0, atol=tolerance) assert_allclose(error_vector[2], 0, atol=tolerance) # Check error bounds using covariance matrix cov *= sigma assert_allclose(cov[0, 0], 0, atol=tolerance) assert_allclose(cov[1, 1], 0, atol=tolerance) assert_allclose(cov[2, 2], 0, atol=tolerance) assert_allclose(rmsd, np.sum((noisy_result - est.apply(vectors))**2)**0.5)
def extract_planes_and_polygons_from_classified_mesh( tri_mesh, avg_peaks, polylidar_kwargs=dict(alpha=0.0, lmax=0.1, min_triangles=2000, z_thresh=0.1, norm_thresh=0.95, norm_thresh_min=0.95, min_hole_vertices=50, task_threads=4), filter_polygons=True, pl_=None, optimized=True, postprocess=dict(filter=dict(hole_area=dict(min=0.025, max=100.0), hole_vertices=dict(min=6), plane_area=dict(min=0.05)), positive_buffer=0.00, negative_buffer=0.00, simplify=0.0)): if pl_ is not None: pl = pl_ else: pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks) t0 = time.perf_counter() all_planes, all_polygons = pl.extract_planes_and_polygons_optimized_classified( tri_mesh, avg_peaks_mat) t1 = time.perf_counter() polylidar_time = (t1 - t0) * 1000 all_planes_shapely = [] all_obstacles_shapely = [] time_filter = [] # all_poly_lines = [] if filter_polygons: vertices = np.asarray(tri_mesh.vertices) for i in range(avg_peaks.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) polygons_for_normal = all_polygons[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: planes_shapely, obstacles_shapely, filter_time = filter_and_create_polygons( vertices, polygons_for_normal, rm=rm, postprocess=postprocess) all_planes_shapely.extend(planes_shapely) all_obstacles_shapely.extend(obstacles_shapely) time_filter.append(filter_time) # all_poly_lines.extend(poly_lines) timings = dict(t_polylidar_planepoly=polylidar_time, t_polylidar_filter=np.array(time_filter).sum()) # all_planes_shapely, all_obstacles_shapely, all_poly_lines, timings return all_planes_shapely, all_obstacles_shapely, timings
def test_align_vectors_no_noise(): rnd = np.random.RandomState(0) c = Rotation.random(random_state=rnd) b = rnd.normal(size=(5, 3)) a = c.apply(b) est, rmsd = Rotation.align_vectors(a, b) assert_allclose(c.as_quat(), est.as_quat()) assert_allclose(rmsd, 0, atol=1e-7)
def test_align_vectors_invalid_input(): with pytest.raises(ValueError, match="Expected input `a` to have shape"): Rotation.align_vectors([1, 2, 3], [[1, 2, 3]]) with pytest.raises(ValueError, match="Expected input `b` to have shape"): Rotation.align_vectors([[1, 2, 3]], [1, 2, 3]) with pytest.raises(ValueError, match="Expected inputs `a` and `b` " "to have same shapes"): Rotation.align_vectors([[1, 2, 3],[4, 5, 6]], [[1, 2, 3]]) with pytest.raises(ValueError, match="Expected `weights` to be 1 dimensional"): Rotation.align_vectors([[1, 2, 3]], [[1, 2, 3]], weights=[[1]]) with pytest.raises(ValueError, match="Expected `weights` to have number of values"): Rotation.align_vectors([[1, 2, 3]], [[1, 2, 3]], weights=[1, 2])
def test_align_vectors_no_noise(): np.random.seed(0) c = Rotation.from_quat(np.random.normal(size=4)) b = np.random.normal(size=(5, 3)) a = c.apply(b) est, rmsd = Rotation.align_vectors(a, b) assert_allclose(c.as_quat(), est.as_quat()) assert_allclose(rmsd, 0, atol=1e-7)
def test_align_vectors_improper_rotation(): # Tests correct logic for issue #10444 x = np.array([[0.89299824, -0.44372674, 0.0752378], [0.60221789, -0.47564102, -0.6411702]]) y = np.array([[0.02386536, -0.82176463, 0.5693271], [-0.27654929, -0.95191427, -0.1318321]]) est, rmsd = Rotation.align_vectors(x, y) assert_allclose(x, est.apply(y), atol=1e-6) assert_allclose(rmsd, 0, atol=1e-7)
def extract_all_dominant_planes(tri_mesh, vertices, polylidar_kwargs, config_pp, ds=50, min_samples=10000): ga = GaussianAccumulatorS2(level=4, max_phi=180) ico = IcoCharts(level=4) triangle_normals = np.asarray(tri_mesh.triangle_normals) num_normals = triangle_normals.shape[0] triangle_normals_ds = down_sample_normals(triangle_normals) # Get the data t0 = time.perf_counter() ga.integrate(MatX3d(triangle_normals_ds)) t1 = time.perf_counter() avg_peaks, _, _, timings = get_image_peaks(ico, ga, level=4, with_o3d=False) timings['t_fastga_integrate'] = (t1 - t0) * 1000 timings['t_fastga_total'] = timings['t_fastga_integrate'] + timings[ 't_fastga_peak'] logging.info("Processing mesh with %d triangles", num_normals) logging.info("Dominant Plane Normals") print(avg_peaks) avg_peaks_selected = np.copy(avg_peaks[[0, 1, 2, 3], :]) pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks_selected) # debugging purposes, ignore tri_set = pl.extract_tri_set(tri_mesh, avg_peaks_mat) t0 = time.perf_counter() all_planes, all_polygons = pl.extract_planes_and_polygons_optimized( tri_mesh, avg_peaks_mat) t1 = time.perf_counter() timings['t_polylidar_planepoly'] = (t1 - t0) * 1000 all_poly_lines = [] for i in range(avg_peaks_selected.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) polygons_for_normal = all_polygons[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: poly_lines, _ = filter_and_create_open3d_polygons( vertices, polygons_for_normal, rm=rm, config_pp=config_pp) all_poly_lines.extend(poly_lines) return all_planes, tri_set, all_poly_lines, timings
def computeReference(self): B = np.array([sin(self.inclination), 0, cos(self.inclination)], dtype=np.float64) A = np.array([0, 0, 1], dtype=np.float64) r, f = Rotation.align_vectors(np.array([A, B]), np.array([self.accel, self.mag]), self.weight) self.referenceOrientation = r.as_euler("xyz") self.r = r.as_quat()
def sample_sphere_cap(n=100, deg=10, normal=[1, 0, 0]): min_z = np.cos(np.radians(deg)) z = np.random.uniform(min_z, 1.0, n) r = np.sqrt(1 - z * z) theta = np.random.uniform(0, 1.0, n) * 2 * np.pi x = r * np.cos(theta) y = r * np.sin(theta) rm, _, = R.align_vectors(np.array([normal]), np.array([[0.0, 0.0, 1.0]])) points = np.column_stack([x, y, z]) points = rm.apply(points) return points
def extract_all_dominant_planes(tri_mesh, vertices, polylidar_kwargs, ds=50, min_samples=10000): ga = GaussianAccumulatorS2(level=4, max_phi=180) triangle_normals = np.asarray(tri_mesh.triangle_normals) num_normals = triangle_normals.shape[0] # Downsample, TODO improve this ds_normals = int(num_normals / ds) to_sample = max(min([num_normals, min_samples]), ds_normals) ds_step = int(num_normals / to_sample) triangle_normals_ds = np.ascontiguousarray( triangle_normals[:num_normals:ds_step, :]) # A copy occurs here for triangle_normals......if done in c++ there would be no copy ga.integrate(MatX3d(triangle_normals_ds)) gaussian_normals = np.asarray(ga.get_bucket_normals()) accumulator_counts = np.asarray(ga.get_normalized_bucket_counts()) _, _, avg_peaks, _ = find_peaks_from_accumulator(gaussian_normals, accumulator_counts) logging.info("Processing mesh with %d triangles", num_normals) logging.info("Dominant Plane Normals") print(avg_peaks) avg_peaks_selected = np.copy(avg_peaks[[0, 1, 2, 3], :]) pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks_selected) tri_set = pl.extract_tri_set(tri_mesh, avg_peaks_mat) t0 = time.perf_counter() all_planes, all_polygons = pl.extract_planes_and_polygons_optimized( tri_mesh, avg_peaks_mat) t1 = time.perf_counter() polylidar_time = (t1 - t0) * 1000 all_poly_lines = [] for i in range(avg_peaks_selected.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) polygons_for_normal = all_polygons[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: poly_lines, _ = filter_and_create_open3d_polygons( vertices, polygons_for_normal, rm=rm) all_poly_lines.extend(poly_lines) return all_planes, tri_set, all_poly_lines, polylidar_time
def create_samples(normal=np.array([0, 1, 0]), std=0.01, size=50): # create 2D samples # import ipdb; ipdb.set_trace() np.random.seed(4) cov = np.identity(2) * std samples_2d = np.random.multivariate_normal([0.0, 0.0], cov, size=size) samples_3d = np.append(samples_2d, np.ones((samples_2d.shape[0], 1)), 1) samples_3d, _ = normalized(samples_3d) # create rotation matrix default_normal = np.array([0, 0, 1]) rm, _, = Rotation.align_vectors([default_normal], [normal]) normals_3d = rm.apply(samples_3d) # TODO normalize return normals_3d
def GetPrincipalAxis(coordinates): cl = np.array(coordinates) n, m = cl.shape mu = cl.mean(axis=0) cl = cl - mu A = np.dot(cl.T, cl) / (n - 1) eigVecs = np.linalg.eig(A)[1] eigVals = np.diag(np.linalg.eig(A)[0]) imax = eigVals.argmax() % 3 L = np.argsort(-eigVals.flatten()) % 3 qalign = R.align_vectors([eigVecs[:, L[0]]], [[0, 1, 0]]) #qalign = R.align_vectors([eigVecs[:,L[0]],eigVecs[:,L[1]]],[[0,1,0],[1,0,0]]) #qalign = R.align_vectors([eigVecs[:,L[0]],eigVecs[:,L[1]],eigVecs[:,L[2]]],[[0,1,0],[1,0,0],[0,0,1]]) #r = R.from_matrix([eigVecs[L[0]],eigVecs[L[1]],eigVecs[L[2]]]) # stage.animationControls.rotate(new NGL.Quaternion( -0.44752089, -0.3509433 , -0.50757132, -0.64725205), 0); return qalign #r.as_quat(), r.inv().as_euler('zxy', degrees=True)
def extract_all_dominant_planes(tri_mesh, vertices, polylidar_kwargs, ds=50, min_samples=10000): ga = GaussianAccumulatorS2Beta(level=4) ico = IcoCharts(level=4) fast_ga_kwargs = dict(find_peaks_kwargs=dict(threshold_abs=15, min_distance=1, exclude_border=False, indices=False), cluster_kwargs=dict(t=0.28, criterion='distance'), average_filter=dict(min_total_weight=0.1)) avg_peaks, _, _, _, alg_timings = extract_all_dominant_plane_normals( tri_mesh, ga_=ga, ico_chart_=ico, **fast_ga_kwargs) logging.info("Dominant Plane Normals") print(avg_peaks) avg_peaks_selected = np.copy(avg_peaks[[0, 1, 2, 3, 4], :]) pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks_selected) tri_set = pl.extract_tri_set(tri_mesh, avg_peaks_mat) t0 = time.perf_counter() all_planes, all_polygons = pl.extract_planes_and_polygons_optimized( tri_mesh, avg_peaks_mat) t1 = time.perf_counter() polylidar_time = (t1 - t0) * 1000 all_poly_lines = [] for i in range(avg_peaks_selected.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) polygons_for_normal = all_polygons[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: poly_lines, _ = filter_and_create_open3d_polygons( vertices, polygons_for_normal, rm=rm) all_poly_lines.extend(poly_lines) return all_planes, tri_set, all_poly_lines, polylidar_time
def hyperNEB(embedder, coords, atomnos, ids, constrained_indexes, title='temp'): ''' Turn a geometry close to TS to a proper TS by getting reagents and products and running a climbing image NEB calculation through ASE. ''' reagents = get_reagent(embedder, coords, atomnos, ids, constrained_indexes, method=embedder.options.theory_level) products = get_product(embedder, coords, atomnos, ids, constrained_indexes, method=embedder.options.theory_level) # get reagents and products for this reaction reagents -= np.mean(reagents, axis=0) products -= np.mean(products, axis=0) # centering both structures on the centroid of reactive atoms aligment_rotation = R.align_vectors(reagents, products) # products = np.array([aligment_rotation @ v for v in products]) products = (aligment_rotation @ products.T).T # rotating the two structures to minimize differences ts_coords, ts_energy, success = ase_neb(embedder, reagents, products, atomnos, title=title) # Use these structures plus the TS guess to run a NEB calculation through ASE return ts_coords, ts_energy, success
def calc_node_orientation(self, unchanged='z', clean_unchanged=True, return_normal=False, flip_normal=1): valid_unchanged = ['x', 'y', 'z'] if unchanged not in valid_unchanged: raise ValueError('unchanged must be one of {}.'.format(unchanged)) cell_normals = self.calc_shell_normals() * flip_normal if clean_unchanged: cell_normals[:, valid_unchanged.index(unchanged)] = 0 cell_normals = normalize_vectors(cell_normals) node_normals = np.zeros((self.number_of_nodes(), 3)) for i, vertices in enumerate(self.shells): node_normals[vertices, :] += cell_normals[i, :] node_normals = normalize_vectors(node_normals) # TODO: which coordinate points outwards global_frame = np.zeros((2, 3)) global_frame[0, 1] = 1 global_frame[1, 2] = 1 local_frame = global_frame.copy() orientation = np.zeros((self.number_of_nodes(), 3)) for i, normal in enumerate(node_normals): local_frame[0, :] = normal rotation = R.align_vectors(global_frame, local_frame) orientation[i, :] = rotation[0].as_euler('xyz') self.node_orientations = orientation if return_normal: return orientation, node_normals return orientation
def _save_aligned_conformers(rdmol, log): elements = [a.GetSymbol() for a in rdmol.GetAtoms()] ref_coords = rdmol.GetConformer(0).GetPositions() xyz_block = "" for conformer in rdmol.GetConformers(): # Get the coordinates of the conformer coords = conformer.GetPositions() # Translate atom 0 to the same position translate = coords[0] - ref_coords[0] coords = coords - translate # Compute two vectors -- atom 1 - atom 0, atom2 - atom0 vec1_ref = ref_coords[1] - ref_coords[0] vec1_ref = vec1_ref / np.linalg.norm(vec1_ref) vec1 = coords[1] - coords[0] vec1 = vec1 / np.linalg.norm(vec1) vec2_ref = ref_coords[2] - ref_coords[0] vec2_ref = vec2_ref / np.linalg.norm(vec2_ref) vec2 = coords[2] - coords[0] vec2 = vec2 / np.linalg.norm(vec2) # Rotate the molecule to best align these two vectors a = np.array([vec1_ref, vec2_ref], dtype=np.float64) b = np.array([vec1, vec2], dtype=np.float64) R, rmsd = Rotation.align_vectors(a, b) coords = R.apply(coords) xyz_block += f"{len(elements)}\n\n" for element, coord in zip(elements, coords): x, y, z = coord xyz_block += f"{element:5s}{x:12.6f}{y:12.6f}{z:12.6f}\n" fpath = Path("structures/aligned_conformers.xyz") with fpath.open("w") as f: f.write(xyz_block) log.log("Wrote all aligned to 'structures/aligned_conformers.xyz'.")
def extract_planes_and_polygons_from_mesh(tri_mesh, avg_peaks, polylidar_kwargs=dict(alpha=0.0, lmax=0.1, min_triangles=2000, z_thresh=0.1, norm_thresh=0.95, norm_thresh_min=0.95, min_hole_vertices=50, task_threads=4), filter_polygons=True, pl_=None, optimized=True, **kwargs): if pl_ is not None: pl = pl_ else: pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks) t0 = time.perf_counter() if optimized: all_planes, all_polygons = pl.extract_planes_and_polygons_optimized(tri_mesh, avg_peaks_mat) else: all_planes, all_polygons = pl.extract_planes_and_polygons(tri_mesh, avg_peaks_mat) t1 = time.perf_counter() polylidar_time = (t1 - t0) * 1000 logger.info("Polygon Extraction - Took (ms): %.2f", polylidar_time) all_poly_lines = [] if filter_polygons: vertices = np.asarray(tri_mesh.vertices) for i in range(avg_peaks.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) polygons_for_normal = all_polygons[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: poly_lines, _ = filter_and_create_open3d_polygons(vertices, polygons_for_normal, rm=rm, **kwargs) all_poly_lines.extend(poly_lines) timings = dict(polylidar=polylidar_time) return all_planes, all_polygons, all_poly_lines, timings
def align_positions( positions_from: Positions, positions_to: Positions, rot_axes: Literal["x", "y", "z", "xyz"], rsmd_threshold=0.4, ) -> Transform: """ Let positions_from be fixed local coordinate frame, and positions_to be some other fixed coordinate system. Further, let the relationship between the two reference systems be described as positions_from = rotation.apply(positions_to) + translation This function finds the rotation and translation by matching the two coordinate systems, and represent the transformation through a Transform object. For robustness it is advised to use more than 2 positions in alignment and using positions with some distance to each other. :param positions_from: Coordinates in a fixed frame :param positions_to: Coordinates in a fixed frame :param rot_axes: Axis of rotation. For rotations in the xy plane (most common), this is set to 'z' :param rsmd_threshold: The root mean square distance threshold, for the coordinate fitting error in matching the two coordinate systems. """ if len(positions_from.positions) != len(positions_to.positions): raise ValueError( f"Expected inputs 'positions_from' and 'positions_to' to have the same shapes" + f" got {len(positions_from.positions)} and {len(positions_to.positions)}, respectively" ) if len(positions_from.positions) < 2: raise ValueError( f" Expected at least 2 positions, got {len(positions_from.positions)}" ) if len(positions_from.positions) < 3 and rot_axes == "xyz": raise ValueError( f" Expected at least 3 positions, got {len(positions_from.positions)}" ) try: edges_1, edges_2 = _get_edges(positions_from, positions_to, rot_axes) except Exception as e: raise ValueError(e) rotation, rmsd_rot, sensitivity = Rotation.align_vectors( edges_2, edges_1, return_sensitivity=True ) translations: Translation = Translation.from_array( np.mean( positions_to.to_array() - rotation.apply(positions_from.to_array()), axis=0, # type:ignore ), from_=positions_from.frame, to_=positions_to.frame, ) # type: ignore transform = Transform( from_=positions_from.frame, to_=positions_to.frame, translation=translations, rotation=rotation, ) try: _check_rsme_treshold(transform, positions_to, positions_from, rsmd_threshold) except Exception as e: raise ValueError(e) return transform
def test_align_vectors_single_vector(): with pytest.warns(UserWarning, match="Optimal rotation is not"): r_estimate, rmsd = Rotation.align_vectors([[1, -1, 1]], [[1, 1, -1]]) assert_allclose(rmsd, 0, atol=1e-16)
def calc_initial_position(self): """Position the molecules in the initial position.""" # calculate the geometric center of the molecule and of the restraints ga_log.info('Positioning molecules in starting conformation') r_c = np.array(self.receptor_coord) l_c = np.array(self.ligand_coord) r_rest_c = np.array(self.receptor_rest_coord) l_rest_c = np.array(self.ligand_rest_coord) r_center = r_c.mean(axis=0) l_center = l_c.mean(axis=0) # Move both to origin r_c -= r_center l_c -= l_center r_rest_c -= r_center l_rest_c -= l_center # get center of interface and molecule r_center = r_c.mean(axis=0) l_center = l_c.mean(axis=0) r_rest_center_c = r_rest_c.mean(axis=0) l_rest_center_c = l_rest_c.mean(axis=0) # ==== # Align the vectors between molecule center and interface a = np.array([r_center, r_rest_center_c]) b = np.array([l_center, l_rest_center_c]) mat, _ = Rotation.align_vectors(a, b) rot_l_c = mat.apply(l_c) rot_l_rest_c = mat.apply(l_rest_c) rot_l_center = rot_l_c.mean(axis=0) rot_l_rest_center = rot_l_rest_c.mean(axis=0) # Move them apart a = np.array([r_center, r_rest_center_c]) b = np.array([rot_l_center, rot_l_rest_center]) _, b_i = np.add(a, b + 5) # +5 to keep them further apart rot_l_c += b_i rot_l_rest_c += b_i # Rotate so that they face each other # Move to origin and rotate again c = rot_l_c.mean(axis=0) rot_l_c -= c rot_l_rest_c -= c rotation_radians = np.radians(180) rotation_axis = np.array([0, 0, 1]) rotation_vector = rotation_radians * rotation_axis rotation = Rotation.from_rotvec(rotation_vector) l_c = rotation.apply(rot_l_c) l_rest_c = rotation.apply(rot_l_rest_c) l_c += c l_rest_c += c self.ligand_coord = l_c self.receptor_coord = r_c
def calibrate(self, cam): cam.compute_relative_RT() print(len(cam.img_T), len(self.RPY)) u0_arr = [] u1_arr = [] w_arr = [] R0_arr = [] R1_arr = [] T0_arr = [] T1_arr = [] for i in range(0, len(self.RPY) - 1): if (i >= len(cam.img_T)): break for j in range(i + 1, len(self.RPY)): if (j >= len(cam.img_T)): break R_, t_ = cam.get_Rt(i, j) if (R_ is None or t_ is None): continue R, t = self.get_Rt(self.T[i], self.RPY[i], self.T[j], self.RPY[j]) if (R is None or t is None): continue u0 = R.as_rotvec() u1 = R_.as_rotvec() e = np.abs(np.linalg.norm(u0) - np.linalg.norm(u1)) if (e > 0.001): continue if (np.linalg.norm(u0) < 0.2): continue w_arr.append(e / np.linalg.norm(u0)) u0 /= np.linalg.norm(u0) u1 /= np.linalg.norm(u1) print(i, j, ':', u0, np.linalg.norm(R.as_rotvec()), u1, np.linalg.norm(R_.as_rotvec())) print(t.transpose()[0], t_.transpose()[0], np.linalg.norm(t) - np.linalg.norm(t_)) u0_arr.append(u0) u1_arr.append(u1) R0_arr.append(R) R1_arr.append(R_) T0_arr.append(t) T1_arr.append(t_) u0 = np.array(u0_arr) u1 = np.array(u1_arr) w = np.array(w_arr) # * 0 + 1 w /= np.linalg.norm(w) R, rmsd = Rotation.align_vectors(u0, u1, weights=w) uerr = np.linalg.norm(u0 - (R.as_dcm() @ u1.transpose()).transpose(), axis=1) merr = np.median(uerr) print("Initial R:", R.as_euler('xyz'), "; median error:", merr) w[uerr > merr] = 0 R, rmsd = Rotation.align_vectors(u0, u1, weights=w) print("Refined R:", R.as_euler('xyz')) a = np.zeros(shape=(0, 3), dtype=np.float) b = np.zeros(shape=(0, 1), dtype=np.float) for i in range(len(R0_arr)): R0 = R0_arr[i] R1 = R1_arr[i] T0 = T0_arr[i] T1 = T1_arr[i] a = np.vstack((a, R0.as_dcm() - np.eye(3))) b = np.vstack((b, R.as_dcm() @ T1 - T0)) print(a.shape, b.shape) T, res, rnk, s = np.linalg.lstsq(a, b, rcond=None) print(R.as_dcm(), rmsd) print() print('R:', R.inv().as_rotvec()) print(T.transpose()[0], res) print((R.as_dcm() @ T).transpose()[0]) print((R.inv().as_dcm() @ T).transpose()[0]) print((R.as_dcm() @ (-1 * T)).transpose()[0]) print((R.inv().as_dcm() @ (-1 * T)).transpose()[0]) print() print("x-y-z-R-P-Y:") print(np.hstack((T.transpose()[0], R.as_euler('xyz')))) return R.as_dcm(), T
def rotate_data_planar(points, normal, inverse=False): with warnings.catch_warnings(): warnings.simplefilter("ignore") rm, _ = R.align_vectors([[0, 0, 1]], [normal]) # Rotating matrix the polygon return rm.apply(points, inverse=inverse)
def get_mapping(self, env: Dict[str, Any]) -> Dict[int, int]: """ Returns mapping of sites from input to this object Pymatgen molecule_matcher does not work unfortunately as it needs to be a reasonably physical molecule. Here, the graph is constructed by connecting the nearest neighbor, and isomorphism is performed to find matches, then kabsch algorithm is performed to make sure it is a match. NetworkX is used for portability. Parameters ---------- env : Dict[str, Any] dictionary that contains information of local environment of a site in datum. See _get_SiteEnvironments definition in the class _SiteEnvironments for what this variable should be. Returns ------- dict : Dict[int, int] Atom mapping from Primitive cell to data point. """ try: import networkx.algorithms.isomorphism as iso except: raise ImportError("This class requires networkx to be installed.") # construct graph G = self._construct_graph(env['pos'], env['sitetypes']) if len(self.G.nodes) != len(G.nodes): s = 'Number of nodes is not equal.\n' raise ValueError(s) elif len(self.G.edges) != len(G.edges): logging.warning("Expected the number of edges to be equal", len(self.G.edges), len(G.edges)) s = 'Number of edges is not equal.\n' s += "- Is the data point's cell a redefined lattice of primitive cell?\n" s += '- If relaxed structure is used, you may want to check structure or increase Gatol\n' raise ValueError(s) GM = iso.GraphMatcher(self.G, G, self._nm, self._em) # Gets the isomorphic mapping. Also the most time consuming part of the code ams = list(GM.isomorphisms_iter()) if not ams: s = 'No isomorphism found.\n' s += "- Is the data point's cell a redefined lattice of primitive cell?\n" s += '- If relaxed structure is used, you may want to check structure or increase rtol\n' raise ValueError(s) rmsd = [] for am in ams: # Loop over isomorphism # reconstruct graph after alinging point order xyz = np.zeros((len(self.pos), 3)) for i in am: xyz[i, :] = env['pos'][am[i], :] rotation, _ = Rotation.align_vectors(self.pos, xyz) R = rotation.as_matrix() # RMSD rmsd.append( np.sqrt( np.mean(np.linalg.norm(np.dot(self.pos, R) - xyz, axis=1)**2))) mini = np.argmin(rmsd) minrmsd = rmsd[mini] if minrmsd < self.tol: return ams[mini] else: s = 'No isomorphism found.\n' s += '-Consider increasing neighbor finding tolerance' raise ValueError(s)
import os import sys if sys.path[0] != os.getcwd(): sys.path.insert(0, os.getcwd()) lib_local_path = os.path.normpath(r'C:\WORKSPACE\DEV\mocaplib') if os.path.exists(lib_local_path): if sys.path[1] != lib_local_path: sys.path.insert(1, lib_local_path) from mocaplib import btkapp as ba import numpy as np from scipy.spatial.transform import Rotation as R #%% cwd = os.path.dirname(__file__) c3d_sample_dir_path = os.path.join(cwd, r'..\Samples_C3D\Sample00\Vicon Motion Systems') src_c3d_path = os.path.join(c3d_sample_dir_path, 'pyCGM2 lower limb CGM24 Walking01.c3d') trc_path = os.path.join(cwd, 'Test02_result.trc') mot_path = os.path.join(cwd, 'Test02_result.mot') acq = ba.open_c3d(src_c3d_path) dict_pts = ba.get_dict_points(acq, tgt_types=['Marker']) mkr_names = [x for x in list(dict_pts['LABELS']) if len(x)<=4] axes_src = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] axes_tgt = [[0, -1, 0], [0, 0, 1], [-1, 0, 0]] rot_ret = R.align_vectors(a=axes_src, b=axes_tgt) rot_obj = rot_ret[0] trf = rot_obj.as_matrix() ba.export_trc(acq, trc_path, rot_mat=trf, filt_fc=20.0, tgt_mkr_names=mkr_names) ret = ba.export_mot(acq, mot_path, rot_mat=trf, filt_fc=20.0, threshold=15.0)
def rotateInDirection(self, position_matrix, direction, axis): estimated_rotation, rmsd = R.align_vectors([direction], [axis]) rotation_matrix = estimated_rotation.as_matrix() rotated_position_matrix = np.dot(rotation_matrix, position_matrix) return rotated_position_matrix