TrackAlgorithm.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009 by Ulrich Friedrich Klank <klank@in.tum.de>
00003  *
00004  * This program is free software; you can redistribute it and/or modify
00005  * it under the terms of the GNU General Public License as published by
00006  * the Free Software Foundation; either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 
00019 /************************************************************************
00020                         TrackAlgorithm.cpp - Copyright klank
00021 
00022 
00023 **************************************************************************/
00024 
00025 #include "TrackAlgorithm.h"
00026 #include <boost/bind.hpp>
00027 #include <boost/thread.hpp>
00028 extern volatile bool g_stopall;
00029 
00030 
00031 
00032 using namespace cop;
00033 
00034 
00035 // Constructors/Destructors
00036 //
00037 
00038 TrackAlgorithm::TrackAlgorithm (PerceptionPrimitive& sig, Evaluator* eval, Algorithm<std::vector<RelPose*> > *alg,
00039                                ImageInputSystem* imageSys) :
00040     m_Running(true),
00041     m_alg(alg),
00042     m_curPrim(sig),
00043     m_imageSys( imageSys),
00044     m_eval(eval)
00045 {
00046     m_trackingThread = new boost::thread( boost::bind(&TrackAlgorithm::threadfunc, this));
00047 }
00048 
00049 
00050 TrackAlgorithm::~TrackAlgorithm ( )
00051 {
00052   /* Stop Thread:*/
00053   m_Running = false;
00054   if(m_trackingThread != NULL)
00055     m_trackingThread->join();
00056     delete m_trackingThread;
00057   m_curPrim.SetTerminated();
00058 
00059     /* Wait for termination and continue destroying*/
00060 }
00061 
00062 //
00063 // Methods
00064 //
00065 //
00066 // Methods
00067 //
00068 void TrackAlgorithm::threadfunc()
00069 {
00070   bool resultReceived = false;
00071   Signature& object = *m_curPrim.GetSignature();
00072   RelPose* lastKnownPose = object.GetObjectPose();
00073   if(lastKnownPose == NULL)
00074     lastKnownPose = RelPoseFactory::FRelPoseWorld();
00075   std::vector<Sensor*> sensors = m_imageSys->GetBestSensor(*lastKnownPose);
00076   while(m_Running && !g_stopall)
00077   {
00078     int numOfObjects = 1;
00079     try
00080     {
00081 
00082 #ifdef _DEBUG
00083       printf("Algorithm for track: %s (%p)\n",m_alg != NULL ? m_alg->GetName().c_str() : "NoName", m_alg );
00084 #endif
00085       if(m_alg != NULL)
00086       {
00087 
00088          double qualityMeasure = 0.0;
00089          boost::xtime t0, t1;
00090          boost::xtime_get(&t0, boost::TIME_UTC);
00091          std::vector<RelPose*> pose = m_alg->Perform(sensors, lastKnownPose, object, numOfObjects, qualityMeasure);
00092          boost::xtime_get(&t1, boost::TIME_UTC);
00093          unsigned long time = ((1000000000 * (t1.sec - t0.sec))+(t1.nsec - t0.nsec));
00094 #ifdef _DEBUG
00095         printf("Calc time: %ld ns\n", time);
00096 #endif
00097         if(pose.size() > 0)
00098         {
00099           pose[0]->m_qualityMeasure = qualityMeasure;
00100           RelPose*& pose_curr = object.GetObjectPose();
00101           if(pose_curr != NULL && pose_curr->m_uniqueID != ID_WORLD && resultReceived)
00102           {
00103             RelPose* temp = pose[0];
00104             printf("temp id: %ld", pose[0]->m_uniqueID);
00105 
00106             pose[0] = RelPoseFactory::FRelPose(pose_curr, temp->m_parentID, temp->GetMatrix(0), temp->GetCovarianceMatrix());
00107             pose[0]->m_qualityMeasure = qualityMeasure;
00108             //RelPoseFactory::FreeRelPose(temp);
00109           }
00110           object.SetPose(pose[0]);
00111           printf("Sent %ld\n", pose[0]->m_uniqueID);
00112           if(!resultReceived)
00113           {
00114             m_curPrim.AddResult(m_alg, m_eval, object.m_ID, qualityMeasure, time);
00115             resultReceived = true;
00116           }
00117         }
00118         else
00119         {
00120            object.SetPose(NULL);
00121         }
00122       }
00123     }
00124     catch(char const* message)
00125     {
00126       printf("Error in Locate: %s\n", message);
00127     }
00129     lastKnownPose = object.GetObjectPose();
00131     sensors[0]->WaitForNewData();
00132   }
00133 }
00134 
00135 XMLTag* TrackAlgorithm::Save()
00136 {
00137     return NULL;
00138 }
00139 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


cognitive_perception
Author(s): Ulrich F Klank
autogenerated on Thu May 23 2013 07:38:35