Apollo  6.0
Open source self driving car software
i_rot.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include "modules/perception/common/core/i_blas.h"
4 #include "modules/perception/common/core/i_rand.h"
5 #include "modules/perception/common/numeric/i_eig.h"
6 #include "modules/perception/common/numeric/i_svd.h"
7 
8 namespace idl {
9 /*This routine force a 2x2 orthogonal matrix R in place (i.e., force it to be
10 rotation maxtrix with positive determinant),
11 /*by unitizing the first row, and taking its cross as the second*/
12 template <typename T>
13 inline void i_force_rot_2x2(T R[4]) {
14  i_unitize2(R);
15  R[2] = -R[1];
16  R[3] = R[0];
17 }
18 
19 /*This routine orthogonalizes a 3x3 near Rotation matrix R in place, i.e., force
20  * it to be orthogonal.*/
21 template <typename T>
22 inline void i_rot_orthogonalize(
23  T R[9], int nr_svd_iter = I_DEFAULT_MAX_SVD_ITERATIONS) {
24  /*obtain SVD*/
25  T Ut[9], w[3], Vt[9];
26  i_svd_3x3(R, Ut, w, Vt, true, nr_svd_iter);
27  if (i_determinant_3x3(Ut) < (T)0.0) {
28  i_neg9(Ut);
29  }
30  if (i_determinant_3x3(Vt) < (T)0.0) {
31  i_neg9(Vt);
32  }
33  i_mult_AtB_3x3_3x3(Ut, Vt, R);
34 }
35 
36 /*Rodrigues' solver for computing the matrix exponential of a 3x3 skew-symmetric
37  * matrix*/
38 template <typename T>
39 inline void i_rot_rodrigues_3x3_solver(T sinc, T mcos, T a0_sqr, T a1_sqr,
40  T a2_sqr, const T a[3], T R[9]) {
41  T tmp1, tmp2;
42  R[0] = (T)1.0 - (a1_sqr + a2_sqr) * mcos;
43  R[4] = (T)1.0 - (a0_sqr + a2_sqr) * mcos;
44  R[8] = (T)1.0 - (a0_sqr + a1_sqr) * mcos;
45  tmp1 = a[0] * a[1] * mcos;
46  tmp2 = a[2] * sinc;
47  R[1] = tmp1 - tmp2;
48  R[3] = tmp1 + tmp2;
49  tmp1 = a[2] * a[0] * mcos;
50  tmp2 = a[1] * sinc;
51  R[2] = tmp1 + tmp2;
52  R[6] = tmp1 - tmp2;
53  tmp1 = a[1] * a[2] * mcos;
54  tmp2 = a[0] * sinc;
55  R[5] = tmp1 - tmp2;
56  R[7] = tmp1 + tmp2;
57 }
58 
59 template <typename T>
60 inline void i_rot_rodrigues_3x3_solver(T sinc, T mcos, T a0_sqr, T a1_sqr,
61  T a2_sqr, const T s_da[3],
62  const T c_da[3], const T a[3], T R[9],
63  T D[9][3]) {
64  T tmp1, tmp2, e_x[9], e_x2[9];
65  T mcos_x_a0 = mcos * a[0];
66  T mcos_x_a1 = mcos * a[1];
67  T mcos_x_a2 = mcos * a[2];
68 
69  T mtwo_mcos_x_a0 = -(2 * mcos_x_a0);
70  T mtwo_mcos_x_a1 = -(2 * mcos_x_a1);
71  T mtwo_mcos_x_a2 = -(2 * mcos_x_a2);
72 
73  R[0] = (T)1.0 - (a1_sqr + a2_sqr) * mcos;
74  R[4] = (T)1.0 - (a0_sqr + a2_sqr) * mcos;
75  R[8] = (T)1.0 - (a0_sqr + a1_sqr) * mcos;
76 
77  tmp1 = a[0] * mcos_x_a1;
78  tmp2 = a[2] * sinc;
79  R[1] = tmp1 - tmp2;
80  R[3] = tmp1 + tmp2;
81  tmp1 = a[2] * mcos_x_a0;
82  tmp2 = a[1] * sinc;
83  R[2] = tmp1 + tmp2;
84  R[6] = tmp1 - tmp2;
85  tmp1 = a[1] * mcos_x_a2;
86  tmp2 = a[0] * sinc;
87  R[5] = tmp1 - tmp2;
88  R[7] = tmp1 + tmp2;
89 
90  /*derivatives*/
91  i_axiator(a, e_x);
92  i_sqr_skew_symmetric_3x3(a, e_x2);
93 
94  // i=0
95  D[0][0] = (T)(/*e_x[0] * s_da[0]*/ /*+ sinc*e_x_da[0][0] +*/ e_x2[0] *
96  c_da[0] /*+ mcos * e_x2_da[0][0]*/);
97  D[0][1] =
98  (T)(/*e_x[0] * s_da[1]*/ /*+ sinc*e_x_da[0][1] +*/ e_x2[0] * c_da[1] +
99  mtwo_mcos_x_a1);
100  D[0][2] =
101  (T)(/*e_x[0] * s_da[2]*/ /*+ sinc*e_x_da[0][2] +*/ e_x2[0] * c_da[2] +
102  mtwo_mcos_x_a2);
103  // i=1
104  D[1][0] = (T)(e_x[1] * s_da[0] /*+ sinc*e_x_da[1][0]*/ + e_x2[1] * c_da[0] +
105  mcos_x_a1);
106  D[1][1] = (T)(e_x[1] * s_da[1] /*+ sinc*e_x_da[1][1]*/ + e_x2[1] * c_da[1] +
107  mcos_x_a0);
108  D[1][2] = (T)(e_x[1] * s_da[2] - sinc +
109  e_x2[1] * c_da[2] /*+ mcos * e_x2_da[1][2]*/);
110  // i=2
111  D[2][0] = (T)(e_x[2] * s_da[0] /*+ sinc*e_x_da[2][0]*/ + e_x2[2] * c_da[0] +
112  mcos_x_a2);
113  D[2][1] = (T)(e_x[2] * s_da[1] + sinc +
114  e_x2[2] * c_da[1] /*+ mcos * e_x2_da[2][1]*/);
115  D[2][2] = (T)(e_x[2] * s_da[2] /*+ sinc*e_x_da[2][2]*/ + e_x2[2] * c_da[2] +
116  mcos_x_a0);
117  // i=3
118  D[3][0] = (T)(e_x[3] * s_da[0] /*+ sinc*e_x_da[3][0]*/ + e_x2[3] * c_da[0] +
119  mcos_x_a1);
120  D[3][1] = (T)(e_x[3] * s_da[1] /*+ sinc*e_x_da[3][1]*/ + e_x2[3] * c_da[1] +
121  mcos_x_a0);
122  D[3][2] = (T)(e_x[3] * s_da[2] + sinc +
123  e_x2[3] * c_da[2] /*+ mcos * e_x2_da[3][2]*/);
124  // i=4
125  D[4][0] =
126  (T)(/*e_x[4] * s_da[0]*/ /*+ sinc*e_x_da[4][0] +*/ e_x2[4] * c_da[0] +
127  mtwo_mcos_x_a0);
128  D[4][1] = (T)(/*e_x[4] * s_da[1]*/ /*+ sinc*e_x_da[4][1] +*/ e_x2[4] *
129  c_da[1] /*+ mcos * e_x2_da[4][1]*/);
130  D[4][2] =
131  (T)(/*e_x[4] * s_da[2]*/ /*+ sinc*e_x_da[4][2] +*/ e_x2[4] * c_da[2] +
132  mtwo_mcos_x_a2);
133  // i=5
134  D[5][0] = (T)(e_x[5] * s_da[0] - sinc +
135  e_x2[5] * c_da[0] /*+ mcos * e_x2_da[5][0]*/);
136  D[5][1] = (T)(e_x[5] * s_da[1] /*+ sinc*e_x_da[5][1]*/ + e_x2[5] * c_da[1] +
137  mcos_x_a2);
138  D[5][2] = (T)(e_x[5] * s_da[2] /*+ sinc*e_x_da[5][2]*/ + e_x2[5] * c_da[2] +
139  mcos_x_a1);
140  // i=6
141  D[6][0] = (T)(e_x[6] * s_da[0] /*+ sinc*e_x_da[6][0]*/ + e_x2[6] * c_da[0] +
142  mcos_x_a2);
143  D[6][1] = (T)(e_x[6] * s_da[1] - sinc +
144  e_x2[6] * c_da[1] /*+ mcos * e_x2_da[6][1]*/);
145  D[6][2] = (T)(e_x[6] * s_da[2] /*+ sinc*e_x_da[6][2]*/ + e_x2[6] * c_da[2] +
146  mcos_x_a0);
147  // i=7
148  D[7][0] = (T)(e_x[7] * s_da[0] + sinc +
149  e_x2[7] * c_da[0] /*+ mcos * e_x2_da[7][0]*/);
150  D[7][1] = (T)(e_x[7] * s_da[1] /*+ sinc*e_x_da[7][1]*/ + e_x2[7] * c_da[1] +
151  mcos_x_a2);
152  D[7][2] = (T)(e_x[7] * s_da[2] /*+ sinc*e_x_da[7][2]*/ + e_x2[7] * c_da[2] +
153  mcos_x_a1);
154  // i=8
155  D[8][0] =
156  (T)(/*e_x[8] * s_da[0]*/ /*+ sinc*e_x_da[8][0] +*/ e_x2[8] * c_da[0] +
157  mtwo_mcos_x_a0);
158  D[8][1] =
159  (T)(/*e_x[8] * s_da[1]*/ /*+ sinc*e_x_da[8][1] +*/ e_x2[8] * c_da[1] +
160  mtwo_mcos_x_a1);
161  D[8][2] = (T)(/*e_x[8] * s_da[2]*/ /*+ sinc*e_x_da[8][2] +*/ e_x2[8] *
162  c_da[2] /*+ mcos * e_x2_da[8][2]*/);
163 }
164 
165 /*Rodrigues' formula for computing the matrix exponential of a 3x3
166  skew-symmetric matrix The 1st argument is the input (a 3-vector) and the 2nd
167  argument is the output (a 3x3 rotation matrix)*/
168 template <typename T>
169 inline void i_rot_rodrigues_3x3(const T a[3], T R[9]) {
170  T sinc, mcos, x[3];
171  i_copy3(a, x);
172  T a0_sqr = i_sqr(x[0]);
173  T a1_sqr = i_sqr(x[1]);
174  T a2_sqr = i_sqr(x[2]);
175  T theta2 = a0_sqr + a1_sqr + a2_sqr;
176  T theta = i_sqrt(theta2);
177 
178  if (theta < Constant<T>::MIN_ABS_SAFE_DIVIDEND()) {
179  /*theta is too small; use a third-order Taylor approximation for sin(theta)
180  * and cos(theta)*/
181  sinc = (T)1.0 - theta2 / (T)6.0;
182  mcos = (T)0.5 - theta2 / (T)24.0;
183  } else if (theta > Constant<T>::PI()) {
184  /*if ||a||>PI, the it maybe replaced by a(1-2PI/||a||), which represenrs the
185  * same rotation*/
186  theta = (T)(1.0) - Constant<T>::TWO_PI() / theta;
187  i_scale3(x, theta);
188  a0_sqr = i_sqr(x[0]);
189  a1_sqr = i_sqr(x[1]);
190  a2_sqr = i_sqr(x[2]);
191  theta2 = a0_sqr + a1_sqr + a2_sqr;
192  theta = i_sqrt(theta2);
193  if (theta < Constant<T>::MIN_ABS_SAFE_DIVIDEND()) {
194  /*theta is too small; use a third-order Taylor approximation for
195  * sin(theta) and cos(theta)*/
196  sinc = (T)1.0 - theta2 / (T)6.0;
197  mcos = (T)0.5 - theta2 / (T)24.0;
198  } else {
199  sinc = i_div(i_sin(theta), theta);
200  mcos = i_div((T)1.0 - i_cos(theta), theta2);
201  }
202  } else {
203  sinc = i_div(i_sin(theta), theta);
204  mcos = i_div((T)1.0 - i_cos(theta), theta2);
205  }
206  i_rot_rodrigues_3x3_solver(sinc, mcos, a0_sqr, a1_sqr, a2_sqr, x, R);
207 }
208 
209 /*Rodrigues' formula for computing the matrix exponential of a 3x3
210 skew-symmetric matrix The 1st argument is the input (a 3-vector), the 2nd
211 argument is the output (a 3x3 rotation matrix), The 3rd argument is the
212 derivative (a 9x3 matrix)*/
213 template <typename T>
214 inline void i_rot_rodrigues_3x3(const T a[3], T R[9], T D[9][3]) {
215  T sinc, mcos, x[3], s_da[3],
216  c_da[3]; /*derivatives of sinc and mcos wrt to a, respectively*/
217  i_copy3(a, x);
218 
219  T a0_sqr = i_sqr(x[0]);
220  T a1_sqr = i_sqr(x[1]);
221  T a2_sqr = i_sqr(x[2]);
222 
223  T theta2 = a0_sqr + a1_sqr + a2_sqr;
224  T theta = i_sqrt(theta2);
225 
226  if (theta < Constant<T>::MIN_ABS_SAFE_DIVIDEND()) {
227  /*theta is too small; use a third-order Taylor approximation for sin(theta)
228  * and cos(theta)*/
229  sinc = (T)1.0 - theta2 / (T)6.0;
230  mcos = (T)0.5 - theta2 / (T)24.0;
231  /*derivatives*/
232  i_scale3(x, s_da, -i_rec((T)3.0));
233  i_scale3(x, c_da, -i_rec((T)12.0));
234  } else if (theta > Constant<T>::PI()) {
235  /*if ||a||>PI, the it maybe replaced by a(1-2PI/||a||), which represenrs the
236  * same rotation*/
237  theta = (T)(1.0) - Constant<T>::TWO_PI() / theta;
238  i_scale3(x, theta);
239  a0_sqr = i_sqr(x[0]);
240  a1_sqr = i_sqr(x[1]);
241  a2_sqr = i_sqr(x[2]);
242  theta2 = a0_sqr + a1_sqr + a2_sqr;
243  theta = i_sqrt(theta2);
244  if (theta < Constant<T>::MIN_ABS_SAFE_DIVIDEND()) {
245  /*theta is too small; use a third-order Taylor approximation for
246  * sin(theta) and cos(theta)*/
247  sinc = (T)1.0 - theta2 / (T)6.0;
248  mcos = (T)0.5 - theta2 / (T)24.0;
249  /*derivatives*/
250  i_scale3(x, s_da, -i_rec((T)3.0));
251  i_scale3(x, c_da, -i_rec((T)12.0));
252  } else {
253  T sin_theta = i_sin(theta);
254  T cos_theta = i_cos(theta);
255  sinc = i_div(sin_theta, theta);
256  mcos = i_div((T)1.0 - cos_theta, theta2);
257  /*derivatives*/
258  T s_dtheta = (cos_theta - sin_theta / theta) / theta2;
259  T c_dtheta = (sinc - (T)(2.0) * mcos) / theta2;
260  i_scale3(x, s_da, s_dtheta);
261  i_scale3(x, c_da, c_dtheta);
262  }
263  } else {
264  T sin_theta = i_sin(theta);
265  T cos_theta = i_cos(theta);
266  sinc = i_div(sin_theta, theta);
267  mcos = i_div((T)1.0 - cos_theta, theta2);
268  /*derivatives*/
269  T s_dtheta = (cos_theta - sin_theta / theta) / theta2;
270  T c_dtheta = (sinc - (T)(2.0) * mcos) / theta2;
271  i_scale3(x, s_da, s_dtheta);
272  i_scale3(x, c_da, c_dtheta);
273  }
274  i_rot_rodrigues_3x3_solver(sinc, mcos, a0_sqr, a1_sqr, a2_sqr, s_da, c_da, x,
275  R, D);
276 }
277 
278 /*Rodrigues' formula for computing the matrix exponential of a 3x3
279 skew-symmetric matrix The 1st argument is the input (a 3-vector), the 2nd
280 argument is the output (a 3x3 rotation matrix), The 3rd argument is the
281 derivative (a 9x3 matrix) Keep this slow implementation as baseline - Liang
282 July/31/2015*/
283 template <typename T>
284 inline void i_rot_rodrigues_3x3_slow(const T a[3], T R[9], T D[9][3]) {
285  T sinc, mcos, tmp1, tmp2;
286  T a0_sqr = i_sqr(a[0]);
287  T a1_sqr = i_sqr(a[1]);
288  T a2_sqr = i_sqr(a[2]);
289 
290  T theta2 = a0_sqr + a1_sqr + a2_sqr;
291  T theta = i_sqrt(theta2);
292  T s_da[3], c_da[3]; /*derivatives of sinc and mcos wrt to a, respectively*/
293 
294  if (theta < Constant<T>::MIN_ABS_SAFE_DIVIDEND()) {
295  /*theta is too small; use a third-order Taylor approximation for sin(theta)
296  * and cos(theta)*/
297  sinc = (T)1.0 - theta2 / (T)6.0;
298  mcos = (T)0.5 - theta2 / (T)24.0;
299  /*derivatives*/
300  i_scale3(a, s_da, -i_rec((T)3.0));
301  i_scale3(a, c_da, -i_rec((T)12.0));
302  // s_da[0] = -a[0]/(T)3.0; c_da[0] = -a[0]/12.0;
303  // s_da[1] = -a[1]/(T)3.0; c_da[1] = -a[1]/12.0;
304  // s_da[2] = -a[2]/(T)3.0; c_da[2] = -a[2]/12.0;
305  } else {
306  T sin_theta = i_sin(theta);
307  T cos_theta = i_cos(theta);
308  sinc = i_div(sin_theta, theta);
309  mcos = i_div((T)1.0 - cos_theta, theta2);
310  /*derivatives*/
311  T s_dtheta = (cos_theta - sin_theta / theta) / theta2;
312  T c_dtheta = (sinc - (T)(2.0) * mcos) / theta2;
313  i_scale3(a, s_da, s_dtheta);
314  i_scale3(a, c_da, c_dtheta);
315  // s_da[0] = s_dtheta*a[0]; c_da[0] = c_dtheta*a[0];
316  // s_da[1] = s_dtheta*a[1]; c_da[1] = c_dtheta*a[1];
317  // s_da[2] = s_dtheta*a[2]; c_da[2] = c_dtheta*a[2];
318  }
319  R[0] = (T)1.0 - (a1_sqr + a2_sqr) * mcos;
320  R[4] = (T)1.0 - (a0_sqr + a2_sqr) * mcos;
321  R[8] = (T)1.0 - (a0_sqr + a1_sqr) * mcos;
322  tmp1 = a[0] * a[1] * mcos;
323  tmp2 = a[2] * sinc;
324  R[1] = tmp1 - tmp2;
325  R[3] = tmp1 + tmp2;
326  tmp1 = a[2] * a[0] * mcos;
327  tmp2 = a[1] * sinc;
328  R[2] = tmp1 + tmp2;
329  R[6] = tmp1 - tmp2;
330  tmp1 = a[1] * a[2] * mcos;
331  tmp2 = a[0] * sinc;
332  R[5] = tmp1 - tmp2;
333  R[7] = tmp1 + tmp2;
334 
335  /*derivatives*/
336  T e_x[9];
337  i_axiator(a, e_x);
338 
339  static const T e_x_da[9][3] = {
340  {(T)0, (T)0, (T)0}, {(T)0, (T)0, -(T)1}, {(T)0, (T)1, (T)0},
341  {(T)0, (T)0, (T)1}, {(T)0, (T)0, (T)0}, {-(T)1, (T)0, (T)0},
342  {(T)0, -(T)1, (T)0}, {(T)1, (T)0, (T)0}, {(T)0, (T)0, (T)0}};
343 
344  T e_x2[9];
345  i_sqr_skew_symmetric_3x3(a, e_x2);
346 
347  //\frac{d {\hat v}^2}{d v}
348  T e_x2_da[9][3];
349  e_x2_da[0][0] = e_x2_da[1][2] = e_x2_da[2][1] = (T)0.0;
350  e_x2_da[3][2] = e_x2_da[4][1] = e_x2_da[5][0] = (T)0.0;
351  e_x2_da[6][1] = e_x2_da[7][0] = e_x2_da[8][2] = (T)0.0;
352  e_x2_da[1][1] = e_x2_da[3][1] = e_x2_da[2][2] = e_x2_da[6][2] = a[0];
353  e_x2_da[1][0] = e_x2_da[3][0] = e_x2_da[5][2] = e_x2_da[7][2] = a[1];
354  e_x2_da[2][0] = e_x2_da[6][0] = e_x2_da[5][1] = e_x2_da[7][1] = a[2];
355  e_x2_da[4][0] = e_x2_da[8][0] = -(T)2.0 * a[0];
356  e_x2_da[0][1] = e_x2_da[8][1] = -(T)2.0 * a[1];
357  e_x2_da[0][2] = e_x2_da[4][2] = -(T)2.0 * a[2];
358 
359  /*for (int i = 0; i<9; ++i)
360  {
361  D[i][0] = (T)(e_x[i] * s_da[0] + sinc*e_x_da[i][0] + e_x2[i] * c_da[0] + mcos
362  * e_x2_da[i][0]); D[i][1] = (T)(e_x[i] * s_da[1] + sinc*e_x_da[i][1] + e_x2[i]
363  * c_da[1] + mcos * e_x2_da[i][1]); D[i][2] = (T)(e_x[i] * s_da[2] +
364  sinc*e_x_da[i][2] + e_x2[i] * c_da[2] + mcos * e_x2_da[i][2]);
365  }*/
366 
367  // i=0
368  D[0][0] = (T)(e_x[0] * s_da[0] + sinc * e_x_da[0][0] + e_x2[0] * c_da[0] +
369  mcos * e_x2_da[0][0]);
370  D[0][1] = (T)(e_x[0] * s_da[1] + sinc * e_x_da[0][1] + e_x2[0] * c_da[1] +
371  mcos * e_x2_da[0][1]);
372  D[0][2] = (T)(e_x[0] * s_da[2] + sinc * e_x_da[0][2] + e_x2[0] * c_da[2] +
373  mcos * e_x2_da[0][2]);
374  // i=1
375  D[1][0] = (T)(e_x[1] * s_da[0] + sinc * e_x_da[1][0] + e_x2[1] * c_da[0] +
376  mcos * e_x2_da[1][0]);
377  D[1][1] = (T)(e_x[1] * s_da[1] + sinc * e_x_da[1][1] + e_x2[1] * c_da[1] +
378  mcos * e_x2_da[1][1]);
379  D[1][2] = (T)(e_x[1] * s_da[2] + sinc * e_x_da[1][2] + e_x2[1] * c_da[2] +
380  mcos * e_x2_da[1][2]);
381  // i=2
382  D[2][0] = (T)(e_x[2] * s_da[0] + sinc * e_x_da[2][0] + e_x2[2] * c_da[0] +
383  mcos * e_x2_da[2][0]);
384  D[2][1] = (T)(e_x[2] * s_da[1] + sinc * e_x_da[2][1] + e_x2[2] * c_da[1] +
385  mcos * e_x2_da[2][1]);
386  D[2][2] = (T)(e_x[2] * s_da[2] + sinc * e_x_da[2][2] + e_x2[2] * c_da[2] +
387  mcos * e_x2_da[2][2]);
388  // i=3
389  D[3][0] = (T)(e_x[3] * s_da[0] + sinc * e_x_da[3][0] + e_x2[3] * c_da[0] +
390  mcos * e_x2_da[3][0]);
391  D[3][1] = (T)(e_x[3] * s_da[1] + sinc * e_x_da[3][1] + e_x2[3] * c_da[1] +
392  mcos * e_x2_da[3][1]);
393  D[3][2] = (T)(e_x[3] * s_da[2] + sinc * e_x_da[3][2] + e_x2[3] * c_da[2] +
394  mcos * e_x2_da[3][2]);
395  // i=4
396  D[4][0] = (T)(e_x[4] * s_da[0] + sinc * e_x_da[4][0] + e_x2[4] * c_da[0] +
397  mcos * e_x2_da[4][0]);
398  D[4][1] = (T)(e_x[4] * s_da[1] + sinc * e_x_da[4][1] + e_x2[4] * c_da[1] +
399  mcos * e_x2_da[4][1]);
400  D[4][2] = (T)(e_x[4] * s_da[2] + sinc * e_x_da[4][2] + e_x2[4] * c_da[2] +
401  mcos * e_x2_da[4][2]);
402  // i=5
403  D[5][0] = (T)(e_x[5] * s_da[0] + sinc * e_x_da[5][0] + e_x2[5] * c_da[0] +
404  mcos * e_x2_da[5][0]);
405  D[5][1] = (T)(e_x[5] * s_da[1] + sinc * e_x_da[5][1] + e_x2[5] * c_da[1] +
406  mcos * e_x2_da[5][1]);
407  D[5][2] = (T)(e_x[5] * s_da[2] + sinc * e_x_da[5][2] + e_x2[5] * c_da[2] +
408  mcos * e_x2_da[5][2]);
409  // i=6
410  D[6][0] = (T)(e_x[6] * s_da[0] + sinc * e_x_da[6][0] + e_x2[6] * c_da[0] +
411  mcos * e_x2_da[6][0]);
412  D[6][1] = (T)(e_x[6] * s_da[1] + sinc * e_x_da[6][1] + e_x2[6] * c_da[1] +
413  mcos * e_x2_da[6][1]);
414  D[6][2] = (T)(e_x[6] * s_da[2] + sinc * e_x_da[6][2] + e_x2[6] * c_da[2] +
415  mcos * e_x2_da[6][2]);
416  // i=7
417  D[7][0] = (T)(e_x[7] * s_da[0] + sinc * e_x_da[7][0] + e_x2[7] * c_da[0] +
418  mcos * e_x2_da[7][0]);
419  D[7][1] = (T)(e_x[7] * s_da[1] + sinc * e_x_da[7][1] + e_x2[7] * c_da[1] +
420  mcos * e_x2_da[7][1]);
421  D[7][2] = (T)(e_x[7] * s_da[2] + sinc * e_x_da[7][2] + e_x2[7] * c_da[2] +
422  mcos * e_x2_da[7][2]);
423  // i=8
424  D[8][0] = (T)(e_x[8] * s_da[0] + sinc * e_x_da[8][0] + e_x2[8] * c_da[0] +
425  mcos * e_x2_da[8][0]);
426  D[8][1] = (T)(e_x[8] * s_da[1] + sinc * e_x_da[8][1] + e_x2[8] * c_da[1] +
427  mcos * e_x2_da[8][1]);
428  D[8][2] = (T)(e_x[8] * s_da[2] + sinc * e_x_da[8][2] + e_x2[8] * c_da[2] +
429  mcos * e_x2_da[8][2]);
430 }
431 
432 /*The inverse Rodrigues' formula for computing the logarithm of a 3x3 rotation
433  matrix The 1st argument is the input (a 3x3 rotation matrix) and the 2nd
434  argument is the output (a 3-vector rotation vector)*/
435 template <typename T>
436 inline void i_rot_invert_rodrigues_3x3(const T R[9],
437  T v[3] /*unnormalized rotation vector*/,
438  T &theta /*rotation angle in radians*/) {
439  T r[3];
440  T Q[9], Ac[9], h[3], htA[3], ss[3];
441  int iv[3];
442  const T *R_ref[3];
443  T *Q_ref[3], *Ac_ref[3];
444  i_make_const_reference_3x3(R, R_ref);
445  i_make_reference_3x3(Q, Q_ref);
446  i_make_reference_3x3(Ac, Ac_ref);
447  /*compute the eigenvector of R corresponding to the eigenvalue 1, every
448  * rotation matrix must have this eigenvalue*/
449  i_eigenvector_from_eigenvalue(R_ref, (T)(1.0), Q_ref, h, htA, ss, Ac_ref, 3,
450  iv);
451  i_copy3(Q + 6, v); /*eigen vector of R, unit rotation axis*/
452  r[0] = R[7] - R[5];
453  r[1] = R[2] - R[6];
454  r[2] = R[3] - R[1];
455  T c = (i_trace_3x3(R) - (T)1.0) *
456  (T)0.5; /*the trace of R is 1 + 2\cos(\theta), equivalent to the sum of
457  its eigenvalues*/
458  T s = i_dot3(r, v) * (T)0.5;
459  theta = i_atan2(s, c);
460  i_scale3(v, theta);
461 }
462 
463 /*This routine finds a rotation matrix R that rotates unit vector an onto unit
464  vector b.
465  a and b are both assumed to be unit vectors. Corner cases are a and b are
466  parallel*/
467 template <typename T>
468 inline bool i_rot_3x3(const T a[3], const T b[3], T R[9]) {
469  T v[3], s, theta; // vx[9], vx2[9];
470  i_eye_3x3(R);
471  /*corner case 1: a == b*/
472  if (i_equal3(a, b)) {
473  return true;
474  }
475  i_cross(a, b, v);
476 
477  /*L2 norm of v:*/
478  s = i_l2_norm3(v); /*sine of angle*/
479 
480  if (s < Constant<T>::MIN_ABS_SAFE_DIVIDEND()) {
481  return false; /*parallel vectors*/
482  }
483 
484  /*normalize v:*/
485  i_scale3(v, i_rec(s));
486 
487  // i_axiator(v, vx);
488  // i_mult_AAt_3x3(vx, vx2);
489  // i_sqr_skew_symmetric_3x3(vx, vx2);
490 
491  // c = i_dot3(a, b);
492  theta = i_acos(i_dot3(a, b)); /*dot product - cos(theta)*/
493 
494  i_scale3(v, theta);
495 
496  i_rot_rodrigues_3x3(v, R);
497 
498  // s = i_sin(theta);
499  // c = (T)1.0 - c;
500 
501  // i_scale9(vx, s);
502  // i_scale9(vx2, c);
503 
505  // i_add9(vx, vx2, R);
506 
507  // R[0] += (T)1.0;
508  // R[4] += (T)1.0;
509  // R[8] += (T)1.0;
510  //
511  // if (i_determinant_3x3(R) < (T)0.0)
512  //{
513  // i_neg9(R);
514  //}
515  return true;
516 }
517 
518 /*Generate a random 3x3 rotation matrix R, if force_proper is set to 1, then R
519  * is forced to be a proper rotation matrix with det(R) = 1*/
520 template <typename T>
521 inline void i_rand_rot_3x3(T R[9], int &seed, int force_proper = 1) {
522  T Q[9], A[9], h[3], htA[3], ss[3];
523  T *R_ref[3], *Q_ref[3], *A_ref[3];
524  int iv[3];
525  i_make_reference_3x3(R, R_ref);
526  i_make_reference_3x3(Q, Q_ref);
527  i_make_reference_3x3(A, A_ref);
528  i_rand_orthogonal<T>(Q_ref, A_ref, R_ref, h, htA, ss, 3, 3, iv, seed);
529  if (force_proper) {
530  if (i_determinant_3x3(R) < (T)0.0) {
531  i_neg9(R);
532  }
533  }
534 }
535 
536 } /* namespace idl */
idl::i_rot_rodrigues_3x3_slow
void i_rot_rodrigues_3x3_slow(const T a[3], T R[9], T D[9][3])
Definition: i_rot.h:284
idl::i_rot_rodrigues_3x3
void i_rot_rodrigues_3x3(const T a[3], T R[9])
Definition: i_rot.h:169
idl::i_rot_3x3
bool i_rot_3x3(const T a[3], const T b[3], T R[9])
Definition: i_rot.h:468
apollo::drivers::camera::A
const size_t A
Definition: util.h:160
idl::i_rot_orthogonalize
void i_rot_orthogonalize(T R[9], int nr_svd_iter=I_DEFAULT_MAX_SVD_ITERATIONS)
Definition: i_rot.h:22
apollo::drivers::hesai::PI
const double PI
Definition: const_var.h:77
idl
Definition: i_rot.h:8
idl::i_force_rot_2x2
void i_force_rot_2x2(T R[4])
Definition: i_rot.h:13
idl::i_rand_rot_3x3
void i_rand_rot_3x3(T R[9], int &seed, int force_proper=1)
Definition: i_rot.h:521
idl::i_rot_rodrigues_3x3_solver
void i_rot_rodrigues_3x3_solver(T sinc, T mcos, T a0_sqr, T a1_sqr, T a2_sqr, const T a[3], T R[9])
Definition: i_rot.h:39
idl::i_rot_invert_rodrigues_3x3
void i_rot_invert_rodrigues_3x3(const T R[9], T v[3], T &theta)
Definition: i_rot.h:436
Generated by   doxygen 1.8.13