-
Notifications
You must be signed in to change notification settings - Fork 8
/
_cy_apriltag.pyx
163 lines (136 loc) · 4.38 KB
/
_cy_apriltag.pyx
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
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
'''
_cy_apriltag.pyx in pyAprilTag
author : cfeng
created : 9/17/18 11:40AM
'''
cimport cython
from libcpp.vector cimport vector
from libcpp.string cimport string
import numpy as np
cimport numpy as np
#ref: https://stackoverflow.com/a/45928289
np.import_array()
from libcpp cimport bool
from cpython.ref cimport PyObject
# References PyObject to OpenCV object conversion code borrowed from OpenCV's own conversion file, cv2.cpp
cdef extern from 'src/pyopencv_converter.cpp':
cdef PyObject* pyopencv_from(const Mat& m)
cdef bool pyopencv_to(PyObject* o, Mat& m)
cdef extern from 'opencv2/core/core.hpp' namespace 'cv':
cdef cppclass Mat:
Mat() except +
int rows
int cols
cdef extern from "src/pyAprilTagDetector.cpp" namespace "py_apriltag":
size_t detect_apriltags(
Mat& frame,
vector[int]& ids,
vector[double]& corners,
vector[double]& centers,
vector[double]& Hs,
const int hammingThresh
)
cdef extern from "src/pyAprilTagDetector.cpp" namespace "py_apriltag":
void set_tagfamilies(string tagid)
cdef extern from "src/pyAprilTagCalibrator.cpp" namespace "py_apriltag":
int calib_by_apriltags(
string rig_filename,
string url,
string log_dir,
int nDistCoeffs,
bool useEachValidPhoto
)
@cython.boundscheck(False)
@cython.wraparound(False)
def _cy_set_tagfamilies(str tagid):
cdef string tagid_ = tagid.encode()
set_tagfamilies(tagid_)
@cython.boundscheck(False)
@cython.wraparound(False)
def _cy_calib_by_apriltags(
str rig_filename,
str url,
str log_dir,
int nDistCoeffs,
bool useEachValidPhoto
):
cdef string rig_filename_ = rig_filename.encode()
cdef string url_ = url.encode()
cdef string log_dir_ = log_dir.encode()
return calib_by_apriltags(rig_filename_, url_, log_dir_, nDistCoeffs, useEachValidPhoto)
@cython.boundscheck(False)
@cython.wraparound(False)
def _cy_find_apriltags(
np.ndarray[np.uint8_t, ndim=2, mode="c"] frame,
int hammingTresh
):
'''
Input:
frame <HxW>: input image
hammingTresh <int>: hamming threshold
Output:
ids <Nx3>: list of detected ids within hammingThresh
corners <Nx(4*2)>: list of tag corners
centers <Nx(2)>: list of tag centers
Hs <Nx(3*3)>: list of homography
'''
cdef vector[int] ids_
cdef vector[double] corners_
cdef vector[double] centers_
cdef vector[double] Hs_
cdef Mat frame_mat = Mat()
pyopencv_to(<PyObject*> frame, frame_mat)
N = detect_apriltags(
frame_mat,
ids_, corners_, centers_, Hs_,
hammingTresh
)
ids = np.array(ids_)
corners = np.array(corners_).reshape(N,4,2)
centers = np.array(centers_).reshape(N,2)
Hs = np.array(Hs_).reshape(N,3,3)
return ids, corners, centers, Hs
@cython.boundscheck(False)
@cython.wraparound(False)
def _cy_find_apriltags_and_vis(
np.ndarray[np.uint8_t, ndim=3, mode="c"] frame,
int hammingTresh
):
'''
Input:
frame <HxWx3>: input color image
hammingTresh <int>: hamming threshold
Output:
ids <Nx3>: list of detected ids within hammingThresh
corners <Nx(4*2)>: list of tag corners
centers <Nx(2)>: list of tag centers
Hs <Nx(3*3)>: list of homography
'''
cdef vector[int] ids_
cdef vector[double] corners_
cdef vector[double] centers_
cdef vector[double] Hs_
cdef Mat frame_mat = Mat()
pyopencv_to(<PyObject*> frame, frame_mat)
N = detect_apriltags(
frame_mat,
ids_, corners_, centers_, Hs_,
hammingTresh
)
ids = np.array(ids_)
corners = np.array(corners_).reshape(N,4,2)
centers = np.array(centers_).reshape(N,2)
Hs = np.array(Hs_).reshape(N,3,3)
return ids, corners, centers, Hs
def set(tagid):
tagid = str(tagid)
_cy_set_tagfamilies(tagid)
def find(img, thresh=0):
img = np.require(img, dtype=np.uint8, requirements=['C'])
thresh = int(thresh)
if img.ndim!=3:
return _cy_find_apriltags(img, thresh)
else:
return _cy_find_apriltags_and_vis(img, thresh)
def calib(rig_filename, url='camera://0', log_dir="", nDistCoeffs=2, useEachValidPhoto=True):
return _cy_calib_by_apriltags(rig_filename, url, log_dir, nDistCoeffs, useEachValidPhoto)