/*
* Create a new person from the provided stamped pose and point cloud, and insert them into the map.
*/
Person& PersonDetector::create_person(const geometry_msgs::Pose& pose, const sensor_msgs::PointCloud2& cloud) {
// Make a new person with the trivial person classifier, for now, and then give them an initial pose.
Person lifeform(_current_uid++);
lifeform.push_pose(pose);
// boost::shared_ptr<PersonClassifier> classifier(new ShirtColorPersonClassifier(cloud, 30.0));
boost::shared_ptr<PersonClassifier> classifier(new TrivialPersonClassifier);
// Then insert them.
tracked()[lifeform.uid()] = lifeform;
classifiers()[lifeform.uid()] = classifier;
// Return a reference to the person in the map.
return tracked()[lifeform.uid()];
}
/*
* Attempts to find the person which is most closely related to the provided cloud.
*/
boost::optional<Person&> PersonDetector::classify_message(const geometry_msgs::Pose& pose, const sensor_msgs::PointCloud2& cloud) {
// First, we need to see if any of the classifiers state that we've seen this person before.
for(std::map<unsigned int, Person>::iterator iter = tracked().begin(); iter != tracked().end(); iter++) {
unsigned int uid = (*iter).first;
boost::optional<PersonClassifier&> classifier = classifier_by_id(uid);
// If the classifier doesn't exist, there isn't much we can do to classify.
if(!classifier.is_initialized())
continue;
if(classifier.get().is_equivalent(pose, cloud))
return boost::optional<Person&>((*iter).second);
}
return boost::none;
}
void IgstkTool::internalTracked(bool value)
{
if (mTracked == value)
return;
mTracked = value;
if (!mTracked && mVisible)
this->internalVisible(false); //Make sure tool is invisible when not tracked
emit tracked(mTracked);
}
/*
* Attempt to find a person by UID. Returns a reference to the person if found, or an empty optional if not.
*/
boost::optional<Person&> PersonDetector::person_by_id(unsigned int uid) {
if(tracked().count(uid) >= 1)
return tracked()[uid];
return boost::none;
}
