Panda3D
|
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 ¶mfile, 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