1 /******************************************************************************
2 * Author: Laurent Kneip *
3 * Contact: kneip.laurent@gmail.com *
4 * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
5 * *
6 * Redistribution and use in source and binary forms, with or without *
7 * modification, are permitted provided that the following conditions *
8 * are met: *
9 * * Redistributions of source code must retain the above copyright *
10 * notice, this list of conditions and the following disclaimer. *
11 * * Redistributions in binary form must reproduce the above copyright *
12 * notice, this list of conditions and the following disclaimer in the *
13 * documentation and/or other materials provided with the distribution. *
14 * * Neither the name of ANU nor the names of its contributors may be *
15 * used to endorse or promote products derived from this software without *
16 * specific prior written permission. *
17 * *
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
19 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
21 * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
24 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
25 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
28 * SUCH DAMAGE. *
29 ******************************************************************************/
30
31
32 #include <opengv/absolute_pose/modules/gpnp4/modules.hpp>
33
34
35 void
groebnerRow5_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)36 opengv::absolute_pose::modules::gpnp4::groebnerRow5_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
37 {
38 double factor = groebnerMatrix(targetRow,23) / groebnerMatrix(5,23);
39 groebnerMatrix(targetRow,23) = 0.0;
40 groebnerMatrix(targetRow,24) -= factor * groebnerMatrix(5,24);
41 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(5,25);
42 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(5,26);
43 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(5,27);
44 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(5,28);
45 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(5,29);
46 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(5,30);
47 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(5,31);
48 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(5,32);
49 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(5,33);
50 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(5,34);
51 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(5,35);
52 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(5,36);
53 }
54
55 void
groebnerRow6_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)56 opengv::absolute_pose::modules::gpnp4::groebnerRow6_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
57 {
58 double factor = groebnerMatrix(targetRow,24) / groebnerMatrix(6,24);
59 groebnerMatrix(targetRow,24) = 0.0;
60 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(6,25);
61 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(6,26);
62 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(6,27);
63 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(6,28);
64 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(6,29);
65 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(6,30);
66 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(6,31);
67 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(6,32);
68 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(6,33);
69 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(6,34);
70 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(6,35);
71 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(6,36);
72 }
73
74 void
groebnerRow7_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)75 opengv::absolute_pose::modules::gpnp4::groebnerRow7_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
76 {
77 double factor = groebnerMatrix(targetRow,25) / groebnerMatrix(7,25);
78 groebnerMatrix(targetRow,25) = 0.0;
79 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(7,26);
80 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(7,27);
81 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(7,28);
82 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(7,29);
83 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(7,30);
84 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(7,31);
85 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(7,32);
86 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(7,33);
87 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(7,34);
88 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(7,35);
89 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(7,36);
90 }
91
92 void
groebnerRow7_0100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)93 opengv::absolute_pose::modules::gpnp4::groebnerRow7_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
94 {
95 double factor = groebnerMatrix(targetRow,9) / groebnerMatrix(7,25);
96 groebnerMatrix(targetRow,9) = 0.0;
97 groebnerMatrix(targetRow,10) -= factor * groebnerMatrix(7,26);
98 groebnerMatrix(targetRow,11) -= factor * groebnerMatrix(7,27);
99 groebnerMatrix(targetRow,15) -= factor * groebnerMatrix(7,28);
100 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(7,29);
101 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(7,30);
102 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(7,31);
103 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(7,32);
104 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(7,33);
105 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(7,34);
106 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(7,35);
107 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(7,36);
108 }
109
110 void
groebnerRow8_0100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)111 opengv::absolute_pose::modules::gpnp4::groebnerRow8_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
112 {
113 double factor = groebnerMatrix(targetRow,10) / groebnerMatrix(8,26);
114 groebnerMatrix(targetRow,10) = 0.0;
115 groebnerMatrix(targetRow,11) -= factor * groebnerMatrix(8,27);
116 groebnerMatrix(targetRow,15) -= factor * groebnerMatrix(8,28);
117 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(8,29);
118 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(8,30);
119 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(8,31);
120 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(8,32);
121 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(8,33);
122 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(8,34);
123 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(8,35);
124 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(8,36);
125 }
126
127 void
groebnerRow5_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)128 opengv::absolute_pose::modules::gpnp4::groebnerRow5_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
129 {
130 double factor = groebnerMatrix(targetRow,13) / groebnerMatrix(5,23);
131 groebnerMatrix(targetRow,13) = 0.0;
132 groebnerMatrix(targetRow,14) -= factor * groebnerMatrix(5,24);
133 groebnerMatrix(targetRow,15) -= factor * groebnerMatrix(5,25);
134 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(5,26);
135 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(5,27);
136 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(5,28);
137 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(5,29);
138 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(5,30);
139 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(5,31);
140 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(5,32);
141 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(5,33);
142 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(5,34);
143 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(5,35);
144 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(5,36);
145 }
146
147 void
groebnerRow6_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)148 opengv::absolute_pose::modules::gpnp4::groebnerRow6_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
149 {
150 double factor = groebnerMatrix(targetRow,14) / groebnerMatrix(6,24);
151 groebnerMatrix(targetRow,14) = 0.0;
152 groebnerMatrix(targetRow,15) -= factor * groebnerMatrix(6,25);
153 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(6,26);
154 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(6,27);
155 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(6,28);
156 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(6,29);
157 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(6,30);
158 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(6,31);
159 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(6,32);
160 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(6,33);
161 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(6,34);
162 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(6,35);
163 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(6,36);
164 }
165
166 void
groebnerRow7_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)167 opengv::absolute_pose::modules::gpnp4::groebnerRow7_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
168 {
169 double factor = groebnerMatrix(targetRow,15) / groebnerMatrix(7,25);
170 groebnerMatrix(targetRow,15) = 0.0;
171 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(7,26);
172 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(7,27);
173 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(7,28);
174 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(7,29);
175 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(7,30);
176 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(7,31);
177 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(7,32);
178 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(7,33);
179 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(7,34);
180 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(7,35);
181 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(7,36);
182 }
183
184 void
groebnerRow8_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)185 opengv::absolute_pose::modules::gpnp4::groebnerRow8_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
186 {
187 double factor = groebnerMatrix(targetRow,16) / groebnerMatrix(8,26);
188 groebnerMatrix(targetRow,16) = 0.0;
189 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(8,27);
190 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(8,28);
191 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(8,29);
192 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(8,30);
193 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(8,31);
194 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(8,32);
195 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(8,33);
196 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(8,34);
197 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(8,35);
198 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(8,36);
199 }
200
201 void
groebnerRow8_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)202 opengv::absolute_pose::modules::gpnp4::groebnerRow8_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
203 {
204 double factor = groebnerMatrix(targetRow,26) / groebnerMatrix(8,26);
205 groebnerMatrix(targetRow,26) = 0.0;
206 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(8,27);
207 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(8,28);
208 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(8,29);
209 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(8,30);
210 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(8,31);
211 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(8,32);
212 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(8,33);
213 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(8,34);
214 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(8,35);
215 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(8,36);
216 }
217
218 void
groebnerRow6_0100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)219 opengv::absolute_pose::modules::gpnp4::groebnerRow6_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
220 {
221 double factor = groebnerMatrix(targetRow,8) / groebnerMatrix(6,24);
222 groebnerMatrix(targetRow,8) = 0.0;
223 groebnerMatrix(targetRow,9) -= factor * groebnerMatrix(6,25);
224 groebnerMatrix(targetRow,10) -= factor * groebnerMatrix(6,26);
225 groebnerMatrix(targetRow,11) -= factor * groebnerMatrix(6,27);
226 groebnerMatrix(targetRow,15) -= factor * groebnerMatrix(6,28);
227 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(6,29);
228 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(6,30);
229 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(6,31);
230 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(6,32);
231 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(6,33);
232 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(6,34);
233 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(6,35);
234 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(6,36);
235 }
236
237 void
groebnerRow9_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)238 opengv::absolute_pose::modules::gpnp4::groebnerRow9_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
239 {
240 double factor = groebnerMatrix(targetRow,11) / groebnerMatrix(9,11);
241 groebnerMatrix(targetRow,11) = 0.0;
242 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(9,17);
243 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(9,18);
244 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(9,19);
245 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(9,20);
246 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(9,21);
247 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(9,27);
248 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(9,28);
249 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(9,29);
250 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(9,30);
251 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(9,31);
252 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(9,32);
253 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(9,33);
254 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(9,34);
255 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(9,35);
256 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(9,36);
257 }
258
259 void
groebnerRow4_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)260 opengv::absolute_pose::modules::gpnp4::groebnerRow4_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
261 {
262 double factor = groebnerMatrix(targetRow,12) / groebnerMatrix(4,22);
263 groebnerMatrix(targetRow,12) = 0.0;
264 groebnerMatrix(targetRow,13) -= factor * groebnerMatrix(4,23);
265 groebnerMatrix(targetRow,14) -= factor * groebnerMatrix(4,24);
266 groebnerMatrix(targetRow,15) -= factor * groebnerMatrix(4,25);
267 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(4,26);
268 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(4,27);
269 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(4,28);
270 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(4,29);
271 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(4,30);
272 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(4,31);
273 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(4,32);
274 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(4,33);
275 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(4,34);
276 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(4,35);
277 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(4,36);
278 }
279
280 void
groebnerRow10_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)281 opengv::absolute_pose::modules::gpnp4::groebnerRow10_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
282 {
283 double factor = groebnerMatrix(targetRow,17) / groebnerMatrix(10,17);
284 groebnerMatrix(targetRow,17) = 0.0;
285 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(10,18);
286 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(10,19);
287 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(10,20);
288 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(10,21);
289 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(10,27);
290 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(10,28);
291 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(10,29);
292 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(10,30);
293 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(10,31);
294 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(10,32);
295 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(10,33);
296 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(10,34);
297 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(10,35);
298 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(10,36);
299 }
300
301 void
groebnerRow4_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)302 opengv::absolute_pose::modules::gpnp4::groebnerRow4_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
303 {
304 double factor = groebnerMatrix(targetRow,22) / groebnerMatrix(4,22);
305 groebnerMatrix(targetRow,22) = 0.0;
306 groebnerMatrix(targetRow,23) -= factor * groebnerMatrix(4,23);
307 groebnerMatrix(targetRow,24) -= factor * groebnerMatrix(4,24);
308 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(4,25);
309 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(4,26);
310 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(4,27);
311 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(4,28);
312 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(4,29);
313 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(4,30);
314 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(4,31);
315 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(4,32);
316 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(4,33);
317 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(4,34);
318 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(4,35);
319 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(4,36);
320 }
321
322 void
groebnerRow5_0100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)323 opengv::absolute_pose::modules::gpnp4::groebnerRow5_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
324 {
325 double factor = groebnerMatrix(targetRow,7) / groebnerMatrix(5,23);
326 groebnerMatrix(targetRow,7) = 0.0;
327 groebnerMatrix(targetRow,8) -= factor * groebnerMatrix(5,24);
328 groebnerMatrix(targetRow,9) -= factor * groebnerMatrix(5,25);
329 groebnerMatrix(targetRow,10) -= factor * groebnerMatrix(5,26);
330 groebnerMatrix(targetRow,11) -= factor * groebnerMatrix(5,27);
331 groebnerMatrix(targetRow,15) -= factor * groebnerMatrix(5,28);
332 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(5,29);
333 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(5,30);
334 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(5,31);
335 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(5,32);
336 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(5,33);
337 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(5,34);
338 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(5,35);
339 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(5,36);
340 }
341
342 void
groebnerRow11_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)343 opengv::absolute_pose::modules::gpnp4::groebnerRow11_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
344 {
345 double factor = groebnerMatrix(targetRow,18) / groebnerMatrix(11,18);
346 groebnerMatrix(targetRow,18) = 0.0;
347 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(11,19);
348 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(11,20);
349 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(11,21);
350 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(11,27);
351 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(11,28);
352 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(11,29);
353 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(11,30);
354 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(11,31);
355 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(11,32);
356 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(11,33);
357 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(11,34);
358 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(11,35);
359 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(11,36);
360 }
361
362 void
groebnerRow6_0010_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)363 opengv::absolute_pose::modules::gpnp4::groebnerRow6_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
364 {
365 double factor = groebnerMatrix(targetRow,5) / groebnerMatrix(6,24);
366 groebnerMatrix(targetRow,5) = 0.0;
367 groebnerMatrix(targetRow,7) -= factor * groebnerMatrix(6,25);
368 groebnerMatrix(targetRow,8) -= factor * groebnerMatrix(6,26);
369 groebnerMatrix(targetRow,10) -= factor * groebnerMatrix(6,27);
370 groebnerMatrix(targetRow,13) -= factor * groebnerMatrix(6,28);
371 groebnerMatrix(targetRow,14) -= factor * groebnerMatrix(6,29);
372 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(6,30);
373 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(6,31);
374 groebnerMatrix(targetRow,23) -= factor * groebnerMatrix(6,32);
375 groebnerMatrix(targetRow,24) -= factor * groebnerMatrix(6,33);
376 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(6,34);
377 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(6,35);
378 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(6,36);
379 }
380
381 void
groebnerRow4_0100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)382 opengv::absolute_pose::modules::gpnp4::groebnerRow4_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
383 {
384 double factor = groebnerMatrix(targetRow,6) / groebnerMatrix(4,22);
385 groebnerMatrix(targetRow,6) = 0.0;
386 groebnerMatrix(targetRow,7) -= factor * groebnerMatrix(4,23);
387 groebnerMatrix(targetRow,8) -= factor * groebnerMatrix(4,24);
388 groebnerMatrix(targetRow,9) -= factor * groebnerMatrix(4,25);
389 groebnerMatrix(targetRow,10) -= factor * groebnerMatrix(4,26);
390 groebnerMatrix(targetRow,11) -= factor * groebnerMatrix(4,27);
391 groebnerMatrix(targetRow,15) -= factor * groebnerMatrix(4,28);
392 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(4,29);
393 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(4,30);
394 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(4,31);
395 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(4,32);
396 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(4,33);
397 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(4,34);
398 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(4,35);
399 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(4,36);
400 }
401
402 void
groebnerRow12_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)403 opengv::absolute_pose::modules::gpnp4::groebnerRow12_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
404 {
405 double factor = groebnerMatrix(targetRow,19) / groebnerMatrix(12,19);
406 groebnerMatrix(targetRow,19) = 0.0;
407 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(12,20);
408 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(12,21);
409 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(12,27);
410 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(12,28);
411 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(12,29);
412 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(12,30);
413 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(12,31);
414 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(12,32);
415 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(12,33);
416 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(12,34);
417 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(12,35);
418 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(12,36);
419 }
420
421 void
groebnerRow5_0010_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)422 opengv::absolute_pose::modules::gpnp4::groebnerRow5_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
423 {
424 double factor = groebnerMatrix(targetRow,4) / groebnerMatrix(5,23);
425 groebnerMatrix(targetRow,4) = 0.0;
426 groebnerMatrix(targetRow,5) -= factor * groebnerMatrix(5,24);
427 groebnerMatrix(targetRow,7) -= factor * groebnerMatrix(5,25);
428 groebnerMatrix(targetRow,8) -= factor * groebnerMatrix(5,26);
429 groebnerMatrix(targetRow,10) -= factor * groebnerMatrix(5,27);
430 groebnerMatrix(targetRow,13) -= factor * groebnerMatrix(5,28);
431 groebnerMatrix(targetRow,14) -= factor * groebnerMatrix(5,29);
432 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(5,30);
433 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(5,31);
434 groebnerMatrix(targetRow,23) -= factor * groebnerMatrix(5,32);
435 groebnerMatrix(targetRow,24) -= factor * groebnerMatrix(5,33);
436 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(5,34);
437 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(5,35);
438 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(5,36);
439 }
440
441 void
groebnerRow13_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)442 opengv::absolute_pose::modules::gpnp4::groebnerRow13_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
443 {
444 double factor = groebnerMatrix(targetRow,20) / groebnerMatrix(13,20);
445 groebnerMatrix(targetRow,20) = 0.0;
446 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(13,21);
447 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(13,27);
448 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(13,28);
449 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(13,29);
450 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(13,30);
451 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(13,31);
452 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(13,32);
453 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(13,33);
454 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(13,34);
455 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(13,35);
456 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(13,36);
457 }
458
459 void
groebnerRow14_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)460 opengv::absolute_pose::modules::gpnp4::groebnerRow14_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
461 {
462 double factor = groebnerMatrix(targetRow,3) / groebnerMatrix(14,21);
463 groebnerMatrix(targetRow,3) = 0.0;
464 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(14,27);
465 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(14,28);
466 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(14,29);
467 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(14,30);
468 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(14,31);
469 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(14,32);
470 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(14,33);
471 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(14,34);
472 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(14,35);
473 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(14,36);
474 }
475
476 void
groebnerRow14_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)477 opengv::absolute_pose::modules::gpnp4::groebnerRow14_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
478 {
479 double factor = groebnerMatrix(targetRow,21) / groebnerMatrix(14,21);
480 groebnerMatrix(targetRow,21) = 0.0;
481 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(14,27);
482 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(14,28);
483 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(14,29);
484 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(14,30);
485 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(14,31);
486 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(14,32);
487 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(14,33);
488 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(14,34);
489 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(14,35);
490 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(14,36);
491 }
492
493 void
groebnerRow13_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)494 opengv::absolute_pose::modules::gpnp4::groebnerRow13_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
495 {
496 double factor = groebnerMatrix(targetRow,2) / groebnerMatrix(13,20);
497 groebnerMatrix(targetRow,2) = 0.0;
498 groebnerMatrix(targetRow,3) -= factor * groebnerMatrix(13,21);
499 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(13,27);
500 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(13,28);
501 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(13,29);
502 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(13,30);
503 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(13,31);
504 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(13,32);
505 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(13,33);
506 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(13,34);
507 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(13,35);
508 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(13,36);
509 }
510
511 void
groebnerRow15_0100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)512 opengv::absolute_pose::modules::gpnp4::groebnerRow15_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
513 {
514 double factor = groebnerMatrix(targetRow,11) / groebnerMatrix(15,27);
515 groebnerMatrix(targetRow,11) = 0.0;
516 groebnerMatrix(targetRow,15) -= factor * groebnerMatrix(15,28);
517 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(15,29);
518 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(15,30);
519 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(15,31);
520 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(15,32);
521 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(15,33);
522 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(15,34);
523 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(15,35);
524 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(15,36);
525 }
526
527 void
groebnerRow15_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)528 opengv::absolute_pose::modules::gpnp4::groebnerRow15_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
529 {
530 double factor = groebnerMatrix(targetRow,17) / groebnerMatrix(15,27);
531 groebnerMatrix(targetRow,17) = 0.0;
532 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(15,28);
533 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(15,29);
534 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(15,30);
535 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(15,31);
536 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(15,32);
537 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(15,33);
538 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(15,34);
539 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(15,35);
540 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(15,36);
541 }
542
543 void
groebnerRow15_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)544 opengv::absolute_pose::modules::gpnp4::groebnerRow15_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
545 {
546 double factor = groebnerMatrix(targetRow,27) / groebnerMatrix(15,27);
547 groebnerMatrix(targetRow,27) = 0.0;
548 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(15,28);
549 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(15,29);
550 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(15,30);
551 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(15,31);
552 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(15,32);
553 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(15,33);
554 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(15,34);
555 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(15,35);
556 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(15,36);
557 }
558
559 void
groebnerRow12_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)560 opengv::absolute_pose::modules::gpnp4::groebnerRow12_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
561 {
562 double factor = groebnerMatrix(targetRow,1) / groebnerMatrix(12,19);
563 groebnerMatrix(targetRow,1) = 0.0;
564 groebnerMatrix(targetRow,2) -= factor * groebnerMatrix(12,20);
565 groebnerMatrix(targetRow,3) -= factor * groebnerMatrix(12,21);
566 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(12,27);
567 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(12,28);
568 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(12,29);
569 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(12,30);
570 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(12,31);
571 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(12,32);
572 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(12,33);
573 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(12,34);
574 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(12,35);
575 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(12,36);
576 }
577
578 void
groebnerRow16_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)579 opengv::absolute_pose::modules::gpnp4::groebnerRow16_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
580 {
581 double factor = groebnerMatrix(targetRow,18) / groebnerMatrix(16,28);
582 groebnerMatrix(targetRow,18) = 0.0;
583 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(16,29);
584 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(16,30);
585 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(16,31);
586 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(16,32);
587 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(16,33);
588 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(16,34);
589 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(16,35);
590 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(16,36);
591 }
592
593 void
groebnerRow16_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)594 opengv::absolute_pose::modules::gpnp4::groebnerRow16_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
595 {
596 double factor = groebnerMatrix(targetRow,28) / groebnerMatrix(16,28);
597 groebnerMatrix(targetRow,28) = 0.0;
598 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(16,29);
599 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(16,30);
600 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(16,31);
601 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(16,32);
602 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(16,33);
603 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(16,34);
604 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(16,35);
605 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(16,36);
606 }
607
608 void
groebnerRow14_0001_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)609 opengv::absolute_pose::modules::gpnp4::groebnerRow14_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
610 {
611 double factor = groebnerMatrix(targetRow,0) / groebnerMatrix(14,21);
612 groebnerMatrix(targetRow,0) = 0.0;
613 groebnerMatrix(targetRow,9) -= factor * groebnerMatrix(14,27);
614 groebnerMatrix(targetRow,12) -= factor * groebnerMatrix(14,28);
615 groebnerMatrix(targetRow,13) -= factor * groebnerMatrix(14,29);
616 groebnerMatrix(targetRow,15) -= factor * groebnerMatrix(14,30);
617 groebnerMatrix(targetRow,18) -= factor * groebnerMatrix(14,31);
618 groebnerMatrix(targetRow,22) -= factor * groebnerMatrix(14,32);
619 groebnerMatrix(targetRow,23) -= factor * groebnerMatrix(14,33);
620 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(14,34);
621 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(14,35);
622 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(14,36);
623 }
624
625 void
groebnerRow14_0010_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)626 opengv::absolute_pose::modules::gpnp4::groebnerRow14_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
627 {
628 double factor = groebnerMatrix(targetRow,1) / groebnerMatrix(14,21);
629 groebnerMatrix(targetRow,1) = 0.0;
630 groebnerMatrix(targetRow,10) -= factor * groebnerMatrix(14,27);
631 groebnerMatrix(targetRow,13) -= factor * groebnerMatrix(14,28);
632 groebnerMatrix(targetRow,14) -= factor * groebnerMatrix(14,29);
633 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(14,30);
634 groebnerMatrix(targetRow,19) -= factor * groebnerMatrix(14,31);
635 groebnerMatrix(targetRow,23) -= factor * groebnerMatrix(14,32);
636 groebnerMatrix(targetRow,24) -= factor * groebnerMatrix(14,33);
637 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(14,34);
638 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(14,35);
639 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(14,36);
640 }
641
642 void
groebnerRow17_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)643 opengv::absolute_pose::modules::gpnp4::groebnerRow17_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
644 {
645 double factor = groebnerMatrix(targetRow,19) / groebnerMatrix(17,29);
646 groebnerMatrix(targetRow,19) = 0.0;
647 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(17,30);
648 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(17,31);
649 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(17,32);
650 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(17,33);
651 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(17,34);
652 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(17,35);
653 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(17,36);
654 }
655
656 void
groebnerRow17_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)657 opengv::absolute_pose::modules::gpnp4::groebnerRow17_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
658 {
659 double factor = groebnerMatrix(targetRow,29) / groebnerMatrix(17,29);
660 groebnerMatrix(targetRow,29) = 0.0;
661 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(17,30);
662 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(17,31);
663 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(17,32);
664 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(17,33);
665 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(17,34);
666 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(17,35);
667 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(17,36);
668 }
669
670 void
groebnerRow18_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)671 opengv::absolute_pose::modules::gpnp4::groebnerRow18_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
672 {
673 double factor = groebnerMatrix(targetRow,30) / groebnerMatrix(18,30);
674 groebnerMatrix(targetRow,30) = 0.0;
675 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(18,31);
676 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(18,32);
677 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(18,33);
678 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(18,34);
679 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(18,35);
680 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(18,36);
681 }
682
683 void
groebnerRow18_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)684 opengv::absolute_pose::modules::gpnp4::groebnerRow18_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
685 {
686 double factor = groebnerMatrix(targetRow,20) / groebnerMatrix(18,30);
687 groebnerMatrix(targetRow,20) = 0.0;
688 groebnerMatrix(targetRow,21) -= factor * groebnerMatrix(18,31);
689 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(18,32);
690 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(18,33);
691 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(18,34);
692 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(18,35);
693 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(18,36);
694 }
695
696 void
groebnerRow19_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)697 opengv::absolute_pose::modules::gpnp4::groebnerRow19_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
698 {
699 double factor = groebnerMatrix(targetRow,21) / groebnerMatrix(19,31);
700 groebnerMatrix(targetRow,21) = 0.0;
701 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(19,32);
702 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(19,33);
703 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(19,34);
704 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(19,35);
705 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(19,36);
706 }
707
708 void
groebnerRow19_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)709 opengv::absolute_pose::modules::gpnp4::groebnerRow19_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
710 {
711 double factor = groebnerMatrix(targetRow,31) / groebnerMatrix(19,31);
712 groebnerMatrix(targetRow,31) = 0.0;
713 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(19,32);
714 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(19,33);
715 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(19,34);
716 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(19,35);
717 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(19,36);
718 }
719
720 void
groebnerRow20_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)721 opengv::absolute_pose::modules::gpnp4::groebnerRow20_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
722 {
723 double factor = groebnerMatrix(targetRow,28) / groebnerMatrix(20,32);
724 groebnerMatrix(targetRow,28) = 0.0;
725 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(20,33);
726 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(20,34);
727 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(20,35);
728 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(20,36);
729 }
730
731 void
groebnerRow20_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)732 opengv::absolute_pose::modules::gpnp4::groebnerRow20_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
733 {
734 double factor = groebnerMatrix(targetRow,32) / groebnerMatrix(20,32);
735 groebnerMatrix(targetRow,32) = 0.0;
736 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(20,33);
737 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(20,34);
738 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(20,35);
739 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(20,36);
740 }
741
742 void
groebnerRow19_0001_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)743 opengv::absolute_pose::modules::gpnp4::groebnerRow19_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
744 {
745 double factor = groebnerMatrix(targetRow,18) / groebnerMatrix(19,31);
746 groebnerMatrix(targetRow,18) = 0.0;
747 groebnerMatrix(targetRow,22) -= factor * groebnerMatrix(19,32);
748 groebnerMatrix(targetRow,23) -= factor * groebnerMatrix(19,33);
749 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(19,34);
750 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(19,35);
751 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(19,36);
752 }
753
754 void
groebnerRow19_0010_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)755 opengv::absolute_pose::modules::gpnp4::groebnerRow19_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
756 {
757 double factor = groebnerMatrix(targetRow,19) / groebnerMatrix(19,31);
758 groebnerMatrix(targetRow,19) = 0.0;
759 groebnerMatrix(targetRow,23) -= factor * groebnerMatrix(19,32);
760 groebnerMatrix(targetRow,24) -= factor * groebnerMatrix(19,33);
761 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(19,34);
762 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(19,35);
763 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(19,36);
764 }
765
766 void
groebnerRow20_0001_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)767 opengv::absolute_pose::modules::gpnp4::groebnerRow20_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
768 {
769 double factor = groebnerMatrix(targetRow,22) / groebnerMatrix(20,32);
770 groebnerMatrix(targetRow,22) = 0.0;
771 groebnerMatrix(targetRow,23) -= factor * groebnerMatrix(20,33);
772 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(20,34);
773 groebnerMatrix(targetRow,28) -= factor * groebnerMatrix(20,35);
774 groebnerMatrix(targetRow,32) -= factor * groebnerMatrix(20,36);
775 }
776
777 void
groebnerRow20_0010_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)778 opengv::absolute_pose::modules::gpnp4::groebnerRow20_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
779 {
780 double factor = groebnerMatrix(targetRow,23) / groebnerMatrix(20,32);
781 groebnerMatrix(targetRow,23) = 0.0;
782 groebnerMatrix(targetRow,24) -= factor * groebnerMatrix(20,33);
783 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(20,34);
784 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(20,35);
785 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(20,36);
786 }
787
788 void
groebnerRow21_0010_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)789 opengv::absolute_pose::modules::gpnp4::groebnerRow21_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
790 {
791 double factor = groebnerMatrix(targetRow,24) / groebnerMatrix(21,33);
792 groebnerMatrix(targetRow,24) = 0.0;
793 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(21,34);
794 groebnerMatrix(targetRow,29) -= factor * groebnerMatrix(21,35);
795 groebnerMatrix(targetRow,33) -= factor * groebnerMatrix(21,36);
796 }
797
798 void
groebnerRow20_0100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)799 opengv::absolute_pose::modules::gpnp4::groebnerRow20_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
800 {
801 double factor = groebnerMatrix(targetRow,25) / groebnerMatrix(20,32);
802 groebnerMatrix(targetRow,25) = 0.0;
803 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(20,33);
804 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(20,34);
805 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(20,35);
806 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(20,36);
807 }
808
809 void
groebnerRow21_0100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)810 opengv::absolute_pose::modules::gpnp4::groebnerRow21_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
811 {
812 double factor = groebnerMatrix(targetRow,26) / groebnerMatrix(21,33);
813 groebnerMatrix(targetRow,26) = 0.0;
814 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(21,34);
815 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(21,35);
816 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(21,36);
817 }
818
819 void
groebnerRow21_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)820 opengv::absolute_pose::modules::gpnp4::groebnerRow21_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
821 {
822 double factor = groebnerMatrix(targetRow,29) / groebnerMatrix(21,33);
823 groebnerMatrix(targetRow,29) = 0.0;
824 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(21,34);
825 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(21,35);
826 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(21,36);
827 }
828
829 void
groebnerRow21_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)830 opengv::absolute_pose::modules::gpnp4::groebnerRow21_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
831 {
832 double factor = groebnerMatrix(targetRow,33) / groebnerMatrix(21,33);
833 groebnerMatrix(targetRow,33) = 0.0;
834 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(21,34);
835 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(21,35);
836 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(21,36);
837 }
838
839 void
groebnerRow20_1100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)840 opengv::absolute_pose::modules::gpnp4::groebnerRow20_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
841 {
842 double factor = groebnerMatrix(targetRow,15) / groebnerMatrix(20,32);
843 groebnerMatrix(targetRow,15) = 0.0;
844 groebnerMatrix(targetRow,16) -= factor * groebnerMatrix(20,33);
845 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(20,34);
846 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(20,35);
847 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(20,36);
848 }
849
850 void
groebnerRow21_1100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)851 opengv::absolute_pose::modules::gpnp4::groebnerRow21_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
852 {
853 double factor = groebnerMatrix(targetRow,16) / groebnerMatrix(21,33);
854 groebnerMatrix(targetRow,16) = 0.0;
855 groebnerMatrix(targetRow,17) -= factor * groebnerMatrix(21,34);
856 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(21,35);
857 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(21,36);
858 }
859
860 void
groebnerRow22_1100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)861 opengv::absolute_pose::modules::gpnp4::groebnerRow22_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
862 {
863 double factor = groebnerMatrix(targetRow,17) / groebnerMatrix(22,34);
864 groebnerMatrix(targetRow,17) = 0.0;
865 groebnerMatrix(targetRow,20) -= factor * groebnerMatrix(22,35);
866 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(22,36);
867 }
868
869 void
groebnerRow19_0100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)870 opengv::absolute_pose::modules::gpnp4::groebnerRow19_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
871 {
872 double factor = groebnerMatrix(targetRow,20) / groebnerMatrix(19,31);
873 groebnerMatrix(targetRow,20) = 0.0;
874 groebnerMatrix(targetRow,25) -= factor * groebnerMatrix(19,32);
875 groebnerMatrix(targetRow,26) -= factor * groebnerMatrix(19,33);
876 groebnerMatrix(targetRow,27) -= factor * groebnerMatrix(19,34);
877 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(19,35);
878 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(19,36);
879 }
880
881 void
groebnerRow22_0100_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)882 opengv::absolute_pose::modules::gpnp4::groebnerRow22_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
883 {
884 double factor = groebnerMatrix(targetRow,27) / groebnerMatrix(22,34);
885 groebnerMatrix(targetRow,27) = 0.0;
886 groebnerMatrix(targetRow,30) -= factor * groebnerMatrix(22,35);
887 groebnerMatrix(targetRow,34) -= factor * groebnerMatrix(22,36);
888 }
889
890 void
groebnerRow22_1000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)891 opengv::absolute_pose::modules::gpnp4::groebnerRow22_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
892 {
893 double factor = groebnerMatrix(targetRow,30) / groebnerMatrix(22,34);
894 groebnerMatrix(targetRow,30) = 0.0;
895 groebnerMatrix(targetRow,31) -= factor * groebnerMatrix(22,35);
896 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(22,36);
897 }
898
899 void
groebnerRow22_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)900 opengv::absolute_pose::modules::gpnp4::groebnerRow22_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
901 {
902 double factor = groebnerMatrix(targetRow,34) / groebnerMatrix(22,34);
903 groebnerMatrix(targetRow,34) = 0.0;
904 groebnerMatrix(targetRow,35) -= factor * groebnerMatrix(22,35);
905 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(22,36);
906 }
907
908 void
groebnerRow23_0000_f(Eigen::Matrix<double,25,37> & groebnerMatrix,int targetRow)909 opengv::absolute_pose::modules::gpnp4::groebnerRow23_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow )
910 {
911 double factor = groebnerMatrix(targetRow,35) / groebnerMatrix(23,35);
912 groebnerMatrix(targetRow,35) = 0.0;
913 groebnerMatrix(targetRow,36) -= factor * groebnerMatrix(23,36);
914 }
915
916