brute-force search

#include <pcl/search/brute_force.h>
#include <pcl/common/common.h>
#include <iostream>

using namespace std;
using namespace pcl;

int main()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    // x+y+z=1平面
    for (float x = -1.0; x <= 1.0; x += 0.5)    
    {    
        for (float y = -1.0; y <= 1.0; y += 0.5)    
        {    
            pcl::PointXYZ cloud_;    
            cloud_.x = x;    
            cloud_.y = y;    
            cloud_.z = 1 - x - y;    
            cloud->push_back(cloud_);    
        }    
    }   

    pcl::search::BruteForce<pcl::PointXYZ> bfsearch;
    bfsearch.setInputCloud(cloud);
    std::vector<int> k_indices;
    std::vector<float> k_distances;
    pcl::PointXYZ p(-1.0,-1.0,3.0);
    bfsearch.nearestKSearch(p,2,k_indices,k_distances);

    return 0;
}

template <typename PointT> int
pcl::search::BruteForce<PointT>::denseKSearch (
    const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const
{
  // container for first k elements -> O(1) for insertion, since order not required here
  std::vector<Entry> result;
  result.reserve (k);
  std::priority_queue<Entry> queue;
 
    Entry entry;
    for (entry.index = 0; entry.index < std::min (static_cast<unsigned> (k), static_cast<unsigned> (input_->size ())); ++entry.index)
    {
      entry.distance = getDistSqr (input_->points[entry.index], point);
      result.push_back (entry);
    }

    queue = std::priority_queue<Entry> (result.begin (), result.end ());

    // add the rest
    for (; entry.index < input_->size (); ++entry.index)
    {
      entry.distance = getDistSqr (input_->points[entry.index], point);
      if (queue.top ().distance > entry.distance)
      {
        queue.pop ();
        queue.push (entry);
      }      
    }

  k_indices.resize (queue.size ());
  k_distances.resize (queue.size ());
  size_t idx = queue.size () - 1;
  while (!queue.empty ())
  {
    k_indices [idx] = queue.top ().index;
    k_distances [idx] = queue.top ().distance;
    queue.pop ();
    --idx;
  }
  
  return (static_cast<int> (k_indices.size ()));
}