def calc_rotation_matrix(mob_v1, mob_v2, ref_v1, ref_v2):
"""
Calculate rotation matrix R, thus R.dot(ref_vx) ~ mob_vx.
:param mob_v1: first mobile vector.
:param mob_v2: second mobile vector.
:param ref_v1: first reference vector.
:param ref_v2: second reference vector.
:return: rotation matrix R.
"""
mob_v1, mob_v2 = np.float32(mob_v1), np.float32(mob_v2)
ref_v1, ref_v2 = np.float32(ref_v1), np.float32(ref_v2)
# rotate reference vector plane to mobile plane
mob_norm = np.cross(mob_v1, mob_v2) # norm vector of mobile vectors
ref_norm = np.cross(ref_v1, ref_v2) # norm vector of reference vectors
if min(norm(mob_norm), norm(ref_norm)) == 0.:
return np.identity(3) # return dummy matrix if co-linear
axis = np.cross(ref_norm, mob_norm)
angle = calc_angle(ref_norm, mob_norm, norm(ref_norm), norm(mob_norm))
R1 = axis_angle_to_rotation_matrix(axis, angle)
rot_ref_v1, rot_ref_v2 = R1.dot(ref_v1), R1.dot(ref_v2)
# rotate reference vectors to mobile vectors approximately
angle1 = calc_angle(rot_ref_v1, mob_v1, norm(rot_ref_v1), norm(mob_v1))
angle2 = calc_angle(rot_ref_v2, mob_v2, norm(rot_ref_v2), norm(mob_v2))
angle = (angle1 + angle2) * 0.5
axis = np.cross(rot_ref_v1, mob_v1) # important!!
R2 = axis_angle_to_rotation_matrix(axis, angle)
R = R2.dot(R1)
return R
def plan(self, waypoints: List[Waypoint]):
if not waypoints:
self.car.enable_cruise_control(0)
self.car.unset_steer_target()
return
self.desired_heading = calc_angle(self.car.position,
waypoints[0].position)
self.adjust_steering()
self.adjust_speed(waypoints)
def search_table(q_pair, table, seed_len_tol=0.001, seed_angle_tol=1.):
"""
Search solution candidates.
q_pair: 2x3 array (q1, q2).
table: reference table.
return: solution cadidates, q1, q2
"""
q1, q2 = q_pair.copy()
q1_len, q2_len = norm(q1), norm(q2)
if q1_len < q2_len:
q1, q2 = q2, q1
q1_len, q2_len = q2_len, q1_len
angle = calc_angle(q1, q2, q1_len, q2_len)
candidates = np.where(
(np.abs(q1_len - table['len_angle'][:, 0]) < seed_len_tol) *
(np.abs(q2_len - table['len_angle'][:, 1]) < seed_len_tol) *
(np.abs(angle - table['len_angle'][:, 2]) < seed_angle_tol))[0]
return candidates, q1, q2
def worker_run():
chunk_size = 10000
stop = False
count = 0
chunk = []
# create table file
table = h5py.File('table-%d.h5' % rank)
while not stop:
job = comm.recv(source=0)
# work on job
for i in job:
for j in range(i + 1, len(hkl)):
q1, q2 = q_vectors[i], q_vectors[j]
len1, len2 = q_lengths[i], q_lengths[j]
len_mean = (len1 + len2) * 0.5
hkl1, hkl2 = hkl[i], hkl[j]
angle = calc_angle(q1, q2, len1, len2)
if len1 >= len2:
row = [
hkl1[0], hkl1[1], hkl1[2], hkl2[0], hkl2[1], hkl2[2],
len1, len2, angle, len_mean
]
else:
row = [
hkl2[0], hkl2[1], hkl2[2], hkl1[0], hkl1[1], hkl1[2],
len2, len1, angle, len_mean
]
chunk.append(row)
count += 1
if count % chunk_size == 0:
save_chunk(chunk, table)
chunk = []
comm.send(job, dest=0)
stop = comm.recv(source=0)
# last chunk
if len(chunk) > 0:
save_chunk(chunk, table)
table.close()
done = True
comm.send(done, dest=0)
def calc_rotation(self):
if len(self.position_history) < 2:
return
self.rotation = calc_angle(begin=self.position_history[-1],
end=self.position_history[-2])
def index_once(peaks, table, transform_matrix, inv_transform_matrix,
seed_pool_size=5,
seed_len_tol=0.003,
seed_angle_tol=3.0,
seed_hkl_tol=0.1,
eval_hkl_tol=0.25,
centering='P',
centering_weight=0.,
refine_mode='global',
refine_cycles=10,
miller_set=None,
nb_top=5,
unindexed_peak_ids=None):
"""
Perform index once.
:param peaks: peak info, including pos_x, pos_y, total_intensity, snr,
qx, qy, qz, resolution.
:param table: reference table dict, including hkl pairs and length, angle.
:param transform_matrix: transform matrix A = [a*, b*, c*] in per angstrom.
:param inv_transform_matrix: inverse of transform matrix A: inv(A).
:param seed_pool_size: size of seed pool.
:param seed_len_tol: length tolerance for seed.
:param seed_angle_tol: angle tolerance for seed.
:param seed_hkl_tol: hkl tolerance for seed.
:param eval_hkl_tol: hkl tolerance for paired peaks.
:param centering: centering type.
:param centering_weight: weight of centering score.
:param refine_mode: minimization mode, global or alternative.
:param refine_cycles: number of refine cycles.
:param miller_set: possible hkl indices.
:param nb_top: number of top solutions.
:param unindexed_peak_ids: indices of unindexed peaks.
:return: best indexing solution.
"""
time_stat = {
'table lookup': 0.,
'evaluation': 0.,
'correction': 0.,
'refinement': 0.,
'total': 0.,
}
time_start = time.time()
qs = peaks[:, 4:7] * 1.E-10 # convert to per angstrom
if unindexed_peak_ids is None:
unindexed_peak_ids = list(range(len(peaks)))
seed_pool = unindexed_peak_ids[:min(
seed_pool_size, len(unindexed_peak_ids))]
seed_pairs = list(combinations(seed_pool, 2))
solutions = []
nb_candidates = 0
seed_len_tol = np.float32(seed_len_tol)
seed_angle_tol = np.float32(seed_angle_tol)
for seed_pair in seed_pairs:
q1, q2 = qs[seed_pair, :]
q1_len, q2_len = np.float32(norm(q1)), np.float32(norm(q2))
if q1_len < q2_len:
q1, q2 = q2, q1
q1_len, q2_len = q2_len, q1_len
angle = np.float32(calc_angle(q1, q2, q1_len, q2_len))
t0 = time.time()
candidates = np.where(
(np.abs(q1_len - table['len_angle'][:, 0]) < seed_len_tol)
* (np.abs(q2_len - table['len_angle'][:, 1]) < seed_len_tol)
* (np.abs(angle - table['len_angle'][:, 2]) < seed_angle_tol)
)[0]
t1 = time.time()
time_stat['table lookup'] += (t1 - t0)
nb_candidates += len(candidates)
t0 = time.time()
for candidate in candidates:
hkl1 = table['hkl1'][candidate]
hkl2 = table['hkl2'][candidate]
ref_q1 = transform_matrix.dot(hkl1)
ref_q2 = transform_matrix.dot(hkl2)
solution = Solution(nb_peaks=len(qs), centering=centering)
solution.rotation_matrix = calc_rotation_matrix(
q1, q2, ref_q1, ref_q2)
solution.transform_matrix = solution.rotation_matrix.dot(
transform_matrix)
eval_solution(solution, qs, inv_transform_matrix, seed_pair,
eval_hkl_tol=eval_hkl_tol,
centering=centering,
centering_weight=centering_weight,
unindexed_peak_ids=unindexed_peak_ids)
if solution.seed_error < seed_hkl_tol:
solutions.append(solution)
t1 = time.time()
time_stat['evaluation'] += (t1 - t0)
if len(solutions) > 0:
solutions.sort(key=lambda x: x.total_score, reverse=True)
total_scores = [solution.total_score for solution in solutions]
if nb_top < 0:
nb_top = len(solutions)
else:
nb_top = min(len(solutions), nb_top)
top_solutions = solutions[:nb_top]
# correct match rate for top solutions using miller set constraints
t0 = time.time()
for solution in top_solutions:
solution.correct_match_rate(
qs, miller_set=miller_set, centering_weight=centering_weight)
top_solutions.sort(
key=lambda x: (x.true_total_score, x.true_pair_dist))
t1 = time.time()
time_stat['correction'] = (t1 - t0)
best_solution = solutions[0]
best_solution.nb_candidates = nb_candidates
best_solution.total_scores = total_scores
# refine best solution
t0 = time.time()
best_solution.transform_matrix = best_solution.rotation_matrix.dot(
transform_matrix)
eXYZs = np.abs(best_solution.transform_matrix.dot(
best_solution.rhkls.T) - qs.T).T
dists = norm(eXYZs, axis=1)
best_solution.pair_dist = dists[best_solution.pair_ids].mean()
refine_solution(best_solution, qs,
mode=refine_mode,
nb_cycles=refine_cycles)
t1 = time.time()
time_stat['refinement'] = t1 - t0
else:
best_solution = Solution()
best_solution.rotation_matrix = np.identity(3)
time_stop = time.time()
time_stat['total'] = time_stop - time_start
best_solution.time_stat = time_stat
return best_solution