C++ vector::getHeight方法代码示例

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
}