Panda3D
 All Classes Functions Variables Enumerations
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_ARGB &&
00125       AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_ABGR &&
00126       AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_RGB &&
00127       AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_BGR) {
00128     vision_cat.error() <<
00129       "The copy of ARToolKit that you are using is not compiled "
00130       "for RGB, BGR, RGBA, BGRA, ARGB or ABGR input.  Panda3D cannot "
00131       "use this copy of ARToolKit. Please modify the ARToolKit's "
00132       "config file and compile it again.\n";
00133     return 0;
00134   }
00135 
00136   if (camera.is_empty()) {
00137     vision_cat.error() << "ARToolKit: invalid camera nodepath\n";
00138     return 0;
00139   }
00140   PandaNode *node = camera.node();
00141   if ((node == 0) || (node->get_type() != Camera::get_class_type())) {
00142     vision_cat.error() << "ARToolKit: invalid camera nodepath\n";
00143     return 0;
00144   }
00145   Camera *cam = DCAST(Camera, node);
00146   Lens *lens = cam->get_lens();
00147   if (lens->get_type() != PerspectiveLens::get_class_type()) {
00148     vision_cat.error() << "ARToolKit: supplied camera node must be perspective.\n";
00149     return 0;
00150   }
00151 
00152   ARParam wparam;
00153   string fn = paramfile.to_os_specific();
00154   if( arParamLoad(fn.c_str(), 1, &wparam) < 0 ) {
00155     vision_cat.error() << "Cannot load ARToolKit camera config\n";
00156     return 0;
00157   }
00158 
00159   arParamDisp(&wparam);
00160   double xfov, yfov;
00161   analyze_fov(wparam.mat, 640, 480, xfov, yfov);
00162 
00163   lens->set_fov(xfov, yfov);
00164 
00165   ARToolKit *result = new ARToolKit();
00166   result->_camera = camera;
00167   result->_camera_param = new ARParam;
00168   result->_threshold = 0.5;
00169   result->_marker_size = marker_size;
00170   result->_have_prev_conv = false;
00171   memcpy(result->_camera_param, &wparam, sizeof(wparam));
00172   return result;
00173 }
00174 
00175 
00176 ////////////////////////////////////////////////////////////////////
00177 //     Function: ARToolKit::cleanup
00178 //       Access: private
00179 //  Description: Pre-destructor deallocation and cleanup.
00180 ////////////////////////////////////////////////////////////////////
00181 void ARToolKit::
00182 cleanup() {
00183   if (_camera_param) {
00184     ARParam *param = (ARParam *)_camera_param;
00185     delete param;
00186     _camera_param = 0;
00187   }
00188 }
00189 
00190 ////////////////////////////////////////////////////////////////////
00191 //     Function: ARToolKit::Constructor
00192 //       Access: Private
00193 //  Description: Use ARToolKit::make to create an ARToolKit.
00194 ////////////////////////////////////////////////////////////////////
00195 ARToolKit::
00196 ARToolKit() : _have_prev_conv(false) {
00197 }
00198 
00199 ////////////////////////////////////////////////////////////////////
00200 //     Function: ARToolKit::Destructor
00201 //       Access: Published
00202 //  Description:
00203 ////////////////////////////////////////////////////////////////////
00204 ARToolKit::
00205 ~ARToolKit() {
00206   cleanup();
00207 }
00208 
00209 ////////////////////////////////////////////////////////////////////
00210 //     Function: ARToolKit::get_pattern
00211 //       Access: Private
00212 //  Description: Load the specified pattern into the toolkit, and
00213 //               return the pattern index.  Initially, the pattern
00214 //               is inactive.
00215 ////////////////////////////////////////////////////////////////////
00216 int ARToolKit::
00217 get_pattern(const Filename &filename) {
00218   PatternTable::iterator ptf = _pattern_table.find(filename);
00219   if (ptf != _pattern_table.end()) {
00220     return (*ptf).second;
00221   }
00222 
00223   string fn = filename.to_os_specific();
00224   int id = arLoadPatt(fn.c_str());
00225   if (id < 0) {
00226     vision_cat.error() << "Could not load AR ToolKit Pattern: " << fn << "\n";
00227     return -1;
00228   }
00229   arDeactivatePatt(id);
00230   _pattern_table[filename] = id;
00231   return id;
00232 }
00233 
00234 ////////////////////////////////////////////////////////////////////
00235 //     Function: ARToolKit::attach_pattern
00236 //       Access: Public
00237 //  Description: Associates the specified glyph with the specified
00238 //               NodePath.  Each time you call analyze, ARToolKit
00239 //               will update the NodePath's transform.  If the node
00240 //               is not visible, its scale will be set to zero.
00241 ////////////////////////////////////////////////////////////////////
00242 void ARToolKit::
00243 attach_pattern(const Filename &filename, NodePath path) {
00244   int patt = get_pattern(filename);
00245   if (patt < 0) return;
00246   _controls[patt] = path;
00247 }
00248 
00249 ////////////////////////////////////////////////////////////////////
00250 //     Function: ARToolKit::detach_patterns
00251 //       Access: Public
00252 //  Description: Dissociates all patterns from all NodePaths.
00253 ////////////////////////////////////////////////////////////////////
00254 void ARToolKit::
00255 detach_patterns() {
00256   _controls.clear();
00257 }
00258 
00259 ////////////////////////////////////////////////////////////////////
00260 //     Function: ARToolKit::analyze
00261 //       Access: Public
00262 //  Description: Analyzes the non-pad region of the specified texture.
00263 //               This causes all attached nodepaths to move.
00264 //               The parameter do_flip_texture is true by default,
00265 //               because Panda's representation of textures is
00266 //               upside down from ARToolKit. If you already have
00267 //               a texture that's upside-down, however, you should
00268 //               set it to false.
00269 ////////////////////////////////////////////////////////////////////
00270 void ARToolKit::
00271 analyze(Texture *tex, bool do_flip_texture) {
00272   // We shouldn't assert on has_ram_image since it also returns false
00273   // when there is a ram image but it's not updated for this frame.
00274   //nassertv(tex->has_ram_image());
00275   nassertv(tex->get_ram_image_compression() == Texture::CM_off);
00276   nassertv(tex->get_component_type() == Texture::T_unsigned_byte);
00277   nassertv(tex->get_texture_type() == Texture::TT_2d_texture);
00278 
00279   if (tex->get_num_components() != 3 && tex->get_num_components() != 4) {
00280     vision_cat.error() << "ARToolKit can only analyze RGB and RGBA textures.\n";
00281     return;
00282   }
00283 
00284   int padx = tex->get_pad_x_size();
00285   int pady = tex->get_pad_y_size();
00286   int xsize = tex->get_x_size() - padx;
00287   int ysize = tex->get_y_size() - pady;
00288   //int pagesize = xsize * ysize * 4;
00289   nassertv((xsize > 0) && (ysize > 0));
00290 
00291   // row length in bytes
00292   int srclen = tex->get_x_size() * tex->get_num_components();
00293 
00294   ARParam cparam;
00295   change_size((ARParam*)_camera_param, xsize, ysize, &cparam);
00296   arInitCparam(&cparam);
00297 
00298   // Pack the data into a buffer with no padding and invert the video
00299   // vertically (panda's representation is upside down from ARToolKit)
00300   // Note: ARToolKit treats the images as grayscale, so the order of
00301   // the individual R, G and B components does not matter.
00302 
00303   CPTA_uchar ri = tex->get_ram_image();
00304   const unsigned char *ram = ri.p();
00305 
00306   if (ram == NULL) {
00307     vision_cat.warning() << "No data in texture!\n";
00308     return;
00309   }
00310 
00311   unsigned char *data;
00312   unsigned char *dstrow;
00313   const unsigned char *srcrow;
00314 
00315   if (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGB ||
00316       AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGR) {
00317     data = new unsigned char[xsize * ysize * 3];
00318     int dstlen = xsize * 3;
00319     if (tex->get_num_components() == 3) {
00320       if (do_flip_texture) {
00321         for (int y = 0; y < ysize; ++y) {
00322           int invy = (ysize - y - 1);
00323           memcpy(data + invy * dstlen, ram + y * srclen, dstlen);
00324         }
00325       } else if (dstlen == srclen) {
00326         memcpy(data, ram, ysize * srclen);
00327       } else {
00328         for (int y = 0; y < ysize; ++y) {
00329           memcpy(data + y * dstlen, ram + y * srclen, dstlen);
00330         }
00331       }
00332     } else {
00333       // Chop off the alpha component.
00334       if (do_flip_texture) {
00335         for (int y = 0; y < ysize; ++y) {
00336           dstrow = data + dstlen * (ysize - y - 1);
00337           srcrow = ram + srclen * y;
00338           for (int x = 0; x < xsize; ++x) {
00339             memcpy(dstrow + x * 3, srcrow + x * 4, 3);
00340           }
00341         }
00342       } else {
00343         for (int y = 0; y < ysize; y++) {
00344           dstrow = data + dstlen * y;
00345           srcrow = ram + srclen * y;
00346           for (int x = 0; x < xsize; x++) {
00347             memcpy(dstrow + x * 3, srcrow + x * 4, 3);
00348           }
00349         }
00350       }
00351     }
00352   } else if (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGBA ||
00353              AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGRA) {
00354     data = new unsigned char[xsize * ysize * 4];
00355     int dstlen = xsize * 4;
00356     if (tex->get_num_components() == 3) {
00357       // We'll have to add an alpha component.
00358       if (do_flip_texture) {
00359         for (int y = 0; y < ysize; ++y) {
00360           dstrow = data + dstlen * (ysize - y - 1);
00361           srcrow = ram + srclen * y;
00362           for (int x = 0; x < xsize; ++x) {
00363             memcpy(dstrow + x * 4, srcrow + x * 3, 3);
00364             dstrow[x * 4 + 3] = 255;
00365           }
00366         }
00367       } else {
00368         for (int y = 0; y < ysize; ++y) {
00369           dstrow = data + dstlen * y;
00370           srcrow = ram + srclen * y;
00371           for (int x = 0; x < xsize; ++x) {
00372             memcpy(dstrow + x * 4, srcrow + x * 3, 3);
00373             dstrow[x * 4 + 3] = 255;
00374           }
00375         }
00376       }
00377     } else {
00378       if (do_flip_texture) {
00379         for (int y = 0; y < ysize; ++y) {
00380           int invy = (ysize - y - 1);
00381           memcpy(data + invy * dstlen, ram + y * srclen, dstlen);
00382         }
00383       } else if (dstlen == srclen) {
00384         memcpy(data, ram, ysize * srclen);
00385       } else {
00386         for (int y = 0; y < ysize; ++y) {
00387           memcpy(data + y * dstlen, ram + y * srclen, dstlen);
00388         }
00389       }
00390     }
00391   } else { // ARToolKit wants ARGB / ABGR.
00392     data = new unsigned char[xsize * ysize * 4];
00393     int dstlen = xsize * 4;
00394     if (tex->get_num_components() == 3) {
00395       // We'll have to add an alpha component.
00396       if (do_flip_texture) {
00397         for (int y = 0; y < ysize; ++y) {
00398           dstrow = data + dstlen * (ysize - y - 1);
00399           srcrow = ram + srclen * y;
00400           for (int x = 0; x < xsize; ++x) {
00401             memcpy(dstrow + x * 4 + 1, srcrow + x * 3, 3);
00402             dstrow[x * 4] = 255;
00403           }
00404         }
00405       } else {
00406         for (int y = 0; y < ysize; ++y) {
00407           dstrow = data + dstlen * y;
00408           srcrow = ram + srclen * y;
00409           for (int x = 0; x < xsize; ++x) {
00410             memcpy(dstrow + x * 4 + 1, srcrow + x * 3, 3);
00411             dstrow[x * 4] = 255;
00412           }
00413         }
00414       }
00415     } else {
00416       if (do_flip_texture) {
00417         for (int y = 0; y < ysize; ++y) {
00418           dstrow = data + dstlen * (ysize - y - 1);
00419           srcrow = ram + srclen * y;
00420           for (int x = 0; x < xsize; ++x) {
00421             memcpy(dstrow + x * 4 + 1, srcrow + x * 4, 3);
00422             dstrow[x * 4] = srcrow[x * 4 + 3];
00423           }
00424         }
00425       } else {
00426         for (int y = 0; y < ysize; ++y) {
00427           dstrow = data + dstlen * y;
00428           srcrow = ram + srclen * y;
00429           for (int x = 0; x < xsize; ++x) {
00430             memcpy(dstrow + x * 4 + 1, srcrow + x * 4, 3);
00431             dstrow[x * 4] = srcrow[x * 4 + 3];
00432           }
00433         }
00434       }
00435     }
00436   }
00437 
00438   // Activate the patterns.
00439   Controls::const_iterator ctrli;
00440   for (ctrli = _controls.begin(); ctrli != _controls.end(); ++ctrli) {
00441     arActivatePatt((*ctrli).first);
00442   }
00443 
00444   ARMarkerInfo *marker_info;
00445   int marker_num;
00446 
00447   if (arDetectMarker(data, _threshold * 256, &marker_info, &marker_num) < 0) {
00448     vision_cat.error() << "ARToolKit detection error.\n";
00449     delete data;
00450     return;
00451   }
00452 
00453   for (ctrli = _controls.begin(); ctrli != _controls.end(); ++ctrli) {
00454     NodePath np = (*ctrli).second;
00455     int pattern = (*ctrli).first;
00456     arDeactivatePatt(pattern);
00457     double conf = -1;
00458     int best = -1;
00459     for (int i = 0; i < marker_num; ++i) {
00460       if (marker_info[i].id == pattern) {
00461         if (marker_info[i].cf >= conf) {
00462           conf = marker_info[i].cf;
00463           best = i;
00464         }
00465       }
00466     }
00467     if (conf > 0.0) {
00468       ARMarkerInfo *inf = &marker_info[best];
00469       double center[2];
00470       center[0] = 0.0;
00471       center[1] = 0.0;
00472       if (_have_prev_conv) {
00473         arGetTransMatCont(inf, _prev_conv, center, _marker_size, _prev_conv);
00474       } else {
00475         arGetTransMat(inf, center, _marker_size, _prev_conv);
00476         _have_prev_conv = true;
00477       }
00478       LMatrix4 mat;
00479       for (int i = 0; i < 4; ++i) {
00480         mat(i, 0) =  _prev_conv[0][i];
00481         mat(i, 1) =  _prev_conv[2][i];
00482         mat(i, 2) = -_prev_conv[1][i];
00483         mat(i, 3) = 0.0;
00484       }
00485       mat(3,3) = 1.0;
00486       LVecBase3 scale, shear, hpr, pos;
00487       decompose_matrix(mat, scale, shear, hpr, pos);
00488 
00489       if (np.get_parent().is_empty()) {
00490         vision_cat.error() << "NodePath must have a parent.\n";
00491       } else {
00492         np.set_pos_hpr(_camera, pos, hpr);
00493       }
00494 
00495       np.show();
00496     } else {
00497       np.hide();
00498     }
00499   }
00500 
00501   delete data;
00502 }
00503 
00504 #endif // HAVE_ARTOOLKIT
00505 
 All Classes Functions Variables Enumerations