ObjectModel.cpp
Go to the documentation of this file.
00001 #include "ObjectModel.h"
00002 #include "Object.h"
00003 
00004 namespace hector_object_tracker {
00005 
00006 ObjectModel::ObjectModel(const std::string& frame_id)
00007 {
00008   setFrameId(frame_id);
00009 }
00010 
00011 ObjectModel::ObjectModel(const ObjectModel &other)
00012 {
00013   *this = other;
00014 }
00015 
00016 ObjectModel::~ObjectModel()
00017 {}
00018 
00019 ObjectModel::ObjectList ObjectModel::getObjects() const
00020 {
00021   boost::recursive_mutex::scoped_lock lock(objectsMutex);
00022   return objects;
00023 }
00024 
00025 ObjectModel::ObjectList ObjectModel::getObjects(const std::string& class_id) const
00026 {
00027   boost::recursive_mutex::scoped_lock lock(objectsMutex);
00028   ObjectList class_list;
00029 
00030   for(const_iterator it = begin(); it != end(); ++it) {
00031     if ((*it)->getClassId() == class_id) class_list.push_back(*it);
00032   }
00033 
00034   return class_list;
00035 }
00036 
00037 ObjectPtr ObjectModel::getObject(const std::string& object_id) const {
00038   boost::recursive_mutex::scoped_lock lock(objectsMutex);
00039 
00040   for(const_iterator it = begin(); it != end(); ++it) {
00041     if ((*it)->getObjectId() == object_id) return *it;
00042   }
00043 
00044   return ObjectPtr();
00045 }
00046 
00047 std_msgs::Header ObjectModel::getHeader() const {
00048   std_msgs::Header temp;
00049   temp.frame_id = header.frame_id;
00050 
00051   for(const_iterator it = begin(); it != end(); ++it) {
00052     if ((*it)->getStamp() > temp.stamp) temp.stamp = (*it)->getStamp();
00053   }
00054   return temp;
00055 }
00056 
00057 void ObjectModel::setFrameId(const std::string &frame_id) {
00058   header.frame_id = frame_id;
00059 }
00060 
00061 void ObjectModel::getMessage(worldmodel_msgs::ObjectModel& model) const {
00062   boost::recursive_mutex::scoped_lock lock(objectsMutex);
00063 
00064   model.header = getHeader();
00065   model.objects.clear();
00066   model.objects.reserve(objects.size());
00067   for(ObjectList::const_iterator it = objects.begin(); it != objects.end(); ++it) {
00068     model.objects.push_back((*it)->getMessage());
00069   }
00070 }
00071 
00072 worldmodel_msgs::ObjectModelPtr ObjectModel::getMessage() const {
00073   worldmodel_msgs::ObjectModelPtr model(new worldmodel_msgs::ObjectModel());
00074   getMessage(*model);
00075   return model;
00076 }
00077 
00078 void ObjectModel::reset()
00079 {
00080   boost::recursive_mutex::scoped_lock lock(objectsMutex);
00081   objects.clear();
00082   Object::reset();
00083 }
00084 
00085 ObjectPtr ObjectModel::add(const std::string& class_id, const std::string& object_id) {
00086   return add(ObjectPtr(new Object(class_id, object_id)));
00087 }
00088 
00089 ObjectPtr ObjectModel::add(ObjectPtr object) {
00090   objects.push_back(object);
00091   return object;
00092 }
00093 
00094 void ObjectModel::remove(ObjectPtr object) {
00095   for(ObjectList::iterator it = objects.begin(); it != objects.end(); ++it) {
00096     if (*it == object) {
00097       remove(it);
00098       return;
00099     }
00100   }
00101 }
00102 
00103 void ObjectModel::remove(iterator it) {
00104   objects.erase(it);
00105 }
00106 ObjectModel& ObjectModel::operator =(const ObjectModel& other)
00107 {
00108   header = other.header;
00109   objects = other.objects;
00110   return *this;
00111 }
00112 
00113 ObjectModel& ObjectModel::operator =(const worldmodel_msgs::ObjectModel& other)
00114 {
00115   header = other.header;
00116 
00117   for(worldmodel_msgs::ObjectModel::_objects_type::const_iterator it = other.objects.begin();
00118       it != other.objects.end();
00119       ++it)
00120   {
00121     ObjectPtr object = getObject(it->info.object_id);
00122     if (!object) {
00123       object.reset(new Object(*it));
00124       add(object);
00125     } else {
00126       *object = *it;
00127     }
00128   }
00129 
00130   return *this;
00131 }
00132 
00133 void ObjectModel::getVisualization(visualization_msgs::MarkerArray &markers) const {
00134   boost::recursive_mutex::scoped_lock lock(objectsMutex);
00135 
00136   markers.markers.clear();
00137   for(ObjectList::const_iterator it = objects.begin(); it != objects.end(); ++it) {
00138     (*it)->getVisualization(markers);
00139   }
00140 }
00141 
00142 float ObjectModel::getBestCorrespondence(ObjectPtr &object, const Eigen::Vector3f& position, const Eigen::Matrix3f& covariance, const std::string& class_id, float max_distance) const
00143 {
00144   float min_distance = max_distance;
00145   if (min_distance <= 0.0) min_distance = FLT_MAX;
00146 
00147   object.reset();
00148 
00149   for(ObjectModel::const_iterator it = begin(); it != end(); ++it) {
00150     ObjectPtr x = *it;
00151     if (!class_id.empty() && class_id != x->getClassId()) continue;
00152     Eigen::Vector3f diff = x->getPosition() - position;
00153     float distance = (diff.transpose() * (x->getCovariance() + covariance).inverse() * diff)[0];
00154     if (distance < min_distance) {
00155       object = x;
00156       min_distance = distance;
00157     }
00158   }
00159 
00160   return min_distance;
00161 }
00162 
00163 void ObjectModel::mergeWith(const ObjectModel &other_model, tf::TransformListener &tf, const std::string& prefix) {
00164   for(ObjectModel::const_iterator other = other_model.begin(); other != other_model.end(); ++other) {
00165     merge(*other, tf, prefix);
00166   }
00167 }
00168 
00169 void ObjectModel::merge(const ObjectPtr& object, tf::TransformListener &tf, const std::string& prefix)
00170 {
00171   // transform other object's pose
00172   ObjectPtr transformed;
00173   try {
00174     transformed = object->transform(tf, header.frame_id, ros::Time());
00175   } catch (tf::TransformException& ex) {
00176     ROS_WARN("Could not transform from frame %s into %s during object merge: %s", object->getHeader().frame_id.c_str(), header.frame_id.c_str(), ex.what());
00177     return;
00178   }
00179 
00180   // search for corresponding objects
00181   ObjectPtr mine;
00182   float distance = getBestCorrespondence(mine, transformed->getPosition(), transformed->getCovariance(), object->getClassId());
00183   if (distance < 1.0) {
00184     // found corresondence
00185     ROS_DEBUG("Merging %s and %s", mine->getObjectId().c_str(), object->getObjectId().c_str());
00186     mine->setObjectId(mine->getObjectId() + "," + prefix + object->getObjectId());
00187     mine->update(transformed->getPosition(), transformed->getCovariance(), object->getSupport());
00188 
00189   } else {
00190     // add as new object
00191     ROS_DEBUG("Adding %s", transformed->getObjectId().c_str());
00192     transformed->setObjectId(prefix + object->getObjectId());
00193     add(transformed);
00194   }
00195 }
00196 
00197 } // namespace hector_object_tracker
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_object_tracker
Author(s): Johannes Meyer
autogenerated on Mon Jul 15 2013 16:50:57