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++;
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::subcluster (std::vector<pcl::people::PersonCluster<PointT> >& clusters)
{
// Check if all mandatory variables have been set:
if (std::isnan(sqrt_ground_coeffs_))
{
PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Floor parameters have not been set or they are not valid!\n");
return;
}
if (cluster_indices_.empty ())
{
PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Cluster indices have not been set!\n");
return;
}
if (cloud_ == nullptr)
{
PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Input cloud has not been set!\n");
return;
}
// Person clusters creation from clusters indices:
for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices_.begin(); it != cluster_indices_.end(); ++it)
{
pcl::people::PersonCluster<PointT> cluster(cloud_, *it, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_); // PersonCluster creation
clusters.push_back(cluster);
}
// Remove clusters with too high height from the ground plane:
std::vector<pcl::people::PersonCluster<PointT> > new_clusters;
for(size_t i = 0; i < clusters.size(); i++) // for every cluster
{
if (clusters[i].getHeight() <= max_height_)
new_clusters.push_back(clusters[i]);
}
clusters = new_clusters;
new_clusters.clear();
// Merge clusters close in floor coordinates:
mergeClustersCloseInFloorCoordinates(clusters, new_clusters);
clusters = new_clusters;
std::vector<pcl::people::PersonCluster<PointT> > subclusters;
int cluster_min_points_sub = int(float(min_points_) * 1.5);
// int cluster_max_points_sub = max_points_;
// create HeightMap2D object:
pcl::people::HeightMap2D<PointT> height_map_obj;
height_map_obj.setGround(ground_coeffs_);
height_map_obj.setInputCloud(cloud_);
height_map_obj.setSensorPortraitOrientation(vertical_);
height_map_obj.setMinimumDistanceBetweenMaxima(heads_minimum_distance_);
for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it) // for every cluster
{
float height = it->getHeight();
int number_of_points = it->getNumberPoints();
if(height > min_height_ && height < max_height_)
{
if (number_of_points > cluster_min_points_sub) // && number_of_points < cluster_max_points_sub)
{
// Compute height map associated to the current cluster and its local maxima (heads):
height_map_obj.compute(*it);
if (height_map_obj.getMaximaNumberAfterFiltering() > 1) // if more than one maximum
{
// create new clusters from the current cluster and put corresponding indices into sub_clusters_indices:
createSubClusters(*it, height_map_obj.getMaximaNumberAfterFiltering(), height_map_obj.getMaximaCloudIndicesFiltered(), subclusters);
}
else
{ // Only one maximum --> copy original cluster:
subclusters.push_back(*it);
}
}
else
{
// Cluster properties not good for sub-clustering --> copy original cluster:
subclusters.push_back(*it);
}
}
}
clusters = subclusters; // substitute clusters with subclusters
}