Panda3D
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"
24 extern "C" {
25  #include <AR/ar.h>
26 };
27 
28 ARToolKit::PatternTable ARToolKit::_pattern_table;
29 
30 static 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 
51 static 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  */
114 ARToolKit *ARToolKit::
115 make(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  */
174 void ARToolKit::
175 cleanup() {
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  */
186 ARToolKit::
187 ARToolKit() : _have_prev_conv(false) {
188 }
189 
190 /**
191  *
192  */
193 ARToolKit::
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  */
202 int ARToolKit::
203 get_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  */
225 void ARToolKit::
226 attach_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  */
235 void ARToolKit::
236 detach_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  */
247 void ARToolKit::
248 analyze(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:39
std::string to_os_specific() const
Converts the filename from our generic Unix-like convention (forward slashes starting with the root a...
Definition: filename.cxx:1123
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:71
get_texture_type
Returns the overall interpretation of the texture.
Definition: texture.h:365
int get_pad_y_size() const
Returns size of the pad region.
Definition: texture.I:629
CPTA_uchar get_ram_image()
Returns the system-RAM image data associated with the texture.
Definition: texture.I:1356
get_ram_image_compression
Returns the compression mode in which the ram image is already stored pre- compressed.
Definition: texture.h:471
get_y_size
Returns the height of the texture image in texels.
Definition: texture.h:346
int get_pad_x_size() const
Returns size of the pad region.
Definition: texture.I:620
get_num_components
Returns the number of color components for each texel of the texture image.
Definition: texture.h:363
get_x_size
Returns the width of the texture image in texels.
Definition: texture.h:342
get_component_type
Returns the numeric interpretation of each component of the texture.
Definition: texture.h:374
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.