/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/knn_cloud.h

Go to the documentation of this file.

// Copyright (C) 2011 Soren Hauberg
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, see <http://www.gnu.org/licenses/>.
 
#ifndef KNN_CLOUD_H
#define KNN_CLOUD_H

#include <ANN/ANN.h> 

class knn_cloud
{
  public:
    knn_cloud (const double epsilon = 0.01)
      : pa (NULL),
        search_tree (NULL),
        eps (epsilon),
        num_points (0)
      {
        index = new ANNidx [1];
        dist = new ANNdist [1];
        query_point = annAllocPt (3);
      }
    
    ~knn_cloud ()
      {
        if (pa != NULL) annDeallocPts (pa);
        if (search_tree != NULL) delete search_tree;
        if (index != NULL) delete [] index;
        if (dist != NULL) delete [] dist;
        if (query_point != NULL) annDeallocPt (query_point);
        
        annClose ();
      }
    
    void refresh (const std::vector<vector3> &points)
      {
        // Clean up after previous call to refresh (if needed)
        if (pa != NULL)
          {
            annDeallocPts (pa);
            pa = NULL;
          }
        if (search_tree != NULL)
          {
            delete search_tree;
            search_tree = NULL;
          }
        
        // Train new kNN structure
        num_points = points.size ();
        if (num_points > 0)
          {
            pa = annAllocPts (num_points, 3);
            for (size_t i = 0; i < num_points; i++)
              {
                const vector3 v = points [i];
                ANNpoint p = pa [i];
                for (size_t d = 0; d < 3; d++)
                  p [d] = v [d];
              }

            search_tree = new ANNkd_tree (pa, num_points, 3);
          }
      }
    
    double nearest_distance (const vector3 &p) const
      {
        double retval;
        // XXX: Can we somehow make ANN thread-safe?
        #pragma omp critical
        {
        for (size_t d = 0; d < 3; d++)
          query_point [d] = p [d];
        
        search_tree->annkSearch (query_point, 1, index, dist, eps);
        retval = dist [0];
        }

        return retval;
      }
    
    size_t knn_search (size_t k, const vector3 &p, std::vector<size_t> &indices, std::vector<double> &distances) const
      {
        if (is_valid ())
          {
            k = std::min (k, num_points);

            // Allocate space
            ANNidxArray l_index = new ANNidx [k];
            ANNdistArray l_dist = new ANNdist [k];
      
            // Perform the search (ANN is not thread-safe; hence the pragma)
            #pragma omp critical
            {
            for (size_t d = 0; d < 3; d++)
              query_point [d] = p [d];
        
            search_tree->annkSearch (query_point, k, l_index, l_dist, eps);
            }
        
            // Copy results
            indices.resize (k);
            distances.resize (k);
            for (size_t j = 0; j < k; j++)
              {
                indices [j] = l_index [j]; 
                distances [j] = l_dist [j];
              }
        
            // Release space
            delete [] l_index;
            delete [] l_dist;
            
            return k;
          }
        else
          return 0;
      }
    
    bool is_valid () const
      {
        return search_tree != NULL;
      }
  
    vector3 operator[] (const size_t idx) const
      {
        vector3 retval;
        for (size_t n = 0; n < 3; n++)
          retval [n] = pa [idx] [n];
        return retval;
      }
    
    size_t size () const
      {
        return num_points;
      }
  
  private:
    ANNpointArray pa;
    ANNkd_tree *search_tree;
    ANNidxArray index;
    ANNdistArray dist;
    ANNpoint query_point;
    const double eps;
    size_t num_points;
};

#endif // KNN_CLOUD_H