-
Notifications
You must be signed in to change notification settings - Fork 4
/
pose_apriltag.cpp
80 lines (72 loc) · 2.81 KB
/
pose_apriltag.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2019 Intel Corporation. All Rights Reserved.
#include "pose_apriltag.hpp"
//
// Re-compute homography between ideal standard tag image and undistorted tag corners for estimage_tag_pose().
//
// @param[in] c is 4 pairs of tag corners on ideal image and undistorted input image.
// @param[out] H is the output homography between ideal and undistorted input image.
// @see static void apriltag_manager::undistort(...)
//
void homography_compute2(const double c[4][4], matd_t* H) {
double A[] = {
c[0][0], c[0][1], 1, 0, 0, 0, -c[0][0]*c[0][2], -c[0][1]*c[0][2], c[0][2],
0, 0, 0, c[0][0], c[0][1], 1, -c[0][0]*c[0][3], -c[0][1]*c[0][3], c[0][3],
c[1][0], c[1][1], 1, 0, 0, 0, -c[1][0]*c[1][2], -c[1][1]*c[1][2], c[1][2],
0, 0, 0, c[1][0], c[1][1], 1, -c[1][0]*c[1][3], -c[1][1]*c[1][3], c[1][3],
c[2][0], c[2][1], 1, 0, 0, 0, -c[2][0]*c[2][2], -c[2][1]*c[2][2], c[2][2],
0, 0, 0, c[2][0], c[2][1], 1, -c[2][0]*c[2][3], -c[2][1]*c[2][3], c[2][3],
c[3][0], c[3][1], 1, 0, 0, 0, -c[3][0]*c[3][2], -c[3][1]*c[3][2], c[3][2],
0, 0, 0, c[3][0], c[3][1], 1, -c[3][0]*c[3][3], -c[3][1]*c[3][3], c[3][3],
};
double epsilon = 1e-10;
// Eliminate.
for (int col = 0; col < 8; col++) {
// Find best row to swap with.
double max_val = 0;
int max_val_idx = -1;
for (int row = col; row < 8; row++) {
double val = fabs(A[row*9 + col]);
if (val > max_val) {
max_val = val;
max_val_idx = row;
}
}
if (max_val < epsilon) {
fprintf(stderr, "WRN: Matrix is singular.\n");
}
// Swap to get best row.
if (max_val_idx != col) {
for (int i = col; i < 9; i++) {
double tmp = A[col*9 + i];
A[col*9 + i] = A[max_val_idx*9 + i];
A[max_val_idx*9 + i] = tmp;
}
}
// Do eliminate.
for (int i = col + 1; i < 8; i++) {
double f = A[i*9 + col]/A[col*9 + col];
A[i*9 + col] = 0;
for (int j = col + 1; j < 9; j++) {
A[i*9 + j] -= f*A[col*9 + j];
}
}
}
// Back solve.
for (int col = 7; col >=0; col--) {
double sum = 0;
for (int i = col + 1; i < 8; i++) {
sum += A[col*9 + i]*A[i*9 + 8];
}
A[col*9 + 8] = (A[col*9 + 8] - sum)/A[col*9 + col];
}
H->data[0] = A[8];
H->data[1] = A[17];
H->data[2] = A[26];
H->data[3] = A[35];
H->data[4] = A[44];
H->data[5] = A[53];
H->data[6] = A[62];
H->data[7] = A[71];
H->data[8] = 1;
}