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


blort
Author(s): Michael Zillich, Thomas Mörwald, Johann Prankl, Andreas Richtsfeld, Bence Magyar (ROS version)
autogenerated on Thu Jan 2 2014 11:38:26