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


hector_object_tracker
Author(s): Johannes Meyer
autogenerated on Mon Jun 27 2016 05:01:21