g2o
dquat2mat_maxima_generated.cpp
Go to the documentation of this file.
1 void compute_dq_dR_w ( Eigen::Matrix<double, 3 , 9 >& dq_dR_w , const double& qw , const double& r00 , const double& r10 , const double& r20 , const double& r01 , const double& r11 , const double& r21 , const double& r02 , const double& r12 , const double& r22 ) {
2  (void) r00;
3  (void) r11;
4  (void) r22;
5  double _aux1 = 1/pow(qw,3) ;
6  double _aux2 = -0.03125*(r21-r12)*_aux1 ;
7  double _aux3 = 1/qw ;
8  double _aux4 = 0.25*_aux3 ;
9  double _aux5 = -0.25*_aux3 ;
10  double _aux6 = 0.03125*(r20-r02)*_aux1 ;
11  double _aux7 = -0.03125*(r10-r01)*_aux1 ;
12  dq_dR_w ( 0 , 0 ) = _aux2 ;
13  dq_dR_w ( 0 , 1 ) = 0 ;
14  dq_dR_w ( 0 , 2 ) = 0 ;
15  dq_dR_w ( 0 , 3 ) = 0 ;
16  dq_dR_w ( 0 , 4 ) = _aux2 ;
17  dq_dR_w ( 0 , 5 ) = _aux4 ;
18  dq_dR_w ( 0 , 6 ) = 0 ;
19  dq_dR_w ( 0 , 7 ) = _aux5 ;
20  dq_dR_w ( 0 , 8 ) = _aux2 ;
21  dq_dR_w ( 1 , 0 ) = _aux6 ;
22  dq_dR_w ( 1 , 1 ) = 0 ;
23  dq_dR_w ( 1 , 2 ) = _aux5 ;
24  dq_dR_w ( 1 , 3 ) = 0 ;
25  dq_dR_w ( 1 , 4 ) = _aux6 ;
26  dq_dR_w ( 1 , 5 ) = 0 ;
27  dq_dR_w ( 1 , 6 ) = _aux4 ;
28  dq_dR_w ( 1 , 7 ) = 0 ;
29  dq_dR_w ( 1 , 8 ) = _aux6 ;
30  dq_dR_w ( 2 , 0 ) = _aux7 ;
31  dq_dR_w ( 2 , 1 ) = _aux4 ;
32  dq_dR_w ( 2 , 2 ) = 0 ;
33  dq_dR_w ( 2 , 3 ) = _aux5 ;
34  dq_dR_w ( 2 , 4 ) = _aux7 ;
35  dq_dR_w ( 2 , 5 ) = 0 ;
36  dq_dR_w ( 2 , 6 ) = 0 ;
37  dq_dR_w ( 2 , 7 ) = 0 ;
38  dq_dR_w ( 2 , 8 ) = _aux7 ;
39 }
40 void compute_dq_dR_x ( Eigen::Matrix<double, 3 , 9 >& dq_dR_x , const double& qx , const double& r00 , const double& r10 , const double& r20 , const double& r01 , const double& r11 , const double& r21 , const double& r02 , const double& r12 , const double& r22 ) {
41  (void) r00;
42  (void) r11;
43  (void) r21;
44  (void) r12;
45  (void) r22;
46  double _aux1 = 1/qx ;
47  double _aux2 = -0.125*_aux1 ;
48  double _aux3 = 1/pow(qx,3) ;
49  double _aux4 = r10+r01 ;
50  double _aux5 = 0.25*_aux1 ;
51  double _aux6 = 0.03125*_aux3*_aux4 ;
52  double _aux7 = r20+r02 ;
53  double _aux8 = 0.03125*_aux3*_aux7 ;
54  dq_dR_x ( 0 , 0 ) = 0.125*_aux1 ;
55  dq_dR_x ( 0 , 1 ) = 0 ;
56  dq_dR_x ( 0 , 2 ) = 0 ;
57  dq_dR_x ( 0 , 3 ) = 0 ;
58  dq_dR_x ( 0 , 4 ) = _aux2 ;
59  dq_dR_x ( 0 , 5 ) = 0 ;
60  dq_dR_x ( 0 , 6 ) = 0 ;
61  dq_dR_x ( 0 , 7 ) = 0 ;
62  dq_dR_x ( 0 , 8 ) = _aux2 ;
63  dq_dR_x ( 1 , 0 ) = -0.03125*_aux3*_aux4 ;
64  dq_dR_x ( 1 , 1 ) = _aux5 ;
65  dq_dR_x ( 1 , 2 ) = 0 ;
66  dq_dR_x ( 1 , 3 ) = _aux5 ;
67  dq_dR_x ( 1 , 4 ) = _aux6 ;
68  dq_dR_x ( 1 , 5 ) = 0 ;
69  dq_dR_x ( 1 , 6 ) = 0 ;
70  dq_dR_x ( 1 , 7 ) = 0 ;
71  dq_dR_x ( 1 , 8 ) = _aux6 ;
72  dq_dR_x ( 2 , 0 ) = -0.03125*_aux3*_aux7 ;
73  dq_dR_x ( 2 , 1 ) = 0 ;
74  dq_dR_x ( 2 , 2 ) = _aux5 ;
75  dq_dR_x ( 2 , 3 ) = 0 ;
76  dq_dR_x ( 2 , 4 ) = _aux8 ;
77  dq_dR_x ( 2 , 5 ) = 0 ;
78  dq_dR_x ( 2 , 6 ) = _aux5 ;
79  dq_dR_x ( 2 , 7 ) = 0 ;
80  dq_dR_x ( 2 , 8 ) = _aux8 ;
81 }
82 void compute_dq_dR_y ( Eigen::Matrix<double, 3 , 9 >& dq_dR_y , const double& qy , const double& r00 , const double& r10 , const double& r20 , const double& r01 , const double& r11 , const double& r21 , const double& r02 , const double& r12 , const double& r22 ) {
83  (void) r00;
84  (void) r20;
85  (void) r11;
86  (void) r02;
87  (void) r22;
88  double _aux1 = 1/pow(qy,3) ;
89  double _aux2 = r10+r01 ;
90  double _aux3 = 0.03125*_aux1*_aux2 ;
91  double _aux4 = 1/qy ;
92  double _aux5 = 0.25*_aux4 ;
93  double _aux6 = -0.125*_aux4 ;
94  double _aux7 = r21+r12 ;
95  double _aux8 = 0.03125*_aux1*_aux7 ;
96  dq_dR_y ( 0 , 0 ) = _aux3 ;
97  dq_dR_y ( 0 , 1 ) = _aux5 ;
98  dq_dR_y ( 0 , 2 ) = 0 ;
99  dq_dR_y ( 0 , 3 ) = _aux5 ;
100  dq_dR_y ( 0 , 4 ) = -0.03125*_aux1*_aux2 ;
101  dq_dR_y ( 0 , 5 ) = 0 ;
102  dq_dR_y ( 0 , 6 ) = 0 ;
103  dq_dR_y ( 0 , 7 ) = 0 ;
104  dq_dR_y ( 0 , 8 ) = _aux3 ;
105  dq_dR_y ( 1 , 0 ) = _aux6 ;
106  dq_dR_y ( 1 , 1 ) = 0 ;
107  dq_dR_y ( 1 , 2 ) = 0 ;
108  dq_dR_y ( 1 , 3 ) = 0 ;
109  dq_dR_y ( 1 , 4 ) = 0.125*_aux4 ;
110  dq_dR_y ( 1 , 5 ) = 0 ;
111  dq_dR_y ( 1 , 6 ) = 0 ;
112  dq_dR_y ( 1 , 7 ) = 0 ;
113  dq_dR_y ( 1 , 8 ) = _aux6 ;
114  dq_dR_y ( 2 , 0 ) = _aux8 ;
115  dq_dR_y ( 2 , 1 ) = 0 ;
116  dq_dR_y ( 2 , 2 ) = 0 ;
117  dq_dR_y ( 2 , 3 ) = 0 ;
118  dq_dR_y ( 2 , 4 ) = -0.03125*_aux1*_aux7 ;
119  dq_dR_y ( 2 , 5 ) = _aux5 ;
120  dq_dR_y ( 2 , 6 ) = 0 ;
121  dq_dR_y ( 2 , 7 ) = _aux5 ;
122  dq_dR_y ( 2 , 8 ) = _aux8 ;
123 }
124 void compute_dq_dR_z ( Eigen::Matrix<double, 3 , 9 >& dq_dR_z , const double& qz , const double& r00 , const double& r10 , const double& r20 , const double& r01 , const double& r11 , const double& r21 , const double& r02 , const double& r12 , const double& r22 ) {
125  (void) r00;
126  (void) r10;
127  (void) r01;
128  (void) r11;
129  (void) r22;
130  double _aux1 = 1/pow(qz,3) ;
131  double _aux2 = r20+r02 ;
132  double _aux3 = 0.03125*_aux1*_aux2 ;
133  double _aux4 = 1/qz ;
134  double _aux5 = 0.25*_aux4 ;
135  double _aux6 = r21+r12 ;
136  double _aux7 = 0.03125*_aux1*_aux6 ;
137  double _aux8 = -0.125*_aux4 ;
138  dq_dR_z ( 0 , 0 ) = _aux3 ;
139  dq_dR_z ( 0 , 1 ) = 0 ;
140  dq_dR_z ( 0 , 2 ) = _aux5 ;
141  dq_dR_z ( 0 , 3 ) = 0 ;
142  dq_dR_z ( 0 , 4 ) = _aux3 ;
143  dq_dR_z ( 0 , 5 ) = 0 ;
144  dq_dR_z ( 0 , 6 ) = _aux5 ;
145  dq_dR_z ( 0 , 7 ) = 0 ;
146  dq_dR_z ( 0 , 8 ) = -0.03125*_aux1*_aux2 ;
147  dq_dR_z ( 1 , 0 ) = _aux7 ;
148  dq_dR_z ( 1 , 1 ) = 0 ;
149  dq_dR_z ( 1 , 2 ) = 0 ;
150  dq_dR_z ( 1 , 3 ) = 0 ;
151  dq_dR_z ( 1 , 4 ) = _aux7 ;
152  dq_dR_z ( 1 , 5 ) = _aux5 ;
153  dq_dR_z ( 1 , 6 ) = 0 ;
154  dq_dR_z ( 1 , 7 ) = _aux5 ;
155  dq_dR_z ( 1 , 8 ) = -0.03125*_aux1*_aux6 ;
156  dq_dR_z ( 2 , 0 ) = _aux8 ;
157  dq_dR_z ( 2 , 1 ) = 0 ;
158  dq_dR_z ( 2 , 2 ) = 0 ;
159  dq_dR_z ( 2 , 3 ) = 0 ;
160  dq_dR_z ( 2 , 4 ) = _aux8 ;
161  dq_dR_z ( 2 , 5 ) = 0 ;
162  dq_dR_z ( 2 , 6 ) = 0 ;
163  dq_dR_z ( 2 , 7 ) = 0 ;
164  dq_dR_z ( 2 , 8 ) = 0.125*_aux4 ;
165 }
166 void compute_dR_dq ( Eigen::Matrix<double, 9 , 3 >& dR_dq , const double& qx , const double& qy , const double& qz , const double& qw ) {
167  double _aux1 = -4*qy ;
168  double _aux2 = -4*qz ;
169  double _aux3 = 1/qw ;
170  double _aux4 = 2*qx*qz ;
171  double _aux5 = -_aux3*(_aux4-2*qw*qy) ;
172  double _aux6 = 2*qy*qz ;
173  double _aux7 = -_aux3*(_aux6-2*qw*qx) ;
174  double _aux8 = -2*pow(qw,2) ;
175  double _aux9 = _aux8+2*pow(qz,2) ;
176  double _aux10 = 2*qw*qz ;
177  double _aux11 = (_aux10+2*qx*qy)*_aux3 ;
178  double _aux12 = _aux8+2*pow(qy,2) ;
179  double _aux13 = _aux3*(_aux6+2*qw*qx) ;
180  double _aux14 = _aux3*(_aux4+2*qw*qy) ;
181  double _aux15 = -4*qx ;
182  double _aux16 = _aux8+2*pow(qx,2) ;
183  double _aux17 = (_aux10-2*qx*qy)*_aux3 ;
184  dR_dq ( 0 , 0 ) = 0 ;
185  dR_dq ( 0 , 1 ) = _aux1 ;
186  dR_dq ( 0 , 2 ) = _aux2 ;
187  dR_dq ( 1 , 0 ) = _aux5 ;
188  dR_dq ( 1 , 1 ) = _aux7 ;
189  dR_dq ( 1 , 2 ) = -_aux3*_aux9 ;
190  dR_dq ( 2 , 0 ) = _aux11 ;
191  dR_dq ( 2 , 1 ) = _aux12*_aux3 ;
192  dR_dq ( 2 , 2 ) = _aux13 ;
193  dR_dq ( 3 , 0 ) = _aux14 ;
194  dR_dq ( 3 , 1 ) = _aux13 ;
195  dR_dq ( 3 , 2 ) = _aux3*_aux9 ;
196  dR_dq ( 4 , 0 ) = _aux15 ;
197  dR_dq ( 4 , 1 ) = 0 ;
198  dR_dq ( 4 , 2 ) = _aux2 ;
199  dR_dq ( 5 , 0 ) = -_aux16*_aux3 ;
200  dR_dq ( 5 , 1 ) = _aux17 ;
201  dR_dq ( 5 , 2 ) = _aux5 ;
202  dR_dq ( 6 , 0 ) = _aux17 ;
203  dR_dq ( 6 , 1 ) = -_aux12*_aux3 ;
204  dR_dq ( 6 , 2 ) = _aux7 ;
205  dR_dq ( 7 , 0 ) = _aux16*_aux3 ;
206  dR_dq ( 7 , 1 ) = _aux11 ;
207  dR_dq ( 7 , 2 ) = _aux14 ;
208  dR_dq ( 8 , 0 ) = _aux15 ;
209  dR_dq ( 8 , 1 ) = _aux1 ;
210  dR_dq ( 8 , 2 ) = 0 ;
211 }
compute_dR_dq
void compute_dR_dq(Eigen::Matrix< double, 9, 3 > &dR_dq, const double &qx, const double &qy, const double &qz, const double &qw)
Definition: dquat2mat_maxima_generated.cpp:166
compute_dq_dR_z
void compute_dq_dR_z(Eigen::Matrix< double, 3, 9 > &dq_dR_z, const double &qz, const double &r00, const double &r10, const double &r20, const double &r01, const double &r11, const double &r21, const double &r02, const double &r12, const double &r22)
Definition: dquat2mat_maxima_generated.cpp:124
compute_dq_dR_x
void compute_dq_dR_x(Eigen::Matrix< double, 3, 9 > &dq_dR_x, const double &qx, const double &r00, const double &r10, const double &r20, const double &r01, const double &r11, const double &r21, const double &r02, const double &r12, const double &r22)
Definition: dquat2mat_maxima_generated.cpp:40
compute_dq_dR_y
void compute_dq_dR_y(Eigen::Matrix< double, 3, 9 > &dq_dR_y, const double &qy, const double &r00, const double &r10, const double &r20, const double &r01, const double &r11, const double &r21, const double &r02, const double &r12, const double &r22)
Definition: dquat2mat_maxima_generated.cpp:82
compute_dq_dR_w
void compute_dq_dR_w(Eigen::Matrix< double, 3, 9 > &dq_dR_w, const double &qw, const double &r00, const double &r10, const double &r20, const double &r01, const double &r11, const double &r21, const double &r02, const double &r12, const double &r22)
Definition: dquat2mat_maxima_generated.cpp:1
Generated on Mon Mar 18 2019 20:14:40 for g2o by   doxygen 1.8.11