$search
00001 00002 #include <blort/Tracker/Tracker.h> 00003 #include <stdexcept> 00004 #include <iostream> 00005 #include <opencv2/imgproc/imgproc.hpp> 00006 00007 using namespace Tracking; 00008 using namespace TomGine; 00009 using namespace std; 00010 00011 // *** PUBLIC *** 00012 00013 Tracker::Parameter::Parameter(){ 00014 // Default parameter 00015 camPar.width = 640; 00016 camPar.height = 480; 00017 model_id_count = 0; 00018 hypotheses_id_count = 0; 00019 num_particles = 100; 00020 num_recursions = 2; 00021 hypotheses_trials = 5; 00022 convergence = 5; 00023 edge_tolerance = 45.0f * pi/180.0f; 00024 num_spreadings = 3; 00025 m_spreadlvl = 2; 00026 variation = Particle(vec3(0.01f, 0.01f, 0.01f), vec3(0.1f,0.1f,0.1f), 0.01f, 0.01f, 0.01f, TomGine::tgQuaternion()); 00027 minTexGrabAngle = 3.0f*PI/4.0f; 00028 max_kernel_size = 3; 00029 kernel_size = 1; 00030 modelPath = string("../Resources/model/"); 00031 texturePath = string("../Resources/texture/"); 00032 shaderPath = string("../Resources/shader/"); 00033 image_sobel_th = 0.02f; 00034 model_sobel_th = 0.03f; 00035 c_th_base=0.6f; 00036 c_th_min=0.3f; 00037 c_th_fair=0.5f; 00038 c_mv_not=0.01f; 00039 c_mv_slow=0.05f; 00040 c_th_lost=0.1f; 00041 pred_no_convergence=0.2f; 00042 } 00043 00044 Tracker::Tracker(){ 00045 // Default flags 00046 m_lock = false; 00047 m_showparticles = false; 00048 m_showmodel = 1; 00049 m_draw_edges = false; 00050 m_tracker_initialized = false; 00051 } 00052 00053 Tracker::~Tracker(){ 00054 unsigned i; 00055 delete(m_tex_frame); 00056 for(i=0; i<m_tex_frame_ip.size(); i++){ 00057 delete(m_tex_frame_ip[i]); 00058 } 00059 for(i=0; i<m_modellist.size(); i++){ 00060 delete(m_modellist[i]); 00061 } 00062 delete g_Resources; 00063 } 00064 00065 bool Tracker::initGL(){ 00066 glEnable(GL_DEPTH_TEST); 00067 glDepthFunc(GL_LEQUAL); 00068 glEnable(GL_CULL_FACE); 00069 glCullFace(GL_BACK); 00070 00071 // commented because this gives a segfault at strstr(.......) 00072 // const GLubyte *str; 00073 // int glOcclusionQueryAvailable; 00074 // 00075 // // Check for Extension 00076 // str = glGetString(GL_EXTENSIONS); 00077 // 00078 // glOcclusionQueryAvailable = (strstr((const char *)str, "GL_ARB_occlusion_query") != NULL); 00079 // if(!glOcclusionQueryAvailable){ 00080 // char errmsg[256]; 00081 // sprintf(errmsg, "[OpenGLControl] Error OpenGL extension 'GL_ARB_occlusion_query' not available. Your graphic card does not support this extension or the hardware driver for your graphic card is not installed properly!"); 00082 // throw std::runtime_error(errmsg); 00083 // } 00084 00085 return true; 00086 } 00087 00088 bool Tracker::loadCamParsFromINI(const char* camCalFile, const char* poseCalFile){ 00089 CDataFile camCDF, poseCDF; 00090 00091 if(!camCDF.Load(camCalFile)){ 00092 char errmsg[128]; 00093 sprintf(errmsg, "[Tracker::loadCamParsFromINI] Cannot open file %s", camCalFile); 00094 throw std::runtime_error(errmsg); 00095 } 00096 00097 // Camera Parameters 00098 params.camPar.width = camCDF.GetInt("w"); 00099 params.camPar.height = camCDF.GetInt("h"); 00100 params.camPar.fx = camCDF.GetFloat("fx"); 00101 params.camPar.fy = camCDF.GetFloat("fy"); 00102 params.camPar.cx = camCDF.GetFloat("cx"); 00103 params.camPar.cy = camCDF.GetFloat("cy"); 00104 params.camPar.k1 = camCDF.GetFloat("k1"); 00105 params.camPar.k2 = camCDF.GetFloat("k2"); 00106 params.camPar.k3 = 0.0f; 00107 params.camPar.p1 = camCDF.GetFloat("p1"); 00108 params.camPar.p2 = camCDF.GetFloat("p2"); 00109 00110 //printf("%d %d, %f %f, %f %f, %f %f %f, %f %f\n", 00111 // params.camPar.width, 00112 // params.camPar.height, 00113 // params.camPar.fx, 00114 // params.camPar.fy, 00115 // params.camPar.cx, 00116 // params.camPar.cy, 00117 // params.camPar.k1, 00118 // params.camPar.k2, 00119 // params.camPar.k3, 00120 // params.camPar.p1, 00121 // params.camPar.p2); 00122 00123 params.camPar.zFar = 4.0f; 00124 params.camPar.zNear = 0.1f; 00125 00126 if(!poseCDF.Load(poseCalFile)){ 00127 char errmsg[128]; 00128 sprintf(errmsg, "[Tracker::loadCamParsFromINI] Cannot open file %s", poseCalFile); 00129 throw std::runtime_error(errmsg); 00130 } 00131 00132 vec3 p, r; 00133 string pose = poseCDF.GetString("pose"); 00134 sscanf( pose.c_str(), "[%f %f %f] [%f %f %f]", 00135 &(p.x), &(p.y), &(p.z), &(r.x), &(r.y), &(r.z) ); 00136 00137 //printf("%s\n", pose.c_str()); 00138 //printf("%f %f %f, %f %f %f\n", p.x, p.y, p.z, r.x, r.y, r.z); 00139 00140 params.camPar.pos.x = p.x; 00141 params.camPar.pos.y = p.y; 00142 params.camPar.pos.z = p.z; 00143 params.camPar.rot.fromRotVector(r); 00144 return true; 00145 } 00146 00147 // load INI file of tracker 00148 bool Tracker::loadTrackParsFromINI(const char* inifile){ 00149 CDataFile cdfParams; 00150 00151 if(!cdfParams.Load(inifile)){ 00152 char errmsg[128]; 00153 sprintf(errmsg, "[Tracker::loadTrackParsFromINI] Cannot open file %s", inifile); 00154 throw std::runtime_error(errmsg); 00155 } 00156 00157 float dpi = pi/180.0f; 00158 00159 // Constraints 00160 params.variation.r.x = cdfParams.GetFloat("r.x", "Constraints") * dpi; 00161 params.variation.r.y = cdfParams.GetFloat("r.y", "Constraints") * dpi; 00162 params.variation.r.z = cdfParams.GetFloat("r.z", "Constraints") * dpi; 00163 params.variation.t.x = cdfParams.GetFloat("t.x", "Constraints"); 00164 params.variation.t.y = cdfParams.GetFloat("t.y", "Constraints"); 00165 params.variation.t.z = cdfParams.GetFloat("t.z", "Constraints"); 00166 params.variation.z = cdfParams.GetFloat("z", "Constraints"); 00167 00168 // Performance 00169 params.num_recursions = cdfParams.GetInt("recursions", "Performance"); 00170 params.num_particles = cdfParams.GetInt("particles", "Performance"); 00171 params.hypotheses_trials = cdfParams.GetInt("hypotheses", "Performance"); 00172 params.convergence = cdfParams.GetInt("convergence", "Performance"); 00173 00174 // Resource Path 00175 params.modelPath = cdfParams.GetString("ModelPath", "ResourcePath"); 00176 params.texturePath = cdfParams.GetString("TexturePath", "ResourcePath"); 00177 params.shaderPath = cdfParams.GetString("ShaderPath", "ResourcePath"); 00178 00179 // Other 00180 params.edge_tolerance = cdfParams.GetFloat("EdgeMatchingTolerance", "Other") * dpi; 00181 params.minTexGrabAngle = cdfParams.GetFloat("MinTextureGrabAngle", "Other") * dpi; 00182 params.num_spreadings = cdfParams.GetInt("NumberOfSpreadings", "Other"); 00183 params.max_kernel_size = cdfParams.GetInt("MaxKernelSize", "Other"); 00184 00185 params.model_sobel_th = cdfParams.GetFloat("ModelSobelThreshold", "Other"); 00186 params.image_sobel_th = cdfParams.GetFloat("ImageSobelThreshold", "Other"); 00187 params.pred_no_convergence = cdfParams.GetFloat("PredictorNoConvergence", "Other"); 00188 00189 params.c_th_base = cdfParams.GetFloat("BaseThreshold", "Qualitative"); 00190 params.c_th_min = cdfParams.GetFloat("MinThreshold", "Qualitative"); 00191 params.c_th_fair = cdfParams.GetFloat("FairThreshold", "Qualitative"); 00192 params.c_th_lost = cdfParams.GetFloat("LostThreshold", "Qualitative"); 00193 00194 params.c_mv_not = cdfParams.GetFloat("NoMovementThreshold", "Movement"); 00195 params.c_mv_slow = cdfParams.GetFloat("SlowMovementThreshold", "Movement"); 00196 00197 return true; 00198 } 00199 00200 void Tracker::setFrameTime(double dTime){ 00201 for(unsigned i=0; i<m_modellist.size(); i++){ 00202 m_modellist[i]->predictor->updateTime(dTime); 00203 } 00204 } 00205 00206 bool Tracker::setCameraParameters(TomGine::tgCamera::Parameter cam_par){ 00207 00208 if(cam_par.zFar <= 0.0){ 00209 ROS_DEBUG("[Tracker::setCameraParameters] Error 'Far Clipping Plane' not valid: %f", cam_par.zFar); 00210 return false; 00211 } 00212 if(cam_par.zNear < 0.0){ 00213 ROS_DEBUG("[Tracker::setCameraParameters] Error 'Near Clipping Plane' not valid: %f", cam_par.zNear); 00214 return false; 00215 } 00216 00217 if(cam_par.zNear >= cam_par.zFar){ 00218 ROS_DEBUG("[Tracker::setCameraParameters] Error 'Clipping Panes' not valid: %f > %f", cam_par.zNear, cam_par.zFar); 00219 return false; 00220 } 00221 00222 m_cam_perspective.Load(cam_par); 00223 return true; 00224 } 00225 00226 bool Tracker::init(const char* trackINIFile, const char* camCalFile, const char* poseCalFile){ 00227 00228 if(m_tracker_initialized){ 00229 ROS_DEBUG("[Tracker::init()] Warning: Tracker already initialized"); 00230 return false; 00231 } 00232 00233 // Load camera parameters 00234 if(!loadCamParsFromINI(camCalFile, poseCalFile)) 00235 return false; 00236 00237 // Load tracker parameter 00238 if(!loadTrackParsFromINI(trackINIFile)) 00239 return false; 00240 00241 // initialise tracker 00242 return init(); 00243 } 00244 00245 bool Tracker::init(const Parameter& trackParam){ 00246 00247 if(m_tracker_initialized){ 00248 ROS_DEBUG("[Tracker::init()] Warning: Tracker already initialized"); 00249 return false; 00250 } 00251 00252 // load tracker parameter 00253 this->params = trackParam; 00254 // initialise tracker 00255 return init(); 00256 } 00257 00258 bool Tracker::init(){ 00259 00260 if(m_tracker_initialized){ 00261 ROS_DEBUG("[Tracker::init()] Warning: Tracker already initialized"); 00262 return false; 00263 } 00264 00265 00266 // OpenGL / devIL 00267 initGL(); 00268 00269 // Set pathes to file resources 00270 g_Resources->SetShaderPath(params.shaderPath.c_str()); 00271 g_Resources->ShowLog(false); 00272 00273 // Load camera parameter 00274 m_cam_perspective.Load(params.camPar); 00275 00276 // Initialize Image Processor 00277 g_Resources->InitImageProcessor(params.camPar.width, params.camPar.height); 00278 m_ip = g_Resources->GetImageProcessor(); 00279 00280 // m_ip->avgInit(1024); 00281 00282 // Textures 00283 m_tex_frame = new Texture(); 00284 glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE); 00285 00286 m_tex_frame_ip.push_back( new Texture() ); 00287 glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE); 00288 for(int i=0; i<(int)params.num_spreadings-1; i++){ 00289 m_tex_frame_ip.push_back( new Texture() ); 00290 glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE); 00291 } 00292 00293 if(params.num_spreadings == 0) 00294 params.m_spreadlvl = 0; 00295 else if(params.m_spreadlvl >= params.num_spreadings) 00296 params.m_spreadlvl = params.num_spreadings - 1; 00297 00298 return (m_tracker_initialized = initInternal()); 00299 } 00300 00301 void Tracker::loadImage(unsigned char* image, GLenum format){ 00302 m_tex_frame->load(image, params.camPar.width, params.camPar.height, format); 00303 } 00304 00305 int Tracker::addModel(TomGine::tgModel& m, tgPose& p, std::string label, bool bfc){ 00306 00307 if(!m_tracker_initialized){ 00308 char errmsg[128]; 00309 sprintf(errmsg, "[Tracker::addModel()] Error tracker not initialised!"); 00310 throw std::runtime_error(errmsg); 00311 } 00312 00313 ModelEntry* modelEntry = new ModelEntry(m); 00314 modelEntry->label = label; 00315 modelEntry->model.setBFC(bfc); 00316 00317 modelEntry->pose = p; 00318 modelEntry->initial_pose = p; 00319 00320 // Calculate Zoom Direction and pass it to the particle filter 00321 TomGine::tgVector3 vCam = m_cam_perspective.GetPos(); 00322 TomGine::tgVector3 vObj = TomGine::tgVector3(p.t.x, p.t.y, p.t.z); 00323 modelEntry->vCam2Model = vObj - vCam; 00324 modelEntry->predictor->setCamViewVector(modelEntry->vCam2Model); 00325 modelEntry->predictor->sample(modelEntry->distribution, params.num_particles, p, params.variation); 00326 modelEntry->predictor->setNoConvergence(params.pred_no_convergence); 00327 00328 modelEntry->id = params.model_id_count++; 00329 modelEntry->num_particles = params.num_particles; 00330 modelEntry->num_recursions = params.num_recursions; 00331 m_modellist.push_back(modelEntry); 00332 00333 return modelEntry->id; 00334 } 00335 00336 int Tracker::addModelFromFile(const char* filename, tgPose& p, std::string label, bool bfc){ 00337 if(!m_tracker_initialized){ 00338 char errmsg[128]; 00339 sprintf(errmsg, "[Tracker::addModelFromFile()] Error tracker not initialised!"); 00340 throw std::runtime_error(errmsg); 00341 } 00342 00343 ModelEntry* modelEntry = new ModelEntry(); 00344 modelEntry->label = label; 00345 modelEntry->model.setBFC(bfc); 00346 00347 ModelLoader modelloader; 00348 if(!modelloader.LoadPly(modelEntry->model, filename)) 00349 return -1; 00350 modelEntry->pose = p; 00351 modelEntry->initial_pose = p; 00352 // Calculate Zoom Direction and pass it to the particle filter 00353 TomGine::tgVector3 vCam = m_cam_perspective.GetPos(); 00354 TomGine::tgVector3 vObj = TomGine::tgVector3(p.t.x, p.t.y, p.t.z); 00355 modelEntry->vCam2Model = vObj - vCam; 00356 modelEntry->predictor->setCamViewVector(modelEntry->vCam2Model); 00357 modelEntry->predictor->sample(modelEntry->distribution, params.num_particles, p, params.variation); 00358 modelEntry->predictor->setNoConvergence(params.pred_no_convergence); 00359 00360 modelEntry->id = params.model_id_count++; 00361 modelEntry->num_particles = params.num_particles; 00362 modelEntry->num_recursions = params.num_recursions; 00363 m_modellist.push_back(modelEntry); 00364 00365 return modelEntry->id; 00366 } 00367 00368 //BENCE: never called 00369 //void Tracker::addPoseHypothesis(int id, tgPose &p, std::string label, bool bfc){ 00370 // if(!m_tracker_initialized){ 00371 // char errmsg[128]; 00372 // sprintf(errmsg, "[Tracker::addPoseHypothesis()] Error tracker not initialised!"); 00373 // throw std::runtime_error(errmsg); 00374 // } 00375 // ModelEntryList::iterator it = m_modellist.begin(); 00376 // while(it != m_modellist.end()){ 00377 // if(id==(*it)->id){ 00378 // ModelEntry* modelEntry = new ModelEntry(); 00379 // modelEntry->label = label; 00380 // modelEntry->model.setBFC(bfc); 00381 // modelEntry->model = m_modellist[id]->model; // TODO maybe buggy 00382 // modelEntry->pose = p; 00383 // modelEntry->initial_pose = p; 00384 // modelEntry->predictor->sample(modelEntry->distribution, params.num_particles, p, params.variation); 00385 // modelEntry->predictor->setNoConvergence(params.pred_no_convergence); 00386 // 00387 // modelEntry->id = params.hypotheses_id_count++; 00388 // modelEntry->num_particles = params.num_particles; 00389 // modelEntry->num_recursions = params.num_recursions; 00390 // modelEntry->hypothesis_id = id; 00391 // m_hypotheses.push_back(modelEntry); 00392 // return; 00393 // } 00394 // ++it; 00395 // } 00396 // ROS_DEBUG("[Tracker::addHypothesis()] Warning model for hypothesis not found: %d - %d", id, (int)m_modellist.size()); 00397 //} 00398 00399 void Tracker::removeModel(int id){ 00400 ModelEntryList::iterator it = m_modellist.begin(); 00401 while(it != m_modellist.end()){ 00402 if(id==(*it)->id){ 00403 delete(*it); 00404 m_modellist.erase(it); 00405 return; 00406 } 00407 ++it; 00408 } 00409 } 00410 00411 ModelEntry* Tracker::getModelEntry(int id){ 00412 ModelEntryList::iterator it = m_modellist.begin(); 00413 00414 while(it != m_modellist.end()){ 00415 if(id==(*it)->id){ 00416 return (*it); 00417 } 00418 ++it; 00419 } 00420 return 0; 00421 } 00422 00423 void Tracker::getModelPose(int id, tgPose& p){ 00424 ModelEntryList::iterator it = m_modellist.begin(); 00425 00426 while(it != m_modellist.end()){ 00427 if(id==(*it)->id){ 00428 p = (*it)->pose; 00429 // p = (*it)->distribution.getParticle(0); 00430 return; 00431 } 00432 ++it; 00433 } 00434 } 00435 00436 void Tracker::getModelPoseLPF(int id, tgPose& p){ 00437 ModelEntryList::iterator it = m_modellist.begin(); 00438 00439 while(it != m_modellist.end()){ 00440 if(id==(*it)->id){ 00441 p = (*it)->pose; 00442 // p = (*it)->distribution.getParticle(0); 00443 return; 00444 } 00445 ++it; 00446 } 00447 } 00448 00449 void Tracker::getModelPoseCamCoords(int id, tgPose& p){ 00450 mat4 mv; 00451 mat3 R; 00452 vec3 t; 00453 00454 mat3 gl2cv; 00455 gl2cv[0]=1.0; gl2cv[3]=0.0; gl2cv[6]=0.0; 00456 gl2cv[1]=0.0; gl2cv[4]=-1.0; gl2cv[7]=0.0; 00457 gl2cv[2]=0.0; gl2cv[5]=0.0; gl2cv[8]=-1.0; 00458 00459 00460 ModelEntryList::iterator it = m_modellist.begin(); 00461 00462 while(it != m_modellist.end()){ 00463 if(id==(*it)->id){ 00464 m_cam_perspective.Activate(); 00465 p = m_modellist[0]->pose; 00466 p.Activate(); 00467 glGetFloatv(GL_MODELVIEW_MATRIX, mv); 00468 p.Deactivate(); 00469 00470 R = gl2cv * mat3(mv); 00471 t = gl2cv * vec3(mv[12], mv[13], mv[14]); 00472 00473 // printf("mv: %f %f %f\n", mv[12], mv[13], mv[14]); 00474 // printf("t: %f %f %f\n", t.x, t.y, t.z); 00475 00476 p.SetPose(R,t); 00477 return; 00478 } 00479 ++it; 00480 } 00481 } 00482 00483 void Tracker::getModelVelocity(int id, float &translational, float &angular){ 00484 ModelEntryList::iterator it = m_modellist.begin(); 00485 00486 while(it != m_modellist.end()){ 00487 if(id==(*it)->id){ 00488 translational = (*it)->speed_translational; 00489 angular = (*it)->speed_angular; 00490 return; 00491 } 00492 ++it; 00493 } 00494 } 00495 00496 00497 void Tracker::getModelMovementState(int id, movement_state &m){ 00498 ModelEntryList::iterator it = m_modellist.begin(); 00499 00500 while(it != m_modellist.end()){ 00501 if(id==(*it)->id){ 00502 m = (*it)->st_movement; 00503 return; 00504 } 00505 ++it; 00506 } 00507 } 00508 00509 void Tracker::getModelQualityState(int id, quality_state &q){ 00510 ModelEntryList::iterator it = m_modellist.begin(); 00511 00512 while(it != m_modellist.end()){ 00513 if(id==(*it)->id){ 00514 q = (*it)->st_quality; 00515 return; 00516 } 00517 ++it; 00518 } 00519 } 00520 00521 void Tracker::getModelConfidenceState(int id, confidence_state &q){ 00522 ModelEntryList::iterator it = m_modellist.begin(); 00523 00524 while(it != m_modellist.end()){ 00525 if(id==(*it)->id){ 00526 q = (*it)->st_confidence; 00527 return; 00528 } 00529 ++it; 00530 } 00531 } 00532 00533 void Tracker::drawTest(){ 00534 tgPose p; 00535 mat4 mv; 00536 mat3 R, Rinv; 00537 vec3 t; 00538 00539 std::vector<vec3> v; 00540 00541 m_cam_perspective.Activate(); 00542 00543 if(!m_modellist.empty()){ 00544 00545 TomGine::tgModel model = m_modellist[0]->model; 00546 for(unsigned i=0; i<model.getVertexSize(); i++){ 00547 v.push_back(model.getVertex(i).pos); 00548 } 00549 00550 p = m_modellist[0]->pose; 00551 00552 p.Activate(); 00553 glGetFloatv(GL_MODELVIEW_MATRIX, mv); 00554 p.Deactivate(); 00555 00556 R = mat3(mv); 00557 t = vec3(mv[12], mv[13], mv[14]); 00558 p.SetPose(R,t); 00559 00560 glMatrixMode(GL_MODELVIEW); 00561 glLoadIdentity(); 00562 glDisable(GL_DEPTH_TEST); 00563 00564 glPointSize(5.0); 00565 glBegin(GL_POINTS); 00566 glColor3f(1.0f, 0.0f, 0.0f); 00567 glVertex3f(t.x, t.y, t.z); 00568 glEnd(); 00569 00570 glPointSize(2.0); 00571 00572 glBegin(GL_POINTS); 00573 glColor3f(1.0f, 0.0f, 0.0f); 00574 00575 for(unsigned int i=0; i<v.size(); i++){ 00576 v[i] = (R * v[i]) + t; 00577 00578 glVertex3f(v[i].x, v[i].y, v[i].z); 00579 } 00580 glEnd(); 00581 00582 } 00583 } 00584 00585 void Tracker::getModelInitialPose(int id, tgPose& p){ 00586 ModelEntryList::iterator it = m_modellist.begin(); 00587 00588 while(it != m_modellist.end()){ 00589 if(id==(*it)->id){ 00590 p = (*it)->initial_pose; 00591 return; 00592 } 00593 ++it; 00594 } 00595 } 00596 00597 void Tracker::getModelConfidence(int id, float& c){ 00598 ModelEntryList::iterator it = m_modellist.begin(); 00599 00600 while(it != m_modellist.end()){ 00601 if(id==(*it)->id){ 00602 c = (*it)->pose.c; 00603 return; 00604 } 00605 ++it; 00606 } 00607 } 00608 00609 void Tracker::getModelConfidenceEdge(int id, float& c){ 00610 ModelEntryList::iterator it = m_modellist.begin(); 00611 00612 while(it != m_modellist.end()){ 00613 if(id==(*it)->id){ 00614 c = (*it)->confidence_edge; 00615 return; 00616 } 00617 ++it; 00618 } 00619 } 00620 00621 void Tracker::getModelConfidenceColor(int id, float& c){ 00622 ModelEntryList::iterator it = m_modellist.begin(); 00623 00624 while(it != m_modellist.end()){ 00625 if(id==(*it)->id){ 00626 c = (*it)->confidence_color; 00627 return; 00628 } 00629 ++it; 00630 } 00631 } 00632 00633 bool Tracker::getModelPoint3D(int id, int x_win, int y_win, double& x3, double& y3, double& z3){ 00634 ModelEntryList::iterator it; 00635 00636 for(it = m_modellist.begin(); it < m_modellist.end(); ++it){ 00637 if(id==(*it)->id){ 00638 // Activate Camera 00639 TomGine::tgCamera cam = m_cam_perspective; 00640 cam.SetZRange(0.0, 1.0); 00641 cam.Activate(); 00642 00643 // Clear Depth Buffer 00644 glClear(GL_DEPTH_BUFFER_BIT); 00645 glEnable(GL_DEPTH_TEST); 00646 00647 // Apply pose 00648 (*it)->pose.Activate(); 00649 00650 // Draw Model Faces 00651 m_lighting.Activate(); 00652 (*it)->model.drawFaces(); 00653 m_lighting.Deactivate(); 00654 00655 // ************************ 00656 int viewport[4]; 00657 double modelview[16]; 00658 double projection[16]; 00659 double result[3]; 00660 00661 glGetDoublev(GL_MODELVIEW_MATRIX, &modelview[0] ); 00662 glGetDoublev(GL_PROJECTION_MATRIX, &projection[0] ); 00663 glGetIntegerv(GL_VIEWPORT, &viewport[0] ); 00664 y_win = viewport[3] - y_win; // flip y for OpenGL 00665 00666 // Read value of depth buffer at position (x_win, y_win) 00667 glReadPixels(x_win, y_win, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &z3 ); 00668 00669 if(z3 > 0.99) 00670 return false; 00671 00672 // calculate intersection of camera viewing vector and model surface 00673 gluUnProject((double)x_win, (double)y_win, (double)z3, modelview, projection, viewport, &result[0], &result[1], &result[2]); 00674 00675 x3 = result[0]; 00676 y3 = result[1]; 00677 z3 = result[2]; 00678 00679 (*it)->pose.Deactivate(); 00680 00681 return true; 00682 } 00683 } 00684 return false; 00685 } 00686 00687 void Tracker::getModelMask(int id, Texture &mask){ 00688 ModelEntryList::iterator it; 00689 00690 for(it = m_modellist.begin(); it < m_modellist.end(); ++it){ 00691 if(id==(*it)->id){ 00692 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 00693 m_cam_perspective.Activate(); 00694 m_lighting.Deactivate(); 00695 glColor3f(1,1,1); 00696 glEnable(GL_DEPTH_TEST); glDepthMask(1); 00697 (*it)->pose.Activate(); 00698 (*it)->model.DrawFaces(); 00699 (*it)->pose.Deactivate(); 00700 glDisable(GL_DEPTH_TEST); glDepthMask(0); 00701 00702 mask.copyTexImage2D(params.camPar.width, params.camPar.height); 00703 } 00704 } 00705 } 00706 00707 void Tracker::getModelEdgeMask(int id, Texture &mask){ 00708 ModelEntryList::iterator it; 00709 00710 for(it = m_modellist.begin(); it < m_modellist.end(); ++it){ 00711 if(id==(*it)->id){ 00712 computeModelEdgeMask((*it), mask); 00713 } 00714 } 00715 } 00716 00717 bool Tracker::getModelMaskOwnEdges(int id){ 00718 ModelEntryList::iterator it; 00719 00720 for(it = m_modellist.begin(); it < m_modellist.end(); ++it){ 00721 if(id==(*it)->id){ 00722 return (*it)->mask_geometry_edges; 00723 } 00724 } 00725 return 0; 00726 } 00727 00728 void Tracker::setModelInitialPose(int id, tgPose& p){ 00729 ModelEntryList::iterator it = m_modellist.begin(); 00730 00731 while(it != m_modellist.end()){ 00732 if(id==(*it)->id){ 00733 (*it)->setInitialPose(p, 0.2f, 0.2f); 00734 return; 00735 } 00736 ++it; 00737 } 00738 } 00739 00740 void Tracker::setModelPredictor(int id, Predictor* predictor){ 00741 ModelEntryList::iterator it = m_modellist.begin(); 00742 while(it != m_modellist.end()){ 00743 if(id==(*it)->id){ 00744 (*it)->predictor = predictor; 00745 return; 00746 } 00747 ++it; 00748 } 00749 } 00750 00751 void Tracker::setModelPredictorNoConvergence(int id, float no_conv){ 00752 ModelEntryList::iterator it = m_modellist.begin(); 00753 while(it != m_modellist.end()){ 00754 if(id==(*it)->id){ 00755 (*it)->predictor->setNoConvergence(no_conv); 00756 return; 00757 } 00758 ++it; 00759 } 00760 } 00761 00762 void Tracker::setModelMask(int id, Texture *mask){ 00763 00764 } 00765 00766 void Tracker::setModelMaskOwnEdges(int id, bool masked){ 00767 ModelEntryList::iterator it = m_modellist.begin(); 00768 while(it != m_modellist.end()){ 00769 if(id==(*it)->id){ 00770 (*it)->mask_geometry_edges = masked; 00771 return; 00772 } 00773 ++it; 00774 } 00775 } 00776 00777 void Tracker::setModelLock(int id, bool lock){ 00778 ModelEntryList::iterator it = m_modellist.begin(); 00779 while(it != m_modellist.end()){ 00780 if(id==(*it)->id){ 00781 (*it)->lock = lock; 00782 return; 00783 } 00784 ++it; 00785 } 00786 } 00787 00788 void Tracker::setModelRecursionsParticle(int id, int num_recursions, int num_particles){ 00789 ModelEntryList::iterator it = m_modellist.begin(); 00790 while(it != m_modellist.end()){ 00791 if(id==(*it)->id){ 00792 (*it)->num_recursions = num_recursions; 00793 (*it)->num_particles = num_particles; 00794 return; 00795 } 00796 ++it; 00797 } 00798 } 00799 00800 void Tracker::saveModel(int id, const char* pathname){ 00801 ModelLoader modelloader; 00802 ModelEntryList::iterator it = m_modellist.begin(); 00803 string name; 00804 while(it != m_modellist.end()){ 00805 if(id==(*it)->id){ 00806 name = string(pathname); 00807 name.append((*it)->label); 00808 modelloader.SavePly((*it)->model, name.c_str()); 00809 return; 00810 } 00811 ++it; 00812 } 00813 } 00814 00815 void Tracker::saveModels(const char* pathname){ 00816 ModelLoader modelloader; 00817 ModelEntryList::iterator it = m_modellist.begin(); 00818 string name; 00819 while(it != m_modellist.end()){ 00820 name = string(pathname); 00821 name.append((*it)->label); 00822 (*it)->model.unwarpTexturesBox_hacky(name.c_str()); 00823 modelloader.SavePly((*it)->model, name.c_str()); 00824 ++it; 00825 } 00826 } 00827 00828 void Tracker::saveScreenshot(const char* filename){ 00829 IplImage* img = cvCreateImage ( cvSize ( params.camPar.width, params.camPar.height ), IPL_DEPTH_8U, 3 ); 00830 glReadPixels(0,0,params.camPar.width,params.camPar.height,GL_RGB,GL_UNSIGNED_BYTE, img->imageData); 00831 cvConvertImage(img, img, CV_CVTIMG_FLIP | CV_CVTIMG_SWAP_RB); 00832 cvSaveImage(filename, img); 00833 cvReleaseImage(&img); 00834 } 00835 00836 cv::Mat Tracker::getImage() 00837 { 00838 IplImage* img = cvCreateImage ( cvSize ( params.camPar.width, params.camPar.height ), IPL_DEPTH_8U, 3 ); 00839 glReadPixels(0,0,params.camPar.width,params.camPar.height,GL_RGB,GL_UNSIGNED_BYTE, img->imageData); 00840 cvConvertImage(img, img, CV_CVTIMG_FLIP | CV_CVTIMG_SWAP_RB); 00841 //FIXME: result(img) does not take ownership of the memory 00842 cv::Mat result(img, true); 00843 cvReleaseImage(&img); 00844 return result; 00845 } 00846 00847 void Tracker::setLockFlag(bool val){ 00848 ModelEntryList::iterator it = m_modellist.begin(); 00849 while(it != m_modellist.end()){ 00850 (*it)->lock = val; 00851 ++it; 00852 } 00853 m_lock = val; 00854 } 00855 00856 void Tracker::drawModel(tgPose p){ 00857 m_cam_perspective.Activate(); 00858 00859 glEnable(GL_DEPTH_TEST); 00860 glClear(GL_DEPTH_BUFFER_BIT); 00861 00862 p.Activate(); 00863 00864 for(unsigned i=0; i<m_modellist.size(); i++){ 00865 00866 glColorMask(0,0,0,0); 00867 m_modellist[i]->model.drawFaces(); 00868 glColorMask(1,1,1,1); 00869 m_modellist[i]->model.drawEdges(); 00870 } 00871 00872 p.Deactivate(); 00873 } 00874 00875 void Tracker::drawModel(const TomGine::tgModel &m, const tgPose &p){ 00876 m_cam_perspective.Activate(); 00877 00878 m_lighting.Activate(); 00879 p.Activate(); 00880 00881 m.DrawFaces(); 00882 00883 p.Deactivate(); 00884 m_lighting.Deactivate(); 00885 } 00886 00887 void Tracker::drawModelWireframe(const TomGine::tgModel &m, const tgPose &p, float linewidth){ 00888 00889 glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); 00890 glLineWidth(linewidth); 00891 00892 m_cam_perspective.Activate(); 00893 00894 m_lighting.Deactivate(); 00895 00896 p.Activate(); 00897 00898 m.DrawFaces(); 00899 00900 p.Deactivate(); 00901 00902 glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); 00903 } 00904 00905 void Tracker::drawCoordinateSystem(float linelength, float linewidth, TomGine::tgPose pose){ 00906 glDisable(GL_DEPTH_TEST); 00907 glDisable(GL_CULL_FACE); 00908 m_cam_perspective.Activate(); 00909 00910 float l = linelength; 00911 glLineWidth(linewidth); 00912 00913 vec3 o = pose.t; 00914 00915 vec3 x = pose * vec3(l,0,0); 00916 vec3 y = pose * vec3(0,l,0); 00917 vec3 z = pose * vec3(0,0,l); 00918 00919 glPushMatrix(); 00920 glColor3f(1,0,0); 00921 glBegin(GL_LINES); 00922 glVertex3f(o.x, o.y, o.z); 00923 glVertex3f(x.x, x.y, x.z); 00924 glEnd(); 00925 00926 glColor3f(0,1,0); 00927 glBegin(GL_LINES); 00928 glVertex3f(o.x, o.y, o.z); 00929 glVertex3f(y.x, y.y, y.z); 00930 glEnd(); 00931 00932 glColor3f(0,0,1); 00933 glBegin(GL_LINES); 00934 glVertex3f(o.x, o.y, o.z); 00935 glVertex3f(z.x, z.y, z.z); 00936 glEnd(); 00937 glPopMatrix(); 00938 } 00939 00940 00941 // render coordinate frame 00942 void Tracker::drawCoordinates(float linelength){ 00943 glDisable(GL_DEPTH_TEST); 00944 glDisable(GL_CULL_FACE); 00945 m_cam_perspective.Activate(); 00946 00947 // float l1 = 0.06f; 00948 // float l2 = 0.02f; 00949 // float b1 = 0.001f; 00950 // float b2 = 0.003f; 00951 00952 drawCoordinateSystem(linelength, 2.0f); 00953 00954 // // X - Axis 00955 // glPushMatrix(); 00956 // glColor3f(1.0,0.0,0.0); 00957 // glBegin(GL_TRIANGLE_FAN); 00958 // glVertex3f(l1, 0.0, b1); 00959 // glVertex3f(0.0, 0.0, b1); 00960 // glVertex3f(0.0, 0.0, -b1); 00961 // glVertex3f(l1, 0.0, -b1); 00962 // glVertex3f(l1, 0.0, -b1-b2); 00963 // glVertex3f(l1+l2, 0.0, 0.0); 00964 // glVertex3f(l1, 0.0, b1+b2); 00965 // glEnd(); 00966 // glPopMatrix(); 00967 // 00968 // // Y - Axis 00969 // glPushMatrix(); 00970 // glColor3f(0.0,1.0,0.0); 00971 // glRotatef(90, 0.0, 0.0, 1.0); 00972 // glBegin(GL_TRIANGLE_FAN); 00973 // glVertex3f(l1, 0.0, b1); 00974 // glVertex3f(0.0, 0.0, b1); 00975 // glVertex3f(0.0, 0.0, -b1); 00976 // glVertex3f(l1, 0.0, -b1); 00977 // glVertex3f(l1, 0.0, -b1-b2); 00978 // glVertex3f(l1+l2, 0.0, 0.0); 00979 // glVertex3f(l1, 0.0, b1+b2); 00980 // glEnd(); 00981 // glPopMatrix(); 00982 // 00983 // // Z - Axis 00984 // glPushMatrix(); 00985 // glColor3f(0.0,0.0,1.0); 00986 // glRotatef(-90, 0.0, 1.0, 0.0); 00987 // glBegin(GL_TRIANGLE_FAN); 00988 // glVertex3f(l1, 0.0, b1); 00989 // glVertex3f(0.0, 0.0, b1); 00990 // glVertex3f(0.0, 0.0, -b1); 00991 // glVertex3f(l1, 0.0, -b1); 00992 // glVertex3f(l1, 0.0, -b1-b2); 00993 // glVertex3f(l1+l2, 0.0, 0.0); 00994 // glVertex3f(l1, 0.0, b1+b2); 00995 // glEnd(); 00996 // glPopMatrix(); 00997 00998 } 00999 01000 void Tracker::drawImage(unsigned char* image){ 01001 glDepthMask(0); 01002 glColor3f(1.0,1.0,1.0); 01003 01004 if(image == NULL){ 01005 if(m_draw_edges) 01006 m_ip->render(m_tex_frame_ip[params.m_spreadlvl]); 01007 else 01008 m_ip->render(m_tex_frame); 01009 }else{ 01010 m_tex_frame->load(image, params.camPar.width, params.camPar.height); 01011 m_ip->flipUpsideDown(m_tex_frame, m_tex_frame); 01012 m_ip->render(m_tex_frame); 01013 } 01014 01015 glDepthMask(1); 01016 } 01017 01018 void Tracker::drawCalibrationPattern(float point_size){ 01019 01020 if(m_calib_points.empty()) 01021 return; 01022 01023 glDisable(GL_DEPTH_TEST); 01024 glDisable(GL_CULL_FACE); 01025 01026 m_cam_perspective.Activate(); 01027 01028 glPointSize(point_size); 01029 01030 glBegin(GL_POINTS); 01031 01032 for( unsigned i=0; i<m_calib_points.size(); i++ ) 01033 glVertex3f(m_calib_points[i].x, m_calib_points[i].y, m_calib_points[i].z); 01034 01035 glEnd(); 01036 } 01037 01038 void Tracker::loadCalibrationPattern(const char* mdl_file){ 01039 FILE* pfile = 0; 01040 01041 pfile = fopen(mdl_file, "r"); 01042 01043 if(pfile!=NULL) { 01044 unsigned n=0; 01045 vec3 p; 01046 rewind(pfile); 01047 int size = fscanf(pfile, "%d", &n); 01048 m_calib_points.clear(); 01049 for(unsigned i=0; i<n; i++){ 01050 size = fscanf(pfile, "%f %f %f", &p.x, &p.y, &p.z); 01051 m_calib_points.push_back(p); 01052 01053 } 01054 printf("size: %d", size); 01055 }else{ 01056 ROS_DEBUG("[Tracker::loadCalibrationPattern] Warning couldn't load model file '%s'\n", mdl_file); 01057 } 01058 01059 fclose(pfile); 01060 } 01061 01062 void Tracker::drawPixel(float u, float v, vec3 color, float size){ 01063 glDisable(GL_DEPTH_TEST); 01064 glDisable(GL_LIGHTING); 01065 m_ip->setCamOrtho(); 01066 01067 glPointSize(size); 01068 glColor3f(color.x, color.y, color.z); 01069 01070 glBegin(GL_POINTS); 01071 glVertex3f(u,v,0.0); 01072 glEnd(); 01073 } 01074 01075 void Tracker::drawPoint(float x, float y, float z, float size){ 01076 glDisable(GL_DEPTH_TEST); 01077 glDisable(GL_LIGHTING); 01078 m_cam_perspective.Activate(); 01079 01080 glPointSize(size); 01081 // glColor3f(1.0,0.0,0.0); 01082 01083 glBegin(GL_POINTS); 01084 glVertex3f(x,y,z); 01085 glEnd(); 01086 } 01087 01088 void Tracker::getGlError(){ 01089 int err = glGetError(); 01090 switch(err){ 01091 case GL_NO_ERROR: 01092 break; 01093 case GL_INVALID_ENUM: 01094 ROS_DEBUG("glGetError: GL_INVALID_ENUM\n"); 01095 break; 01096 case GL_INVALID_VALUE: 01097 ROS_DEBUG("glGetError: GL_INVALID_VALUE\n"); 01098 break; 01099 case GL_INVALID_OPERATION: 01100 ROS_DEBUG("glGetError: GL_INVALID_OPERATION\n"); 01101 break; 01102 case GL_STACK_OVERFLOW: 01103 ROS_DEBUG("glGetError: GL_STACK_OVERFLOW\n"); 01104 break; 01105 case GL_STACK_UNDERFLOW: 01106 ROS_DEBUG("glGetError: GL_STACK_UNDERFLOW\n"); 01107 break; 01108 case GL_OUT_OF_MEMORY: 01109 ROS_DEBUG("glGetError: GL_OUT_OF_MEMORY\n"); 01110 break; 01111 default: 01112 ROS_DEBUG("glGetError: no known error\n"); 01113 break; 01114 } 01115 } 01116 01117 void Tracker::computeModelEdgeMask(ModelEntry* modelEntry, Texture &mask) 01118 { 01119 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 01120 m_cam_perspective.Activate(); 01121 m_lighting.Activate(); 01122 glEnable(GL_DEPTH_TEST); glDepthMask(1); 01123 modelEntry->pose.Activate(); 01124 modelEntry->model.DrawFaces(); 01125 modelEntry->pose.Deactivate(); 01126 glDisable(GL_DEPTH_TEST); glDepthMask(0); 01127 m_lighting.Deactivate(); 01128 01129 mask.copyTexImage2D(params.camPar.width, params.camPar.height); 01130 m_ip->sobel(&mask, &mask, 0.01, true, true); 01131 } 01132 01133 01134 // Show performance and likelihood 01135 void Tracker::printStatistics(){ 01136 printf("\n\nStatistics: \n"); 01137 01138 for(unsigned i=0; i<m_modellist.size(); i++){ 01139 tgPose pMean = m_modellist[i]->distribution.getMean(); 01140 printf(" Object %d '%s'\n", i, m_modellist[i]->label.c_str()); 01141 printf(" FPS: %.1f\n", 1.0f/m_ftime); 01142 printf(" Textured: %d\n", m_modellist[i]->model.m_textured); 01143 printf(" Recursions: %i\n", m_modellist[i]->num_recursions ); 01144 printf(" Particles: %i\n", m_modellist[i]->distribution.size() ); 01145 printf(" Pose: %f %f %f\n", m_modellist[i]->pose.t.x, m_modellist[i]->pose.t.y, m_modellist[i]->pose.t.z); 01146 printf(" Variance: %f \n", m_modellist[i]->distribution.getVariancePt() ); 01147 printf(" Confidence: %f \n", m_modellist[i]->pose.c); 01148 printf(" Weight: %f \n", m_modellist[i]->distribution.getMaxW()); 01149 printf(" Spread: %d / %d\n", params.m_spreadlvl, params.num_spreadings); 01150 printf(" Kernel: %d \n", params.kernel_size); 01151 printf(" ImageSobelTh: %f \n", params.image_sobel_th); 01152 printf(" ModelSobelTh: %f \n", params.model_sobel_th); 01153 } 01154 } 01155 01156 void Tracker::reset() 01157 { 01158 for(unsigned i=0; i<m_modellist.size(); i++) 01159 { 01160 m_modellist[i]->predictor->sample( 01161 m_modellist[i]->distribution, params.num_particles, 01162 m_modellist[i]->initial_pose, params.variation); 01163 m_modellist[i]->pose = m_modellist[i]->initial_pose; 01164 // TODO evaluate Distribution::v_max (for the right confidence value when the model is locked) 01165 //m_modellist[i]->m_lpf_cl.Set(0.0f); 01166 } 01167 } 01168 01169 void Tracker::reset(int id) 01170 { 01171 if(id >= 0 && id<static_cast<int>(m_modellist.size())) 01172 { 01173 m_modellist[id]->predictor->sample( 01174 m_modellist[id]->distribution, params.num_particles, 01175 m_modellist[id]->initial_pose, params.variation); 01176 m_modellist[id]->pose = m_modellist[id]->initial_pose; 01177 //m_modellist[id]->m_lpf_cl.Set(0.0f); 01178 } 01179 } 01180 01181 void Tracker::resetUnlockLock() 01182 { 01183 this->reset(); 01184 this->setLockFlag(false); 01185 const unsigned int num_frames(30); 01186 for(unsigned i=0; i<num_frames; i++) 01187 this->track(); 01188 this->setLockFlag(true); 01189 } 01190 01191 01192