Panda3D
Loading...
Searching...
No Matches
arToolKit.cxx
Go to the documentation of this file.
1/**
2 * PANDA 3D SOFTWARE
3 * Copyright (c) Carnegie Mellon University. All rights reserved.
4 *
5 * All use of this software is subject to the terms of the revised BSD
6 * license. You should have received a copy of this license along
7 * with this source code in a file named "LICENSE."
8 *
9 * @file arToolKit.cxx
10 * @author jyelon
11 * @date 2007-11-01
12 */
13
14#include "arToolKit.h"
15
16#ifdef HAVE_ARTOOLKIT
17
18#include "pandaNode.h"
19#include "camera.h"
20#include "perspectiveLens.h"
21#include "lvecBase3.h"
22#include "compose_matrix.h"
23#include "config_vision.h"
24extern "C" {
25 #include <AR/ar.h>
26};
27
28ARToolKit::PatternTable ARToolKit::_pattern_table;
29
30static void change_size( ARParam *source, int xsize, int ysize, ARParam *newparam )
31{
32 int i;
33
34 newparam->xsize = xsize;
35 newparam->ysize = ysize;
36
37 double xscale = (double)xsize / (double)(source->xsize);
38 double yscale = (double)ysize / (double)(source->ysize);
39 for( i = 0; i < 4; i++ ) {
40 newparam->mat[0][i] = source->mat[0][i] * xscale;
41 newparam->mat[1][i] = source->mat[1][i] * yscale;
42 newparam->mat[2][i] = source->mat[2][i];
43 }
44
45 newparam->dist_factor[0] = source->dist_factor[0] * xscale;
46 newparam->dist_factor[1] = source->dist_factor[1] * yscale;
47 newparam->dist_factor[2] = source->dist_factor[2] / (xscale*yscale);
48 newparam->dist_factor[3] = source->dist_factor[3];
49}
50
51static void analyze_fov(double cparam[3][4], int width, int height, double &xfov, double &yfov)
52{
53 double gnear = 10.0;
54 double gfar = 1000.0;
55 double icpara[3][4];
56 double trans[3][4];
57 double p[3][3], q[4][4];
58 double xval, yval;
59 int i, j;
60
61 if( arParamDecompMat(cparam, icpara, trans) < 0 ) {
62 printf("gConvGLcpara: Parameter error!!\n");
63 exit(0);
64 }
65
66 for( i = 0; i < 3; i++ ) {
67 for( j = 0; j < 3; j++ ) {
68 p[i][j] = icpara[i][j] / icpara[2][2];
69 }
70 }
71 q[0][0] = (2.0 * p[0][0] / width);
72 q[0][1] = (2.0 * p[0][1] / width);
73 q[0][2] = ((2.0 * p[0][2] / width) - 1.0);
74 q[0][3] = 0.0;
75
76 q[1][0] = 0.0;
77 q[1][1] = (2.0 * p[1][1] / height);
78 q[1][2] = ((2.0 * p[1][2] / height) - 1.0);
79 q[1][3] = 0.0;
80
81 q[2][0] = 0.0;
82 q[2][1] = 0.0;
83 q[2][2] = (gfar + gnear)/(gfar - gnear);
84 q[2][3] = -2.0 * gfar * gnear / (gfar - gnear);
85
86 q[3][0] = 0.0;
87 q[3][1] = 0.0;
88 q[3][2] = 1.0;
89 q[3][3] = 0.0;
90
91 xval =
92 q[0][0] * trans[0][0] +
93 q[0][1] * trans[1][0] +
94 q[0][2] * trans[2][0];
95 yval =
96 q[1][0] * trans[0][1] +
97 q[1][1] * trans[1][1] +
98 q[1][2] * trans[2][1];
99
100 xfov = 2.0 * atan(1.0/xval) * (180.0/3.141592654);
101 yfov = 2.0 * atan(1.0/yval) * (180.0/3.141592654);
102}
103
104/**
105 * Create a new ARToolKit instance.
106 *
107 * Camera must be the nodepath of a panda camera object. The panda camera's
108 * field of view is initialized to match the field of view of the physical
109 * webcam. Each time you call analyze, all marker nodepaths will be moved
110 * into a position which is relative to this camera. The marker_size
111 * parameter indicates how large you printed the physical markers. You should
112 * use the same size units that you wish to use in the panda code.
113 */
114ARToolKit *ARToolKit::
115make(NodePath camera, const Filename &paramfile, double marker_size) {
116
117 if (AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_BGRA &&
118 AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_RGBA &&
119 AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_ARGB &&
120 AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_ABGR &&
121 AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_RGB &&
122 AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_BGR) {
123 vision_cat.error() <<
124 "The copy of ARToolKit that you are using is not compiled "
125 "for RGB, BGR, RGBA, BGRA, ARGB or ABGR input. Panda3D cannot "
126 "use this copy of ARToolKit. Please modify the ARToolKit's "
127 "config file and compile it again.\n";
128 return 0;
129 }
130
131 if (camera.is_empty()) {
132 vision_cat.error() << "ARToolKit: invalid camera nodepath\n";
133 return 0;
134 }
135 PandaNode *node = camera.node();
136 if ((node == 0) || (node->get_type() != Camera::get_class_type())) {
137 vision_cat.error() << "ARToolKit: invalid camera nodepath\n";
138 return 0;
139 }
140 Camera *cam = DCAST(Camera, node);
141 Lens *lens = cam->get_lens();
142 if (lens->get_type() != PerspectiveLens::get_class_type()) {
143 vision_cat.error() << "ARToolKit: supplied camera node must be perspective.\n";
144 return 0;
145 }
146
147 ARParam wparam;
148 std::string fn = paramfile.to_os_specific();
149 if( arParamLoad(fn.c_str(), 1, &wparam) < 0 ) {
150 vision_cat.error() << "Cannot load ARToolKit camera config\n";
151 return 0;
152 }
153
154 arParamDisp(&wparam);
155 double xfov, yfov;
156 analyze_fov(wparam.mat, 640, 480, xfov, yfov);
157
158 lens->set_fov(xfov, yfov);
159
160 ARToolKit *result = new ARToolKit();
161 result->_camera = camera;
162 result->_camera_param = new ARParam;
163 result->_threshold = 0.5;
164 result->_marker_size = marker_size;
165 result->_have_prev_conv = false;
166 memcpy(result->_camera_param, &wparam, sizeof(wparam));
167 return result;
168}
169
170
171/**
172 * Pre-destructor deallocation and cleanup.
173 */
174void ARToolKit::
175cleanup() {
176 if (_camera_param) {
177 ARParam *param = (ARParam *)_camera_param;
178 delete param;
179 _camera_param = 0;
180 }
181}
182
183/**
184 * Use ARToolKit::make to create an ARToolKit.
185 */
186ARToolKit::
187ARToolKit() : _have_prev_conv(false) {
188}
189
190/**
191 *
192 */
193ARToolKit::
194~ARToolKit() {
195 cleanup();
196}
197
198/**
199 * Load the specified pattern into the toolkit, and return the pattern index.
200 * Initially, the pattern is inactive.
201 */
202int ARToolKit::
203get_pattern(const Filename &filename) {
204 PatternTable::iterator ptf = _pattern_table.find(filename);
205 if (ptf != _pattern_table.end()) {
206 return (*ptf).second;
207 }
208
209 std::string fn = filename.to_os_specific();
210 int id = arLoadPatt(fn.c_str());
211 if (id < 0) {
212 vision_cat.error() << "Could not load AR ToolKit Pattern: " << fn << "\n";
213 return -1;
214 }
215 arDeactivatePatt(id);
216 _pattern_table[filename] = id;
217 return id;
218}
219
220/**
221 * Associates the specified glyph with the specified NodePath. Each time you
222 * call analyze, ARToolKit will update the NodePath's transform. If the node
223 * is not visible, its scale will be set to zero.
224 */
225void ARToolKit::
226attach_pattern(const Filename &filename, NodePath path) {
227 int patt = get_pattern(filename);
228 if (patt < 0) return;
229 _controls[patt] = path;
230}
231
232/**
233 * Dissociates all patterns from all NodePaths.
234 */
235void ARToolKit::
236detach_patterns() {
237 _controls.clear();
238}
239
240/**
241 * Analyzes the non-pad region of the specified texture. This causes all
242 * attached nodepaths to move. The parameter do_flip_texture is true by
243 * default, because Panda's representation of textures is upside down from
244 * ARToolKit. If you already have a texture that's upside-down, however, you
245 * should set it to false.
246 */
247void ARToolKit::
248analyze(Texture *tex, bool do_flip_texture) {
249 // We shouldn't assert on has_ram_image since it also returns false when
250 // there is a ram image but it's not updated for this frame.
251 // nassertv(tex->has_ram_image());
252 nassertv(tex->get_ram_image_compression() == Texture::CM_off);
253 nassertv(tex->get_component_type() == Texture::T_unsigned_byte);
254 nassertv(tex->get_texture_type() == Texture::TT_2d_texture);
255
256 if (tex->get_num_components() != 3 && tex->get_num_components() != 4) {
257 vision_cat.error() << "ARToolKit can only analyze RGB and RGBA textures.\n";
258 return;
259 }
260
261 int padx = tex->get_pad_x_size();
262 int pady = tex->get_pad_y_size();
263 int xsize = tex->get_x_size() - padx;
264 int ysize = tex->get_y_size() - pady;
265 // int pagesize = xsize * ysize * 4;
266 nassertv((xsize > 0) && (ysize > 0));
267
268 // row length in bytes
269 int srclen = tex->get_x_size() * tex->get_num_components();
270
271 ARParam cparam;
272 change_size((ARParam*)_camera_param, xsize, ysize, &cparam);
273 arInitCparam(&cparam);
274
275 // Pack the data into a buffer with no padding and invert the video
276 // vertically (panda's representation is upside down from ARToolKit) Note:
277 // ARToolKit treats the images as grayscale, so the order of the individual
278 // R, G and B components does not matter.
279
280 CPTA_uchar ri = tex->get_ram_image();
281 const unsigned char *ram = ri.p();
282
283 if (ram == nullptr) {
284 vision_cat.warning() << "No data in texture!\n";
285 return;
286 }
287
288 unsigned char *data;
289 unsigned char *dstrow;
290 const unsigned char *srcrow;
291
292 if (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGB ||
293 AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGR) {
294 data = new unsigned char[xsize * ysize * 3];
295 int dstlen = xsize * 3;
296 if (tex->get_num_components() == 3) {
297 if (do_flip_texture) {
298 for (int y = 0; y < ysize; ++y) {
299 int invy = (ysize - y - 1);
300 memcpy(data + invy * dstlen, ram + y * srclen, dstlen);
301 }
302 } else if (dstlen == srclen) {
303 memcpy(data, ram, ysize * srclen);
304 } else {
305 for (int y = 0; y < ysize; ++y) {
306 memcpy(data + y * dstlen, ram + y * srclen, dstlen);
307 }
308 }
309 } else {
310 // Chop off the alpha component.
311 if (do_flip_texture) {
312 for (int y = 0; y < ysize; ++y) {
313 dstrow = data + dstlen * (ysize - y - 1);
314 srcrow = ram + srclen * y;
315 for (int x = 0; x < xsize; ++x) {
316 memcpy(dstrow + x * 3, srcrow + x * 4, 3);
317 }
318 }
319 } else {
320 for (int y = 0; y < ysize; y++) {
321 dstrow = data + dstlen * y;
322 srcrow = ram + srclen * y;
323 for (int x = 0; x < xsize; x++) {
324 memcpy(dstrow + x * 3, srcrow + x * 4, 3);
325 }
326 }
327 }
328 }
329 } else if (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGBA ||
330 AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGRA) {
331 data = new unsigned char[xsize * ysize * 4];
332 int dstlen = xsize * 4;
333 if (tex->get_num_components() == 3) {
334 // We'll have to add an alpha component.
335 if (do_flip_texture) {
336 for (int y = 0; y < ysize; ++y) {
337 dstrow = data + dstlen * (ysize - y - 1);
338 srcrow = ram + srclen * y;
339 for (int x = 0; x < xsize; ++x) {
340 memcpy(dstrow + x * 4, srcrow + x * 3, 3);
341 dstrow[x * 4 + 3] = 255;
342 }
343 }
344 } else {
345 for (int y = 0; y < ysize; ++y) {
346 dstrow = data + dstlen * y;
347 srcrow = ram + srclen * y;
348 for (int x = 0; x < xsize; ++x) {
349 memcpy(dstrow + x * 4, srcrow + x * 3, 3);
350 dstrow[x * 4 + 3] = 255;
351 }
352 }
353 }
354 } else {
355 if (do_flip_texture) {
356 for (int y = 0; y < ysize; ++y) {
357 int invy = (ysize - y - 1);
358 memcpy(data + invy * dstlen, ram + y * srclen, dstlen);
359 }
360 } else if (dstlen == srclen) {
361 memcpy(data, ram, ysize * srclen);
362 } else {
363 for (int y = 0; y < ysize; ++y) {
364 memcpy(data + y * dstlen, ram + y * srclen, dstlen);
365 }
366 }
367 }
368 } else { // ARToolKit wants ARGB / ABGR.
369 data = new unsigned char[xsize * ysize * 4];
370 int dstlen = xsize * 4;
371 if (tex->get_num_components() == 3) {
372 // We'll have to add an alpha component.
373 if (do_flip_texture) {
374 for (int y = 0; y < ysize; ++y) {
375 dstrow = data + dstlen * (ysize - y - 1);
376 srcrow = ram + srclen * y;
377 for (int x = 0; x < xsize; ++x) {
378 memcpy(dstrow + x * 4 + 1, srcrow + x * 3, 3);
379 dstrow[x * 4] = 255;
380 }
381 }
382 } else {
383 for (int y = 0; y < ysize; ++y) {
384 dstrow = data + dstlen * y;
385 srcrow = ram + srclen * y;
386 for (int x = 0; x < xsize; ++x) {
387 memcpy(dstrow + x * 4 + 1, srcrow + x * 3, 3);
388 dstrow[x * 4] = 255;
389 }
390 }
391 }
392 } else {
393 if (do_flip_texture) {
394 for (int y = 0; y < ysize; ++y) {
395 dstrow = data + dstlen * (ysize - y - 1);
396 srcrow = ram + srclen * y;
397 for (int x = 0; x < xsize; ++x) {
398 memcpy(dstrow + x * 4 + 1, srcrow + x * 4, 3);
399 dstrow[x * 4] = srcrow[x * 4 + 3];
400 }
401 }
402 } else {
403 for (int y = 0; y < ysize; ++y) {
404 dstrow = data + dstlen * y;
405 srcrow = ram + srclen * y;
406 for (int x = 0; x < xsize; ++x) {
407 memcpy(dstrow + x * 4 + 1, srcrow + x * 4, 3);
408 dstrow[x * 4] = srcrow[x * 4 + 3];
409 }
410 }
411 }
412 }
413 }
414
415 // Activate the patterns.
416 Controls::const_iterator ctrli;
417 for (ctrli = _controls.begin(); ctrli != _controls.end(); ++ctrli) {
418 arActivatePatt((*ctrli).first);
419 }
420
421 ARMarkerInfo *marker_info;
422 int marker_num;
423
424 if (arDetectMarker(data, _threshold * 256, &marker_info, &marker_num) < 0) {
425 vision_cat.error() << "ARToolKit detection error.\n";
426 delete[] data;
427 return;
428 }
429
430 for (ctrli = _controls.begin(); ctrli != _controls.end(); ++ctrli) {
431 NodePath np = (*ctrli).second;
432 int pattern = (*ctrli).first;
433 arDeactivatePatt(pattern);
434 double conf = -1;
435 int best = -1;
436 for (int i = 0; i < marker_num; ++i) {
437 if (marker_info[i].id == pattern) {
438 if (marker_info[i].cf >= conf) {
439 conf = marker_info[i].cf;
440 best = i;
441 }
442 }
443 }
444 if (conf > 0.0) {
445 ARMarkerInfo *inf = &marker_info[best];
446 double center[2];
447 center[0] = 0.0;
448 center[1] = 0.0;
449 if (_have_prev_conv) {
450 arGetTransMatCont(inf, _prev_conv, center, _marker_size, _prev_conv);
451 } else {
452 arGetTransMat(inf, center, _marker_size, _prev_conv);
453 _have_prev_conv = true;
454 }
455 LMatrix4 mat;
456 for (int i = 0; i < 4; ++i) {
457 mat(i, 0) = _prev_conv[0][i];
458 mat(i, 1) = _prev_conv[2][i];
459 mat(i, 2) = -_prev_conv[1][i];
460 mat(i, 3) = 0.0;
461 }
462 mat(3,3) = 1.0;
463 LVecBase3 scale, shear, hpr, pos;
464 decompose_matrix(mat, scale, shear, hpr, pos);
465
466 if (np.get_parent().is_empty()) {
467 vision_cat.error() << "NodePath must have a parent.\n";
468 } else {
469 np.set_pos_hpr(_camera, pos, hpr);
470 }
471
472 np.show();
473 } else {
474 np.hide();
475 }
476 }
477
478 delete[] data;
479}
480
481#endif // HAVE_ARTOOLKIT
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
A node that can be positioned around in the scene graph to represent a point of view for rendering a ...
Definition camera.h:35
const Element * p() const
Function p() is similar to the function from ConstPointerTo.
The name of a file, such as a texture file or an Egg file.
Definition filename.h:44
std::string to_os_specific() const
Converts the filename from our generic Unix-like convention (forward slashes starting with the root a...
Lens * get_lens(int index=0) const
Returns a pointer to the particular Lens associated with this LensNode, or NULL if there is not yet a...
Definition lensNode.I:47
A base class for any number of different kinds of lenses, linear and otherwise.
Definition lens.h:41
set_fov
Sets the horizontal field of view of the lens without changing the aspect ratio.
Definition lens.h:101
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
Definition nodePath.h:159
void show()
Undoes the effect of a previous hide() on this node: makes the referenced node (and the entire subgra...
Definition nodePath.I:1796
void hide()
Makes the referenced node (and the entire subgraph below this node) invisible to all cameras.
Definition nodePath.I:1853
bool is_empty() const
Returns true if the NodePath contains no nodes.
Definition nodePath.I:188
PandaNode * node() const
Returns the referenced node of the path.
Definition nodePath.I:227
get_parent
Returns the NodePath to the parent of the referenced node: that is, this NodePath,...
Definition nodePath.h:242
void set_pos_hpr(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z, PN_stdfloat h, PN_stdfloat p, PN_stdfloat r)
Sets the translation and rotation component of the transform, leaving scale untouched.
Definition nodePath.I:728
void clear()
Sets this NodePath to the empty NodePath.
Definition nodePath.I:119
A basic node of the scene graph or data graph.
Definition pandaNode.h:65
Represents a texture object, which is typically a single 2-d image but may also represent a 1-d or 3-...
Definition texture.h:72
get_texture_type
Returns the overall interpretation of the texture.
Definition texture.h:366
int get_pad_y_size() const
Returns size of the pad region.
Definition texture.I:630
CPTA_uchar get_ram_image()
Returns the system-RAM image data associated with the texture.
Definition texture.I:1357
get_ram_image_compression
Returns the compression mode in which the ram image is already stored pre- compressed.
Definition texture.h:472
get_y_size
Returns the height of the texture image in texels.
Definition texture.h:347
int get_pad_x_size() const
Returns size of the pad region.
Definition texture.I:621
get_num_components
Returns the number of color components for each texel of the texture image.
Definition texture.h:364
get_x_size
Returns the width of the texture image in texels.
Definition texture.h:343
get_component_type
Returns the numeric interpretation of each component of the texture.
Definition texture.h:375
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.