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