template<typename PointT> void PersonClusterVisualizer<PointT>::visualize(const std::string& nameSpace, std::vector<pcl::people::PersonCluster<PointT> >& clusters) { // Save us some computation time if there are no subscribers. if(_markerArrayPublisher.getNumSubscribers() == 0) return; // Look up values for this particular namespace std::set<unsigned int>& oldMarkerIds = _oldMarkerIds[nameSpace]; unsigned int& lastMarkerId = _lastMarkerIds[nameSpace]; visualization_msgs::MarkerArray markerArray; // Remove old clusters for(std::set<unsigned int>::const_iterator it = oldMarkerIds.begin(); it != oldMarkerIds.end(); ++it) { visualization_msgs::Marker oldClusterMarker = MarkerUtils::createMarker(nameSpace, _detectionFrame); oldClusterMarker.id = *it; oldClusterMarker.action = visualization_msgs::Marker::DELETE; markerArray.markers.push_back(oldClusterMarker); } oldMarkerIds.clear(); double lifetime = 1.0 / 15; unsigned int k = lastMarkerId + 1; for (typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it) { // // 3D bounding box // visualization_msgs::Marker clusterMarker = MarkerUtils::createMarker(nameSpace, _detectionFrame); clusterMarker.id = k; clusterMarker.type = visualization_msgs::Marker::CUBE; clusterMarker.color = MarkerUtils::getPaletteColor(_visualizationCounter); clusterMarker.color.a = 0.35; clusterMarker.scale.x = it->getMax().x() - it->getMin().x(); clusterMarker.scale.y = it->getHeight(); // note that in the detection frame, y is up clusterMarker.scale.z = it->getMax().z() - it->getMin().z(); clusterMarker.lifetime.fromSec(lifetime); // just to be safe Eigen::Vector3f tcenter = it->getTCenter(); tf::poseEigenToMsg(Eigen::Translation3d(tcenter.cast<double>()) * Eigen::Affine3d::Identity(), clusterMarker.pose); markerArray.markers.push_back(clusterMarker); oldMarkerIds.insert(clusterMarker.id); k++; // // Center of gravity // visualization_msgs::Marker cogMarker = MarkerUtils::createMarker(nameSpace, _detectionFrame); cogMarker.id = k; cogMarker.type = visualization_msgs::Marker::SPHERE; cogMarker.color = MarkerUtils::getPaletteColor(_visualizationCounter); cogMarker.color.a = 1.0; cogMarker.scale.x = cogMarker.scale.y = cogMarker.scale.z = 0.1; // 10 cm cogMarker.lifetime.fromSec(lifetime); // just to be safe Eigen::Vector3f center = it->getCenter(); tf::poseEigenToMsg(Eigen::Translation3d(center.cast<double>()) * Eigen::Affine3d::Identity(), cogMarker.pose); markerArray.markers.push_back(cogMarker); oldMarkerIds.insert(cogMarker.id); k++; // // Person confidence (if set) // if(it->getPersonConfidence() == it->getPersonConfidence()) { // must not be NaN std::stringstream confidenceStr; confidenceStr << std::fixed << std::setprecision(1) << it->getPersonConfidence(); visualization_msgs::Marker confidenceMarker = MarkerUtils::createMarker(nameSpace, _detectionFrame); confidenceMarker.id = k; confidenceMarker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; //confidenceMarker.color = MarkerUtils::getPaletteColor(_visualizationCounter); confidenceMarker.color.r = confidenceMarker.color.g = confidenceMarker.color.b = 1.0; confidenceMarker.color.a = 1.0; confidenceMarker.scale.z = 0.2; // height of letters in m(?) confidenceMarker.lifetime.fromSec(lifetime); // just to be safe confidenceMarker.text = confidenceStr.str(); Eigen::Vector3f top = it->getTop(); tf::poseEigenToMsg(Eigen::Translation3d(top.cast<double>()) * Eigen::Affine3d::Identity(), confidenceMarker.pose); markerArray.markers.push_back(confidenceMarker); oldMarkerIds.insert(confidenceMarker.id); k++; } } lastMarkerId = k; _markerArrayPublisher.publish(markerArray); _visualizationCounter++; }