1 #include "orb_algos.h"
2 #include <stdlib.h>
3 #include <stdio.h>
4 #include <string.h>
5 #include <math.h>
6 #include "xcam_log.h"
7
8 #define EPS 1e-10
9
10 #include <time.h>
11 double begin_tick;
12 double end_tick;
13
getTime()14 unsigned long getTime()
15 {
16 struct timespec ts;
17 clock_gettime(1, &ts);
18 return (ts.tv_sec * 1000 + ts.tv_nsec / 1000000);
19 }
20
work_begin()21 void work_begin()
22 {
23 begin_tick = getTime();
24 }
25
work_end(const char * module_name)26 void work_end(const char* module_name)
27 {
28 end_tick = getTime() - begin_tick;
29 LOGE_AORB("[%s] TIME = %lf ms \n", module_name, end_tick);
30 }
31
initList(U32 bytes)32 ORBList* initList(U32 bytes) {
33 ORBList* list = (ORBList*)malloc(sizeof(ORBList));
34 list->bytes = bytes;
35 list->start = NULL;
36 list->end = NULL;
37 list->length = 0;
38 return list;
39 }
40
freeList(ORBList * list)41 void freeList(ORBList* list) {
42 if (list != NULL) {
43 for(Node* item = list->start; item != NULL; item = item->next) {
44 if (item->data != NULL) {
45 free(item->data);
46 item->data = NULL;
47 }
48 }
49 free(list);
50 list = NULL;
51 }
52 }
53
push(ORBList * list,void * data)54 int push(ORBList* list, void* data) {
55
56 Node* node = (Node*)malloc(sizeof(Node));
57 node->data = malloc(list->bytes);
58 node->next = NULL;
59 memcpy(node->data, data, list->bytes);
60 if (list->start == NULL) {
61 list->start = node;
62 list->end = node;
63 } else {
64 list->end->next = node;
65 list->end = node;
66 }
67
68 list->length++;
69 return list->length;
70 }
71
init_matchpoints(orb_matched_point_t * point,U32 row1,U32 col1,U32 row2,U32 col2,U32 score)72 void init_matchpoints(orb_matched_point_t* point, U32 row1, U32 col1, U32 row2, U32 col2, U32 score) {
73 if (point == NULL) {
74 return;
75 }
76
77 point->col1 = col1;
78 point->row1 = row1;
79 point->col2 = col2;
80 point->row2 = row2;
81 point->score = score;
82 }
83
get_roi_points_list(rk_aiq_orb_algo_stat_t * keypoints,orb_rect_t roi)84 ORBList* get_roi_points_list (rk_aiq_orb_algo_stat_t* keypoints, orb_rect_t roi)
85 {
86 ORBList* roi_points_list = initList(sizeof(rk_aiq_orb_featue_point));
87
88 for (unsigned int i = 0; i < keypoints->num_points; i++) {
89 rk_aiq_orb_featue_point* point = keypoints->points + i;
90 if (point->x < roi.left ||
91 point->x > roi.right ||
92 point->y < roi.top ||
93 point->y > roi.bottom) {
94 continue;
95 }
96
97 push(roi_points_list, point);
98 }
99
100 #if 1
101 FILE* fp = fopen("/tmp/points0.txt", "wb");
102 for (unsigned int i = 0; i < keypoints->num_points; i++) {
103 rk_aiq_orb_featue_point* point = keypoints->points + i;
104 fprintf(fp, "%d, %d\n", point->x, point->y);
105 }
106 fflush(fp);
107 fclose(fp);
108 #endif
109 return roi_points_list;
110 }
111
matching(ORBList * roi_points_list,rk_aiq_orb_algo_stat_t * keypoints2,orb_rect_t roi)112 ORBList* matching(ORBList* roi_points_list, rk_aiq_orb_algo_stat_t* keypoints2, orb_rect_t roi) {
113 orb_matched_point_t keypoint{};
114 ORBList* matched_list = initList(sizeof(orb_matched_point_t));
115
116 for(Node* point = roi_points_list->start; point != NULL; point = point->next) {
117 rk_aiq_orb_featue_point* descriptor1 = (rk_aiq_orb_featue_point*)point->data;
118 keypoint.score = 0;
119
120 for (unsigned int j = 0; j < keypoints2->num_points; j++) {
121 rk_aiq_orb_featue_point* descriptor2 = keypoints2->points + j;
122
123 unsigned int success_num = 120;
124 for(int k = 0; k < DESCRIPTOR_SIZE; k++) {
125 U8 xor_val = descriptor1->brief[k] ^ descriptor2->brief[k];
126 while (xor_val) {
127 if (xor_val & 1) {
128 success_num--;
129 }
130 xor_val = xor_val >> 1;
131 }
132 }
133
134 if(success_num > 110) { //120
135 if (success_num > keypoint.score) {
136 init_matchpoints(&keypoint,
137 descriptor1->y, descriptor1->x,
138 descriptor2->y, descriptor2->x,
139 success_num);
140 }
141
142 //orb_matched_point_t* f = (orb_matched_point_t*)(matched_list->start);
143 //LOGE_AORB("found match: (%d-%d)-(%d-%d)",
144 // f->col1, f->row1, f->col2, f->row2);
145 }
146 }
147
148 if (keypoint.score > 0) {
149 push(matched_list, &keypoint);
150 }
151
152 if (matched_list->length > 30) {
153 break;
154 }
155 }
156
157 #if 1
158 FILE* fp = fopen("/tmp/points1.txt", "wb");
159 for (unsigned int i = 0; i < keypoints2->num_points; i++) {
160 rk_aiq_orb_featue_point* point = keypoints2->points + i;
161 fprintf(fp, "%d, %d\n", point->x, point->y);
162 }
163 fflush(fp);
164 fclose(fp);
165 #endif
166
167 return matched_list;
168 }
169
gaussian_elimination(double * input,int n)170 void gaussian_elimination(double *input, int n)
171 {
172 double * A = input;
173 int i = 0;
174 int j = 0;
175 //m = 8 rows, n = 9 cols
176 int m = n - 1;
177 while (i < m && j < n)
178 {
179 // Find pivot in column j, starting in row i:
180 int maxi = i;
181 for(int k = i + 1; k < m; k++)
182 {
183 if(fabs(A[k * n + j]) > fabs(A[maxi * n + j]))
184 {
185 maxi = k;
186 }
187 }
188 if (A[maxi * n + j] != 0)
189 {
190 //swap rows i and maxi, but do not change the value of i
191 if(i != maxi)
192 for(int k = 0; k < n; k++)
193 {
194 float aux = A[i * n + k];
195 A[i * n + k] = A[maxi * n + k];
196 A[maxi * n + k] = aux;
197 }
198 //Now A[i,j] will contain the old value of A[maxi,j].
199 //divide each entry in row i by A[i,j]
200 float A_ij = A[i * n + j];
201 for(int k = 0; k < n; k++)
202 {
203 A[i * n + k] /= A_ij;
204 }
205 //Now A[i,j] will have the value 1.
206 for(int u = i + 1; u < m; u++)
207 {
208 //subtract A[u,j] * row i from row u
209 float A_uj = A[u * n + j];
210 for(int k = 0; k < n; k++)
211 {
212 A[u * n + k] -= A_uj * A[i * n + k];
213 }
214 //Now A[u,j] will be 0, since A[u,j] - A[i,j] * A[u,j] = A[u,j] - 1 * A[u,j] = 0.
215 }
216 i++;
217 }
218 j++;
219 }
220
221 //back substitution
222 for(int i = m - 2; i >= 0; i--)
223 {
224 for(int j = i + 1; j < n - 1; j++)
225 {
226 A[i * n + m] -= A[i * n + j] * A[j * n + m];
227 }
228 }
229 }
230
get_trusted_four_points(ORBList * matched_keypoints,point_t src[4],point_t dst[4])231 int get_trusted_four_points(ORBList* matched_keypoints, point_t src[4], point_t dst[4])
232 {
233 point_t center{};
234
235 if (matched_keypoints->length < 4) {
236 return -1;
237 }
238
239 for(Node* point = matched_keypoints->start; point != NULL; point = point->next) {
240 orb_matched_point_t* keypoints = (orb_matched_point_t*)point->data;
241 center.col += keypoints->col1;
242 center.row += keypoints->row1;
243 }
244
245 center.col = center.col / matched_keypoints->length;
246 center.row = center.row / matched_keypoints->length;
247
248 int subx, suby, dist;
249 int max_dist_lt = 0;
250 int max_dist_rt = 0;
251 int max_dist_lb = 0;
252 int max_dist_rb = 0;
253 bool found_lt, found_rt, found_lb, found_rb;
254
255 found_lt = found_rt = found_lb = found_rb = false;
256 for(Node* point = matched_keypoints->start; point != NULL; point = point->next) {
257 orb_matched_point_t* keypoints = (orb_matched_point_t*)point->data;
258 if (keypoints->col1 < center.col &&
259 keypoints->row1 < center.row) {
260 subx = keypoints->col1 - center.col;
261 suby = keypoints->row1 - center.row;
262 dist = subx * subx + suby * suby;
263 if (dist > max_dist_lt) {
264 found_lt = true;
265 max_dist_lt = dist;
266 src[0].col = keypoints->col1;
267 src[0].row = keypoints->row1;
268 dst[0].col = keypoints->col2;
269 dst[0].row = keypoints->row2;
270 }
271 }
272
273 if (keypoints->col1 > center.col &&
274 keypoints->row1 < center.row) {
275 subx = keypoints->col1 - center.col;
276 suby = keypoints->row1 - center.row;
277 dist = subx * subx + suby * suby;
278 if (dist > max_dist_rt) {
279 found_rt = true;
280 max_dist_rt = dist;
281 src[1].col = keypoints->col1;
282 src[1].row = keypoints->row1;
283 dst[1].col = keypoints->col2;
284 dst[1].row = keypoints->row2;
285 }
286 }
287
288 if (keypoints->col1 < center.col &&
289 keypoints->row1 > center.row) {
290 subx = keypoints->col1 - center.col;
291 suby = keypoints->row1 - center.row;
292 dist = subx * subx + suby * suby;
293 if (dist > max_dist_lb) {
294 found_lb = true;
295 max_dist_lb = dist;
296 src[2].col = keypoints->col1;
297 src[2].row = keypoints->row1;
298 dst[2].col = keypoints->col2;
299 dst[2].row = keypoints->row2;
300 }
301 }
302
303 if (keypoints->col1 > center.col &&
304 keypoints->row1 > center.row) {
305 subx = keypoints->col1 - center.col;
306 suby = keypoints->row1 - center.row;
307 dist = subx * subx + suby * suby;
308 if (dist > max_dist_rb) {
309 found_rb = true;
310 max_dist_rb = dist;
311 src[3].col = keypoints->col1;
312 src[3].row = keypoints->row1;
313 dst[3].col = keypoints->col2;
314 dst[3].row = keypoints->row2;
315 }
316 }
317 }
318
319 if (!found_lt || !found_rt || !found_lb || !found_rb) {
320 return -1;
321 }
322
323 return 0;
324 }
325
find_homography_by_four_points(ORBList * matched_keypoints,double homography[9])326 int find_homography_by_four_points(ORBList* matched_keypoints, double homography[9])
327 {
328 int ret = 0;
329 point_t src[4]{};
330 point_t dst[4]{};
331
332 ret = get_trusted_four_points(matched_keypoints, src, dst);
333 if (ret < 0) {
334 return -1;
335 }
336
337 // create the equation system to be solved
338 // src and dst must implement [] operator for point access
339 //
340 // from: Multiple View Geometry in Computer Vision 2ed
341 // Hartley R. and Zisserman A.
342 //
343 // x' = xH
344 // where H is the homography: a 3 by 3 matrix
345 // that transformed to inhomogeneous coordinates for each point
346 // gives the following equations for each point:
347 //
348 // x' * (h31*x + h32*y + h33) = h11*x + h12*y + h13
349 // y' * (h31*x + h32*y + h33) = h21*x + h22*y + h23
350 //
351 // as the homography is scale independent we can let h33 be 1 (indeed any of the terms)
352 // so for 4 points we have 8 equations for 8 terms to solve: h11 - h32
353 // after ordering the terms it gives the following matrix
354 // that can be solved with gaussian elimination:
355
356 double P[8][9] =
357 {
358 {-src[0].col, -src[0].row, -1, 0, 0, 0, src[0].col * dst[0].col, src[0].row * dst[0].col, -dst[0].col }, // h11
359 { 0, 0, 0, -src[0].col, -src[0].row, -1, src[0].col * dst[0].row, src[0].row * dst[0].row, -dst[0].row }, // h12
360
361 {-src[1].col, -src[1].row, -1, 0, 0, 0, src[1].col * dst[1].col, src[1].row * dst[1].col, -dst[1].col }, // h13
362 { 0, 0, 0, -src[1].col, -src[1].row, -1, src[1].col * dst[1].row, src[1].row * dst[1].row, -dst[1].row }, // h21
363
364 {-src[2].col, -src[2].row, -1, 0, 0, 0, src[2].col * dst[2].col, src[2].row * dst[2].col, -dst[2].col }, // h22
365 { 0, 0, 0, -src[2].col, -src[2].row, -1, src[2].col * dst[2].row, src[2].row * dst[2].row, -dst[2].row }, // h23
366
367 {-src[3].col, -src[3].row, -1, 0, 0, 0, src[3].col * dst[3].col, src[3].row * dst[3].col, -dst[3].col }, // h31
368 { 0, 0, 0, -src[3].col, -src[3].row, -1, src[3].col * dst[3].row, src[3].row * dst[3].row, -dst[3].row }, // h32
369 };
370
371 gaussian_elimination((&P[0][0]), 9);
372
373 // gaussian elimination gives the results of the equation system
374 // in the last column of the original matrix.
375 // opengl needs the transposed 4x4 matrix:
376 double aux_H[] = { P[0][8], P[1][8], P[2][8], // h11 h21 0 h31
377 P[3][8], P[4][8], P[5][8], // h12 h22 0 h32
378 P[6][8], P[7][8], 1
379 }; // h13 h23 0 h33
380
381 for(int i = 0; i < 3; i++)
382 {
383 for(int j = 0; j < 3; j++)
384 {
385 homography[i * 3 + j] = aux_H[i * 3 + j];
386 }
387 }
388
389 return 0;
390 }
391
get_affine_matrix(double m[3][5],int dim,double affineM[9])392 void get_affine_matrix(double m[3][5], int dim, double affineM[9]) {
393 int index = 0;
394 char res[1024] = {0};
395
396 strcat(res, "\n");
397 for(int j = 0; j < dim; j++) {
398 char str1[128];
399 sprintf(str1, "x%d' = ", j);
400 strcat(res, str1);
401 for(int i = 0; i < dim; i++) {
402 char str2[128];
403 sprintf(str2, "x%d * %f + ", i, m[i][j + dim + 1]);
404 strcat(res, str2);
405
406 affineM[index] = m[i][j + dim + 1];
407 index++;
408 }
409 char str3[128];
410 sprintf(str3, "%f\n", m[dim][j + dim + 1]);
411 strcat(res, str3);
412
413 affineM[index] = m[dim][j + dim + 1];
414 index++;
415 }
416
417 affineM[8] = 1;
418
419 LOGE_ORB("\n-------------------------------------\n");
420 LOGE_ORB("%s", res);
421 }
422
gauss_jordan(double m[3][5],int length,int dim)423 bool gauss_jordan(double m[3][5], int length, int dim) {
424 int h = length;
425 int w = dim;
426 /*
427 LOGV_ORB("gauss1: \n[[%f, %f, %f, %f, %f]\n[%f, %f, %f, %f, %f]\n[%f, %f, %f, %f, %f]]\n",
428 m[0][0], m[0][1], m[0][2], m[0][3], m[0][4],
429 m[1][0], m[1][1], m[1][2], m[1][3], m[1][4],
430 m[2][0], m[2][1], m[2][2], m[2][3], m[2][4]);
431 */
432 for(int y = 0; y < h; y++) {
433 int maxrow = y;
434 for(int y2 = y + 1; y2 < h; y2++) {
435 if(fabs(m[y2][y]) > fabs(m[maxrow][y])) {
436 maxrow = y2;
437 }
438 }
439
440 //(m[y], m[maxrow]) = (m[maxrow], m[y])
441 if (y != maxrow) {
442 for(int i = 0; i < w; i++) {
443 double temp = m[y][i];
444 m[y][i] = m[maxrow][i];
445 m[maxrow][i] = temp;
446 }
447 }
448
449 if(fabs(m[y][y]) <= EPS) {
450 return false;
451 }
452
453 for(int y2 = y + 1; y2 < h; y2++) {
454 double c = m[y2][y] / m[y][y];
455 for(int x = y; x < w; x++) {
456 m[y2][x] -= m[y][x] * c;
457 }
458 }
459 }
460
461 for(int y = h - 1; y > -1; y--) {
462 double c = m[y][y];
463 for(int y2 = 0; y2 < y; y2++) {
464 for(int x = w - 1; x > y - 1; x--) {
465 m[y2][x] -= m[y][x] * m[y2][y] / c;
466 }
467 }
468 m[y][y] /= c;
469 for(int x = h; x < w; x++) {
470 m[y][x] /= c;
471 }
472 }
473
474 return true;
475 }
476
affine_fit(double fpt[][2],double tpt[][2],int length,int dim,double affineM[9])477 int affine_fit(double fpt[][2], double tpt[][2], int length, int dim, double affineM[9]) {
478 if (length <= dim) {
479 LOGE_ORB("number of points must bigger than dimension\n");
480 return -1;
481 }
482
483 // dim+1 x dim
484 double c[3][2]{};
485 for(int j = 0; j < dim; j++) {
486 for(int k = 0; k < (dim + 1); k++) {
487 for(int i = 0; i < length; i++) {
488 double qt[3] = {fpt[i][0], fpt[i][1], 1};
489 c[k][j] += qt[k] * tpt[i][j];
490 }
491 }
492 }
493
494 // dim+1 x dim+1
495 double Q[3][3]{};
496 for(int k = 0; k < length; k++) {
497 double qt[3] = {fpt[k][0], fpt[k][1], 1};
498 for(int i = 0; i < (dim + 1); i++) {
499 for(int j = 0; j < (dim + 1); j++) {
500 Q[i][j] += qt[i] * qt[j];
501 }
502 }
503 }
504
505 double M[3][5];
506 for(int i = 0; i < 3; i++) {
507 for(int j = 0; j < 3; j++) {
508 M[i][j] = Q[i][j];
509 }
510 }
511
512 for(int i = 0; i < 3; i++) {
513 for(int j = 3; j < 5; j++) {
514 M[i][j] = c[i][j - 3];
515 }
516 }
517
518 gauss_jordan(M, 3, 5);
519 get_affine_matrix(M, 2, affineM);
520
521 return 0;
522 }
523
elimate_affine_transform(ORBList * matched_keypoints,double homography[9])524 int elimate_affine_transform(ORBList* matched_keypoints, double homography[9])
525 {
526 int ret = 0;
527 int i = 0, j = 0, k = 0;
528
529 if (matched_keypoints->length < 3) {
530 LOGE_ORB("matched keypoints num(%d) not enough", matched_keypoints->length);
531 k = 0;
532 for(Node* point = matched_keypoints->start; point != NULL; point = point->next) {
533 orb_matched_point_t* keypoint = (orb_matched_point_t*)point->data;
534 LOGE_AORB("<%d>-[%d - %d]-[%d - %d]",
535 keypoint->score,
536 keypoint->col1, keypoint->row1,
537 keypoint->col2, keypoint->row2);
538 k++;
539 }
540 return -1;
541 }
542
543 int max, M0, count = 0;
544 int maxLength = 10;
545 int subX, subY;
546 int length = matched_keypoints->length > maxLength ? maxLength : matched_keypoints->length;
547 int distance = -1, distanceAvg = 0;
548 int* distanceArray = (int*)malloc(sizeof(int) * matched_keypoints->length);
549 int* distanceSortedArray = (int*)malloc(sizeof(int) * matched_keypoints->length);
550 #if 0 // fix compile error for Android
551 int (*M0_stats)[matched_keypoints->length] =
552 (int (*)[matched_keypoints->length])malloc(sizeof(int) * matched_keypoints->length * 2);
553 for (k = 0; k < matched_keypoints->length; k++) {
554 M0_stats[0][k] = M0_stats[1][k] = -1;
555 }
556 #else
557 int* M0_stats[2];
558 for (int i = 0; i < 2 ;i++)
559 M0_stats[i] = (int*)malloc(sizeof(int) * matched_keypoints->length);
560 for (k = 0; k < matched_keypoints->length; k++) {
561 M0_stats[0][k] = M0_stats[1][k] = -1;
562 }
563 #endif
564 double (*from_pts)[2];
565 from_pts = (double (*)[2])malloc(sizeof(double) * length * 2);
566
567 double (*to_pts)[2];
568 to_pts = (double (*)[2])malloc(sizeof(double) * length * 2);
569
570
571 LOGD_ORB("matched points:");
572 //work_begin();
573 for(Node* point = matched_keypoints->start; point != NULL; point = point->next) {
574 orb_matched_point_t* keypoint = (orb_matched_point_t*)point->data;
575
576 subX = (keypoint->col1 - keypoint->col2);
577 subY = (keypoint->row1 - keypoint->row2);
578 distanceArray[i] = subX * subX + subY * subY;
579 distanceSortedArray[i] = distanceArray[i];
580 distanceAvg += distanceArray[i];
581 i++;
582 }
583
584 distanceAvg /= matched_keypoints->length;
585
586 //sort
587 for (k = 0; k < matched_keypoints->length; k++) {
588 for (j = k; j < matched_keypoints->length; j++) {
589 if (distanceSortedArray[k] < distanceSortedArray[j]) {
590 max = distanceSortedArray[j];
591 distanceSortedArray[j] = distanceSortedArray[k];
592 distanceSortedArray[k] = max;
593 }
594 }
595 }
596
597 //find mode
598 for (k = 0, j = 0; k < matched_keypoints->length - 1; k++) {
599 if (distanceSortedArray[k] == distanceSortedArray[k + 1]) {
600 M0_stats[0][j] = distanceSortedArray[k];
601 if (M0_stats[1][j] == -1) {
602 M0_stats[1][j] = 2;
603 } else {
604 M0_stats[1][j] += 1;
605 }
606 } else {
607 if(M0_stats[0][j] == -1) {
608 M0_stats[0][j] = distanceSortedArray[k];
609 M0_stats[1][j] = 1;
610 }
611 j++;
612
613 if (k == matched_keypoints->length - 2) {
614 M0_stats[0][j] = distanceSortedArray[k + 1];
615 M0_stats[1][j] = 1;
616 }
617 }
618 }
619
620 for (k = 1; k < matched_keypoints->length; k++) {
621 LOGE_AORB("distanceSortedArray[%d]: %d", k, distanceSortedArray[k]);
622
623 }
624
625 max = M0_stats[1][0];
626 distance = M0_stats[0][0];
627 for (k = 1; k < matched_keypoints->length; k++) {
628 LOGE_AORB("M0_stats[%d]: %d, count: %d", k, M0_stats[0][k], M0_stats[1][k]);
629 if (M0_stats[0][k] == -1) {
630 break;
631 }
632
633 if (M0_stats[1][k] >= max) {
634 max = M0_stats[1][k];
635 distance = M0_stats[0][k];
636 }
637 }
638
639 LOGE_AORB("mesured distance: %d, count: %d", distance, max);
640
641 for (k = 0; k < matched_keypoints->length; k++) {
642 if (abs(distanceArray[k] - distance) > distance * 10) {
643 distanceArray[k] = -1;
644 }
645 }
646
647 //work_end("calc-dist");
648 //work_begin();
649
650 i = 0;
651 k = 0;
652 for(Node* point = matched_keypoints->start; point != NULL && i < maxLength; point = point->next) {
653 orb_matched_point_t* keypoint = (orb_matched_point_t*)point->data;
654
655 if (distanceArray[k] == -1) {
656 k++;
657 continue;
658 }
659
660 from_pts[i][0] = keypoint->col1;
661 from_pts[i][1] = keypoint->row1;
662 to_pts[i][0] = keypoint->col2;
663 to_pts[i][1] = keypoint->row2;
664 i++;
665 k++;
666 }
667
668 if (i < 3) {
669 k = 0;
670 for(Node* point = matched_keypoints->start; point != NULL; point = point->next) {
671 orb_matched_point_t* keypoint = (orb_matched_point_t*)point->data;
672 LOGE_AORB("<%d>-[%d - %d]-[%d - %d]-<%d>",
673 keypoint->score,
674 keypoint->col1, keypoint->row1,
675 keypoint->col2, keypoint->row2,
676 distanceArray[k]);
677 k++;
678 }
679 LOGE_ORB("valid matched keypoints %d less than 6", i);
680 ret = -2;
681 goto out;
682 //return -2;
683 }
684
685 ret = affine_fit(from_pts, to_pts, i, 2, homography);
686 //work_end("calc-affine");
687
688 out:
689 free(from_pts);
690 free(to_pts);
691 free(distanceArray);
692 free(distanceSortedArray);
693 #if 0 // fix compile error for Android
694 free(M0_stats);
695 #else
696 free(M0_stats[0]);
697 free(M0_stats[1]);
698 #endif
699 return ret;
700 }
701
702
map_point(double mat[9],point_t * point_src,point_t * point_dst)703 void map_point(double mat[9], point_t* point_src, point_t* point_dst)
704 {
705 // P[0], P[1], P[2],
706 // P[3], P[4], P[5],
707 // P[6], P[7], P[8]
708 double D = point_src->col * mat[6] + point_src->row * mat[7] + mat[8];
709 point_dst->col = (point_src->col * mat[0] + point_src->row * mat[1] + mat[2]) / D;
710 point_dst->row = (point_src->col * mat[3] + point_src->row * mat[4] + mat[5]) / D;
711
712 }
713
map_rect(double homography[9],orb_rect_t * rect_src)714 orb_rect_t map_rect(double homography[9], orb_rect_t* rect_src)
715 {
716 orb_rect_t rect_dst;
717 point_t src, dst;
718 src.col = rect_src->left;
719 src.row = rect_src->top;
720 map_point(homography, &src, &dst);
721 rect_dst.left = dst.col;
722 rect_dst.top = dst.row;
723
724 src.col = rect_src->right;
725 src.row = rect_src->bottom;
726 map_point(homography, &src, &dst);
727 rect_dst.right = dst.col;
728 rect_dst.bottom = dst.row;
729
730 rect_dst.width = rect_dst.right - rect_dst.left;
731 rect_dst.height = rect_dst.bottom - rect_dst.top;
732
733 return rect_dst;
734 }
735
736