openCV中的K-D Tree

template<class T> 

class FlannKdTree 
{ 
protected: 
    /* 
     * @brief build_params is a structure containing the parameters passed to the function 
     */ 
    FLANNParameters build_params; 

    /* 
     * @brief index is a structure containing the kdtree 
     */ 
    FLANN_INDEX index; 

public: 
    /* 
     * @brief Constructor, this function builds an index from the input point cloud 
     * @param pointcloud : vector of points (the access to point coordinates is done using pointcloud[i].x,pointcloud[i].y,pointcloud[i].z) 
     */ 
    FlannKdTree(std::vector<T> pointcloud) 
    { 
        int dim = 3; // default number of dimensions (3 = xyz) 

        // check the number of points in the point cloud 
        if( pointcloud.size() == 0 ){ 
            printf("[FlannKdTree] Could not create kd-tree for %d points!",pointcloud.size()); 
            return; 
        } 

        // Allocate enough data 
        float * points = (float*) malloc(pointcloud.size() * 3 * sizeof(float)); // default number of dimensions (3 = xyz) 

        for( unsigned int cp = 0; cp < pointcloud.size(); cp++ ){ 
            points[cp * 3 + 0] = pointcloud[cp].x; 
            points[cp * 3 + 1] = pointcloud[cp].y; 
            points[cp * 3 + 2] = pointcloud[cp].z; 
        } 

        // Create the kd-tree representation 
        float speedup; 
        build_params.algorithm = KDTREE; // choose the type of the tree 
        build_params.log_level = FLANN_LOG_NONE; // controls the verbosity of the messages generated by the FLANN library functions 

        build_params.trees = 1; 
        build_params.target_precision = -1; 
        build_params.checks = 128; 

        printf("Building index\n"); 
        index = flann_build_index(points, pointcloud.size(), dim, &speedup, 
                                  &build_params); 
        printf("Index built\n"); 
    } 

    /* 
     * @brief Default destructor 
     */ 
    ~FlannKdTree() 
    { 
        // ANN Cleanup 
        flann_free_index(index, &build_params); 
    } 

    /* 
     * @brief This function searches for the nearest neighbors of the query point using an index already built 
     * @param queryPoint : reference input point (the access to the coordinates is done by queryPoint.x,queryPoint.y,queryPoint.z) 
     * @param knn : number of point nearest neighbors 
     * @param indices : vector of indices to the nearest neighbors (indices of the vector used to build the index) 
     * @param distances : vector of distances to the nearest neighbors (distances between the query point and the corresponding points in indices) 
     */ 
    void knnSearch(T queryPoint, int knn, std::vector<int> &indices, 
                   std::vector<float> &distances) 
    { 
        float * inputPoint = (float*) malloc(3 * sizeof(float)); // default number of dimensions (3 = xyz) 
        inputPoint[0] = queryPoint.x; 
        inputPoint[1] = queryPoint.y; 
        inputPoint[2] = queryPoint.z; 

        flann_find_nearest_neighbors_index(index, inputPoint, 1, &indices[0],&distances[0], knn, &build_params); 
    } 

}; 

1）建立查詢樹 : 
std::vector<cv::Point3f> cvTargetCloud; 
// fill the cvTargetCloud with the cartesian coordinates (x, y z) 
cv::flann::KDTreeIndexParams indexParams; 
cv::flann::Index kdtree(cv::Mat(cvTargetCloud).reshape(1), indexParams); 

2） 尋找 : 
//cv::Point3f pt declared 
std::vector<float> query; 
query.push_back(pt.x); 
query.push_back(pt.y); 
query.push_back(pt.z); 
int k = 4; //number of nearest neighbors 
std::vector<int> indices(k); 
std::vector<float> dists(k); 
kdtree.knnSearch(query, indices, dists, k,cv::flann::SearchParams(64));