00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
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_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
00178
00179
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
00192
00193
00194
00195 ARToolKit::
00196 ARToolKit() : _have_prev_conv(false) {
00197 }
00198
00199
00200
00201
00202
00203
00204 ARToolKit::
00205 ~ARToolKit() {
00206 cleanup();
00207 }
00208
00209
00210
00211
00212
00213
00214
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
00236
00237
00238
00239
00240
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
00251
00252
00253
00254 void ARToolKit::
00255 detach_patterns() {
00256 _controls.clear();
00257 }
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270 void ARToolKit::
00271 analyze(Texture *tex, bool do_flip_texture) {
00272
00273
00274
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
00289 nassertv((xsize > 0) && (ysize > 0));
00290
00291
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
00299
00300
00301
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
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
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 {
00392 data = new unsigned char[xsize * ysize * 4];
00393 int dstlen = xsize * 4;
00394 if (tex->get_num_components() == 3) {
00395
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
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