@@ -140,71 +140,6 @@ def grid_3d(n, shifted=False, normalized=True):
140140 }
141141
142142
143- def qrand (nrot , seed = 0 ):
144-
145- """
146- Generate a set of quaternions from random normal distribution.
147-
148- Each quaternions is a four-elements column vector. Returns a matrix of
149- size 4xn.
150-
151- The 3-sphere S^3 in R^4 is a double cover of the rotation group SO(3), SO(3) = RP^3.
152- We identify unit norm quaternions a^2+b^2+c^2+d^2=1 with group elements.
153- The antipodal points (-a,-b,-c,-d) and (a,b,c,d) are identified as the same group elements,
154- so we take a>=0.
155- :param nrot: The number of quaternions for rotations.
156- :param seed: The random seed.
157- :return: An array consists of 4 dimensions quaternions
158- """
159- q = randn (4 , nrot , seed = seed )
160- l2_norm = np .sqrt (q [0 , :]** 2 + q [1 , :]** 2 + q [2 , :]** 2 + q [3 , :]** 2 )
161- for i in range (4 ):
162- q [i , :] = q [i , :] / l2_norm
163-
164- for k in range (nrot ):
165- if q [0 , k ] < 0 :
166- q [:, k ] = - q [:, k ]
167-
168- return q
169-
170-
171- def q_to_rot (q ):
172- """
173- Convert the quaternions into a rotation matrices.
174-
175- :param q: Array of quaternions. May be a vector of dimensions 4 x n
176- :return rot_mat: n-by-3-by-3 array of 3x3 rotation matrices.
177- """
178-
179- nrot = np .size (q , 1 )
180- rot_mat = np .zeros ((nrot , 3 , 3 ), dtype = q .dtype )
181-
182- rot_mat [:, 0 , 0 ] = q [0 , :]** 2 + q [1 , :]** 2 - q [2 , :]** 2 - q [3 , :]** 2
183- rot_mat [:, 0 , 1 ] = 2 * q [1 , :]* q [2 , :] - 2 * q [0 , :]* q [3 , :]
184- rot_mat [:, 0 , 2 ] = 2 * q [0 , :]* q [2 , :] + 2 * q [1 , :]* q [3 , :]
185-
186- rot_mat [:, 1 , 0 ] = 2 * q [1 , :]* q [2 , :] + 2 * q [0 , :]* q [3 , :]
187- rot_mat [:, 1 , 1 ] = q [0 , :]** 2 - q [1 , :]** 2 + q [2 , :]** 2 - q [3 , :]** 2
188- rot_mat [:, 1 , 2 ] = - 2 * q [0 , :]* q [1 , :] + 2 * q [2 , :]* q [3 , :]
189-
190- rot_mat [:, 2 , 0 ] = - 2 * q [0 , :]* q [2 , :] + 2 * q [1 , :]* q [3 , :]
191- rot_mat [:, 2 , 1 ] = 2 * q [0 , :]* q [1 , :] + 2 * q [2 , :]* q [3 , :]
192- rot_mat [:, 2 , 2 ] = q [0 , :]** 2 - q [1 , :]** 2 - q [2 , :]** 2 + q [3 , :]** 2
193- return rot_mat
194-
195-
196- def qrand_rots (nrot , seed = 0 ):
197- """
198- Generate random rotations from quaternions
199-
200- :param nrot: The totalArray of quaternions. May be a vector of dimensions 4 x n
201- :return: Array of 3x3 rotation matrices.
202- """
203- qs = qrand (nrot , seed )
204-
205- return q_to_rot (qs )
206-
207-
208143def uniform_random_angles (n , seed = None ):
209144 """
210145 Generate random 3D rotation angles
0 commit comments