/home/vittorfp/Artoolkit/examples/simpleLite/ta...

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

1 de 3

le:///tmp/tmpvd4f38.html

// ============================================================================
//
Includes / Namespaces
// ============================================================================
#include <AR/ar.h>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;
// ============================================================================
//
Constantes
// ============================================================================
#define VIEW_SCALEFACTOR
#define VIEW_DISTANCE_MIN
#define VIEW_DISTANCE_MAX

1.0
40.0
10000.0

// Units received from ARToolKit tracking will be multiplied by this factor before being used in OpenGL drawing.
// Objects closer to the camera than this will not be displayed. OpenGL units.
// Objects further away from the camera than this will not be displayed. OpenGL units.

// ============================================================================
//
Variaveis Globais
// ============================================================================
// Initial window width, also updated during program execution.
// Initial window height, also updated during program execution.
// Fullscreen mode bit depth.

static int windowWidth = 640;

static int windowHeight = 480;
static int windowDepth = 32;

// Image acquisition.
static ARUint8
*gARTImage = NULL;
// Marker detection.
static ARHandle

*gARHandle = NULL;

//Pattern Handler
static ARPattHandle

*PattHandle = NULL;

//Contador FPS
static long

gCallCountMarkerDetect = 0;

// Transformation matrix retrieval.

static AR3DHandle
*gAR3DHandle = NULL;
//Tamanho dos markers impressos
static ARdouble
gPatt_width

= 80.0;

//tamanho das TAGs

//Matrizes de rotao/translao dos respectivos tags em relao ao sistema de coordenadas da camera.

//Os mesmos sero setados e atualizados em todos os frames onde o
static ARdouble
terrines_trans[3][4];
static ARdouble
exchange_trans[3][4];
static ARdouble
tank_trans[3][4];
//FLAGS que indicam, a cada frame processado, se o respectivo tag foi detectado
static int
terrines_found = FALSE;
static int
tank_found = FALSE;
static int
exchange_found = FALSE;
//ID de cada marker, utilizado ao carregar os dados referentes aos markers e ao detecta-los
static int
terrines_id;
static int
exchange_id;
static int
tank_id;
//Parametro da camera
static ARParamLT
*gCparamLT = NULL;
static ARParam
cparam;
//Limiar para o erro na deteco de tags
static float
th = 0.7;
//Imagem opencv
static Mat
imgOpenCV;
static VideoCapture capture;
static int
cameraId = 0;
// ============================================================================
//
Functions
// ============================================================================
//Funo que conecta com a camera, a configura e faz checagem de erros
static int setupCamera(const char *cparam_name, char *vconf, ARParamLT **cparamLT_p, ARHandle **arhandle, AR3DHandle **ar3dhandle){
int
xsize, ysize;
AR_PIXEL_FORMAT pixFormat;
xsize = 640;
ysize = 480;
ARLOGi("Camera image size (x,y) = (%d,%d)\n", xsize, ysize);

//Carrega os parametros de configurao da camera.

if (arParamLoad(cparam_name, 1, &cparam) < 0) {
ARLOGe("setupCamera(): Error loading parameter file %s for camera.\n", cparam_name);
return (FALSE);
}
//Redimensiona o tamanho do frame captado pela camera
if (cparam.xsize != xsize || cparam.ysize != ysize) {
ARLOGw("*** Camera Parameter resized from %d, %d. ***\n", cparam.xsize, cparam.ysize);
arParamChangeSize(&cparam, xsize, ysize, &cparam);
}
//Configura os parametros e Handlers da camera
if ((*cparamLT_p = arParamLTCreate(&cparam, AR_PARAM_LT_DEFAULT_OFFSET)) == NULL) {
ARLOGe("setupCamera(): Error: arParamLTCreate.\n");
return (FALSE);
}
if ((*arhandle = arCreateHandle(*cparamLT_p)) == NULL) {
ARLOGe("setupCamera(): Error: arCreateHandle.\n");
return (FALSE);
}
if (arSetPixelFormat(*arhandle, pixFormat) < 0) {
ARLOGe("setupCamera(): Error: arSetPixelFormat.\n");
return (FALSE);

02/06/2016 10:26

/home/vittorfp/Artoolkit/examples/simpleLite/ta...
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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227

2 de 3

le:///tmp/tmpvd4f38.html

}
if (arSetDebugMode(*arhandle, AR_DEBUG_DISABLE) < 0) {
ARLOGe("setupCamera(): Error: arSetDebugMode.\n");
return (FALSE);
}
if ((*ar3dhandle = ar3DCreateHandle(&cparam)) == NULL) {
ARLOGe("setupCamera(): Error: ar3DCreateHandle.\n");
return (FALSE);
}
return (TRUE);
}

