EdgeTracker.cpp
Go to the documentation of this file.
00001 #include <ros/console.h>
00002 #include <blort/Tracker/EdgeTracker.h>
00003 
00004 using namespace Tracking;
00005 using namespace TomGine;
00006 
00007 // *** PRIVATE ***
00008 
00009 // Process camera image (gauss, sobel, thinning, spreading, rendering)
00010 void EdgeTracker::image_processing(unsigned char* image, GLenum format){
00011         
00012         // Load camera image to texture
00013         m_tex_frame->load(image, params.camPar.width, params.camPar.height, format);
00014         
00015         // Preprocessing for camera image
00016         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00017         glDepthMask(0);
00018         glColor3f(1,1,1);
00019         m_ip->flipUpsideDown(m_tex_frame, m_tex_frame);
00020         m_ip->gauss(m_tex_frame, m_tex_frame_ip[0]);
00021         m_ip->sobel(m_tex_frame_ip[0], m_tex_frame_ip[0], params.image_sobel_th);
00022         glDepthMask(1);
00023 }
00024 
00025 // Process camera image (gauss, sobel, thinning, spreading, rendering)
00026 void EdgeTracker::image_processing(unsigned char* image, const TomGine::tgModel &m, const tgPose &p, GLenum format){
00027         
00028         // Load camera image to texture
00029         m_tex_frame->load(image, params.camPar.width, params.camPar.height, format);
00030         
00031         // Preprocessing for camera image
00032         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00033         glDepthMask(0);
00034         glColor3f(1,1,1);
00035         m_ip->flipUpsideDown(m_tex_frame, m_tex_frame);
00036 
00037         // Draw model (i.e. a virutal occluder)
00038         glDepthMask(1);
00039         glEnable(GL_DEPTH_TEST);
00040         drawModel(m,p);
00041         glDisable(GL_DEPTH_TEST);
00042         glDepthMask(0);
00043         m_tex_frame->copyTexImage2D(params.camPar.width, params.camPar.height);
00044 
00045         m_ip->gauss(m_tex_frame, m_tex_frame_ip[0]);
00046         m_ip->sobel(m_tex_frame_ip[0], m_tex_frame_ip[0], 0.03f);
00047         glDepthMask(1);
00048 }
00049 
00050 // particle filtering
00051 void EdgeTracker::particle_filtering(ModelEntry* modelEntry){
00052         
00053         m_cam_perspective.Activate();
00054         
00055         // Calculate Zoom Direction and pass it to the particle filter
00056         TomGine::tgVector3 vCam = m_cam_perspective.GetPos();
00057         TomGine::tgVector3 vObj = TomGine::tgVector3(modelEntry->pose.t.x, modelEntry->pose.t.y, modelEntry->pose.t.z);
00058         modelEntry->vCam2Model = vObj - vCam;
00059         modelEntry->predictor->setCamViewVector(modelEntry->vCam2Model);
00060         
00061         
00062         glLineWidth(5);
00063         glColor3f(0.0f,0.0f,0.0f);
00064         m_tex_frame_ip[0]->bind();      // bind camera image
00065         float ns = 0.5f;
00066         
00067         for(int i=0; i<params.num_recursions; i++){
00068                 if(params.num_recursions > 1)
00069                         ns = 1.0f - float(i)/(params.num_recursions-1);
00070                 
00071                 glColor3f(0.0f,1.0f-ns,1.0f-ns);
00072                 
00073                 modelEntry->distribution.updateLikelihood(modelEntry->model, m_shadeEdgeCompare, false, params.convergence, m_showparticles);
00074                 
00075                 modelEntry->predictor->resample(modelEntry->distribution, modelEntry->num_particles, params.variation);
00076         }
00077         modelEntry->pose_prev = modelEntry->pose;
00078         modelEntry->pose = modelEntry->distribution.getMean();
00079         modelEntry->confidence_edge = modelEntry->pose.c;
00080 }
00081 
00082 
00083 // *** PUBLIC ***
00084 
00085 EdgeTracker::EdgeTracker(){
00086         m_lock = false;
00087         m_showparticles = false;
00088         m_showmodel = true;
00089         m_draw_edges = false;
00090         m_tracker_initialized = false;
00091 }
00092 
00093 // Initialise function (must be called before tracking)
00094 bool EdgeTracker::initInternal(){       
00095         
00096         int id;
00097         // Shader
00098         if((id = g_Resources->AddShader("edgetest", "edgetest.vert", "edgetest.frag")) == -1)
00099                 exit(1);
00100         m_shadeEdgeCompare = g_Resources->GetShader(id);
00101         
00102         m_shadeEdgeCompare->bind();
00103         m_shadeEdgeCompare->setUniform("texFrame", 0);
00104         m_shadeEdgeCompare->setUniform("width", params.camPar.width);
00105         m_shadeEdgeCompare->setUniform("height", params.camPar.height);
00106         m_shadeEdgeCompare->unbind();
00107         
00108         return true;
00109 }
00110 
00111 bool EdgeTracker::track(){
00112         
00113         if(!m_tracker_initialized){
00114                 ROS_DEBUG("[EdgeTracker::track()] Error tracker not initialised!");
00115                 return false;
00116         }
00117         
00118         drawImage(0);
00119         
00120         for(unsigned i=0; i<m_modellist.size(); i++){
00121                 // Recursive particle filtering
00122                 if(!m_lock){
00123                         particle_filtering(m_modellist[i]);             
00124                 }
00125         }
00126         return true;
00127 }
00128 
00129 bool EdgeTracker::track(int id){
00130         ModelEntryList::iterator it = m_modellist.begin();
00131         while(it != m_modellist.end()){
00132                 if(id==(*it)->id){
00133                         if(!m_lock){
00134                                 particle_filtering(m_modellist[id]);            
00135                         }
00136                         return true;
00137                 }
00138                 ++it;
00139         }
00140         return false;
00141 }
00142 
00143 // Draw result of edge tracking (particle with maximum likelihood)
00144 void EdgeTracker::drawResult(float linewidth){
00145 //      float var = 0.0;
00146 //      var = m_distribution->getVariance(params.number_of_particles);
00147 //      glColor3f(1000*var,1-1000*var,0.0);
00148         m_cam_perspective.Activate();
00149         
00150         glColor3f(1.0,0.0,0.0);
00151         glLineWidth(linewidth);
00152         
00153 //              if(i==0){
00154 //                      glDepthMask(0);
00155 //                      if(m_draw_edges)
00156 //                              m_ip->render(m_tex_frame_ip[params.m_spreadlvl]);
00157 //                      else
00158 //                              m_ip->render(m_tex_model_ip[params.m_spreadlvl]);
00159 //                      glDepthMask(1);
00160 //              }
00161         
00162         for(unsigned i=0; i<m_modellist.size(); i++){
00163         
00164                 m_modellist[i]->pose.Activate();
00165                 
00166                         glColorMask(0,0,0,0); glDepthMask(1);
00167                         glClear(GL_DEPTH_BUFFER_BIT);
00168                         m_modellist[i]->model.drawFaces();
00169                         glColorMask(1,1,1,1);
00170                         
00171                         switch(m_showmodel){
00172                                 case 0:
00173                                         m_tex_frame_ip[0]->bind();
00174                                         m_shadeEdgeCompare->bind();
00175                                         m_shadeEdgeCompare->setUniform("analyze", true);
00176                                         m_modellist[i]->model.drawEdges();
00177                                         m_shadeEdgeCompare->unbind();
00178                                         break;
00179                                 case 1:
00180                                         m_modellist[i]->model.drawEdges();
00181                                         break;
00182                                 default:
00183                                         m_showmodel = 0;
00184                                         break;
00185                         }
00186                         //m_opengl.RenderSettings(true, true);
00187                         
00188                         
00189                         glColor3f(1.0,1.0,1.0);
00190                         
00191                 m_modellist[i]->pose.Deactivate();
00192         }
00193         
00194 }
00195 
00196 


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