1 #include <math.h>
2 #include <stdio.h>
3
4 #include "apriltag_pose.h"
5 #include "apriltag_math.h"
6 #include "common/homography.h"
7 #include "common/image_u8x3.h"
8
9
10 /**
11 * Calculate projection operator from image points.
12 */
calculate_F(matd_t * v)13 matd_t* calculate_F(matd_t* v) {
14 matd_t* outer_product = matd_op("MM'", v, v, v, v);
15 matd_t* inner_product = matd_op("M'M", v, v);
16 matd_scale_inplace(outer_product, 1.0/inner_product->data[0]);
17 matd_destroy(inner_product);
18 return outer_product;
19 }
20
21 /**
22 * Returns the value of the supplied scalar matrix 'a' and destroys the matrix.
23 */
matd_to_double(matd_t * a)24 double matd_to_double(matd_t *a)
25 {
26 assert(matd_is_scalar(a));
27 double d = a->data[0];
28 matd_destroy(a);
29 return d;
30 }
31
32 /**
33 * @param v Image points on the image plane.
34 * @param p Object points in object space.
35 * @outparam t Optimal translation.
36 * @param R In/Outparam. Should be set to initial guess at R. Will be modified to be the optimal translation.
37 * @param n_points Number of points.
38 * @param n_steps Number of iterations.
39 *
40 * @return Object-space error after iteration.
41 *
42 * Implementation of Orthogonal Iteration from Lu, 2000.
43 */
orthogonal_iteration(matd_t ** v,matd_t ** p,matd_t ** t,matd_t ** R,int n_points,int n_steps)44 double orthogonal_iteration(matd_t** v, matd_t** p, matd_t** t, matd_t** R, int n_points, int n_steps) {
45 matd_t* p_mean = matd_create(3, 1);
46 for (int i = 0; i < n_points; i++) {
47 matd_add_inplace(p_mean, p[i]);
48 }
49 matd_scale_inplace(p_mean, 1.0/n_points);
50
51 matd_t** p_res = (matd_t**)malloc(sizeof(matd_t *)*n_points);
52 for (int i = 0; i < n_points; i++) {
53 p_res[i] = matd_op("M-M", p[i], p_mean);
54 }
55
56 // Compute M1_inv.
57 matd_t** F = (matd_t**)malloc(sizeof(matd_t *)*n_points);
58 matd_t *avg_F = matd_create(3, 3);
59 for (int i = 0; i < n_points; i++) {
60 F[i] = calculate_F(v[i]);
61 matd_add_inplace(avg_F, F[i]);
62 }
63 matd_scale_inplace(avg_F, 1.0/n_points);
64 matd_t *I3 = matd_identity(3);
65 matd_t *M1 = matd_subtract(I3, avg_F);
66 matd_t *M1_inv = matd_inverse(M1);
67 matd_destroy(avg_F);
68 matd_destroy(M1);
69
70 double prev_error = HUGE_VAL;
71 // Iterate.
72 for (int i = 0; i < n_steps; i++) {
73 // Calculate translation.
74 matd_t *M2 = matd_create(3, 1);
75 for (int j = 0; j < n_points; j++) {
76 matd_t* M2_update = matd_op("(M - M)*M*M", F[j], I3, *R, p[j]);
77 matd_add_inplace(M2, M2_update);
78 matd_destroy(M2_update);
79 }
80 matd_scale_inplace(M2, 1.0/n_points);
81 matd_destroy(*t);
82 *t = matd_multiply(M1_inv, M2);
83 matd_destroy(M2);
84
85 // Calculate rotation.
86 matd_t** q = (matd_t**)malloc(sizeof(matd_t *)*n_points);
87 matd_t* q_mean = matd_create(3, 1);
88 for (int j = 0; j < n_points; j++) {
89 q[j] = matd_op("M*(M*M+M)", F[j], *R, p[j], *t);
90 matd_add_inplace(q_mean, q[j]);
91 }
92 matd_scale_inplace(q_mean, 1.0/n_points);
93
94 matd_t* M3 = matd_create(3, 3);
95 for (int j = 0; j < n_points; j++) {
96 matd_t *M3_update = matd_op("(M-M)*M'", q[j], q_mean, p_res[j]);
97 matd_add_inplace(M3, M3_update);
98 matd_destroy(M3_update);
99 }
100 matd_svd_t M3_svd = matd_svd(M3);
101 matd_destroy(M3);
102 matd_destroy(*R);
103 *R = matd_op("M*M'", M3_svd.U, M3_svd.V);
104 matd_destroy(M3_svd.U);
105 matd_destroy(M3_svd.S);
106 matd_destroy(M3_svd.V);
107 matd_destroy(q_mean);
108 for (int j = 0; j < n_points; j++) {
109 matd_destroy(q[j]);
110 }
111
112 double error = 0;
113 for (int i = 0; i < 4; i++) {
114 matd_t* err_vec = matd_op("(M-M)(MM+M)", I3, F[i], *R, p[i], *t);
115 error += matd_to_double(matd_op("M'M", err_vec, err_vec));
116 matd_destroy(err_vec);
117 }
118 prev_error = error;
119
120 free(q);
121 }
122
123 matd_destroy(I3);
124 matd_destroy(M1_inv);
125 for (int i = 0; i < n_points; i++) {
126 matd_destroy(p_res[i]);
127 matd_destroy(F[i]);
128 }
129 free(p_res);
130 free(F);
131 matd_destroy(p_mean);
132 return prev_error;
133 }
134
135 /**
136 * Evaluates polynomial p at x.
137 */
polyval(double * p,int degree,double x)138 double polyval(double* p, int degree, double x) {
139 double ret = 0;
140 for (int i = 0; i <= degree; i++) {
141 ret += p[i]*pow(x, i);
142 }
143 return ret;
144 }
145
146 /**
147 * Numerically solve small degree polynomials. This is a customized method. It
148 * ignores roots larger than 1000 and only gives small roots approximately.
149 *
150 * @param p Array of parameters s.t. p(x) = p[0] + p[1]*x + ...
151 * @param degree The degree of p(x).
152 * @outparam roots
153 * @outparam n_roots
154 */
solve_poly_approx(double * p,int degree,double * roots,int * n_roots)155 void solve_poly_approx(double* p, int degree, double* roots, int* n_roots) {
156 static const int MAX_ROOT = 1000;
157 if (degree == 1) {
158 if (fabs(p[0]) > MAX_ROOT*fabs(p[1])) {
159 *n_roots = 0;
160 } else {
161 roots[0] = -p[0]/p[1];
162 *n_roots = 1;
163 }
164 return;
165 }
166
167 // Calculate roots of derivative.
168 double *p_der = (double *)malloc(sizeof(double)*degree);
169 for (int i = 0; i < degree; i++) {
170 p_der[i] = (i + 1) * p[i+1];
171 }
172
173 double *der_roots = (double *)malloc(sizeof(double)*(degree - 1));
174 int n_der_roots;
175 solve_poly_approx(p_der, degree - 1, der_roots, &n_der_roots);
176
177
178 // Go through all possibilities for roots of the polynomial.
179 *n_roots = 0;
180 for (int i = 0; i <= n_der_roots; i++) {
181 double min;
182 if (i == 0) {
183 min = -MAX_ROOT;
184 } else {
185 min = der_roots[i - 1];
186 }
187
188 double max;
189 if (i == n_der_roots) {
190 max = MAX_ROOT;
191 } else {
192 max = der_roots[i];
193 }
194
195 if (polyval(p, degree, min)*polyval(p, degree, max) < 0) {
196 // We have a zero-crossing in this interval, use a combination of Newton' and bisection.
197 // Some thanks to Numerical Recipes in C.
198
199 double lower;
200 double upper;
201 if (polyval(p, degree, min) < polyval(p, degree, max)) {
202 lower = min;
203 upper = max;
204 } else {
205 lower = max;
206 upper = min;
207 }
208 double root = 0.5*(lower + upper);
209 double dx_old = upper - lower;
210 double dx = dx_old;
211 double f = polyval(p, degree, root);
212 double df = polyval(p_der, degree - 1, root);
213
214 for (int j = 0; j < 100; j++) {
215 if (((f + df*(upper - root))*(f + df*(lower - root)) > 0)
216 || (fabs(2*f) > fabs(dx_old*df))) {
217 dx_old = dx;
218 dx = 0.5*(upper - lower);
219 root = lower + dx;
220 } else {
221 dx_old = dx;
222 dx = -f/df;
223 root += dx;
224 }
225
226 if (root == upper || root == lower) {
227 break;
228 }
229
230 f = polyval(p, degree, root);
231 df = polyval(p_der, degree - 1, root);
232
233 if (f > 0) {
234 upper = root;
235 } else {
236 lower = root;
237 }
238 }
239
240 roots[(*n_roots)++] = root;
241 } else if(polyval(p, degree, max) == 0) {
242 // Double/triple root.
243 roots[(*n_roots)++] = max;
244 }
245 }
246
247 free(der_roots);
248 free(p_der);
249 }
250
251 /**
252 * Given a local minima of the pose error tries to find the other minima.
253 */
fix_pose_ambiguities(matd_t ** v,matd_t ** p,matd_t * t,matd_t * R,int n_points)254 matd_t* fix_pose_ambiguities(matd_t** v, matd_t** p, matd_t* t, matd_t* R, int n_points) {
255 matd_t* I3 = matd_identity(3);
256
257 // 1. Find R_t
258 matd_t* R_t_3 = matd_vec_normalize(t);
259
260 matd_t* e_x = matd_create(3, 1);
261 MATD_EL(e_x, 0, 0) = 1;
262 matd_t* R_t_1_tmp = matd_op("M-(M'*M)*M", e_x, e_x, R_t_3, R_t_3);
263 matd_t* R_t_1 = matd_vec_normalize(R_t_1_tmp);
264 matd_destroy(e_x);
265 matd_destroy(R_t_1_tmp);
266
267 matd_t* R_t_2 = matd_crossproduct(R_t_3, R_t_1);
268
269 double data_[] = {MATD_EL(R_t_1, 0, 0), MATD_EL(R_t_1, 0, 1), MATD_EL(R_t_1, 0, 2),
270 MATD_EL(R_t_2, 0, 0), MATD_EL(R_t_2, 0, 1), MATD_EL(R_t_2, 0, 2),
271 MATD_EL(R_t_3, 0, 0), MATD_EL(R_t_3, 0, 1), MATD_EL(R_t_3, 0, 2)};
272 matd_t* R_t = matd_create_data(3, 3, data_);
273 matd_destroy(R_t_1);
274 matd_destroy(R_t_2);
275 matd_destroy(R_t_3);
276
277 // 2. Find R_z
278 matd_t* R_1_prime = matd_multiply(R_t, R);
279 double r31 = MATD_EL(R_1_prime, 2, 0);
280 double r32 = MATD_EL(R_1_prime, 2, 1);
281 double hypotenuse = sqrt(r31*r31 + r32*r32);
282 if (hypotenuse < 1e-100) {
283 r31 = 1;
284 r32 = 0;
285 hypotenuse = 1;
286 }
287 double data_R_z[] = {r31/hypotenuse, -r32/hypotenuse, 0,
288 r32/hypotenuse, r31/hypotenuse, 0,
289 0, 0, 1};
290 matd_t* R_z = matd_create_data(3, 3, data_R_z);
291
292 // 3. Calculate parameters of Eos
293 matd_t* R_trans = matd_multiply(R_1_prime, R_z);
294 double sin_gamma = -MATD_EL(R_trans, 0, 1);
295 double cos_gamma = MATD_EL(R_trans, 1, 1);
296 double data_R_gamma[] = {cos_gamma, -sin_gamma, 0,
297 sin_gamma, cos_gamma, 0,
298 0, 0, 1};
299 matd_t* R_gamma = matd_create_data(3, 3, data_R_gamma);
300
301 double sin_beta = -MATD_EL(R_trans, 2, 0);
302 double cos_beta = MATD_EL(R_trans, 2, 2);
303 double t_initial = atan2(sin_beta, cos_beta);
304 matd_destroy(R_trans);
305
306 matd_t** v_trans = (matd_t**)malloc(sizeof(matd_t *)*n_points);
307 matd_t** p_trans = (matd_t**)malloc(sizeof(matd_t *)*n_points);
308 matd_t** F_trans = (matd_t**)malloc(sizeof(matd_t *)*n_points);
309 matd_t* avg_F_trans = matd_create(3, 3);
310 for (int i = 0; i < n_points; i++) {
311 p_trans[i] = matd_op("M'*M", R_z, p[i]);
312 v_trans[i] = matd_op("M*M", R_t, v[i]);
313 F_trans[i] = calculate_F(v_trans[i]);
314 matd_add_inplace(avg_F_trans, F_trans[i]);
315 }
316 matd_scale_inplace(avg_F_trans, 1.0/n_points);
317
318 matd_t* G = matd_op("(M-M)^-1", I3, avg_F_trans);
319 matd_scale_inplace(G, 1.0/n_points);
320
321 double data_M1[] = {0, 0, 2,
322 0, 0, 0,
323 -2, 0, 0};
324 matd_t* M1 = matd_create_data(3, 3, data_M1);
325 double data_M2[] = {-1, 0, 0,
326 0, 1, 0,
327 0, 0, -1};
328 matd_t* M2 = matd_create_data(3, 3, data_M2);
329
330 matd_t* b0 = matd_create(3, 1);
331 matd_t* b1 = matd_create(3, 1);
332 matd_t* b2 = matd_create(3, 1);
333 for (int i = 0; i < n_points; i++) {
334 matd_t* op_tmp1 = matd_op("(M-M)MM", F_trans[i], I3, R_gamma, p_trans[i]);
335 matd_t* op_tmp2 = matd_op("(M-M)MMM", F_trans[i], I3, R_gamma, M1, p_trans[i]);
336 matd_t* op_tmp3 = matd_op("(M-M)MMM", F_trans[i], I3, R_gamma, M2, p_trans[i]);
337
338 matd_add_inplace(b0, op_tmp1);
339 matd_add_inplace(b1, op_tmp2);
340 matd_add_inplace(b2, op_tmp3);
341
342 matd_destroy(op_tmp1);
343 matd_destroy(op_tmp2);
344 matd_destroy(op_tmp3);
345 }
346 matd_t* b0_ = matd_multiply(G, b0);
347 matd_t* b1_ = matd_multiply(G, b1);
348 matd_t* b2_ = matd_multiply(G, b2);
349
350 double a0 = 0;
351 double a1 = 0;
352 double a2 = 0;
353 double a3 = 0;
354 double a4 = 0;
355 for (int i = 0; i < n_points; i++) {
356 matd_t* c0 = matd_op("(M-M)(MM+M)", I3, F_trans[i], R_gamma, p_trans[i], b0_);
357 matd_t* c1 = matd_op("(M-M)(MMM+M)", I3, F_trans[i], R_gamma, M1, p_trans[i], b1_);
358 matd_t* c2 = matd_op("(M-M)(MMM+M)", I3, F_trans[i], R_gamma, M2, p_trans[i], b2_);
359
360 a0 += matd_to_double(matd_op("M'M", c0, c0));
361 a1 += matd_to_double(matd_op("2M'M", c0, c1));
362 a2 += matd_to_double(matd_op("M'M+2M'M", c1, c1, c0, c2));
363 a3 += matd_to_double(matd_op("2M'M", c1, c2));
364 a4 += matd_to_double(matd_op("M'M", c2, c2));
365
366 matd_destroy(c0);
367 matd_destroy(c1);
368 matd_destroy(c2);
369 }
370
371 matd_destroy(b0);
372 matd_destroy(b1);
373 matd_destroy(b2);
374 matd_destroy(b0_);
375 matd_destroy(b1_);
376 matd_destroy(b2_);
377
378 for (int i = 0; i < n_points; i++) {
379 matd_destroy(p_trans[i]);
380 matd_destroy(v_trans[i]);
381 matd_destroy(F_trans[i]);
382 }
383 free(p_trans);
384 free(v_trans);
385 free(F_trans);
386 matd_destroy(avg_F_trans);
387 matd_destroy(G);
388
389
390 // 4. Solve for minima of Eos.
391 double p0 = a1;
392 double p1 = 2*a2 - 4*a0;
393 double p2 = 3*a3 - 3*a1;
394 double p3 = 4*a4 - 2*a2;
395 double p4 = -a3;
396
397 double roots[4];
398 int n_roots;
399 double data_points[] = {p0, p1, p2, p3, p4};
400 solve_poly_approx(data_points, 4, roots, &n_roots);
401
402 double minima[4];
403 int n_minima = 0;
404 for (int i = 0; i < n_roots; i++) {
405 double t1 = roots[i];
406 double t2 = t1*t1;
407 double t3 = t1*t2;
408 double t4 = t1*t3;
409 double t5 = t1*t4;
410 // Check extrema is a minima.
411 if (a2 - 2*a0 + (3*a3 - 6*a1)*t1 + (6*a4 - 8*a2 + 10*a0)*t2 + (-8*a3 + 6*a1)*t3 + (-6*a4 + 3*a2)*t4 + a3*t5 >= 0) {
412 // And that it corresponds to an angle different than the known minimum.
413 double t = 2*atan(roots[i]);
414 // We only care about finding a second local minima which is qualitatively
415 // different than the first.
416 if (fabs(t - t_initial) > 0.1) {
417 minima[n_minima++] = roots[i];
418 }
419 }
420 }
421
422 // 5. Get poses for minima.
423 matd_t* ret = NULL;
424 if (n_minima == 1) {
425 double t = minima[0];
426 matd_t* R_beta = matd_copy(M2);
427 matd_scale_inplace(R_beta, t);
428 matd_add_inplace(R_beta, M1);
429 matd_scale_inplace(R_beta, t);
430 matd_add_inplace(R_beta, I3);
431 matd_scale_inplace(R_beta, 1/(1 + t*t));
432 ret = matd_op("M'MMM'", R_t, R_gamma, R_beta, R_z);
433 matd_destroy(R_beta);
434 } else if (n_minima > 1) {
435 // This can happen if our prior pose estimate was not very good.
436 //fprintf(stderr, "Error, more than one new minima found.\n");
437 }
438 matd_destroy(I3);
439 matd_destroy(M1);
440 matd_destroy(M2);
441 matd_destroy(R_t);
442 matd_destroy(R_gamma);
443 matd_destroy(R_z);
444 matd_destroy(R_1_prime);
445 return ret;
446 }
447
448 /**
449 * Estimate pose of the tag using the homography method.
450 */
estimate_pose_for_tag_homography(apriltag_detection_info_t * info,apriltag_pose_t * solution)451 void estimate_pose_for_tag_homography(apriltag_detection_info_t* info, apriltag_pose_t* solution) {
452 double scale = info->tagsize/2.0;
453
454 matd_t *M_H = homography_to_pose(info->det->H, -info->fx, info->fy, info->cx, info->cy);
455 MATD_EL(M_H, 0, 3) *= scale;
456 MATD_EL(M_H, 1, 3) *= scale;
457 MATD_EL(M_H, 2, 3) *= scale;
458
459 matd_t* fix = matd_create(4, 4);
460 MATD_EL(fix, 0, 0) = 1;
461 MATD_EL(fix, 1, 1) = -1;
462 MATD_EL(fix, 2, 2) = -1;
463 MATD_EL(fix, 3, 3) = 1;
464
465 matd_t* initial_pose = matd_multiply(fix, M_H);
466 matd_destroy(M_H);
467 matd_destroy(fix);
468
469 solution->R = matd_create(3, 3);
470 for (unsigned int i = 0; i < 3; i++) {
471 for (unsigned int j = 0; j < 3; j++) {
472 MATD_EL(solution->R, i, j) = MATD_EL(initial_pose, i, j);
473 }
474 }
475
476 solution->t = matd_create(3, 1);
477 for (unsigned int i = 0; i < 3; i++) {
478 MATD_EL(solution->t, i, 0) = MATD_EL(initial_pose, i, 3);
479 }
480 matd_destroy(initial_pose);
481 }
482
483 /**
484 * Estimate tag pose using orthogonal iteration.
485 */
estimate_tag_pose_orthogonal_iteration(apriltag_detection_info_t * info,double * err1,apriltag_pose_t * solution1,double * err2,apriltag_pose_t * solution2,int nIters)486 void estimate_tag_pose_orthogonal_iteration(
487 apriltag_detection_info_t* info,
488 double* err1,
489 apriltag_pose_t* solution1,
490 double* err2,
491 apriltag_pose_t* solution2,
492 int nIters) {
493 double scale = info->tagsize/2.0;
494 double data_p0[] = {-scale, scale, 0};
495 double data_p1[] = {scale, scale, 0};
496 double data_p2[] = {scale, -scale, 0};
497 double data_p3[] = {-scale, -scale, 0};
498 matd_t* p[4] = {
499 matd_create_data(3, 1, data_p0),
500 matd_create_data(3, 1, data_p1),
501 matd_create_data(3, 1, data_p2),
502 matd_create_data(3, 1, data_p3)};
503 matd_t* v[4];
504 for (int i = 0; i < 4; i++) {
505 double data_v[] = {(info->det->p[i][0] - info->cx)/info->fx, (info->det->p[i][1] - info->cy)/info->fy, 1};
506 v[i] = matd_create_data(3, 1, data_v);
507 }
508
509 estimate_pose_for_tag_homography(info, solution1);
510 *err1 = orthogonal_iteration(v, p, &solution1->t, &solution1->R, 4, nIters);
511 // solution2->R = fix_pose_ambiguities(v, p, solution1->t, solution1->R, 4);
512 // if (solution2->R) {
513 // solution2->t = matd_create(3, 1);
514 // *err2 = orthogonal_iteration(v, p, &solution2->t, &solution2->R, 4, nIters);
515 // } else {
516 // *err2 = HUGE_VAL;
517 // }
518 get_second_solution(v, p, solution1, solution2, nIters, err2);
519
520 for (int i = 0; i < 4; i++) {
521 matd_destroy(p[i]);
522 matd_destroy(v[i]);
523 }
524 }
525
526 /**
527 * Estimate tag pose.
528 */
estimate_tag_pose(apriltag_detection_info_t * info,apriltag_pose_t * pose)529 double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose) {
530 double err1, err2;
531 apriltag_pose_t pose1, pose2;
532 estimate_tag_pose_orthogonal_iteration(info, &err1, &pose1, &err2, &pose2, 50);
533 if (err1 <= err2) {
534 pose->R = pose1.R;
535 pose->t = pose1.t;
536 if (pose2.R) {
537 matd_destroy(pose2.t);
538 }
539 matd_destroy(pose2.R);
540 return err1;
541 } else {
542 pose->R = pose2.R;
543 pose->t = pose2.t;
544 matd_destroy(pose1.R);
545 matd_destroy(pose1.t);
546 return err2;
547 }
548 }
549
get_second_solution(matd_t * v[4],matd_t * p[4],apriltag_pose_t * solution1,apriltag_pose_t * solution2,int nIters,double * err2)550 void get_second_solution(matd_t* v[4], matd_t* p[4], apriltag_pose_t* solution1, apriltag_pose_t* solution2, int nIters, double* err2) {
551 solution2->R = fix_pose_ambiguities(v, p, solution1->t, solution1->R, 4);
552 if (solution2->R) {
553 solution2->t = matd_create(3, 1);
554 *err2 = orthogonal_iteration(v, p, &solution2->t, &solution2->R, 4, nIters);
555 } else {
556 *err2 = HUGE_VAL;
557 }
558 }
559