//Limpa todos os dados temporarios e desativa a camera ao fim do programa

static void cleanup(void){
arPattDetach(gARHandle);
arPattDeleteHandle(PattHandle);
ar3DDeleteHandle(&gAR3DHandle);
arDeleteHandle(gARHandle);
arParamLTFree(&gCparamLT);
}
//Main Loop do Artoolkit.
//Nele, o frame carregado da camera e processado
static void mainLoop(void) {
static int imageNumber = 0;
//static int ms_prev;
ARdouble err;
Mat view;

//Pega um frame na camera

capture >> view;
//converte para o formato de imagem do artoolkit
gARTImage = view.data;
gCallCountMarkerDetect++; // Incrementa o contador FPS do ARToolKit

//Procura pelas TAGs no frame

if (arDetectMarker(gARHandle, gARTImage) < 0) {
exit(-1);
}
int
int
int
int

k = -1;
l = -1;
m = -1;
j;

//Percorre o vetor de tags encotradas, as classifica pelo ID e compara para ver qual tem melhor confiabilidade
for (j = 0; j < gARHandle->marker_num; j++) {
if(gARHandle->markerInfo[j].cf >= th){
if (gARHandle->markerInfo[j].id == terrines_id) {
if (k == -1)
k = j; // First marker detected.
else if (gARHandle->markerInfo[j].cf > gARHandle->markerInfo[k].cf)
k = j; // Higher confidence marker detected.
}else if (gARHandle->markerInfo[j].id == tank_id) {
if (m == -1)
m = j; // First marker detected.
else if (gARHandle->markerInfo[j].cf > gARHandle->markerInfo[m].cf)
m = j; // Higher confidence marker detected.
}else if (gARHandle->markerInfo[j].id == exchange_id) {
if (l == -1)
l = j; // First marker detected.
else if (gARHandle->markerInfo[j].cf > gARHandle->markerInfo[l].cf)
l = j; // Higher confidence marker detected.
}
}
}

//Ve quais dos tags foram detectados e pega a matriz de rotaao/translao deles
//seta as flags de detecao
if (k != -1) {
//Pega matriz de rotao/translao
err = arGetTransMatSquare(gAR3DHandle, &(gARHandle->markerInfo[k]), gPatt_width, terrines_trans);
terrines_found = TRUE;
Point p = Point( (int)(terrines_trans[0][3]+(cparam.xsize/2)) , (int)( terrines_trans[1][3]+(cparam.ysize/2))
circle( view, p, 3, Scalar(0,0,255), 3, LINE_AA);
putText(view, "Terrines zone", p, FONT_HERSHEY_PLAIN, 1.0, CV_RGB(0,255,0), 2.0);

);

} else {
terrines_found = FALSE;
}

if (l != -1) {

//Pega matriz de rotao/translao

err = arGetTransMatSquare(gAR3DHandle, &(gARHandle->markerInfo[l]), gPatt_width, exchange_trans);
Point p = Point( (int)(exchange_trans[0][3]+(cparam.xsize/2)) , (int)( exchange_trans[1][3]+(cparam.ysize/2))
circle( view, p, 3, Scalar(0,0,255), 3, LINE_AA);
putText(view, "Exchange zone", p, FONT_HERSHEY_PLAIN, 1.0, CV_RGB(0,255,0), 2.0);
exchange_found = TRUE;

);

} else {
exchange_found = FALSE;

02/06/2016 10:26

/home/vittorfp/Artoolkit/examples/simpleLite/ta...
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336

3 de 3

le:///tmp/tmpvd4f38.html

if (m != -1) {

//Pega matriz de rotao/translao

err = arGetTransMatSquare(gAR3DHandle, &(gARHandle->markerInfo[m]), gPatt_width, tank_trans);
Point p = Point( (int)(tank_trans[0][3]+(cparam.xsize/2)) , (int)( tank_trans[1][3]+(cparam.ysize/2))
circle( view, p, 3, Scalar(0,0,255), 3, LINE_AA);
putText(view, "Milk tank", p, FONT_HERSHEY_PLAIN, 1.0, CV_RGB(0,255,0), 2.0);
tank_found = TRUE;

);

} else {
tank_found = FALSE;
}

//Mostra o frame processado

//Para utilizar sem exibir os frames, excluir as linhas a seguir
//INICIO
cv::imshow("Camera", view);
int key = 0xff & waitKey(capture.isOpened() ? 50 : 500);
//FIM
if(gCallCountMarkerDetect == 120){
printf("*** Camera - %lf (frame/sec)\n", (double)gCallCountMarkerDetect/arUtilTimer());
gCallCountMarkerDetect = 0;
arUtilTimerReset();
}

//Funo principal. Inicializa os recursos necessarios para a execuo do programa e executa o loop
//necessario para o Artoolkit
int main(int argc, char** argv){
char glutGamemode[32];
char cparam_name[] = "Data/camera_para.dat";
char vconf[] = "";
int marker_num = 3;
//Seta o modo de ajuste no treshold da deteco de tags
AR_LABELING_THRESH_MODE modea;
modea = AR_LABELING_THRESH_MODE_AUTO_BRACKETING;
arSetLabelingThreshMode(gARHandle, modea);
//Seta o modo de conexao com a camera para Video4Linux2
//OBS.: Modo usado no meu computador. Em outros computadores podeser necessario utilizar o GStreamer
int s = system("export ARTOOLKIT5_VCONF=\"-device=LinuxV4L2\"");
//inicia a captura de frames usando o opencv
capture.open(cameraId);
//Inicializa parametros de camera
if (!setupCamera(cparam_name, vconf, &gCparamLT, &gARHandle, &gAR3DHandle)) {
ARLOGe("main(): Unable to set up AR camera.\n");
exit(-1);
}
//inicializa os pattHandlers
if ((PattHandle = arPattCreateHandle()) == NULL) {
ARLOGe("main(): Error: arPattCreateHandle.\n");
return (FALSE);
}
//Carrega as informaes de cada tag para a memoria
if ((terrines_id = arPattLoad(PattHandle, "Data/terrines.patt" ) ) < 0) {
ARLOGe("main(): Error loading pattern file Data/terrines.patt.\n");
arPattDeleteHandle(PattHandle);
return (FALSE);
}
if ((tank_id = arPattLoad(PattHandle, "Data/milk_tank.patt")) < 0) {
ARLOGe("main(): Error loading pattern file Data/milk_tank.patt.\n");
arPattDeleteHandle(PattHandle);
return (FALSE);
}

if ((exchange_id = arPattLoad(PattHandle, "Data/exchange_zone.patt")) < 0) {

ARLOGe("main(): Error loading pattern file Data/exchange_zone.patt.\n");
arPattDeleteHandle(PattHandle);
return (FALSE);
}

//Atrela o Pattern Handler com o ARHandler

if(arPattAttach(gARHandle, PattHandle) == -1){
ARLOGe("main(): Error attatching the Pattern handler to the AR Handler.\n");
return(FALSE);
}
//Inicia o timer, para a mediao de FPS
arUtilTimerReset();
//Executa o main loop do Artoolkit
while(1){
mainLoop();
}
cleanup();
return (0);
}

02/06/2016 10:26