LocateAlgorithm.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                         LocateAlgorithm.cpp - Copyright klank
00021 **************************************************************************/
00022 
00023 #include "LocateAlgorithm.h"
00024 #include "XMLTag.h"
00025 
00026 #include <pluginlib/class_loader.h>
00027 
00028 
00029 // Constructors/Destructors
00030 //
00031 #include <time.h>
00032 #define TRACKING_POSSIBLE_MS 2
00033 
00034 using namespace cop;
00035 
00036 
00037 
00038 pluginlib::ClassLoader<LocateAlgorithm> s_localg_loader("cognitive_perception", "LocateAlgorithm");
00039 
00040 LocateAlgorithm* LocateAlgorithm::LocAlgFactory(XMLTag* tag)
00041 {
00042   std::string name = tag->GetName();
00043   LocateAlgorithm* alg = NULL;
00044   try
00045   {
00046     alg = s_localg_loader.createClassInstance(name);
00047     if(alg != NULL)
00048       alg->SetData(tag);
00049   }
00050   catch(pluginlib::PluginlibException& ex)
00051   {
00052   //handle the class failing to load
00053     ROS_WARN("LocateAlgorithm: The plugin failed to load for some reason. Error: %s\n", ex.what());
00054     ROS_WARN("LocateAlgorithm: Tag failed: %s\n", tag->GetName().c_str());
00055   }
00056         return alg;
00057 }
00058 
00059 
00060 bool LocateAlgorithm::TrackingPossible(const Reading& img, const Signature& sig, RelPose* pose)
00061 {
00062         unsigned long l = (unsigned long)time(NULL);
00063         l -= sig.date();
00064 #ifdef _DEBUG
00065         printf("TrackingPossible? Last Match diff: %ld\n", l);
00066 #endif
00067         const RelPose* old_pose = sig.GetObjectPose();
00068         if(old_pose != NULL)
00069         {
00070           RelPose* copy = RelPoseFactory::FRelPose(old_pose->m_uniqueID);
00071           if(l > 0 && l < TRACKING_POSSIBLE_MS && pose != NULL && copy != NULL)
00072           {
00073                 Matrix m = copy->GetCovarianceMatrix(0);
00074                 //TODO Check Timestamp
00075 #ifdef _DEBUG
00076                 printf("Check 3 Cov Values of %ld: %f < 0.1; %f < 0.1;%f < 0.1;\n", old_pose->m_uniqueID, m.data()[0],m.data()[7], m.data()[14]);
00077 #endif
00078                 return m.data()[0] < 0.1 && m.data()[7] < 0.1 && m.data()[14] < 0.1;
00079           }
00080           RelPoseFactory::FreeRelPose(&copy);
00081   }
00082         return false;
00083 
00084 }
00085 
00086 
00087 // Other methods
00088 //
00089 
00090 
 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:34