Panda3D

arToolKit.cxx

00001 // Filename: arToolKit.cxx
00002 // Created by: jyelon (01Nov2007)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 #include "arToolKit.h"
00016 
00017 #ifdef HAVE_ARTOOLKIT
00018 
00019 #include "pandaNode.h"
00020 #include "camera.h"
00021 #include "perspectiveLens.h"
00022 #include "lvecBase3.h"
00023 #include "compose_matrix.h"
00024 #include "config_vision.h"
00025 extern "C" {
00026   #include "AR/ar.h"
00027 };
00028 
00029 ARToolKit::PatternTable ARToolKit::_pattern_table;
00030 
00031 static void change_size( ARParam *source, int xsize, int ysize, ARParam *newparam )
00032 {
00033     int     i;
00034 
00035     newparam->xsize = xsize;
00036     newparam->ysize = ysize;
00037 
00038     double xscale = (double)xsize / (double)(source->xsize);
00039     double yscale = (double)ysize / (double)(source->ysize);
00040     for( i = 0; i < 4; i++ ) {
00041       newparam->mat[0][i] = source->mat[0][i] * xscale;
00042       newparam->mat[1][i] = source->mat[1][i] * yscale;
00043       newparam->mat[2][i] = source->mat[2][i];
00044     }
00045 
00046     newparam->dist_factor[0] = source->dist_factor[0] * xscale;
00047     newparam->dist_factor[1] = source->dist_factor[1] * yscale;
00048     newparam->dist_factor[2] = source->dist_factor[2] / (xscale*yscale);
00049     newparam->dist_factor[3] = source->dist_factor[3];
00050 }
00051 
00052 static void analyze_fov(double cparam[3][4], int width, int height, double &xfov, double &yfov)
00053 {
00054   double   gnear = 10.0;
00055   double gfar = 1000.0;
00056   double   icpara[3][4];
00057   double   trans[3][4];
00058   double   p[3][3], q[4][4];
00059   double   xval, yval;
00060   int      i, j;
00061 
00062   if( arParamDecompMat(cparam, icpara, trans) < 0 ) {
00063     printf("gConvGLcpara: Parameter error!!\n");
00064     exit(0);
00065   }
00066 
00067   for( i = 0; i < 3; i++ ) {
00068     for( j = 0; j < 3; j++ ) {
00069       p[i][j] = icpara[i][j] / icpara[2][2];
00070     }
00071   }
00072   q[0][0] = (2.0 * p[0][0] / width);
00073   q[0][1] = (2.0 * p[0][1] / width);
00074   q[0][2] = ((2.0 * p[0][2] / width)  - 1.0);
00075   q[0][3] = 0.0;
00076 
00077   q[1][0] = 0.0;
00078   q[1][1] = (2.0 * p[1][1] / height);
00079   q[1][2] = ((2.0 * p[1][2] / height) - 1.0);
00080   q[1][3] = 0.0;
00081 
00082   q[2][0] = 0.0;
00083   q[2][1] = 0.0;
00084   q[2][2] = (gfar + gnear)/(gfar - gnear);
00085   q[2][3] = -2.0 * gfar * gnear / (gfar - gnear);
00086 
00087   q[3][0] = 0.0;
00088   q[3][1] = 0.0;
00089   q[3][2] = 1.0;
00090   q[3][3] = 0.0;
00091 
00092   xval =
00093     q[0][0] * trans[0][0] +
00094     q[0][1] * trans[1][0] +
00095     q[0][2] * trans[2][0];
00096   yval =
00097     q[1][0] * trans[0][1] +
00098     q[1][1] * trans[1][1] +
00099     q[1][2] * trans[2][1];
00100 
00101   xfov = 2.0 * atan(1.0/xval) * (180.0/3.141592654);
00102   yfov = 2.0 * atan(1.0/yval) * (180.0/3.141592654);
00103 }
00104 
00105 ////////////////////////////////////////////////////////////////////
00106 //     Function: ARToolKit::make
00107 //       Access: Private
00108 //  Description: Create a new ARToolKit instance.
00109 //
00110 //               Camera must be the nodepath of a panda camera object.
00111 //               The panda camera's field of view is initialized to match
00112 //               the field of view of the physical webcam.  Each time
00113 //               you call analyze, all marker nodepaths will be moved
00114 //               into a position which is relative to this camera.
00115 //               The marker_size parameter indicates how large you
00116 //               printed the physical markers.  You should use the same
00117 //               size units that you wish to use in the panda code.
00118 ////////////////////////////////////////////////////////////////////
00119 ARToolKit *ARToolKit::
00120 make(NodePath camera, const Filename &paramfile, double marker_size) {
00121 
00122   if (AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_BGRA &&
00123       AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_RGBA &&
00124       AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_RGB &&
00125       AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_BGR) {
00126     vision_cat.error() <<
00127       "The copy of ARToolKit that you are using is not compiled "
00128       "for RGB, BGR, RGBA or BGRA input.  Panda3D cannot use "
00129       "this copy of ARToolKit. Please modify the ARToolKit's "
00130       "config file and compile it again.\n";
00131     return 0;
00132   }
00133 
00134   if (camera.is_empty()) {
00135     vision_cat.error() << "ARToolKit: invalid camera nodepath\n";
00136     return 0;
00137   }
00138   PandaNode *node = camera.node();
00139   if ((node == 0) || (node->get_type() != Camera::get_class_type())) {
00140     vision_cat.error() << "ARToolKit: invalid camera nodepath\n";
00141     return 0;
00142   }
00143   Camera *cam = DCAST(Camera, node);
00144   Lens *lens = cam->get_lens();
00145   if (lens->get_type() != PerspectiveLens::get_class_type()) {
00146     vision_cat.error() << "ARToolKit: supplied camera node must be perspective.\n";
00147     return 0;
00148   }
00149 
00150   ARParam wparam;
00151   string fn = paramfile.to_os_specific();
00152   if( arParamLoad(fn.c_str(), 1, &wparam) < 0 ) {
00153     vision_cat.error() << "Cannot load ARToolKit camera config\n";
00154     return 0;
00155   }
00156 
00157   arParamDisp(&wparam);
00158   double xfov, yfov;
00159   analyze_fov(wparam.mat, 640, 480, xfov, yfov);
00160 
00161   lens->set_fov(xfov, yfov);
00162 
00163   ARToolKit *result = new ARToolKit();
00164   result->_camera = camera;
00165   result->_camera_param = new ARParam;
00166   result->_threshold = 0.5;
00167   result->_marker_size = marker_size;
00168   result->_have_prev_conv = false;
00169   memcpy(result->_camera_param, &wparam, sizeof(wparam));
00170   return result;
00171 }
00172 
00173 
00174 ////////////////////////////////////////////////////////////////////
00175 //     Function: ARToolKit::cleanup
00176 //       Access: private
00177 //  Description: Pre-destructor deallocation and cleanup.
00178 ////////////////////////////////////////////////////////////////////
00179 void ARToolKit::
00180 cleanup() {
00181   if (_camera_param) {
00182     ARParam *param = (ARParam *)_camera_param;
00183     delete param;
00184     _camera_param = 0;
00185   }
00186 }
00187 
00188 ////////////////////////////////////////////////////////////////////
00189 //     Function: ARToolKit::Constructor
00190 //       Access: Private
00191 //  Description: Use ARToolKit::make to create an ARToolKit.
00192 ////////////////////////////////////////////////////////////////////
00193 ARToolKit::
00194 ARToolKit() {
00195 }
00196 
00197 ////////////////////////////////////////////////////////////////////
00198 //     Function: ARToolKit::Destructor
00199 //       Access: Published
00200 //  Description:
00201 ////////////////////////////////////////////////////////////////////
00202 ARToolKit::
00203 ~ARToolKit() {
00204   cleanup();
00205 }
00206 
00207 ////////////////////////////////////////////////////////////////////
00208 //     Function: ARToolKit::get_pattern
00209 //       Access: Private
00210 //  Description: Load the specified pattern into the toolkit, and
00211 //               return the pattern index.  Initially, the pattern
00212 //               is inactive.
00213 ////////////////////////////////////////////////////////////////////
00214 int ARToolKit::
00215 get_pattern(const Filename &filename) {
00216   PatternTable::iterator ptf = _pattern_table.find(filename);
00217   if (ptf != _pattern_table.end()) {
00218     return (*ptf).second;
00219   }
00220 
00221   string fn = filename.to_os_specific();
00222   int id = arLoadPatt(fn.c_str());
00223   if (id < 0) {
00224     vision_cat.error() << "Could not load AR ToolKit Pattern: " << fn << "\n";
00225     return -1;
00226   }
00227   arDeactivatePatt(id);
00228   _pattern_table[filename] = id;
00229   return id;
00230 }
00231 
00232 ////////////////////////////////////////////////////////////////////
00233 //     Function: ARToolKit::attach_pattern
00234 //       Access: Public
00235 //  Description: Associates the specified glyph with the specified
00236 //               NodePath.  Each time you call analyze, ARToolKit
00237 //               will update the NodePath's transform.  If the node
00238 //               is not visible, its scale will be set to zero.
00239 ////////////////////////////////////////////////////////////////////
00240 void ARToolKit::
00241 attach_pattern(const Filename &filename, NodePath path) {
00242   int patt = get_pattern(filename);
00243   if (patt < 0) return;
00244   _controls[patt] = path;
00245 }
00246 
00247 ////////////////////////////////////////////////////////////////////
00248 //     Function: ARToolKit::detach_patterns
00249 //       Access: Public
00250 //  Description: Dissociates all patterns from all NodePaths.
00251 ////////////////////////////////////////////////////////////////////
00252 void ARToolKit::
00253 detach_patterns() {
00254   _controls.clear();
00255 }
00256 
00257 ////////////////////////////////////////////////////////////////////
00258 //     Function: ARToolKit::analyze
00259 //       Access: Public
00260 //  Description: Analyzes the non-pad region of the specified texture.
00261 //               This causes all attached nodepaths to move.
00262 //               The parameter do_flip_texture is true by default,
00263 //               because Panda's representation of textures is
00264 //               upside down from ARToolKit. If you already have
00265 //               a texture that's upside-down, however, you should
00266 //               set it to false.
00267 ////////////////////////////////////////////////////////////////////
00268 void ARToolKit::
00269 analyze(Texture *tex, bool do_flip_texture) {
00270   // We shouldn't assert on has_ram_image since it also returns false
00271   // when there is a ram image but its not updated for this frame.
00272   //nassertv(tex->has_ram_image());
00273   nassertv(tex->get_ram_image_compression() == Texture::CM_off);
00274   nassertv(tex->get_component_type() == Texture::T_unsigned_byte);
00275   nassertv(tex->get_texture_type() == Texture::TT_2d_texture);
00276 
00277   if (tex->get_num_components() != 3 && tex->get_num_components() != 4) {
00278     vision_cat.error() << "ARToolKit can only analyze RGB and RGBA textures.\n";
00279     return;
00280   }
00281 
00282   int padx = tex->get_pad_x_size();
00283   int pady = tex->get_pad_y_size();
00284   int xsize = tex->get_x_size() - padx;
00285   int ysize = tex->get_y_size() - pady;
00286   //int pagesize = xsize * ysize * 4;
00287   nassertv((xsize > 0) && (ysize > 0));
00288 
00289   ARParam cparam;
00290   change_size( (ARParam*)_camera_param, xsize, ysize, &cparam );
00291   arInitCparam( &cparam );
00292 
00293   // Pack the data into a buffer with no padding and invert the video
00294   // vertically (panda's representation is upside down from ARToolKit)
00295 
00296   CPTA_uchar ri = tex->get_ram_image();
00297   const unsigned char *ram = ri.p();
00298 
00299   unsigned char *data;
00300   if (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGB ||
00301       AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGR) {
00302     data = new unsigned char[xsize * ysize * 3];
00303     int dstlen = xsize * 3;
00304     if (tex->get_num_components() == 3) {
00305       int srclen = (xsize + padx) * 3;
00306       if (do_flip_texture) {
00307         for (int y=0; y<ysize; y++) {
00308           int invy = (ysize - y - 1);
00309           memcpy(data + invy * dstlen, ram + y * srclen, dstlen);
00310         }
00311       } else if (dstlen == srclen) {
00312         memcpy(data, ram, ysize * srclen);
00313       } else {
00314         for (int y=0; y<ysize; y++) {
00315           memcpy(data + y * dstlen, ram + y * srclen, dstlen);
00316         }
00317       }
00318     } else {
00319       // Chop off the alpha component.
00320       int srclen = (xsize + padx) * 4;
00321       if (do_flip_texture) {
00322         for (int y=0; y<ysize; y++) {
00323           int invy = (ysize - y - 1);
00324           for (int x=0; x<xsize; x++) {
00325             memcpy(data + invy * dstlen + x * 3, ram + y * srclen + x * 4, 3);
00326           }
00327         }
00328       } else {
00329         for (int y=0; y<ysize; y++) {
00330           for (int x=0; x<xsize; x++) {
00331             memcpy(data + y * dstlen + x * 3, ram + y * srclen + x * 4, 3);
00332           }
00333         }
00334       }
00335     }
00336   } else { // ARToolKit wants RGBA.
00337     data = new unsigned char[xsize * ysize * 4];
00338     int dstlen = xsize * 4;
00339     if (tex->get_num_components() == 3) {
00340       // Until we find a better solution, we'll need to add the Alpha component ourselves.
00341       int srclen = (xsize + padx) * 3;
00342       if (do_flip_texture) {
00343         for (int y=0; y<ysize; y++) {
00344           int invy = (ysize - y - 1);
00345           memcpy(data + invy * dstlen, ram + y * srclen, dstlen - 1);
00346           for (int x=0; x<xsize; x++) {
00347             data[invy * dstlen + x * 4 + 3] = -1;
00348           }
00349         }
00350       } else {
00351         for (int y=0; y<ysize; y++) {
00352           memcpy(data + y * dstlen, ram + y * srclen, dstlen - 1);
00353           for (int x=0; x<xsize; x++) {
00354             data[y * dstlen + x * 4 + 3] = -1;
00355           }
00356         }
00357       }
00358     } else {
00359       int srclen = (xsize + padx) * 4;
00360       if (do_flip_texture) {
00361         for (int y=0; y<ysize; y++) {
00362           int invy = (ysize - y - 1);
00363           memcpy(data + invy * dstlen, ram + y * srclen, dstlen);
00364         }
00365       } else if (dstlen == srclen) {
00366         memcpy(data, ram, ysize * srclen);
00367       } else {
00368         for (int y=0; y<ysize; y++) {
00369           memcpy(data + y * dstlen, ram + y * srclen, dstlen);
00370         }
00371       }
00372     }
00373   }
00374 
00375   Controls::const_iterator ctrli;
00376   for (ctrli=_controls.begin(); ctrli!=_controls.end(); ctrli++) {
00377     arActivatePatt((*ctrli).first);
00378   }
00379 
00380   ARMarkerInfo *marker_info;
00381   int marker_num;
00382 
00383   if (arDetectMarker(data, _threshold * 256, &marker_info, &marker_num) < 0) {
00384     vision_cat.error() << "ARToolKit detection error.\n";
00385     delete data;
00386     return;
00387   }
00388 
00389   for (ctrli=_controls.begin(); ctrli!=_controls.end(); ctrli++) {
00390     NodePath np = (*ctrli).second;
00391     int pattern = (*ctrli).first;
00392     arDeactivatePatt(pattern);
00393     double conf = -1;
00394     int best = -1;
00395     for (int i=0; i<marker_num; i++) {
00396       if (marker_info[i].id == pattern) {
00397         if (marker_info[i].cf >= conf) {
00398           conf = marker_info[i].cf;
00399           best = i;
00400         }
00401       }
00402     }
00403     if (conf > 0.0) {
00404       ARMarkerInfo *inf = &marker_info[best];
00405       double center[2];
00406       center[0] = 0.0;
00407       center[1] = 0.0;
00408       if (_have_prev_conv) {
00409         arGetTransMatCont(inf, _prev_conv, center, _marker_size, _prev_conv);
00410       } else {
00411         arGetTransMat(inf, center, _marker_size, _prev_conv);
00412         _have_prev_conv = true;
00413       }
00414       LMatrix4 mat;
00415       for (int i=0; i<4; i++) {
00416         mat(i,0) =  _prev_conv[0][i];
00417         mat(i,1) =  _prev_conv[2][i];
00418         mat(i,2) = -_prev_conv[1][i];
00419         mat(i,3) = 0.0;
00420       }
00421       mat(3,3) = 1.0;
00422       LVecBase3 scale, shear, hpr, pos;
00423       decompose_matrix(mat, scale, shear, hpr, pos);
00424 
00425       if (np.get_parent().is_empty()) {
00426         vision_cat.error() << "NodePath must have a parent.\n";
00427       } else {
00428         np.set_pos_hpr(_camera, pos, hpr);
00429       }
00430 
00431       np.show();
00432     } else {
00433       np.hide();
00434     }
00435   }
00436 
00437   delete data;
00438 }
00439 
00440 #endif // HAVE_ARTOOLKIT
00441 
 All Classes Functions Variables Enumerations