├── utils ├── __init__.py ├── align_trajectory.py ├── mat_utils.py ├── trace_parser.py ├── align_utils.py ├── tfs_utils.py └── transformations.py ├── plots └── .gitignore ├── data ├── .gitignore └── download.sh ├── RAL18_Zhang.pdf ├── img ├── cov_mat.png ├── pos_cov.png ├── spectrum.png ├── fixed_cov_mat.png ├── fixed_pos_cov.png ├── free_cov_mat.png ├── free_pos_cov.png ├── free_cov_transformed_mat.png └── free_pos_cov_transformed.png ├── README.md ├── cov_transformation.py └── compare_cov_free_fixed.py /utils/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /plots/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore 3 | -------------------------------------------------------------------------------- /data/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore 3 | !download.sh 4 | -------------------------------------------------------------------------------- /RAL18_Zhang.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_vi_cov_transformation/HEAD/RAL18_Zhang.pdf -------------------------------------------------------------------------------- /img/cov_mat.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_vi_cov_transformation/HEAD/img/cov_mat.png -------------------------------------------------------------------------------- /img/pos_cov.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_vi_cov_transformation/HEAD/img/pos_cov.png -------------------------------------------------------------------------------- /img/spectrum.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_vi_cov_transformation/HEAD/img/spectrum.png -------------------------------------------------------------------------------- /img/fixed_cov_mat.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_vi_cov_transformation/HEAD/img/fixed_cov_mat.png -------------------------------------------------------------------------------- /img/fixed_pos_cov.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_vi_cov_transformation/HEAD/img/fixed_pos_cov.png -------------------------------------------------------------------------------- /img/free_cov_mat.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_vi_cov_transformation/HEAD/img/free_cov_mat.png -------------------------------------------------------------------------------- /img/free_pos_cov.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_vi_cov_transformation/HEAD/img/free_pos_cov.png -------------------------------------------------------------------------------- /data/download.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | wget http://rpg.ifi.uzh.ch/datasets/ral18zhang_data.tar.gz -O - | tar -xz 4 | -------------------------------------------------------------------------------- /img/free_cov_transformed_mat.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_vi_cov_transformation/HEAD/img/free_cov_transformed_mat.png -------------------------------------------------------------------------------- /img/free_pos_cov_transformed.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_vi_cov_transformation/HEAD/img/free_pos_cov_transformed.png -------------------------------------------------------------------------------- /utils/align_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | 4 | import numpy as np 5 | from tfs_utils import exp_so3, log_so3 6 | 7 | 8 | def align_se3(model, data, only_yaw=False): 9 | """Align two trajectories using the method of Horn (closed-form). 10 | 11 | Input: 12 | model -- first trajectory (nx3), numpy array type 13 | data -- second trajectory (nx3), numpy array type 14 | only_yaw -- constrain the rotation to yaw only 15 | 16 | Output: 17 | R -- rotation matrix (3x3) 18 | t -- translation vector (3x1) 19 | 20 | """ 21 | np.set_printoptions(precision=3, suppress=True) 22 | mu_M = model.mean(0) 23 | mu_D = data.mean(0) 24 | model_zerocentered = model - mu_M 25 | data_zerocentered = data - mu_D 26 | 27 | W = np.zeros((3, 3)) 28 | for row in range(model.shape[0]): 29 | W += np.outer(model_zerocentered[row, :], data_zerocentered[row, :]) 30 | U, d, Vh = np.linalg.linalg.svd(W.transpose()) 31 | S = np.matrix(np.identity(3)) 32 | if np.linalg.det(U) * np.linalg.det(Vh) < 0: 33 | S[2, 2] = -1 34 | R = U*S*Vh 35 | if only_yaw: 36 | log_R = log_so3(R) 37 | yaw_R = np.array([0, 0, log_R[2]]) 38 | R = exp_so3(yaw_R) 39 | t = mu_D - np.dot(R, mu_M) 40 | 41 | return R, t 42 | -------------------------------------------------------------------------------- /utils/mat_utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | import numpy as np 4 | 5 | 6 | def permuteHesCov(cov, num_kf): 7 | ''' 8 | permutate the covriance/hessian matrix so that 9 | positions, rotations and velocities appear together. 10 | assum that the states are ordered [p0, q0, v0; p1, q1, v1; ...] 11 | ''' 12 | assert cov.shape[0] == 9 * num_kf 13 | num_states = 9 * num_kf 14 | 15 | des_index = np.zeros((num_states, )) 16 | for i in range(num_kf): 17 | cur_pos_s = i * 3 18 | old_pos_s = i * 9 19 | des_index[cur_pos_s] = old_pos_s 20 | des_index[cur_pos_s+1] = old_pos_s+1 21 | des_index[cur_pos_s+2] = old_pos_s+2 22 | cur_rot_s = 3 * num_kf + i * 3 23 | old_rot_s = i * 9 + 3 24 | des_index[cur_rot_s] = old_rot_s 25 | des_index[cur_rot_s+1] = old_rot_s+1 26 | des_index[cur_rot_s+2] = old_rot_s+2 27 | cur_vel_s = 6 * num_kf + i * 3 28 | old_vel_s = i * 9 + 6 29 | des_index[cur_vel_s] = old_vel_s 30 | des_index[cur_vel_s+1] = old_vel_s+1 31 | des_index[cur_vel_s+2] = old_vel_s+2 32 | 33 | new_cov = cov 34 | cur_index = np.array(range(num_states)) 35 | 36 | for i in range(num_states): 37 | des_id = des_index[i] 38 | loc_des_state = np.argwhere(cur_index == des_id)[0][0] 39 | if loc_des_state == i: 40 | continue 41 | # swap i and loc_des_state 42 | tmp = cur_index[i] 43 | cur_index[i] = cur_index[loc_des_state] 44 | cur_index[loc_des_state] = tmp 45 | new_cov[:, [i, loc_des_state]] = new_cov[:, [loc_des_state, i]] 46 | new_cov[[i, loc_des_state], :] = new_cov[[loc_des_state, i], :] 47 | 48 | return new_cov 49 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Covariance Transformation for Visual-Inertial Systems 2 | 3 | This repository contains the implementation of the covariance transformation method in the following publication: 4 | 5 | Zichao Zhang, Guillermo Gallego and Davide Scaramuzza, "*On the Comparison of Gauge Freedom Handling in Optimization-Based Visual-Inertial State Estimation*," in IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 2710-2717, July 2018. 6 | 7 | If the code is used, please cite: 8 | 9 | ```latex 10 | @Article{Zhang18ral, 11 | author={Zichao Zhang and Guillermo Gallego and Davide Scaramuzza}, 12 | journal={IEEE Robotics and Automation Letters}, 13 | title={On the Comparison of Gauge Freedom Handling in Optimization-Based Visual-Inertial State Estimation}, 14 | year={2018}, 15 | month={July} 16 | } 17 | ``` 18 | 19 | ## Handling unobservable degrees of freedom 20 | 21 | It is well known that visual-inertial systems have four degrees of freedom that are not observable: the global position and the rotation around gravity. These unobservable degrees of freedom (called *gauge freedom*) have to be handled properly in visual-inertial state estimation to obtain a unique state estimate. In optimization-based methods, three approaches are usually used: 22 | 23 | * *Gauge Fixation*: fixing the initial state, 24 | * *Gauge Prior*: adding a prior to the initial state, 25 | * *Free Gauge*: allowing the parameters to evolve freely during optimization. 26 | 27 | ![Spectrum](img/spectrum.png) 28 | 29 | While these three methods do not have significant differences in terms of accuracy and efficiency, the estimated covariance (inverse/pseudo inverse of the Fisher Information Matrix) can be very different. The following figure shows the estimated position uncertainties using fixation (left) and free gauge method (right). 30 | 31 | **Position uncertainties of different methods** 32 | 33 | ![Position covariance](img/pos_cov.png) 34 | 35 | While the estimated covariance from the gauge fixation method shows that the uncertainty grows over time (time increases from bottom to top), the uncertainties from the free gauge approach cannot be interpreted intuitively. This is due to the fact that no reference is chosen for the free gauge approach, and therefore the uncertainty is not associated with any reference frame. However, we show that by observing the gauge constraint, we can *transform the estimated covariance from the free gauge method to any desired gauge fixation*. The result is shown in the middle plot, where we can see that the transformed free gauge uncertainties agree very well with the ones from the gauge fixation case. 36 | 37 | Such transformations can be useful, for example, when the optimization process is used as a black box, and the covariance of a certain fixation is desired. We publicly release the code to the community to benefit related research. 38 | 39 | 40 | **Covariance matrices of different methods** 41 | 42 | The following figure shows how the covariance matrix (in position `p`, orientation `phi` and velocity `v` of the camera) from the free gauge method is transformed to a desired gauge, that is, to a desired reference frame. The yellow areas denote fixed parameters. 43 | 44 | ![Position covariance](img/cov_mat.png) 45 | 46 | 47 | ## Instructions to Run the Code 48 | First download the data 49 | 50 | ```shell 51 | cd data && ./download.sh 52 | cd .. 53 | ``` 54 | 55 | Then simply run 56 | 57 | ```python 58 | python2 compare_cov_free_fixed.py 59 | ``` 60 | 61 | The transformed covariance/uncertainties will be plotted and saved in `plots`. 62 | 63 | The script calls the function in `cov_transformation.py` to transform the free gauge covariance to the gauge fixation case. We provide several example datasets in `data` folder. You can try different ones by changing the code at the beginning of the script `cov_transformation.py`. 64 | 65 | The covariance transformation method is implemented in `cov_transformation.py`, and it does not depend on our data and its format. Please see the documentation there for details (e.g., input and output). 66 | 67 | -------------------------------------------------------------------------------- /utils/trace_parser.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | import numpy as np 4 | import os 5 | 6 | 7 | def parseTrajRes(filename): 8 | trace = open(filename, 'r') 9 | num_trials = int(trace.readline()) 10 | 11 | mc_est = [] 12 | mc_gt = [] 13 | mc_pts_est = [] 14 | mc_pts_gt = [] 15 | mc_cov = [] 16 | mc_iter_num = [] 17 | mc_reproj_err = [] 18 | mc_rot_err = [] 19 | mc_vel_err = [] 20 | mc_pos_err = [] 21 | mc_prior_yaw_err = [] # position error 22 | mc_prior_pos_err = [] # rotation error 23 | mc_time_sec = [] 24 | mc_q_inc = [] 25 | for i in range(num_trials): 26 | num_kfs = int(trace.readline()) 27 | est_single = np.zeros((num_kfs, 10)) # 3 + 4 + 3 28 | gt_single = np.zeros((num_kfs, 10)) 29 | for j in range(num_kfs): 30 | line = (trace.readline()).split() 31 | est_single[j, :] = [float(e) for e in line] 32 | for j in range(num_kfs): 33 | line = (trace.readline()).split() 34 | gt_single[j, :] = [float(e) for e in line] 35 | 36 | num_pts_est = int(trace.readline()) 37 | pts_est_single = np.zeros((num_pts_est, 3)) 38 | for j in range(num_pts_est): 39 | line = (trace.readline()).split() 40 | pts_est_single[j, :] = [float(e) for e in line] 41 | 42 | num_pts_gt = int(trace.readline()) 43 | assert num_pts_gt == num_pts_est 44 | pts_gt_single = np.zeros((num_pts_gt, 3)) 45 | for j in range(num_pts_gt): 46 | line = (trace.readline()).split() 47 | pts_gt_single[j, :] = [float(e) for e in line] 48 | 49 | cov_rows = int(trace.readline()) 50 | cov_cols = int(trace.readline()) 51 | cov_single = np.zeros((cov_rows, cov_cols)) 52 | for j in range(cov_rows): 53 | line = (trace.readline()).split() 54 | cov_single[j, :] = [float(e) for e in line] 55 | 56 | iter_num = int(trace.readline()) 57 | line = (trace.readline()).split() 58 | reproj_err = [float(e) for e in line] 59 | 60 | trace.readline() 61 | line = (trace.readline()).split() 62 | preinte_err = [float(e) for e in line] 63 | 64 | trace.readline() 65 | line = (trace.readline()).split() 66 | prior_err = [float(e) for e in line] 67 | 68 | time_sec = float(trace.readline()) 69 | 70 | trace.readline() 71 | line = (trace.readline()).split() 72 | q_inc = np.array([float(e) for e in line]) 73 | q_inc = q_inc.reshape((num_kfs, 3)) 74 | 75 | mc_est.append(est_single) 76 | mc_gt.append(gt_single) 77 | mc_pts_est.append(pts_est_single) 78 | mc_pts_gt.append(pts_gt_single) 79 | mc_cov.append(cov_single) 80 | mc_iter_num.append(iter_num) 81 | mc_reproj_err.append(reproj_err) 82 | mc_rot_err.append(preinte_err[0::3]) 83 | mc_vel_err.append(preinte_err[1::3]) 84 | mc_pos_err.append(preinte_err[2::3]) 85 | mc_prior_yaw_err.append(prior_err[0::2]) 86 | mc_prior_pos_err.append(prior_err[1::2]) 87 | 88 | mc_time_sec.append(time_sec) 89 | 90 | mc_q_inc.append(q_inc) 91 | 92 | trace.close() 93 | mc_res = {'mc_est': mc_est, 'mc_gt': mc_gt, 94 | 'mc_pts_est': mc_pts_est, 'mc_pts_gt': mc_pts_gt, 95 | 'mc_cov': mc_cov, 96 | 'mc_iter': mc_iter_num, 'mc_reproj_err': mc_reproj_err, 97 | 'mc_rot_err': mc_rot_err, 'mc_vel_err': mc_vel_err, 98 | 'mc_pos_err': mc_pos_err, 99 | 'mc_prior_yaw_err': mc_prior_yaw_err, 100 | 'mc_prior_pos_err': mc_prior_pos_err, 101 | 'mc_time_sec': mc_time_sec, 102 | 'mc_q_inc': mc_q_inc} 103 | return mc_res 104 | 105 | 106 | def parseTrajResBatch(trace_dir): 107 | catalog = open(os.path.join(trace_dir, 'trajectories.txt'), 'r') 108 | traj_files = [os.path.join(trace_dir, l[0: -1]) for l in catalog] 109 | mc_res_vec = [] 110 | for i, traj in enumerate(traj_files): 111 | mc_res = parseTrajRes(traj) 112 | mc_res_vec.append(mc_res) 113 | return mc_res_vec 114 | 115 | 116 | def parseSingle(l): 117 | return l[0:3], l[3:7], l[7:10] 118 | -------------------------------------------------------------------------------- /utils/align_utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | 4 | import numpy as np 5 | import transformations as tfs # [w, x, y, z] 6 | from tfs_utils import exp_so3, log_so3 7 | import align_trajectory as align 8 | import trace_parser as tp 9 | 10 | 11 | # utility functions 12 | def _assertDim(states): 13 | # 10: without bias; 16: with bias 14 | assert states.shape[1] == 10 or states.shape[1] == 16 15 | 16 | 17 | def _getIndices(n_aligned, total_n): 18 | if n_aligned == -1: 19 | idxs = np.arange(0, total_n) 20 | else: 21 | assert n_aligned <= total_n and n_aligned >= 1 22 | idxs = np.arange(0, n_aligned) 23 | return idxs 24 | 25 | 26 | # align by a 4-DoF transformation 27 | def alignPositionYawSingleExpm(est, gt): 28 | ''' 29 | calcualte the 4DOF transformation: yaw R and translation t so that: 30 | gt = R * est + t 31 | ''' 32 | 33 | _assertDim(est) 34 | _assertDim(gt) 35 | est_pos, est_quat, est_vel = tp.parseSingle(est[0, :]) 36 | g_pos, g_quat, g_vel = tp.parseSingle(gt[0, :]) 37 | g_rot = tfs.quaternion_matrix(g_quat) 38 | g_rot = g_rot[0:3, 0:3] 39 | est_rot = tfs.quaternion_matrix(est_quat) 40 | est_rot = est_rot[0:3, 0:3] 41 | 42 | R_full = np.dot(g_rot, np.transpose(est_rot)) 43 | log_R = log_so3(R_full) 44 | yaw_R = np.array([0, 0, log_R[2]]) 45 | if np.linalg.norm(yaw_R) < 1e-7: 46 | R = np.identity(3) 47 | else: 48 | R = exp_so3(yaw_R) 49 | t = g_pos - np.dot(R, est_pos) 50 | 51 | return R, t 52 | 53 | 54 | def alignPositionYaw(est, gt, n_aligned=1): 55 | _assertDim(est) 56 | _assertDim(gt) 57 | if n_aligned == 1: 58 | R, t = alignPositionYawSingleExpm(est, gt) 59 | return R, t 60 | else: 61 | idxs = _getIndices(n_aligned, est.shape[0]) 62 | est_pos = est[idxs, 0:3] 63 | gt_pos = gt[idxs, 0:3] 64 | R, t = align.align_se3(gt_pos, est_pos, 65 | only_yaw=True) # note the order 66 | t = np.array(t) 67 | t = t.reshape((3, )) 68 | R = np.array(R) 69 | return R, t 70 | 71 | 72 | # align by a SE3 transformation 73 | def alignSE3Single(est, gt): 74 | ''' 75 | Calculate SE3 transformation R and t so that: 76 | gt = R * est + t 77 | Using only the first poses of est and gt 78 | ''' 79 | _assertDim(est) 80 | _assertDim(gt) 81 | est_pos, est_quat, est_vel = tp.parseSingle(est[0, :]) 82 | g_pos, g_quat, g_vel = tp.parseSingle(gt[0, :]) 83 | 84 | g_rot = tfs.quaternion_matrix(g_quat) 85 | g_rot = g_rot[0:3, 0:3] 86 | est_rot = tfs.quaternion_matrix(est_quat) 87 | est_rot = est_rot[0:3, 0:3] 88 | 89 | R = np.dot(g_rot, np.transpose(est_rot)) 90 | t = g_pos - np.dot(R, est_pos) 91 | 92 | return R, t 93 | 94 | 95 | def alignSE3(est, gt, n_aligned=-1): 96 | ''' 97 | Calculate SE3 transformation R and t so that: 98 | gt = R * est + t 99 | ''' 100 | _assertDim(est) 101 | _assertDim(gt) 102 | if n_aligned == 1: 103 | R, t = alignSE3Single(est, gt) 104 | return R, t 105 | else: 106 | idxs = _getIndices(n_aligned, est.shape[0]) 107 | est_pos = est[idxs, 0:3] 108 | gt_pos = gt[idxs, 0:3] 109 | R, t = align.align_se3(gt_pos, est_pos) # note the order 110 | t = np.array(t) 111 | t = t.reshape((3, )) 112 | R = np.array(R) 113 | return R, t 114 | 115 | 116 | # transformations 117 | def transformStates(states, R, t, scale=1): 118 | states_new = np.zeros(states.shape) 119 | for i, e in enumerate(states): 120 | p, q, v = tp.parseSingle(e) 121 | p_new = scale * np.dot(R, p) + t 122 | v_new = np.dot(R, v) 123 | rot = tfs.quaternion_matrix(q) 124 | rot = rot[0:3, 0:3] 125 | q_new = tfs.quaternion_from_matrix(np.dot(R, rot)) 126 | states_new[i, 0:10] = np.hstack((p_new, q_new, v_new)) 127 | 128 | return states_new 129 | 130 | 131 | def transformPoints(pts, R, t): 132 | pts_new = np.zeros(pts.shape) 133 | for i, pt in enumerate(pts): 134 | pt_new = np.dot(R, pt) + t 135 | pts_new[i] = pt_new 136 | 137 | return pts_new 138 | 139 | 140 | def transformRinc(incs, R): 141 | ''' 142 | Exp(inc_new) = R Exp(inc) 143 | ''' 144 | assert incs.shape[1] == 3 145 | 146 | new_incs = np.zeros(incs.shape) 147 | for i, inc in enumerate(incs): 148 | R_inc = exp_so3(inc) 149 | R_new_inc = np.dot(R, R_inc) 150 | new_inc = log_so3(R_new_inc) 151 | new_incs[i, :] = new_inc 152 | 153 | return new_incs 154 | 155 | 156 | if __name__ == '__main__': 157 | pass 158 | -------------------------------------------------------------------------------- /utils/tfs_utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | 4 | import numpy as np 5 | import transformations as tfs 6 | 7 | 8 | def skewv3(v): 9 | return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) 10 | 11 | 12 | def transformPt(T, pt): 13 | assert T.shape == (4, 4) 14 | return np.dot(T[0:3, 0:3], pt) + T[0:3, 3] 15 | 16 | 17 | def wxyz2xyzw(quat): 18 | quat_new = np.zeros((4, )) 19 | quat_new[0:3], quat_new[3] = quat[1:4], quat[0] 20 | return quat_new 21 | 22 | 23 | def xyzw2wxyz(quat): 24 | quat_new = np.zeros((4, )) 25 | quat_new[1:4], quat_new[0] = quat[0:3], quat[3] 26 | return quat_new 27 | 28 | 29 | def exp_so3(v): 30 | """ 31 | Grassia, F. S. (1998). 32 | Practical parameterization of rotations using the exponential map 33 | """ 34 | angle = np.linalg.norm(v) 35 | 36 | if angle < np.power(np.finfo(float).eps, 0.25): 37 | na = 0.5 + angle * angle * 1.0 / 48.0 38 | else: 39 | na = np.sin(angle * 0.5) / angle 40 | 41 | ct = np.cos(angle * 0.5) 42 | 43 | T = tfs.quaternion_matrix([ct, na * v[0], na * v[1], na * v[2]]) 44 | return T[0:3, 0:3] 45 | 46 | 47 | def log_so3(R): 48 | assert R.shape == (3, 3) 49 | T = np.identity(4) 50 | T[0:3, 0:3] = R 51 | angle, axis, _ = tfs.rotation_from_matrix(T) 52 | 53 | return angle * axis 54 | 55 | 56 | def exp_se3(v): 57 | vs = np.squeeze(v) 58 | assert vs.shape[0] == 6 59 | t = vs[0:3] 60 | rotv = vs[3:6] 61 | R = exp_so3(rotv) 62 | T = np.identity(4) 63 | T[0:3, 0:3] = R 64 | T[0:3, 3] = t 65 | 66 | return T 67 | 68 | 69 | def log_se3(T): 70 | assert T.shape == (4, 4) 71 | R = T[0:3, 0:3] 72 | t = T[0:3, 3] 73 | rotv = log_so3(R) 74 | 75 | return np.hstack((t, rotv)) 76 | 77 | 78 | def randomTranformation(): 79 | T = tfs.random_rotation_matrix() 80 | T[0:3, 3] = np.random.rand(3) 81 | 82 | return T 83 | 84 | 85 | def calTyaw(theta): 86 | return exp_se3(np.hstack((np.zeros(5), theta))) 87 | 88 | 89 | def reorderAngle(angles): 90 | n_angle = angles.shape[0] 91 | # convert to 0-2pi 92 | angles_2pi = np.array([a % (2 * np.pi) for a in angles]) 93 | for v in angles_2pi: 94 | assert v >= 0 and v <= 2 * np.pi 95 | 96 | # loop over and count wrap around 97 | bias = 0 98 | angles_re = angles_2pi 99 | for i in np.arange(1, n_angle): 100 | # calculate current bias 101 | s = angles_re[i-1] % (2 * np.pi) 102 | e = angles_re[i] % (2 * np.pi) 103 | if s > e and s > np.pi and (s - e) > np.pi: 104 | bias = bias + 2 * np.pi 105 | elif e > s and s < np.pi and (e - s) > np.pi: 106 | bias = bias - 2 * np.pi 107 | angles_re[i] += bias 108 | return angles_re 109 | 110 | 111 | def leftJacobian(v): 112 | if np.linalg.norm(v) < 1e-10: 113 | return np.eye(3) 114 | s_v = skewv3(v) 115 | n_v = np.linalg.norm(v) 116 | I = np.identity(3) 117 | Jl = I + ((1 - np.cos(n_v)) / (n_v*n_v)) * s_v + \ 118 | ((n_v - np.sin(n_v))/(n_v*n_v*n_v)) * np.dot(s_v, s_v) 119 | return Jl 120 | 121 | 122 | def invLeftJacobian(v): 123 | if np.linalg.norm(v) < 1e-10: 124 | return np.eye(3) 125 | s_v = skewv3(v) 126 | n_v = np.linalg.norm(v) 127 | I = np.identity(3) 128 | Jl_inv = I - 0.5 * s_v + \ 129 | (1/(n_v*n_v) - (1+np.cos(n_v))/(2*n_v*np.sin(n_v))) * np.dot(s_v, s_v) 130 | return Jl_inv 131 | 132 | 133 | def rightJacobian(v): 134 | if np.linalg.norm(v) < 1e-10: 135 | return np.eye(3) 136 | s_v = skewv3(v) 137 | n_v = np.linalg.norm(v) 138 | I = np.identity(3) 139 | 140 | Jr = I - ((1 - np.cos(n_v)) / (n_v*n_v)) * s_v + \ 141 | ((n_v - np.sin(n_v)) / (n_v * n_v * n_v)) * np.dot(s_v, s_v) 142 | 143 | return Jr 144 | 145 | 146 | def invRightJacobian(v): 147 | if np.linalg.norm(v) < 1e-10: 148 | return np.eye(3) 149 | s_v = skewv3(v) 150 | n_v = np.linalg.norm(v) 151 | I = np.identity(3) 152 | 153 | Jr_inv = I + 0.5 * s_v + \ 154 | (1/(n_v*n_v) - (1+np.cos(n_v))/(2*n_v*np.sin(n_v))) * np.dot(s_v, s_v) 155 | return Jr_inv 156 | 157 | 158 | if __name__ == "__main__": 159 | axis = np.random.rand(3) 160 | axis = axis / np.linalg.norm(axis) 161 | angle = np.random.rand() - 0.5 # -0.5~0.5 no singularity 162 | rotv = angle * axis 163 | 164 | R = exp_so3(rotv) 165 | 166 | so3_vec = log_so3(R) 167 | 168 | print("Original so3 vector:\n{0}".format(rotv)) 169 | print("Rotation matrix:\n{0}".format(R)) 170 | print("so3 vector:\n{0}".format(so3_vec)) 171 | 172 | t = np.random.rand(3) 173 | se3vec = np.hstack((t, rotv)) 174 | T = exp_se3(se3vec) 175 | log_se3vec = log_se3(T) 176 | 177 | print("Original se3 vector:\n{0}".format(se3vec)) 178 | print("Rotation matrix:\n{0}".format(T)) 179 | print("so3 vector:\n{0}".format(log_se3vec)) 180 | 181 | angles = np.array([0.1, -0.1, 0.5, 3.14, 5, 0.1]) 182 | re_angles = reorderAngle(angles) 183 | print("raw angles are:\n {0}".format(angles)) 184 | print("reordered angles are:\n {0}".format(re_angles)) 185 | 186 | angles = np.array([0, np.pi-0.1, 0]) 187 | re_angles = reorderAngle(angles) 188 | print("raw angles are:\n {0}".format(angles)) 189 | print("reordered angles are:\n {0}".format(re_angles)) 190 | 191 | angles = np.array([0, np.pi+0.1, 0]) 192 | re_angles = reorderAngle(angles) 193 | print("raw angles are:\n {0}".format(angles)) 194 | print("reordered angles are:\n {0}".format(re_angles)) 195 | 196 | angles = np.array([0, np.pi-0.1, -np.pi+0.1, 0]) 197 | re_angles = reorderAngle(angles) 198 | print("raw angles are:\n {0}".format(angles)) 199 | print("reordered angles are:\n {0}".format(re_angles)) 200 | 201 | -------------------------------------------------------------------------------- /cov_transformation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | 4 | import numpy as np 5 | 6 | import utils.trace_parser as tp 7 | import utils.tfs_utils as tu 8 | import utils.align_utils as au 9 | 10 | 11 | def transform_vi_covariance(cov_free, 12 | q_inc_free, q_inc_fixed, 13 | est_free, est_fixed, 14 | all_inc_rot=False): 15 | """Transform the free gauge covariance to the same reference frame as 16 | the gauge fixation one 17 | 18 | Suppose N is the number of keyframes. 19 | 20 | Input: 21 | cov_free -- 9N x 9N array 22 | The free gauge covariance to be transformed. 23 | The states are ordered as [position, rotation, velocity] 24 | 25 | q_inc_free -- list of length N, each of which is a (3,) numpy array. 26 | The estimated incremental rotation of the free gauge method. 27 | If all_inc_rot is False, only the first element is used. 28 | 29 | q_inc_fixed -- list of length N, each of which is a (3,) numpy array 30 | The estimated incremental rotations of the fixed gauge 31 | method. If all_inc_rot is False, only the first element is 32 | meaningful and used. 33 | 34 | est_free -- list of length N, each of which is a (10,) numpy array. 35 | The estimated state of the free gauge method. 36 | Each state is ordered as [position, rotation, velocity] 37 | 38 | est_fixed -- list of length N, each of which is a (10,) numpy array. 39 | The estimated state of the fixed gauge method. 40 | Each state is ordered as [position, rotation, velocity] 41 | 42 | all_inc_rot -- If this parameter is True, the incremental rotation 43 | parameterization is used for all rotations. 44 | Otherwise, it is only used for the first rotation. 45 | This affects the transformation of the rotation-related 46 | covariance. 47 | 48 | Output: 49 | cov_proj -- The final transformed covaraince (9N x 9N) 50 | cov_aligend -- The intermediate covariance before the oblique projection. 51 | Q -- The oblique projection matrix. 52 | 53 | """ 54 | n_states = cov_free.shape[0] 55 | assert n_states % 9 == 0 56 | n_kfs = n_states / 9 57 | 58 | pos_from_0, quat_from_0, vel_from_0 = tp.parseSingle(est_free[0, :]) 59 | pos_to_0, quat_to_0, vel_to_0 = tp.parseSingle(est_fixed[0, :]) 60 | 61 | print("Going to transform covariance of {0} keyframes".format(n_kfs)) 62 | 63 | # 0 calcualte transformation from the first state 64 | R_inc_0_from = tu.exp_so3(q_inc_free[0]) 65 | R_inc_0_to = tu.exp_so3(q_inc_fixed[0]) 66 | 67 | R_full = np.dot(R_inc_0_to, np.transpose(R_inc_0_from)) 68 | log_R = tu.log_so3(R_full) 69 | yaw_R = np.array([0, 0, log_R[2]]) 70 | if np.linalg.norm(yaw_R) < 1e-7: 71 | R = np.identity(3) 72 | else: 73 | R = tu.exp_so3(yaw_R) 74 | t = pos_to_0 - np.dot(R, pos_from_0) 75 | 76 | print("The transformation to be applied is\n{0}\n{1}".format(R, t)) 77 | 78 | # align the estimation 79 | q_inc_aligned = au.transformRinc(q_inc_free, R) 80 | est_aligned = au.transformStates(est_free, R, t) 81 | 82 | # 1 transform the covariance 83 | tfs_jac = np.zeros((9*n_kfs, 9*n_kfs)) 84 | for j in range(n_kfs): 85 | cs = j * 3 86 | 87 | # positions 88 | tfs_jac[cs:cs+3, cs:cs+3] = R 89 | 90 | # velocity 91 | tfs_jac[6*n_kfs + cs: 6*n_kfs + cs+3, 92 | 6*n_kfs + cs: 6*n_kfs + cs+3] = R 93 | 94 | # q_inc 95 | if all_inc_rot or j == 0: 96 | inv_Jr = tu.invRightJacobian(q_inc_aligned[j]) 97 | Jr = tu.rightJacobian(q_inc_free[j]) 98 | tfs_jac[3*n_kfs + cs: 3*n_kfs + cs+3, 99 | 3*n_kfs + cs: 3*n_kfs + cs+3] = np.dot(inv_Jr, Jr) 100 | else: 101 | tfs_jac[3*n_kfs + cs: 3*n_kfs + cs+3, 102 | 3*n_kfs + cs: 3*n_kfs + cs+3] = R 103 | 104 | cov_aligned = np.dot(tfs_jac, 105 | np.dot(cov_free, np.transpose(tfs_jac))) 106 | 107 | # 2 project the covariance 108 | az = np.array([0, 0, 1]) 109 | s_az = tu.skewv3(az) 110 | # U 111 | U = np.zeros((n_states, 4)) 112 | for j in range(n_kfs): 113 | cs = 3 * j 114 | # position 115 | U[cs: cs + 3, 0] = np.dot(s_az, est_aligned[j, 0:3]) 116 | U[cs: cs + 3, 1:4] = np.identity(3) 117 | 118 | # rotation 119 | if j == 0 or all_inc_rot: 120 | U[3*n_kfs+cs: 3*n_kfs+cs+3, 0] = \ 121 | np.dot(tu.invLeftJacobian(q_inc_aligned[j]), az) 122 | U[3*n_kfs+cs: 3*n_kfs+cs + 3, 1:4] = np.zeros((3, 3)) 123 | else: 124 | U[3*n_kfs+cs: 3*n_kfs+cs+3, 0] = az 125 | U[3*n_kfs+cs: 3*n_kfs+cs + 3, 1:4] = np.zeros((3, 3)) 126 | 127 | # velocity 128 | U[6*n_kfs+cs: 6*n_kfs+cs+3, 0] = \ 129 | np.dot(s_az, est_aligned[j, 7:10]) 130 | 131 | U[6*n_kfs+cs: 6*n_kfs+cs + 3, 1:4] = np.zeros((3, 3)) 132 | 133 | # V 134 | V = np.zeros((n_states, 4)) 135 | V[0:3, 1:4] = np.identity(3) 136 | V[3*n_kfs + 2, 0] = 1 137 | 138 | VTU = np.dot(np.transpose(V), U) 139 | invVTU = np.linalg.inv(VTU) 140 | print("VTU is\n{0} \nand its inverse is\n{1}".format(VTU, invVTU)) 141 | Q = np.identity(n_states) - np.dot(U, np.dot(invVTU, np.transpose(V))) 142 | 143 | print("The rank of Q is {0} of size {1}".format(np.linalg.matrix_rank(Q), 144 | Q.shape[0])) 145 | 146 | cov_proj = np.dot(Q, np.dot(cov_aligned, np.transpose(Q))) 147 | 148 | return cov_proj, cov_aligned, Q 149 | 150 | if __name__ == '__main__': 151 | pass 152 | -------------------------------------------------------------------------------- /compare_cov_free_fixed.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | 4 | import os 5 | 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | from matplotlib import rc 9 | from matplotlib.patches import Ellipse 10 | 11 | import utils.mat_utils as mu 12 | import utils.trace_parser as tp 13 | 14 | import cov_transformation as ct 15 | 16 | rc('font', **{'family': 'serif', 'serif': ['Cardo'], 'size': 20}) 17 | rc('text', usetex=True) 18 | 19 | # directories 20 | cur_file_dir = os.path.dirname(os.path.abspath(__file__)) 21 | data_dir = os.path.join(cur_file_dir, 'data/compare_cov_free_fix') 22 | plot_dir = os.path.join(cur_file_dir, 'plots') 23 | 24 | 25 | # NOTE: choose different datasets here 26 | # results that used incremental rotation for ALL orientations 27 | # all_inc_rot = True 28 | # free_trace = os.path.join(data_dir, 'inc_rot_all/free_sine.txt') 29 | # fix_trace = os.path.join(data_dir, 'inc_rot_all/fix_sine.txt') 30 | 31 | # results that used incremental rotation ONLY for the first orientation 32 | all_inc_rot = False 33 | free_trace = os.path.join(data_dir, 'inc_rot_first/free_mh_30.txt') 34 | fix_trace = os.path.join(data_dir, 'inc_rot_first/fix_mh_30.txt') 35 | 36 | # parsing 37 | print("#####################") 38 | print("Parsing results...") 39 | print("#####################") 40 | mc_res_free = tp.parseTrajRes(free_trace) 41 | mc_res_fix = tp.parseTrajRes(fix_trace) 42 | num_trials = len(mc_res_free['mc_est']) 43 | num_kfs = mc_res_free['mc_est'][0].shape[0] 44 | num_sts = 9 * num_kfs 45 | 46 | print("Current trace has {0} trials with {1} keyframes.".format(num_trials, 47 | num_kfs)) 48 | valid_cov_ids = [0] 49 | # permute covaraince: put position/rotation/velocity together 50 | mc_perm_cov_free = [] 51 | for i in valid_cov_ids: 52 | cur_num_kfs = mc_res_free['mc_est'][i].shape[0] 53 | perm_cov = mu.permuteHesCov( 54 | mc_res_free['mc_cov'][i][0:9*num_kfs, 0:9*num_kfs], num_kfs) 55 | mc_perm_cov_free.append(perm_cov) 56 | mc_res_free['mc_cov'] = mc_perm_cov_free 57 | 58 | mc_perm_cov_fix = [] 59 | for i in valid_cov_ids: 60 | cur_num_kfs = mc_res_fix['mc_est'][i].shape[0] 61 | perm_cov = mu.permuteHesCov( 62 | mc_res_fix['mc_cov'][i][0:9*num_kfs, 0:9*num_kfs], num_kfs) 63 | mc_perm_cov_fix.append(perm_cov) 64 | mc_res_fix['mc_cov'] = mc_perm_cov_fix 65 | 66 | # visualization one trial 67 | viz_id = 0 68 | cov_free = mc_res_free['mc_cov'][viz_id] 69 | cov_fix = mc_res_fix['mc_cov'][viz_id] 70 | q_inc_free = mc_res_free['mc_q_inc'][viz_id] 71 | q_inc_fix = mc_res_fix['mc_q_inc'][viz_id] 72 | est_free = mc_res_free['mc_est'][viz_id] 73 | est_fix = mc_res_fix['mc_est'][viz_id] 74 | 75 | # transform covariance 76 | print("#####################") 77 | print("Transform covaraince for trial {0}...".format(viz_id)) 78 | print("#####################") 79 | cov_free_transformed, cov_free_aligned, Q = \ 80 | ct.transform_vi_covariance(cov_free, 81 | q_inc_free, q_inc_fix, 82 | est_free, est_fix, all_inc_rot) 83 | gt_fixed = mc_res_fix['mc_gt'][viz_id] 84 | 85 | # visualize the covaraince as matrices 86 | print("#####################") 87 | print("Plotting covariance matrices and saving results...") 88 | print("#####################") 89 | fig = plt.figure() 90 | ax = fig.add_subplot(111) 91 | plt.imshow(Q, interpolation='none', cmap='gray_r') 92 | plt.colorbar() 93 | fig.savefig(plot_dir + '/Q.pdf', bbox_inches='tight') 94 | 95 | dim_sts = cov_free.shape[0] 96 | num_kfs = dim_sts / 9 97 | one_third = dim_sts / 3 98 | print("Visualization: {0} states and {1} keyframes.".format(dim_sts, num_kfs)) 99 | ticks = [0.5 * one_third, 1.5 * one_third, 2.5 * one_third] 100 | st_labels = ['$\mathbf{p}$', '$\phi$', '$\mathbf{v}$'] 101 | 102 | zero_region = np.zeros((dim_sts, dim_sts)) 103 | zero_region[:, 0:3] = 1.0 104 | zero_region[0:3, :] = 1.0 105 | zero_region[3*num_kfs+2, :] = 1.0 106 | zero_region[:, 3*num_kfs+2] = 1.0 107 | zero_region = np.ma.masked_where(zero_region < 0.5, zero_region) 108 | 109 | disp_offset = 1e-6 110 | 111 | fig = plt.figure() 112 | fig.canvas.set_window_title('Free Covariance') 113 | ax = fig.add_subplot(111) 114 | plt.imshow(np.log10(disp_offset + np.abs(cov_free)), interpolation='none', 115 | cmap='gray_r') 116 | plt.colorbar() 117 | ax.set_xticks(ticks) 118 | ax.set_xticklabels(st_labels) 119 | ax.set_yticks(ticks) 120 | ax.set_yticklabels(st_labels) 121 | ax.tick_params(axis=u'both', which=u'both', length=0) 122 | plt.tight_layout() 123 | fig.savefig(plot_dir + '/free_cov_mat.pdf', bbox_inches='tight') 124 | 125 | fig = plt.figure() 126 | fig.canvas.set_window_title('Aligned Free Covariance') 127 | ax = fig.add_subplot(111) 128 | plt.imshow(np.log10(disp_offset + np.abs(cov_free_aligned)), 129 | interpolation='none', cmap='gray_r') 130 | plt.colorbar() 131 | ax.set_xticks(ticks) 132 | ax.set_xticklabels(st_labels) 133 | ax.set_yticks(ticks) 134 | ax.set_yticklabels(st_labels) 135 | ax.tick_params(axis=u'both', which=u'both', length=0) 136 | plt.tight_layout() 137 | fig.savefig(plot_dir + '/aligned_free_cov_mat.pdf', bbox_inches='tight') 138 | 139 | 140 | cmap = 'autumn' 141 | 142 | fig = plt.figure() 143 | fig.canvas.set_window_title('Transformed Free Covariance') 144 | ax = fig.add_subplot(111) 145 | plt.imshow(np.log10(disp_offset + np.abs(cov_free_transformed)), 146 | interpolation='none', cmap='gray_r') 147 | plt.colorbar() 148 | plt.imshow(zero_region, interpolation='none', cmap=cmap, vmax=1.0, vmin=0.0, 149 | alpha=1.0) 150 | ax.set_xticks(ticks) 151 | ax.set_xticklabels(st_labels) 152 | ax.set_yticks(ticks) 153 | ax.set_yticklabels(st_labels) 154 | ax.tick_params(axis=u'both', which=u'both', length=0) 155 | plt.tight_layout() 156 | fig.savefig(plot_dir + '/transformed_cov_mat.pdf', bbox_inches='tight') 157 | 158 | fig = plt.figure() 159 | fig.canvas.set_window_title('Fixed Covariance') 160 | ax = fig.add_subplot(111) 161 | plt.imshow(np.log10(disp_offset + np.abs(cov_fix)), interpolation='none', 162 | cmap='gray_r') 163 | plt.colorbar() 164 | plt.imshow(zero_region, interpolation='none', cmap=cmap, vmax=1.0, vmin=0.0, 165 | alpha=1.0) 166 | ax.set_xticks(ticks) 167 | ax.set_xticklabels(st_labels) 168 | ax.set_yticks(ticks) 169 | ax.set_yticklabels(st_labels) 170 | ax.tick_params(axis=u'both', which=u'both', length=0) 171 | plt.tight_layout() 172 | fig.savefig(plot_dir + '/fixed_cov_mat.pdf', bbox_inches='tight') 173 | 174 | # visualize position uncertainties 175 | print("#####################") 176 | print("Plotting 2D position uncertainties and saving results...") 177 | print("#####################") 178 | c_free = 'r' 179 | c_fixed = 'b' 180 | free_viz_s = 1e3 181 | free_align_var_ellips = [] 182 | for i in range(num_kfs): 183 | cur_var_e = Ellipse(gt_fixed[i, 0:2], 184 | free_viz_s * cov_free_aligned[i*3, i*3], 185 | free_viz_s * cov_free_aligned[i*3+1, i*3+1], 186 | fill=False) 187 | free_align_var_ellips.append(cur_var_e) 188 | 189 | fix_viz_s = 3e3 190 | fix_var_ellips = [] 191 | for i in range(num_kfs): 192 | cur_var_e = Ellipse(gt_fixed[i, 0:2], 193 | fix_viz_s * cov_fix[i*3, i*3], 194 | fix_viz_s * cov_fix[i*3+1, i*3+1], 195 | fill=False) 196 | fix_var_ellips.append(cur_var_e) 197 | 198 | free_proj_var_ellips = [] 199 | for i in range(num_kfs): 200 | cur_var_e = Ellipse(gt_fixed[i, 0:2], 201 | fix_viz_s * cov_free_transformed[i*3, i*3], 202 | fix_viz_s * cov_free_transformed[i*3+1, i*3+1], 203 | fill=False) 204 | free_proj_var_ellips.append(cur_var_e) 205 | 206 | fig = plt.figure(figsize=(6, 6)) 207 | fig.canvas.set_window_title('Free 2D') 208 | ax = fig.add_subplot(111) 209 | ax.plot(gt_fixed[:, 0], gt_fixed[:, 1], color='k') 210 | for e in free_align_var_ellips: 211 | ax.add_artist(e) 212 | e.set_facecolor('none') 213 | e.set_color(c_free) 214 | e.set(label='Free') 215 | # ax.legend() 216 | plt.tight_layout() 217 | plt.xlabel('x (m)') 218 | plt.ylabel('y (m)') 219 | plt.axis('equal') 220 | fig.savefig(plot_dir + '/aligned_free_cov.pdf', bbox_inches='tight') 221 | 222 | fig = plt.figure(figsize=(6, 6)) 223 | fig.canvas.set_window_title('Fixed 2D') 224 | ax = fig.add_subplot(111) 225 | ax.plot(gt_fixed[:, 0], gt_fixed[:, 1], color='k') 226 | for e in fix_var_ellips: 227 | ax.add_artist(e) 228 | e.set_facecolor('none') 229 | e.set_color(c_fixed) 230 | e.set_label('Fixed') 231 | # ax.legend() 232 | plt.tight_layout() 233 | plt.xlabel('x (m)') 234 | plt.ylabel('y (m)') 235 | plt.axis('equal') 236 | fig.savefig(plot_dir + '/fixed_cov.pdf', bbox_inches='tight') 237 | 238 | fig = plt.figure(figsize=(6, 6)) 239 | fig.canvas.set_window_title('Transformed 2D') 240 | ax = fig.add_subplot(111) 241 | ax.plot(gt_fixed[:, 0], gt_fixed[:, 1], color='k') 242 | for e in free_proj_var_ellips: 243 | ax.add_artist(e) 244 | e.set_facecolor('none') 245 | e.set_color(c_free) 246 | e.set_label('Transformed Free') 247 | # ax.legend() 248 | plt.tight_layout() 249 | plt.xlabel('x (m)') 250 | plt.ylabel('y (m)') 251 | plt.axis('equal') 252 | fig.savefig(plot_dir + '/transformed_free_cov.pdf', bbox_inches='tight') 253 | 254 | # calculate statistics 255 | print("#####################") 256 | print("Calculating statistics...") 257 | print("#####################") 258 | transformed_diff_cov = cov_free_transformed - cov_fix 259 | org_diff_cov = cov_free - cov_fix 260 | 261 | n_org_diff_cov = np.linalg.norm(org_diff_cov, 'fro') 262 | n_transformed_diff_cov = np.linalg.norm(transformed_diff_cov, 'fro') 263 | 264 | n_org_free_cov = np.linalg.norm(cov_free, 'fro') 265 | n_transformed_free_cov = np.linalg.norm(cov_free_transformed, 'fro') 266 | n_fix_cov = np.linalg.norm(cov_fix, 'fro') 267 | 268 | print("The Frobenius norm of covariance difference" 269 | " before transformation is {0}.".format(n_org_diff_cov)) 270 | print("The Frobenius norm of covariance difference" 271 | " after transformation {0}.".format(n_transformed_diff_cov)) 272 | 273 | print(">>> Relative difference before transformation:") 274 | print("The relative difference is" 275 | " {0} %.".format(n_org_diff_cov / n_fix_cov * 100)) 276 | print("The relative difference is" 277 | " {0} %.".format(n_org_diff_cov / n_org_free_cov * 100)) 278 | 279 | print(">>> Relative difference after transformation:") 280 | print("The relative difference is" 281 | " {0} %.".format(n_transformed_diff_cov / n_fix_cov * 100)) 282 | print("The relative difference is" 283 | " {0} %.".format(n_transformed_diff_cov / n_transformed_free_cov * 100)) 284 | 285 | 286 | # plt.show() 287 | -------------------------------------------------------------------------------- /utils/transformations.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # transformations.py 3 | 4 | # Copyright (c) 2006-2017, Christoph Gohlke 5 | # Copyright (c) 2006-2017, The Regents of the University of California 6 | # Produced at the Laboratory for Fluorescence Dynamics 7 | # All rights reserved. 8 | # 9 | # Redistribution and use in source and binary forms, with or without 10 | # modification, are permitted provided that the following conditions are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above copyright 15 | # notice, this list of conditions and the following disclaimer in the 16 | # documentation and/or other materials provided with the distribution. 17 | # * Neither the name of the copyright holders nor the names of any 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | """Homogeneous Transformation Matrices and Quaternions. 34 | 35 | A library for calculating 4x4 matrices for translating, rotating, reflecting, 36 | scaling, shearing, projecting, orthogonalizing, and superimposing arrays of 37 | 3D homogeneous coordinates as well as for converting between rotation matrices, 38 | Euler angles, and quaternions. Also includes an Arcball control object and 39 | functions to decompose transformation matrices. 40 | 41 | :Author: 42 | `Christoph Gohlke `_ 43 | 44 | :Organization: 45 | Laboratory for Fluorescence Dynamics, University of California, Irvine 46 | 47 | :Version: 2017.02.17 48 | 49 | Requirements 50 | ------------ 51 | * `CPython 2.7 or 3.5 `_ 52 | * `Numpy 1.11 `_ 53 | * `Transformations.c 2017.02.17 `_ 54 | (recommended for speedup of some functions) 55 | 56 | Notes 57 | ----- 58 | The API is not stable yet and is expected to change between revisions. 59 | 60 | This Python code is not optimized for speed. Refer to the transformations.c 61 | module for a faster implementation of some functions. 62 | 63 | Documentation in HTML format can be generated with epydoc. 64 | 65 | Matrices (M) can be inverted using numpy.linalg.inv(M), be concatenated using 66 | numpy.dot(M0, M1), or transform homogeneous coordinate arrays (v) using 67 | numpy.dot(M, v) for shape (4, \*) column vectors, respectively 68 | numpy.dot(v, M.T) for shape (\*, 4) row vectors ("array of points"). 69 | 70 | This module follows the "column vectors on the right" and "row major storage" 71 | (C contiguous) conventions. The translation components are in the right column 72 | of the transformation matrix, i.e. M[:3, 3]. 73 | The transpose of the transformation matrices may have to be used to interface 74 | with other graphics systems, e.g. with OpenGL's glMultMatrixd(). See also [16]. 75 | 76 | Calculations are carried out with numpy.float64 precision. 77 | 78 | Vector, point, quaternion, and matrix function arguments are expected to be 79 | "array like", i.e. tuple, list, or numpy arrays. 80 | 81 | Return types are numpy arrays unless specified otherwise. 82 | 83 | Angles are in radians unless specified otherwise. 84 | 85 | Quaternions w+ix+jy+kz are represented as [w, x, y, z]. 86 | 87 | A triple of Euler angles can be applied/interpreted in 24 ways, which can 88 | be specified using a 4 character string or encoded 4-tuple: 89 | 90 | *Axes 4-string*: e.g. 'sxyz' or 'ryxy' 91 | 92 | - first character : rotations are applied to 's'tatic or 'r'otating frame 93 | - remaining characters : successive rotation axis 'x', 'y', or 'z' 94 | 95 | *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) 96 | 97 | - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. 98 | - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed 99 | by 'z', or 'z' is followed by 'x'. Otherwise odd (1). 100 | - repetition : first and last axis are same (1) or different (0). 101 | - frame : rotations are applied to static (0) or rotating (1) frame. 102 | 103 | Other Python packages and modules for 3D transformations and quaternions: 104 | 105 | * `Transforms3d `_ 106 | includes most code of this module. 107 | * `Blender.mathutils `_ 108 | * `numpy-dtypes `_ 109 | 110 | References 111 | ---------- 112 | (1) Matrices and transformations. Ronald Goldman. 113 | In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. 114 | (2) More matrices and transformations: shear and pseudo-perspective. 115 | Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 116 | (3) Decomposing a matrix into simple transformations. Spencer Thomas. 117 | In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 118 | (4) Recovering the data from the transformation matrix. Ronald Goldman. 119 | In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. 120 | (5) Euler angle conversion. Ken Shoemake. 121 | In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. 122 | (6) Arcball rotation control. Ken Shoemake. 123 | In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. 124 | (7) Representing attitude: Euler angles, unit quaternions, and rotation 125 | vectors. James Diebel. 2006. 126 | (8) A discussion of the solution for the best rotation to relate two sets 127 | of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. 128 | (9) Closed-form solution of absolute orientation using unit quaternions. 129 | BKP Horn. J Opt Soc Am A. 1987. 4(4):629-642. 130 | (10) Quaternions. Ken Shoemake. 131 | http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf 132 | (11) From quaternion to matrix and back. JMP van Waveren. 2005. 133 | http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm 134 | (12) Uniform random rotations. Ken Shoemake. 135 | In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. 136 | (13) Quaternion in molecular modeling. CFF Karney. 137 | J Mol Graph Mod, 25(5):595-604 138 | (14) New method for extracting the quaternion from a rotation matrix. 139 | Itzhack Y Bar-Itzhack, J Guid Contr Dynam. 2000. 23(6): 1085-1087. 140 | (15) Multiple View Geometry in Computer Vision. Hartley and Zissermann. 141 | Cambridge University Press; 2nd Ed. 2004. Chapter 4, Algorithm 4.7, p 130. 142 | (16) Column Vectors vs. Row Vectors. 143 | http://steve.hollasch.net/cgindex/math/matrix/column-vec.html 144 | 145 | Examples 146 | -------- 147 | >>> alpha, beta, gamma = 0.123, -1.234, 2.345 148 | >>> origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1] 149 | >>> I = identity_matrix() 150 | >>> Rx = rotation_matrix(alpha, xaxis) 151 | >>> Ry = rotation_matrix(beta, yaxis) 152 | >>> Rz = rotation_matrix(gamma, zaxis) 153 | >>> R = concatenate_matrices(Rx, Ry, Rz) 154 | >>> euler = euler_from_matrix(R, 'rxyz') 155 | >>> numpy.allclose([alpha, beta, gamma], euler) 156 | True 157 | >>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') 158 | >>> is_same_transform(R, Re) 159 | True 160 | >>> al, be, ga = euler_from_matrix(Re, 'rxyz') 161 | >>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) 162 | True 163 | >>> qx = quaternion_about_axis(alpha, xaxis) 164 | >>> qy = quaternion_about_axis(beta, yaxis) 165 | >>> qz = quaternion_about_axis(gamma, zaxis) 166 | >>> q = quaternion_multiply(qx, qy) 167 | >>> q = quaternion_multiply(q, qz) 168 | >>> Rq = quaternion_matrix(q) 169 | >>> is_same_transform(R, Rq) 170 | True 171 | >>> S = scale_matrix(1.23, origin) 172 | >>> T = translation_matrix([1, 2, 3]) 173 | >>> Z = shear_matrix(beta, xaxis, origin, zaxis) 174 | >>> R = random_rotation_matrix(numpy.random.rand(3)) 175 | >>> M = concatenate_matrices(T, R, Z, S) 176 | >>> scale, shear, angles, trans, persp = decompose_matrix(M) 177 | >>> numpy.allclose(scale, 1.23) 178 | True 179 | >>> numpy.allclose(trans, [1, 2, 3]) 180 | True 181 | >>> numpy.allclose(shear, [0, math.tan(beta), 0]) 182 | True 183 | >>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) 184 | True 185 | >>> M1 = compose_matrix(scale, shear, angles, trans, persp) 186 | >>> is_same_transform(M, M1) 187 | True 188 | >>> v0, v1 = random_vector(3), random_vector(3) 189 | >>> M = rotation_matrix(angle_between_vectors(v0, v1), vector_product(v0, v1)) 190 | >>> v2 = numpy.dot(v0, M[:3,:3].T) 191 | >>> numpy.allclose(unit_vector(v1), unit_vector(v2)) 192 | True 193 | 194 | """ 195 | 196 | from __future__ import division, print_function 197 | 198 | import math 199 | 200 | import numpy 201 | 202 | __version__ = '2017.02.17' 203 | __docformat__ = 'restructuredtext en' 204 | __all__ = () 205 | 206 | 207 | def identity_matrix(): 208 | """Return 4x4 identity/unit matrix. 209 | 210 | >>> I = identity_matrix() 211 | >>> numpy.allclose(I, numpy.dot(I, I)) 212 | True 213 | >>> numpy.sum(I), numpy.trace(I) 214 | (4.0, 4.0) 215 | >>> numpy.allclose(I, numpy.identity(4)) 216 | True 217 | 218 | """ 219 | return numpy.identity(4) 220 | 221 | 222 | def translation_matrix(direction): 223 | """Return matrix to translate by direction vector. 224 | 225 | >>> v = numpy.random.random(3) - 0.5 226 | >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) 227 | True 228 | 229 | """ 230 | M = numpy.identity(4) 231 | M[:3, 3] = direction[:3] 232 | return M 233 | 234 | 235 | def translation_from_matrix(matrix): 236 | """Return translation vector from translation matrix. 237 | 238 | >>> v0 = numpy.random.random(3) - 0.5 239 | >>> v1 = translation_from_matrix(translation_matrix(v0)) 240 | >>> numpy.allclose(v0, v1) 241 | True 242 | 243 | """ 244 | return numpy.array(matrix, copy=False)[:3, 3].copy() 245 | 246 | 247 | def reflection_matrix(point, normal): 248 | """Return matrix to mirror at plane defined by point and normal vector. 249 | 250 | >>> v0 = numpy.random.random(4) - 0.5 251 | >>> v0[3] = 1. 252 | >>> v1 = numpy.random.random(3) - 0.5 253 | >>> R = reflection_matrix(v0, v1) 254 | >>> numpy.allclose(2, numpy.trace(R)) 255 | True 256 | >>> numpy.allclose(v0, numpy.dot(R, v0)) 257 | True 258 | >>> v2 = v0.copy() 259 | >>> v2[:3] += v1 260 | >>> v3 = v0.copy() 261 | >>> v2[:3] -= v1 262 | >>> numpy.allclose(v2, numpy.dot(R, v3)) 263 | True 264 | 265 | """ 266 | normal = unit_vector(normal[:3]) 267 | M = numpy.identity(4) 268 | M[:3, :3] -= 2.0 * numpy.outer(normal, normal) 269 | M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal 270 | return M 271 | 272 | 273 | def reflection_from_matrix(matrix): 274 | """Return mirror plane point and normal vector from reflection matrix. 275 | 276 | >>> v0 = numpy.random.random(3) - 0.5 277 | >>> v1 = numpy.random.random(3) - 0.5 278 | >>> M0 = reflection_matrix(v0, v1) 279 | >>> point, normal = reflection_from_matrix(M0) 280 | >>> M1 = reflection_matrix(point, normal) 281 | >>> is_same_transform(M0, M1) 282 | True 283 | 284 | """ 285 | M = numpy.array(matrix, dtype=numpy.float64, copy=False) 286 | # normal: unit eigenvector corresponding to eigenvalue -1 287 | w, V = numpy.linalg.eig(M[:3, :3]) 288 | i = numpy.where(abs(numpy.real(w) + 1.0) < 1e-8)[0] 289 | if not len(i): 290 | raise ValueError("no unit eigenvector corresponding to eigenvalue -1") 291 | normal = numpy.real(V[:, i[0]]).squeeze() 292 | # point: any unit eigenvector corresponding to eigenvalue 1 293 | w, V = numpy.linalg.eig(M) 294 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 295 | if not len(i): 296 | raise ValueError("no unit eigenvector corresponding to eigenvalue 1") 297 | point = numpy.real(V[:, i[-1]]).squeeze() 298 | point /= point[3] 299 | return point, normal 300 | 301 | 302 | def rotation_matrix(angle, direction, point=None): 303 | """Return matrix to rotate about axis defined by point and direction. 304 | 305 | >>> R = rotation_matrix(math.pi/2, [0, 0, 1], [1, 0, 0]) 306 | >>> numpy.allclose(numpy.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1]) 307 | True 308 | >>> angle = (random.random() - 0.5) * (2*math.pi) 309 | >>> direc = numpy.random.random(3) - 0.5 310 | >>> point = numpy.random.random(3) - 0.5 311 | >>> R0 = rotation_matrix(angle, direc, point) 312 | >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) 313 | >>> is_same_transform(R0, R1) 314 | True 315 | >>> R0 = rotation_matrix(angle, direc, point) 316 | >>> R1 = rotation_matrix(-angle, -direc, point) 317 | >>> is_same_transform(R0, R1) 318 | True 319 | >>> I = numpy.identity(4, numpy.float64) 320 | >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) 321 | True 322 | >>> numpy.allclose(2, numpy.trace(rotation_matrix(math.pi/2, 323 | ... direc, point))) 324 | True 325 | 326 | """ 327 | sina = math.sin(angle) 328 | cosa = math.cos(angle) 329 | direction = unit_vector(direction[:3]) 330 | # rotation matrix around unit vector 331 | R = numpy.diag([cosa, cosa, cosa]) 332 | R += numpy.outer(direction, direction) * (1.0 - cosa) 333 | direction *= sina 334 | R += numpy.array([[ 0.0, -direction[2], direction[1]], 335 | [ direction[2], 0.0, -direction[0]], 336 | [-direction[1], direction[0], 0.0]]) 337 | M = numpy.identity(4) 338 | M[:3, :3] = R 339 | if point is not None: 340 | # rotation not around origin 341 | point = numpy.array(point[:3], dtype=numpy.float64, copy=False) 342 | M[:3, 3] = point - numpy.dot(R, point) 343 | return M 344 | 345 | 346 | def rotation_from_matrix(matrix): 347 | """Return rotation angle and axis from rotation matrix. 348 | 349 | >>> angle = (random.random() - 0.5) * (2*math.pi) 350 | >>> direc = numpy.random.random(3) - 0.5 351 | >>> point = numpy.random.random(3) - 0.5 352 | >>> R0 = rotation_matrix(angle, direc, point) 353 | >>> angle, direc, point = rotation_from_matrix(R0) 354 | >>> R1 = rotation_matrix(angle, direc, point) 355 | >>> is_same_transform(R0, R1) 356 | True 357 | 358 | """ 359 | R = numpy.array(matrix, dtype=numpy.float64, copy=False) 360 | R33 = R[:3, :3] 361 | # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 362 | w, W = numpy.linalg.eig(R33.T) 363 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 364 | if not len(i): 365 | raise ValueError("no unit eigenvector corresponding to eigenvalue 1") 366 | direction = numpy.real(W[:, i[-1]]).squeeze() 367 | # point: unit eigenvector of R33 corresponding to eigenvalue of 1 368 | w, Q = numpy.linalg.eig(R) 369 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 370 | if not len(i): 371 | raise ValueError("no unit eigenvector corresponding to eigenvalue 1") 372 | point = numpy.real(Q[:, i[-1]]).squeeze() 373 | point /= point[3] 374 | # rotation angle depending on direction 375 | cosa = (numpy.trace(R33) - 1.0) / 2.0 376 | if abs(direction[2]) > 1e-8: 377 | sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] 378 | elif abs(direction[1]) > 1e-8: 379 | sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] 380 | else: 381 | sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] 382 | angle = math.atan2(sina, cosa) 383 | return angle, direction, point 384 | 385 | 386 | def scale_matrix(factor, origin=None, direction=None): 387 | """Return matrix to scale by factor around origin in direction. 388 | 389 | Use factor -1 for point symmetry. 390 | 391 | >>> v = (numpy.random.rand(4, 5) - 0.5) * 20 392 | >>> v[3] = 1 393 | >>> S = scale_matrix(-1.234) 394 | >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) 395 | True 396 | >>> factor = random.random() * 10 - 5 397 | >>> origin = numpy.random.random(3) - 0.5 398 | >>> direct = numpy.random.random(3) - 0.5 399 | >>> S = scale_matrix(factor, origin) 400 | >>> S = scale_matrix(factor, origin, direct) 401 | 402 | """ 403 | if direction is None: 404 | # uniform scaling 405 | M = numpy.diag([factor, factor, factor, 1.0]) 406 | if origin is not None: 407 | M[:3, 3] = origin[:3] 408 | M[:3, 3] *= 1.0 - factor 409 | else: 410 | # nonuniform scaling 411 | direction = unit_vector(direction[:3]) 412 | factor = 1.0 - factor 413 | M = numpy.identity(4) 414 | M[:3, :3] -= factor * numpy.outer(direction, direction) 415 | if origin is not None: 416 | M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction 417 | return M 418 | 419 | 420 | def scale_from_matrix(matrix): 421 | """Return scaling factor, origin and direction from scaling matrix. 422 | 423 | >>> factor = random.random() * 10 - 5 424 | >>> origin = numpy.random.random(3) - 0.5 425 | >>> direct = numpy.random.random(3) - 0.5 426 | >>> S0 = scale_matrix(factor, origin) 427 | >>> factor, origin, direction = scale_from_matrix(S0) 428 | >>> S1 = scale_matrix(factor, origin, direction) 429 | >>> is_same_transform(S0, S1) 430 | True 431 | >>> S0 = scale_matrix(factor, origin, direct) 432 | >>> factor, origin, direction = scale_from_matrix(S0) 433 | >>> S1 = scale_matrix(factor, origin, direction) 434 | >>> is_same_transform(S0, S1) 435 | True 436 | 437 | """ 438 | M = numpy.array(matrix, dtype=numpy.float64, copy=False) 439 | M33 = M[:3, :3] 440 | factor = numpy.trace(M33) - 2.0 441 | try: 442 | # direction: unit eigenvector corresponding to eigenvalue factor 443 | w, V = numpy.linalg.eig(M33) 444 | i = numpy.where(abs(numpy.real(w) - factor) < 1e-8)[0][0] 445 | direction = numpy.real(V[:, i]).squeeze() 446 | direction /= vector_norm(direction) 447 | except IndexError: 448 | # uniform scaling 449 | factor = (factor + 2.0) / 3.0 450 | direction = None 451 | # origin: any eigenvector corresponding to eigenvalue 1 452 | w, V = numpy.linalg.eig(M) 453 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 454 | if not len(i): 455 | raise ValueError("no eigenvector corresponding to eigenvalue 1") 456 | origin = numpy.real(V[:, i[-1]]).squeeze() 457 | origin /= origin[3] 458 | return factor, origin, direction 459 | 460 | 461 | def projection_matrix(point, normal, direction=None, 462 | perspective=None, pseudo=False): 463 | """Return matrix to project onto plane defined by point and normal. 464 | 465 | Using either perspective point, projection direction, or none of both. 466 | 467 | If pseudo is True, perspective projections will preserve relative depth 468 | such that Perspective = dot(Orthogonal, PseudoPerspective). 469 | 470 | >>> P = projection_matrix([0, 0, 0], [1, 0, 0]) 471 | >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) 472 | True 473 | >>> point = numpy.random.random(3) - 0.5 474 | >>> normal = numpy.random.random(3) - 0.5 475 | >>> direct = numpy.random.random(3) - 0.5 476 | >>> persp = numpy.random.random(3) - 0.5 477 | >>> P0 = projection_matrix(point, normal) 478 | >>> P1 = projection_matrix(point, normal, direction=direct) 479 | >>> P2 = projection_matrix(point, normal, perspective=persp) 480 | >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) 481 | >>> is_same_transform(P2, numpy.dot(P0, P3)) 482 | True 483 | >>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0]) 484 | >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20 485 | >>> v0[3] = 1 486 | >>> v1 = numpy.dot(P, v0) 487 | >>> numpy.allclose(v1[1], v0[1]) 488 | True 489 | >>> numpy.allclose(v1[0], 3-v1[1]) 490 | True 491 | 492 | """ 493 | M = numpy.identity(4) 494 | point = numpy.array(point[:3], dtype=numpy.float64, copy=False) 495 | normal = unit_vector(normal[:3]) 496 | if perspective is not None: 497 | # perspective projection 498 | perspective = numpy.array(perspective[:3], dtype=numpy.float64, 499 | copy=False) 500 | M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) 501 | M[:3, :3] -= numpy.outer(perspective, normal) 502 | if pseudo: 503 | # preserve relative depth 504 | M[:3, :3] -= numpy.outer(normal, normal) 505 | M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) 506 | else: 507 | M[:3, 3] = numpy.dot(point, normal) * perspective 508 | M[3, :3] = -normal 509 | M[3, 3] = numpy.dot(perspective, normal) 510 | elif direction is not None: 511 | # parallel projection 512 | direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) 513 | scale = numpy.dot(direction, normal) 514 | M[:3, :3] -= numpy.outer(direction, normal) / scale 515 | M[:3, 3] = direction * (numpy.dot(point, normal) / scale) 516 | else: 517 | # orthogonal projection 518 | M[:3, :3] -= numpy.outer(normal, normal) 519 | M[:3, 3] = numpy.dot(point, normal) * normal 520 | return M 521 | 522 | 523 | def projection_from_matrix(matrix, pseudo=False): 524 | """Return projection plane and perspective point from projection matrix. 525 | 526 | Return values are same as arguments for projection_matrix function: 527 | point, normal, direction, perspective, and pseudo. 528 | 529 | >>> point = numpy.random.random(3) - 0.5 530 | >>> normal = numpy.random.random(3) - 0.5 531 | >>> direct = numpy.random.random(3) - 0.5 532 | >>> persp = numpy.random.random(3) - 0.5 533 | >>> P0 = projection_matrix(point, normal) 534 | >>> result = projection_from_matrix(P0) 535 | >>> P1 = projection_matrix(*result) 536 | >>> is_same_transform(P0, P1) 537 | True 538 | >>> P0 = projection_matrix(point, normal, direct) 539 | >>> result = projection_from_matrix(P0) 540 | >>> P1 = projection_matrix(*result) 541 | >>> is_same_transform(P0, P1) 542 | True 543 | >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) 544 | >>> result = projection_from_matrix(P0, pseudo=False) 545 | >>> P1 = projection_matrix(*result) 546 | >>> is_same_transform(P0, P1) 547 | True 548 | >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) 549 | >>> result = projection_from_matrix(P0, pseudo=True) 550 | >>> P1 = projection_matrix(*result) 551 | >>> is_same_transform(P0, P1) 552 | True 553 | 554 | """ 555 | M = numpy.array(matrix, dtype=numpy.float64, copy=False) 556 | M33 = M[:3, :3] 557 | w, V = numpy.linalg.eig(M) 558 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 559 | if not pseudo and len(i): 560 | # point: any eigenvector corresponding to eigenvalue 1 561 | point = numpy.real(V[:, i[-1]]).squeeze() 562 | point /= point[3] 563 | # direction: unit eigenvector corresponding to eigenvalue 0 564 | w, V = numpy.linalg.eig(M33) 565 | i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] 566 | if not len(i): 567 | raise ValueError("no eigenvector corresponding to eigenvalue 0") 568 | direction = numpy.real(V[:, i[0]]).squeeze() 569 | direction /= vector_norm(direction) 570 | # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 571 | w, V = numpy.linalg.eig(M33.T) 572 | i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] 573 | if len(i): 574 | # parallel projection 575 | normal = numpy.real(V[:, i[0]]).squeeze() 576 | normal /= vector_norm(normal) 577 | return point, normal, direction, None, False 578 | else: 579 | # orthogonal projection, where normal equals direction vector 580 | return point, direction, None, None, False 581 | else: 582 | # perspective projection 583 | i = numpy.where(abs(numpy.real(w)) > 1e-8)[0] 584 | if not len(i): 585 | raise ValueError( 586 | "no eigenvector not corresponding to eigenvalue 0") 587 | point = numpy.real(V[:, i[-1]]).squeeze() 588 | point /= point[3] 589 | normal = - M[3, :3] 590 | perspective = M[:3, 3] / numpy.dot(point[:3], normal) 591 | if pseudo: 592 | perspective -= normal 593 | return point, normal, None, perspective, pseudo 594 | 595 | 596 | def clip_matrix(left, right, bottom, top, near, far, perspective=False): 597 | """Return matrix to obtain normalized device coordinates from frustum. 598 | 599 | The frustum bounds are axis-aligned along x (left, right), 600 | y (bottom, top) and z (near, far). 601 | 602 | Normalized device coordinates are in range [-1, 1] if coordinates are 603 | inside the frustum. 604 | 605 | If perspective is True the frustum is a truncated pyramid with the 606 | perspective point at origin and direction along z axis, otherwise an 607 | orthographic canonical view volume (a box). 608 | 609 | Homogeneous coordinates transformed by the perspective clip matrix 610 | need to be dehomogenized (divided by w coordinate). 611 | 612 | >>> frustum = numpy.random.rand(6) 613 | >>> frustum[1] += frustum[0] 614 | >>> frustum[3] += frustum[2] 615 | >>> frustum[5] += frustum[4] 616 | >>> M = clip_matrix(perspective=False, *frustum) 617 | >>> numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) 618 | array([-1., -1., -1., 1.]) 619 | >>> numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1]) 620 | array([ 1., 1., 1., 1.]) 621 | >>> M = clip_matrix(perspective=True, *frustum) 622 | >>> v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) 623 | >>> v / v[3] 624 | array([-1., -1., -1., 1.]) 625 | >>> v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1]) 626 | >>> v / v[3] 627 | array([ 1., 1., -1., 1.]) 628 | 629 | """ 630 | if left >= right or bottom >= top or near >= far: 631 | raise ValueError("invalid frustum") 632 | if perspective: 633 | if near <= _EPS: 634 | raise ValueError("invalid frustum: near <= 0") 635 | t = 2.0 * near 636 | M = [[t/(left-right), 0.0, (right+left)/(right-left), 0.0], 637 | [0.0, t/(bottom-top), (top+bottom)/(top-bottom), 0.0], 638 | [0.0, 0.0, (far+near)/(near-far), t*far/(far-near)], 639 | [0.0, 0.0, -1.0, 0.0]] 640 | else: 641 | M = [[2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)], 642 | [0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)], 643 | [0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)], 644 | [0.0, 0.0, 0.0, 1.0]] 645 | return numpy.array(M) 646 | 647 | 648 | def shear_matrix(angle, direction, point, normal): 649 | """Return matrix to shear by angle along direction vector on shear plane. 650 | 651 | The shear plane is defined by a point and normal vector. The direction 652 | vector must be orthogonal to the plane's normal vector. 653 | 654 | A point P is transformed by the shear matrix into P" such that 655 | the vector P-P" is parallel to the direction vector and its extent is 656 | given by the angle of P-P'-P", where P' is the orthogonal projection 657 | of P onto the shear plane. 658 | 659 | >>> angle = (random.random() - 0.5) * 4*math.pi 660 | >>> direct = numpy.random.random(3) - 0.5 661 | >>> point = numpy.random.random(3) - 0.5 662 | >>> normal = numpy.cross(direct, numpy.random.random(3)) 663 | >>> S = shear_matrix(angle, direct, point, normal) 664 | >>> numpy.allclose(1, numpy.linalg.det(S)) 665 | True 666 | 667 | """ 668 | normal = unit_vector(normal[:3]) 669 | direction = unit_vector(direction[:3]) 670 | if abs(numpy.dot(normal, direction)) > 1e-6: 671 | raise ValueError("direction and normal vectors are not orthogonal") 672 | angle = math.tan(angle) 673 | M = numpy.identity(4) 674 | M[:3, :3] += angle * numpy.outer(direction, normal) 675 | M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction 676 | return M 677 | 678 | 679 | def shear_from_matrix(matrix): 680 | """Return shear angle, direction and plane from shear matrix. 681 | 682 | >>> angle = (random.random() - 0.5) * 4*math.pi 683 | >>> direct = numpy.random.random(3) - 0.5 684 | >>> point = numpy.random.random(3) - 0.5 685 | >>> normal = numpy.cross(direct, numpy.random.random(3)) 686 | >>> S0 = shear_matrix(angle, direct, point, normal) 687 | >>> angle, direct, point, normal = shear_from_matrix(S0) 688 | >>> S1 = shear_matrix(angle, direct, point, normal) 689 | >>> is_same_transform(S0, S1) 690 | True 691 | 692 | """ 693 | M = numpy.array(matrix, dtype=numpy.float64, copy=False) 694 | M33 = M[:3, :3] 695 | # normal: cross independent eigenvectors corresponding to the eigenvalue 1 696 | w, V = numpy.linalg.eig(M33) 697 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-4)[0] 698 | if len(i) < 2: 699 | raise ValueError("no two linear independent eigenvectors found %s" % w) 700 | V = numpy.real(V[:, i]).squeeze().T 701 | lenorm = -1.0 702 | for i0, i1 in ((0, 1), (0, 2), (1, 2)): 703 | n = numpy.cross(V[i0], V[i1]) 704 | w = vector_norm(n) 705 | if w > lenorm: 706 | lenorm = w 707 | normal = n 708 | normal /= lenorm 709 | # direction and angle 710 | direction = numpy.dot(M33 - numpy.identity(3), normal) 711 | angle = vector_norm(direction) 712 | direction /= angle 713 | angle = math.atan(angle) 714 | # point: eigenvector corresponding to eigenvalue 1 715 | w, V = numpy.linalg.eig(M) 716 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 717 | if not len(i): 718 | raise ValueError("no eigenvector corresponding to eigenvalue 1") 719 | point = numpy.real(V[:, i[-1]]).squeeze() 720 | point /= point[3] 721 | return angle, direction, point, normal 722 | 723 | 724 | def decompose_matrix(matrix): 725 | """Return sequence of transformations from transformation matrix. 726 | 727 | matrix : array_like 728 | Non-degenerative homogeneous transformation matrix 729 | 730 | Return tuple of: 731 | scale : vector of 3 scaling factors 732 | shear : list of shear factors for x-y, x-z, y-z axes 733 | angles : list of Euler angles about static x, y, z axes 734 | translate : translation vector along x, y, z axes 735 | perspective : perspective partition of matrix 736 | 737 | Raise ValueError if matrix is of wrong type or degenerative. 738 | 739 | >>> T0 = translation_matrix([1, 2, 3]) 740 | >>> scale, shear, angles, trans, persp = decompose_matrix(T0) 741 | >>> T1 = translation_matrix(trans) 742 | >>> numpy.allclose(T0, T1) 743 | True 744 | >>> S = scale_matrix(0.123) 745 | >>> scale, shear, angles, trans, persp = decompose_matrix(S) 746 | >>> scale[0] 747 | 0.123 748 | >>> R0 = euler_matrix(1, 2, 3) 749 | >>> scale, shear, angles, trans, persp = decompose_matrix(R0) 750 | >>> R1 = euler_matrix(*angles) 751 | >>> numpy.allclose(R0, R1) 752 | True 753 | 754 | """ 755 | M = numpy.array(matrix, dtype=numpy.float64, copy=True).T 756 | if abs(M[3, 3]) < _EPS: 757 | raise ValueError("M[3, 3] is zero") 758 | M /= M[3, 3] 759 | P = M.copy() 760 | P[:, 3] = 0.0, 0.0, 0.0, 1.0 761 | if not numpy.linalg.det(P): 762 | raise ValueError("matrix is singular") 763 | 764 | scale = numpy.zeros((3, )) 765 | shear = [0.0, 0.0, 0.0] 766 | angles = [0.0, 0.0, 0.0] 767 | 768 | if any(abs(M[:3, 3]) > _EPS): 769 | perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) 770 | M[:, 3] = 0.0, 0.0, 0.0, 1.0 771 | else: 772 | perspective = numpy.array([0.0, 0.0, 0.0, 1.0]) 773 | 774 | translate = M[3, :3].copy() 775 | M[3, :3] = 0.0 776 | 777 | row = M[:3, :3].copy() 778 | scale[0] = vector_norm(row[0]) 779 | row[0] /= scale[0] 780 | shear[0] = numpy.dot(row[0], row[1]) 781 | row[1] -= row[0] * shear[0] 782 | scale[1] = vector_norm(row[1]) 783 | row[1] /= scale[1] 784 | shear[0] /= scale[1] 785 | shear[1] = numpy.dot(row[0], row[2]) 786 | row[2] -= row[0] * shear[1] 787 | shear[2] = numpy.dot(row[1], row[2]) 788 | row[2] -= row[1] * shear[2] 789 | scale[2] = vector_norm(row[2]) 790 | row[2] /= scale[2] 791 | shear[1:] /= scale[2] 792 | 793 | if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: 794 | numpy.negative(scale, scale) 795 | numpy.negative(row, row) 796 | 797 | angles[1] = math.asin(-row[0, 2]) 798 | if math.cos(angles[1]): 799 | angles[0] = math.atan2(row[1, 2], row[2, 2]) 800 | angles[2] = math.atan2(row[0, 1], row[0, 0]) 801 | else: 802 | # angles[0] = math.atan2(row[1, 0], row[1, 1]) 803 | angles[0] = math.atan2(-row[2, 1], row[1, 1]) 804 | angles[2] = 0.0 805 | 806 | return scale, shear, angles, translate, perspective 807 | 808 | 809 | def compose_matrix(scale=None, shear=None, angles=None, translate=None, 810 | perspective=None): 811 | """Return transformation matrix from sequence of transformations. 812 | 813 | This is the inverse of the decompose_matrix function. 814 | 815 | Sequence of transformations: 816 | scale : vector of 3 scaling factors 817 | shear : list of shear factors for x-y, x-z, y-z axes 818 | angles : list of Euler angles about static x, y, z axes 819 | translate : translation vector along x, y, z axes 820 | perspective : perspective partition of matrix 821 | 822 | >>> scale = numpy.random.random(3) - 0.5 823 | >>> shear = numpy.random.random(3) - 0.5 824 | >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) 825 | >>> trans = numpy.random.random(3) - 0.5 826 | >>> persp = numpy.random.random(4) - 0.5 827 | >>> M0 = compose_matrix(scale, shear, angles, trans, persp) 828 | >>> result = decompose_matrix(M0) 829 | >>> M1 = compose_matrix(*result) 830 | >>> is_same_transform(M0, M1) 831 | True 832 | 833 | """ 834 | M = numpy.identity(4) 835 | if perspective is not None: 836 | P = numpy.identity(4) 837 | P[3, :] = perspective[:4] 838 | M = numpy.dot(M, P) 839 | if translate is not None: 840 | T = numpy.identity(4) 841 | T[:3, 3] = translate[:3] 842 | M = numpy.dot(M, T) 843 | if angles is not None: 844 | R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') 845 | M = numpy.dot(M, R) 846 | if shear is not None: 847 | Z = numpy.identity(4) 848 | Z[1, 2] = shear[2] 849 | Z[0, 2] = shear[1] 850 | Z[0, 1] = shear[0] 851 | M = numpy.dot(M, Z) 852 | if scale is not None: 853 | S = numpy.identity(4) 854 | S[0, 0] = scale[0] 855 | S[1, 1] = scale[1] 856 | S[2, 2] = scale[2] 857 | M = numpy.dot(M, S) 858 | M /= M[3, 3] 859 | return M 860 | 861 | 862 | def orthogonalization_matrix(lengths, angles): 863 | """Return orthogonalization matrix for crystallographic cell coordinates. 864 | 865 | Angles are expected in degrees. 866 | 867 | The de-orthogonalization matrix is the inverse. 868 | 869 | >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90]) 870 | >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) 871 | True 872 | >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) 873 | >>> numpy.allclose(numpy.sum(O), 43.063229) 874 | True 875 | 876 | """ 877 | a, b, c = lengths 878 | angles = numpy.radians(angles) 879 | sina, sinb, _ = numpy.sin(angles) 880 | cosa, cosb, cosg = numpy.cos(angles) 881 | co = (cosa * cosb - cosg) / (sina * sinb) 882 | return numpy.array([ 883 | [ a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0], 884 | [-a*sinb*co, b*sina, 0.0, 0.0], 885 | [ a*cosb, b*cosa, c, 0.0], 886 | [ 0.0, 0.0, 0.0, 1.0]]) 887 | 888 | 889 | def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True): 890 | """Return affine transform matrix to register two point sets. 891 | 892 | v0 and v1 are shape (ndims, \*) arrays of at least ndims non-homogeneous 893 | coordinates, where ndims is the dimensionality of the coordinate space. 894 | 895 | If shear is False, a similarity transformation matrix is returned. 896 | If also scale is False, a rigid/Euclidean transformation matrix 897 | is returned. 898 | 899 | By default the algorithm by Hartley and Zissermann [15] is used. 900 | If usesvd is True, similarity and Euclidean transformation matrices 901 | are calculated by minimizing the weighted sum of squared deviations 902 | (RMSD) according to the algorithm by Kabsch [8]. 903 | Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9] 904 | is used, which is slower when using this Python implementation. 905 | 906 | The returned matrix performs rotation, translation and uniform scaling 907 | (if specified). 908 | 909 | >>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]] 910 | >>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]] 911 | >>> affine_matrix_from_points(v0, v1) 912 | array([[ 0.14549, 0.00062, 675.50008], 913 | [ 0.00048, 0.14094, 53.24971], 914 | [ 0. , 0. , 1. ]]) 915 | >>> T = translation_matrix(numpy.random.random(3)-0.5) 916 | >>> R = random_rotation_matrix(numpy.random.random(3)) 917 | >>> S = scale_matrix(random.random()) 918 | >>> M = concatenate_matrices(T, R, S) 919 | >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 920 | >>> v0[3] = 1 921 | >>> v1 = numpy.dot(M, v0) 922 | >>> v0[:3] += numpy.random.normal(0, 1e-8, 300).reshape(3, -1) 923 | >>> M = affine_matrix_from_points(v0[:3], v1[:3]) 924 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 925 | True 926 | 927 | More examples in superimposition_matrix() 928 | 929 | """ 930 | v0 = numpy.array(v0, dtype=numpy.float64, copy=True) 931 | v1 = numpy.array(v1, dtype=numpy.float64, copy=True) 932 | 933 | ndims = v0.shape[0] 934 | if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape: 935 | raise ValueError("input arrays are of wrong shape or type") 936 | 937 | # move centroids to origin 938 | t0 = -numpy.mean(v0, axis=1) 939 | M0 = numpy.identity(ndims+1) 940 | M0[:ndims, ndims] = t0 941 | v0 += t0.reshape(ndims, 1) 942 | t1 = -numpy.mean(v1, axis=1) 943 | M1 = numpy.identity(ndims+1) 944 | M1[:ndims, ndims] = t1 945 | v1 += t1.reshape(ndims, 1) 946 | 947 | if shear: 948 | # Affine transformation 949 | A = numpy.concatenate((v0, v1), axis=0) 950 | u, s, vh = numpy.linalg.svd(A.T) 951 | vh = vh[:ndims].T 952 | B = vh[:ndims] 953 | C = vh[ndims:2*ndims] 954 | t = numpy.dot(C, numpy.linalg.pinv(B)) 955 | t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1) 956 | M = numpy.vstack((t, ((0.0,)*ndims) + (1.0,))) 957 | elif usesvd or ndims != 3: 958 | # Rigid transformation via SVD of covariance matrix 959 | u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) 960 | # rotation matrix from SVD orthonormal bases 961 | R = numpy.dot(u, vh) 962 | if numpy.linalg.det(R) < 0.0: 963 | # R does not constitute right handed system 964 | R -= numpy.outer(u[:, ndims-1], vh[ndims-1, :]*2.0) 965 | s[-1] *= -1.0 966 | # homogeneous transformation matrix 967 | M = numpy.identity(ndims+1) 968 | M[:ndims, :ndims] = R 969 | else: 970 | # Rigid transformation matrix via quaternion 971 | # compute symmetric matrix N 972 | xx, yy, zz = numpy.sum(v0 * v1, axis=1) 973 | xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) 974 | xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) 975 | N = [[xx+yy+zz, 0.0, 0.0, 0.0], 976 | [yz-zy, xx-yy-zz, 0.0, 0.0], 977 | [zx-xz, xy+yx, yy-xx-zz, 0.0], 978 | [xy-yx, zx+xz, yz+zy, zz-xx-yy]] 979 | # quaternion: eigenvector corresponding to most positive eigenvalue 980 | w, V = numpy.linalg.eigh(N) 981 | q = V[:, numpy.argmax(w)] 982 | q /= vector_norm(q) # unit quaternion 983 | # homogeneous transformation matrix 984 | M = quaternion_matrix(q) 985 | 986 | if scale and not shear: 987 | # Affine transformation; scale is ratio of RMS deviations from centroid 988 | v0 *= v0 989 | v1 *= v1 990 | M[:ndims, :ndims] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) 991 | 992 | # move centroids back 993 | M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0)) 994 | M /= M[ndims, ndims] 995 | return M 996 | 997 | 998 | def superimposition_matrix(v0, v1, scale=False, usesvd=True): 999 | """Return matrix to transform given 3D point set into second point set. 1000 | 1001 | v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 points. 1002 | 1003 | The parameters scale and usesvd are explained in the more general 1004 | affine_matrix_from_points function. 1005 | 1006 | The returned matrix is a similarity or Euclidean transformation matrix. 1007 | This function has a fast C implementation in transformations.c. 1008 | 1009 | >>> v0 = numpy.random.rand(3, 10) 1010 | >>> M = superimposition_matrix(v0, v0) 1011 | >>> numpy.allclose(M, numpy.identity(4)) 1012 | True 1013 | >>> R = random_rotation_matrix(numpy.random.random(3)) 1014 | >>> v0 = [[1,0,0], [0,1,0], [0,0,1], [1,1,1]] 1015 | >>> v1 = numpy.dot(R, v0) 1016 | >>> M = superimposition_matrix(v0, v1) 1017 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1018 | True 1019 | >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 1020 | >>> v0[3] = 1 1021 | >>> v1 = numpy.dot(R, v0) 1022 | >>> M = superimposition_matrix(v0, v1) 1023 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1024 | True 1025 | >>> S = scale_matrix(random.random()) 1026 | >>> T = translation_matrix(numpy.random.random(3)-0.5) 1027 | >>> M = concatenate_matrices(T, R, S) 1028 | >>> v1 = numpy.dot(M, v0) 1029 | >>> v0[:3] += numpy.random.normal(0, 1e-9, 300).reshape(3, -1) 1030 | >>> M = superimposition_matrix(v0, v1, scale=True) 1031 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1032 | True 1033 | >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) 1034 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1035 | True 1036 | >>> v = numpy.empty((4, 100, 3)) 1037 | >>> v[:, :, 0] = v0 1038 | >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) 1039 | >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) 1040 | True 1041 | 1042 | """ 1043 | v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] 1044 | v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] 1045 | return affine_matrix_from_points(v0, v1, shear=False, 1046 | scale=scale, usesvd=usesvd) 1047 | 1048 | 1049 | def euler_matrix(ai, aj, ak, axes='sxyz'): 1050 | """Return homogeneous rotation matrix from Euler angles and axis sequence. 1051 | 1052 | ai, aj, ak : Euler's roll, pitch and yaw angles 1053 | axes : One of 24 axis sequences as string or encoded tuple 1054 | 1055 | >>> R = euler_matrix(1, 2, 3, 'syxz') 1056 | >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) 1057 | True 1058 | >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) 1059 | >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) 1060 | True 1061 | >>> ai, aj, ak = (4*math.pi) * (numpy.random.random(3) - 0.5) 1062 | >>> for axes in _AXES2TUPLE.keys(): 1063 | ... R = euler_matrix(ai, aj, ak, axes) 1064 | >>> for axes in _TUPLE2AXES.keys(): 1065 | ... R = euler_matrix(ai, aj, ak, axes) 1066 | 1067 | """ 1068 | try: 1069 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] 1070 | except (AttributeError, KeyError): 1071 | _TUPLE2AXES[axes] # validation 1072 | firstaxis, parity, repetition, frame = axes 1073 | 1074 | i = firstaxis 1075 | j = _NEXT_AXIS[i+parity] 1076 | k = _NEXT_AXIS[i-parity+1] 1077 | 1078 | if frame: 1079 | ai, ak = ak, ai 1080 | if parity: 1081 | ai, aj, ak = -ai, -aj, -ak 1082 | 1083 | si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) 1084 | ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) 1085 | cc, cs = ci*ck, ci*sk 1086 | sc, ss = si*ck, si*sk 1087 | 1088 | M = numpy.identity(4) 1089 | if repetition: 1090 | M[i, i] = cj 1091 | M[i, j] = sj*si 1092 | M[i, k] = sj*ci 1093 | M[j, i] = sj*sk 1094 | M[j, j] = -cj*ss+cc 1095 | M[j, k] = -cj*cs-sc 1096 | M[k, i] = -sj*ck 1097 | M[k, j] = cj*sc+cs 1098 | M[k, k] = cj*cc-ss 1099 | else: 1100 | M[i, i] = cj*ck 1101 | M[i, j] = sj*sc-cs 1102 | M[i, k] = sj*cc+ss 1103 | M[j, i] = cj*sk 1104 | M[j, j] = sj*ss+cc 1105 | M[j, k] = sj*cs-sc 1106 | M[k, i] = -sj 1107 | M[k, j] = cj*si 1108 | M[k, k] = cj*ci 1109 | return M 1110 | 1111 | 1112 | def euler_from_matrix(matrix, axes='sxyz'): 1113 | """Return Euler angles from rotation matrix for specified axis sequence. 1114 | 1115 | axes : One of 24 axis sequences as string or encoded tuple 1116 | 1117 | Note that many Euler angle triplets can describe one matrix. 1118 | 1119 | >>> R0 = euler_matrix(1, 2, 3, 'syxz') 1120 | >>> al, be, ga = euler_from_matrix(R0, 'syxz') 1121 | >>> R1 = euler_matrix(al, be, ga, 'syxz') 1122 | >>> numpy.allclose(R0, R1) 1123 | True 1124 | >>> angles = (4*math.pi) * (numpy.random.random(3) - 0.5) 1125 | >>> for axes in _AXES2TUPLE.keys(): 1126 | ... R0 = euler_matrix(axes=axes, *angles) 1127 | ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) 1128 | ... if not numpy.allclose(R0, R1): print(axes, "failed") 1129 | 1130 | """ 1131 | try: 1132 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 1133 | except (AttributeError, KeyError): 1134 | _TUPLE2AXES[axes] # validation 1135 | firstaxis, parity, repetition, frame = axes 1136 | 1137 | i = firstaxis 1138 | j = _NEXT_AXIS[i+parity] 1139 | k = _NEXT_AXIS[i-parity+1] 1140 | 1141 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] 1142 | if repetition: 1143 | sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) 1144 | if sy > _EPS: 1145 | ax = math.atan2( M[i, j], M[i, k]) 1146 | ay = math.atan2( sy, M[i, i]) 1147 | az = math.atan2( M[j, i], -M[k, i]) 1148 | else: 1149 | ax = math.atan2(-M[j, k], M[j, j]) 1150 | ay = math.atan2( sy, M[i, i]) 1151 | az = 0.0 1152 | else: 1153 | cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) 1154 | if cy > _EPS: 1155 | ax = math.atan2( M[k, j], M[k, k]) 1156 | ay = math.atan2(-M[k, i], cy) 1157 | az = math.atan2( M[j, i], M[i, i]) 1158 | else: 1159 | ax = math.atan2(-M[j, k], M[j, j]) 1160 | ay = math.atan2(-M[k, i], cy) 1161 | az = 0.0 1162 | 1163 | if parity: 1164 | ax, ay, az = -ax, -ay, -az 1165 | if frame: 1166 | ax, az = az, ax 1167 | return ax, ay, az 1168 | 1169 | 1170 | def euler_from_quaternion(quaternion, axes='sxyz'): 1171 | """Return Euler angles from quaternion for specified axis sequence. 1172 | 1173 | >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0]) 1174 | >>> numpy.allclose(angles, [0.123, 0, 0]) 1175 | True 1176 | 1177 | """ 1178 | return euler_from_matrix(quaternion_matrix(quaternion), axes) 1179 | 1180 | 1181 | def quaternion_from_euler(ai, aj, ak, axes='sxyz'): 1182 | """Return quaternion from Euler angles and axis sequence. 1183 | 1184 | ai, aj, ak : Euler's roll, pitch and yaw angles 1185 | axes : One of 24 axis sequences as string or encoded tuple 1186 | 1187 | >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') 1188 | >>> numpy.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435]) 1189 | True 1190 | 1191 | """ 1192 | try: 1193 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 1194 | except (AttributeError, KeyError): 1195 | _TUPLE2AXES[axes] # validation 1196 | firstaxis, parity, repetition, frame = axes 1197 | 1198 | i = firstaxis + 1 1199 | j = _NEXT_AXIS[i+parity-1] + 1 1200 | k = _NEXT_AXIS[i-parity] + 1 1201 | 1202 | if frame: 1203 | ai, ak = ak, ai 1204 | if parity: 1205 | aj = -aj 1206 | 1207 | ai /= 2.0 1208 | aj /= 2.0 1209 | ak /= 2.0 1210 | ci = math.cos(ai) 1211 | si = math.sin(ai) 1212 | cj = math.cos(aj) 1213 | sj = math.sin(aj) 1214 | ck = math.cos(ak) 1215 | sk = math.sin(ak) 1216 | cc = ci*ck 1217 | cs = ci*sk 1218 | sc = si*ck 1219 | ss = si*sk 1220 | 1221 | q = numpy.empty((4, )) 1222 | if repetition: 1223 | q[0] = cj*(cc - ss) 1224 | q[i] = cj*(cs + sc) 1225 | q[j] = sj*(cc + ss) 1226 | q[k] = sj*(cs - sc) 1227 | else: 1228 | q[0] = cj*cc + sj*ss 1229 | q[i] = cj*sc - sj*cs 1230 | q[j] = cj*ss + sj*cc 1231 | q[k] = cj*cs - sj*sc 1232 | if parity: 1233 | q[j] *= -1.0 1234 | 1235 | return q 1236 | 1237 | 1238 | def quaternion_about_axis(angle, axis): 1239 | """Return quaternion for rotation about axis. 1240 | 1241 | >>> q = quaternion_about_axis(0.123, [1, 0, 0]) 1242 | >>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0]) 1243 | True 1244 | 1245 | """ 1246 | q = numpy.array([0.0, axis[0], axis[1], axis[2]]) 1247 | qlen = vector_norm(q) 1248 | if qlen > _EPS: 1249 | q *= math.sin(angle/2.0) / qlen 1250 | q[0] = math.cos(angle/2.0) 1251 | return q 1252 | 1253 | 1254 | def quaternion_matrix(quaternion): 1255 | """Return homogeneous rotation matrix from quaternion. 1256 | 1257 | >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0]) 1258 | >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0])) 1259 | True 1260 | >>> M = quaternion_matrix([1, 0, 0, 0]) 1261 | >>> numpy.allclose(M, numpy.identity(4)) 1262 | True 1263 | >>> M = quaternion_matrix([0, 1, 0, 0]) 1264 | >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1])) 1265 | True 1266 | 1267 | """ 1268 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 1269 | n = numpy.dot(q, q) 1270 | if n < _EPS: 1271 | return numpy.identity(4) 1272 | q *= math.sqrt(2.0 / n) 1273 | q = numpy.outer(q, q) 1274 | return numpy.array([ 1275 | [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], 1276 | [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], 1277 | [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], 1278 | [ 0.0, 0.0, 0.0, 1.0]]) 1279 | 1280 | 1281 | def quaternion_from_matrix(matrix, isprecise=False): 1282 | """Return quaternion from rotation matrix. 1283 | 1284 | If isprecise is True, the input matrix is assumed to be a precise rotation 1285 | matrix and a faster algorithm is used. 1286 | 1287 | >>> q = quaternion_from_matrix(numpy.identity(4), True) 1288 | >>> numpy.allclose(q, [1, 0, 0, 0]) 1289 | True 1290 | >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1])) 1291 | >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0]) 1292 | True 1293 | >>> R = rotation_matrix(0.123, (1, 2, 3)) 1294 | >>> q = quaternion_from_matrix(R, True) 1295 | >>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786]) 1296 | True 1297 | >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0], 1298 | ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]] 1299 | >>> q = quaternion_from_matrix(R) 1300 | >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611]) 1301 | True 1302 | >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0], 1303 | ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]] 1304 | >>> q = quaternion_from_matrix(R) 1305 | >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603]) 1306 | True 1307 | >>> R = random_rotation_matrix() 1308 | >>> q = quaternion_from_matrix(R) 1309 | >>> is_same_transform(R, quaternion_matrix(q)) 1310 | True 1311 | >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False), 1312 | ... quaternion_from_matrix(R, isprecise=True)) 1313 | True 1314 | >>> R = euler_matrix(0.0, 0.0, numpy.pi/2.0) 1315 | >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False), 1316 | ... quaternion_from_matrix(R, isprecise=True)) 1317 | True 1318 | 1319 | """ 1320 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] 1321 | if isprecise: 1322 | q = numpy.empty((4, )) 1323 | t = numpy.trace(M) 1324 | if t > M[3, 3]: 1325 | q[0] = t 1326 | q[3] = M[1, 0] - M[0, 1] 1327 | q[2] = M[0, 2] - M[2, 0] 1328 | q[1] = M[2, 1] - M[1, 2] 1329 | else: 1330 | i, j, k = 0, 1, 2 1331 | if M[1, 1] > M[0, 0]: 1332 | i, j, k = 1, 2, 0 1333 | if M[2, 2] > M[i, i]: 1334 | i, j, k = 2, 0, 1 1335 | t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] 1336 | q[i] = t 1337 | q[j] = M[i, j] + M[j, i] 1338 | q[k] = M[k, i] + M[i, k] 1339 | q[3] = M[k, j] - M[j, k] 1340 | q = q[[3, 0, 1, 2]] 1341 | q *= 0.5 / math.sqrt(t * M[3, 3]) 1342 | else: 1343 | m00 = M[0, 0] 1344 | m01 = M[0, 1] 1345 | m02 = M[0, 2] 1346 | m10 = M[1, 0] 1347 | m11 = M[1, 1] 1348 | m12 = M[1, 2] 1349 | m20 = M[2, 0] 1350 | m21 = M[2, 1] 1351 | m22 = M[2, 2] 1352 | # symmetric matrix K 1353 | K = numpy.array([[m00-m11-m22, 0.0, 0.0, 0.0], 1354 | [m01+m10, m11-m00-m22, 0.0, 0.0], 1355 | [m02+m20, m12+m21, m22-m00-m11, 0.0], 1356 | [m21-m12, m02-m20, m10-m01, m00+m11+m22]]) 1357 | K /= 3.0 1358 | # quaternion is eigenvector of K that corresponds to largest eigenvalue 1359 | w, V = numpy.linalg.eigh(K) 1360 | q = V[[3, 0, 1, 2], numpy.argmax(w)] 1361 | if q[0] < 0.0: 1362 | numpy.negative(q, q) 1363 | return q 1364 | 1365 | 1366 | def quaternion_multiply(quaternion1, quaternion0): 1367 | """Return multiplication of two quaternions. 1368 | 1369 | >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7]) 1370 | >>> numpy.allclose(q, [28, -44, -14, 48]) 1371 | True 1372 | 1373 | """ 1374 | w0, x0, y0, z0 = quaternion0 1375 | w1, x1, y1, z1 = quaternion1 1376 | return numpy.array([ 1377 | -x1*x0 - y1*y0 - z1*z0 + w1*w0, 1378 | x1*w0 + y1*z0 - z1*y0 + w1*x0, 1379 | -x1*z0 + y1*w0 + z1*x0 + w1*y0, 1380 | x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=numpy.float64) 1381 | 1382 | 1383 | def quaternion_conjugate(quaternion): 1384 | """Return conjugate of quaternion. 1385 | 1386 | >>> q0 = random_quaternion() 1387 | >>> q1 = quaternion_conjugate(q0) 1388 | >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:]) 1389 | True 1390 | 1391 | """ 1392 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 1393 | numpy.negative(q[1:], q[1:]) 1394 | return q 1395 | 1396 | 1397 | def quaternion_inverse(quaternion): 1398 | """Return inverse of quaternion. 1399 | 1400 | >>> q0 = random_quaternion() 1401 | >>> q1 = quaternion_inverse(q0) 1402 | >>> numpy.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0]) 1403 | True 1404 | 1405 | """ 1406 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 1407 | numpy.negative(q[1:], q[1:]) 1408 | return q / numpy.dot(q, q) 1409 | 1410 | 1411 | def quaternion_real(quaternion): 1412 | """Return real part of quaternion. 1413 | 1414 | >>> quaternion_real([3, 0, 1, 2]) 1415 | 3.0 1416 | 1417 | """ 1418 | return float(quaternion[0]) 1419 | 1420 | 1421 | def quaternion_imag(quaternion): 1422 | """Return imaginary part of quaternion. 1423 | 1424 | >>> quaternion_imag([3, 0, 1, 2]) 1425 | array([ 0., 1., 2.]) 1426 | 1427 | """ 1428 | return numpy.array(quaternion[1:4], dtype=numpy.float64, copy=True) 1429 | 1430 | 1431 | def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): 1432 | """Return spherical linear interpolation between two quaternions. 1433 | 1434 | >>> q0 = random_quaternion() 1435 | >>> q1 = random_quaternion() 1436 | >>> q = quaternion_slerp(q0, q1, 0) 1437 | >>> numpy.allclose(q, q0) 1438 | True 1439 | >>> q = quaternion_slerp(q0, q1, 1, 1) 1440 | >>> numpy.allclose(q, q1) 1441 | True 1442 | >>> q = quaternion_slerp(q0, q1, 0.5) 1443 | >>> angle = math.acos(numpy.dot(q0, q)) 1444 | >>> numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) or \ 1445 | numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle) 1446 | True 1447 | 1448 | """ 1449 | q0 = unit_vector(quat0[:4]) 1450 | q1 = unit_vector(quat1[:4]) 1451 | if fraction == 0.0: 1452 | return q0 1453 | elif fraction == 1.0: 1454 | return q1 1455 | d = numpy.dot(q0, q1) 1456 | if abs(abs(d) - 1.0) < _EPS: 1457 | return q0 1458 | if shortestpath and d < 0.0: 1459 | # invert rotation 1460 | d = -d 1461 | numpy.negative(q1, q1) 1462 | angle = math.acos(d) + spin * math.pi 1463 | if abs(angle) < _EPS: 1464 | return q0 1465 | isin = 1.0 / math.sin(angle) 1466 | q0 *= math.sin((1.0 - fraction) * angle) * isin 1467 | q1 *= math.sin(fraction * angle) * isin 1468 | q0 += q1 1469 | return q0 1470 | 1471 | 1472 | def random_quaternion(rand=None): 1473 | """Return uniform random unit quaternion. 1474 | 1475 | rand: array like or None 1476 | Three independent random variables that are uniformly distributed 1477 | between 0 and 1. 1478 | 1479 | >>> q = random_quaternion() 1480 | >>> numpy.allclose(1, vector_norm(q)) 1481 | True 1482 | >>> q = random_quaternion(numpy.random.random(3)) 1483 | >>> len(q.shape), q.shape[0]==4 1484 | (1, True) 1485 | 1486 | """ 1487 | if rand is None: 1488 | rand = numpy.random.rand(3) 1489 | else: 1490 | assert len(rand) == 3 1491 | r1 = numpy.sqrt(1.0 - rand[0]) 1492 | r2 = numpy.sqrt(rand[0]) 1493 | pi2 = math.pi * 2.0 1494 | t1 = pi2 * rand[1] 1495 | t2 = pi2 * rand[2] 1496 | return numpy.array([numpy.cos(t2)*r2, numpy.sin(t1)*r1, 1497 | numpy.cos(t1)*r1, numpy.sin(t2)*r2]) 1498 | 1499 | 1500 | def random_rotation_matrix(rand=None): 1501 | """Return uniform random rotation matrix. 1502 | 1503 | rand: array like 1504 | Three independent random variables that are uniformly distributed 1505 | between 0 and 1 for each returned quaternion. 1506 | 1507 | >>> R = random_rotation_matrix() 1508 | >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) 1509 | True 1510 | 1511 | """ 1512 | return quaternion_matrix(random_quaternion(rand)) 1513 | 1514 | 1515 | class Arcball(object): 1516 | """Virtual Trackball Control. 1517 | 1518 | >>> ball = Arcball() 1519 | >>> ball = Arcball(initial=numpy.identity(4)) 1520 | >>> ball.place([320, 320], 320) 1521 | >>> ball.down([500, 250]) 1522 | >>> ball.drag([475, 275]) 1523 | >>> R = ball.matrix() 1524 | >>> numpy.allclose(numpy.sum(R), 3.90583455) 1525 | True 1526 | >>> ball = Arcball(initial=[1, 0, 0, 0]) 1527 | >>> ball.place([320, 320], 320) 1528 | >>> ball.setaxes([1, 1, 0], [-1, 1, 0]) 1529 | >>> ball.constrain = True 1530 | >>> ball.down([400, 200]) 1531 | >>> ball.drag([200, 400]) 1532 | >>> R = ball.matrix() 1533 | >>> numpy.allclose(numpy.sum(R), 0.2055924) 1534 | True 1535 | >>> ball.next() 1536 | 1537 | """ 1538 | def __init__(self, initial=None): 1539 | """Initialize virtual trackball control. 1540 | 1541 | initial : quaternion or rotation matrix 1542 | 1543 | """ 1544 | self._axis = None 1545 | self._axes = None 1546 | self._radius = 1.0 1547 | self._center = [0.0, 0.0] 1548 | self._vdown = numpy.array([0.0, 0.0, 1.0]) 1549 | self._constrain = False 1550 | if initial is None: 1551 | self._qdown = numpy.array([1.0, 0.0, 0.0, 0.0]) 1552 | else: 1553 | initial = numpy.array(initial, dtype=numpy.float64) 1554 | if initial.shape == (4, 4): 1555 | self._qdown = quaternion_from_matrix(initial) 1556 | elif initial.shape == (4, ): 1557 | initial /= vector_norm(initial) 1558 | self._qdown = initial 1559 | else: 1560 | raise ValueError("initial not a quaternion or matrix") 1561 | self._qnow = self._qpre = self._qdown 1562 | 1563 | def place(self, center, radius): 1564 | """Place Arcball, e.g. when window size changes. 1565 | 1566 | center : sequence[2] 1567 | Window coordinates of trackball center. 1568 | radius : float 1569 | Radius of trackball in window coordinates. 1570 | 1571 | """ 1572 | self._radius = float(radius) 1573 | self._center[0] = center[0] 1574 | self._center[1] = center[1] 1575 | 1576 | def setaxes(self, *axes): 1577 | """Set axes to constrain rotations.""" 1578 | if axes is None: 1579 | self._axes = None 1580 | else: 1581 | self._axes = [unit_vector(axis) for axis in axes] 1582 | 1583 | @property 1584 | def constrain(self): 1585 | """Return state of constrain to axis mode.""" 1586 | return self._constrain 1587 | 1588 | @constrain.setter 1589 | def constrain(self, value): 1590 | """Set state of constrain to axis mode.""" 1591 | self._constrain = bool(value) 1592 | 1593 | def down(self, point): 1594 | """Set initial cursor window coordinates and pick constrain-axis.""" 1595 | self._vdown = arcball_map_to_sphere(point, self._center, self._radius) 1596 | self._qdown = self._qpre = self._qnow 1597 | if self._constrain and self._axes is not None: 1598 | self._axis = arcball_nearest_axis(self._vdown, self._axes) 1599 | self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) 1600 | else: 1601 | self._axis = None 1602 | 1603 | def drag(self, point): 1604 | """Update current cursor window coordinates.""" 1605 | vnow = arcball_map_to_sphere(point, self._center, self._radius) 1606 | if self._axis is not None: 1607 | vnow = arcball_constrain_to_axis(vnow, self._axis) 1608 | self._qpre = self._qnow 1609 | t = numpy.cross(self._vdown, vnow) 1610 | if numpy.dot(t, t) < _EPS: 1611 | self._qnow = self._qdown 1612 | else: 1613 | q = [numpy.dot(self._vdown, vnow), t[0], t[1], t[2]] 1614 | self._qnow = quaternion_multiply(q, self._qdown) 1615 | 1616 | def next(self, acceleration=0.0): 1617 | """Continue rotation in direction of last drag.""" 1618 | q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) 1619 | self._qpre, self._qnow = self._qnow, q 1620 | 1621 | def matrix(self): 1622 | """Return homogeneous rotation matrix.""" 1623 | return quaternion_matrix(self._qnow) 1624 | 1625 | 1626 | def arcball_map_to_sphere(point, center, radius): 1627 | """Return unit sphere coordinates from window coordinates.""" 1628 | v0 = (point[0] - center[0]) / radius 1629 | v1 = (center[1] - point[1]) / radius 1630 | n = v0*v0 + v1*v1 1631 | if n > 1.0: 1632 | # position outside of sphere 1633 | n = math.sqrt(n) 1634 | return numpy.array([v0/n, v1/n, 0.0]) 1635 | else: 1636 | return numpy.array([v0, v1, math.sqrt(1.0 - n)]) 1637 | 1638 | 1639 | def arcball_constrain_to_axis(point, axis): 1640 | """Return sphere point perpendicular to axis.""" 1641 | v = numpy.array(point, dtype=numpy.float64, copy=True) 1642 | a = numpy.array(axis, dtype=numpy.float64, copy=True) 1643 | v -= a * numpy.dot(a, v) # on plane 1644 | n = vector_norm(v) 1645 | if n > _EPS: 1646 | if v[2] < 0.0: 1647 | numpy.negative(v, v) 1648 | v /= n 1649 | return v 1650 | if a[2] == 1.0: 1651 | return numpy.array([1.0, 0.0, 0.0]) 1652 | return unit_vector([-a[1], a[0], 0.0]) 1653 | 1654 | 1655 | def arcball_nearest_axis(point, axes): 1656 | """Return axis, which arc is nearest to point.""" 1657 | point = numpy.array(point, dtype=numpy.float64, copy=False) 1658 | nearest = None 1659 | mx = -1.0 1660 | for axis in axes: 1661 | t = numpy.dot(arcball_constrain_to_axis(point, axis), point) 1662 | if t > mx: 1663 | nearest = axis 1664 | mx = t 1665 | return nearest 1666 | 1667 | 1668 | # epsilon for testing whether a number is close to zero 1669 | _EPS = numpy.finfo(float).eps * 4.0 1670 | 1671 | # axis sequences for Euler angles 1672 | _NEXT_AXIS = [1, 2, 0, 1] 1673 | 1674 | # map axes strings to/from tuples of inner axis, parity, repetition, frame 1675 | _AXES2TUPLE = { 1676 | 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), 1677 | 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), 1678 | 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), 1679 | 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), 1680 | 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), 1681 | 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), 1682 | 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), 1683 | 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} 1684 | 1685 | _TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) 1686 | 1687 | 1688 | def vector_norm(data, axis=None, out=None): 1689 | """Return length, i.e. Euclidean norm, of ndarray along axis. 1690 | 1691 | >>> v = numpy.random.random(3) 1692 | >>> n = vector_norm(v) 1693 | >>> numpy.allclose(n, numpy.linalg.norm(v)) 1694 | True 1695 | >>> v = numpy.random.rand(6, 5, 3) 1696 | >>> n = vector_norm(v, axis=-1) 1697 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) 1698 | True 1699 | >>> n = vector_norm(v, axis=1) 1700 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 1701 | True 1702 | >>> v = numpy.random.rand(5, 4, 3) 1703 | >>> n = numpy.empty((5, 3)) 1704 | >>> vector_norm(v, axis=1, out=n) 1705 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 1706 | True 1707 | >>> vector_norm([]) 1708 | 0.0 1709 | >>> vector_norm([1]) 1710 | 1.0 1711 | 1712 | """ 1713 | data = numpy.array(data, dtype=numpy.float64, copy=True) 1714 | if out is None: 1715 | if data.ndim == 1: 1716 | return math.sqrt(numpy.dot(data, data)) 1717 | data *= data 1718 | out = numpy.atleast_1d(numpy.sum(data, axis=axis)) 1719 | numpy.sqrt(out, out) 1720 | return out 1721 | else: 1722 | data *= data 1723 | numpy.sum(data, axis=axis, out=out) 1724 | numpy.sqrt(out, out) 1725 | 1726 | 1727 | def unit_vector(data, axis=None, out=None): 1728 | """Return ndarray normalized by length, i.e. Euclidean norm, along axis. 1729 | 1730 | >>> v0 = numpy.random.random(3) 1731 | >>> v1 = unit_vector(v0) 1732 | >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) 1733 | True 1734 | >>> v0 = numpy.random.rand(5, 4, 3) 1735 | >>> v1 = unit_vector(v0, axis=-1) 1736 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) 1737 | >>> numpy.allclose(v1, v2) 1738 | True 1739 | >>> v1 = unit_vector(v0, axis=1) 1740 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) 1741 | >>> numpy.allclose(v1, v2) 1742 | True 1743 | >>> v1 = numpy.empty((5, 4, 3)) 1744 | >>> unit_vector(v0, axis=1, out=v1) 1745 | >>> numpy.allclose(v1, v2) 1746 | True 1747 | >>> list(unit_vector([])) 1748 | [] 1749 | >>> list(unit_vector([1])) 1750 | [1.0] 1751 | 1752 | """ 1753 | if out is None: 1754 | data = numpy.array(data, dtype=numpy.float64, copy=True) 1755 | if data.ndim == 1: 1756 | data /= math.sqrt(numpy.dot(data, data)) 1757 | return data 1758 | else: 1759 | if out is not data: 1760 | out[:] = numpy.array(data, copy=False) 1761 | data = out 1762 | length = numpy.atleast_1d(numpy.sum(data*data, axis)) 1763 | numpy.sqrt(length, length) 1764 | if axis is not None: 1765 | length = numpy.expand_dims(length, axis) 1766 | data /= length 1767 | if out is None: 1768 | return data 1769 | 1770 | 1771 | def random_vector(size): 1772 | """Return array of random doubles in the half-open interval [0.0, 1.0). 1773 | 1774 | >>> v = random_vector(10000) 1775 | >>> numpy.all(v >= 0) and numpy.all(v < 1) 1776 | True 1777 | >>> v0 = random_vector(10) 1778 | >>> v1 = random_vector(10) 1779 | >>> numpy.any(v0 == v1) 1780 | False 1781 | 1782 | """ 1783 | return numpy.random.random(size) 1784 | 1785 | 1786 | def vector_product(v0, v1, axis=0): 1787 | """Return vector perpendicular to vectors. 1788 | 1789 | >>> v = vector_product([2, 0, 0], [0, 3, 0]) 1790 | >>> numpy.allclose(v, [0, 0, 6]) 1791 | True 1792 | >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] 1793 | >>> v1 = [[3], [0], [0]] 1794 | >>> v = vector_product(v0, v1) 1795 | >>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]]) 1796 | True 1797 | >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] 1798 | >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] 1799 | >>> v = vector_product(v0, v1, axis=1) 1800 | >>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]]) 1801 | True 1802 | 1803 | """ 1804 | return numpy.cross(v0, v1, axis=axis) 1805 | 1806 | 1807 | def angle_between_vectors(v0, v1, directed=True, axis=0): 1808 | """Return angle between vectors. 1809 | 1810 | If directed is False, the input vectors are interpreted as undirected axes, 1811 | i.e. the maximum angle is pi/2. 1812 | 1813 | >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3]) 1814 | >>> numpy.allclose(a, math.pi) 1815 | True 1816 | >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False) 1817 | >>> numpy.allclose(a, 0) 1818 | True 1819 | >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] 1820 | >>> v1 = [[3], [0], [0]] 1821 | >>> a = angle_between_vectors(v0, v1) 1822 | >>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532]) 1823 | True 1824 | >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] 1825 | >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] 1826 | >>> a = angle_between_vectors(v0, v1, axis=1) 1827 | >>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532]) 1828 | True 1829 | 1830 | """ 1831 | v0 = numpy.array(v0, dtype=numpy.float64, copy=False) 1832 | v1 = numpy.array(v1, dtype=numpy.float64, copy=False) 1833 | dot = numpy.sum(v0 * v1, axis=axis) 1834 | dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis) 1835 | return numpy.arccos(dot if directed else numpy.fabs(dot)) 1836 | 1837 | 1838 | def inverse_matrix(matrix): 1839 | """Return inverse of square transformation matrix. 1840 | 1841 | >>> M0 = random_rotation_matrix() 1842 | >>> M1 = inverse_matrix(M0.T) 1843 | >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) 1844 | True 1845 | >>> for size in range(1, 7): 1846 | ... M0 = numpy.random.rand(size, size) 1847 | ... M1 = inverse_matrix(M0) 1848 | ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print(size) 1849 | 1850 | """ 1851 | return numpy.linalg.inv(matrix) 1852 | 1853 | 1854 | def concatenate_matrices(*matrices): 1855 | """Return concatenation of series of transformation matrices. 1856 | 1857 | >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 1858 | >>> numpy.allclose(M, concatenate_matrices(M)) 1859 | True 1860 | >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) 1861 | True 1862 | 1863 | """ 1864 | M = numpy.identity(4) 1865 | for i in matrices: 1866 | M = numpy.dot(M, i) 1867 | return M 1868 | 1869 | 1870 | def is_same_transform(matrix0, matrix1): 1871 | """Return True if two matrices perform same transformation. 1872 | 1873 | >>> is_same_transform(numpy.identity(4), numpy.identity(4)) 1874 | True 1875 | >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) 1876 | False 1877 | 1878 | """ 1879 | matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) 1880 | matrix0 /= matrix0[3, 3] 1881 | matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) 1882 | matrix1 /= matrix1[3, 3] 1883 | return numpy.allclose(matrix0, matrix1) 1884 | 1885 | 1886 | def is_same_quaternion(q0, q1): 1887 | """Return True if two quaternions are equal.""" 1888 | q0 = numpy.array(q0) 1889 | q1 = numpy.array(q1) 1890 | return numpy.allclose(q0, q1) or numpy.allclose(q0, -q1) 1891 | 1892 | 1893 | def _import_module(name, package=None, warn=True, prefix='_py_', ignore='_'): 1894 | """Try import all public attributes from module into global namespace. 1895 | 1896 | Existing attributes with name clashes are renamed with prefix. 1897 | Attributes starting with underscore are ignored by default. 1898 | 1899 | Return True on successful import. 1900 | 1901 | """ 1902 | import warnings 1903 | from importlib import import_module 1904 | try: 1905 | if not package: 1906 | module = import_module(name) 1907 | else: 1908 | module = import_module('.' + name, package=package) 1909 | except ImportError: 1910 | if warn: 1911 | pass 1912 | # warnings.warn("failed to import module %s" % name) 1913 | else: 1914 | for attr in dir(module): 1915 | if ignore and attr.startswith(ignore): 1916 | continue 1917 | if prefix: 1918 | if attr in globals(): 1919 | globals()[prefix + attr] = globals()[attr] 1920 | elif warn: 1921 | warnings.warn("no Python implementation of " + attr) 1922 | globals()[attr] = getattr(module, attr) 1923 | return True 1924 | 1925 | 1926 | _import_module('_transformations') 1927 | 1928 | if __name__ == "__main__": 1929 | import doctest 1930 | import random # noqa: used in doctests 1931 | numpy.set_printoptions(suppress=True, precision=5) 1932 | doctest.testmod() 1933 | --------------------------------------------------------------------------------