k-d树搜索最近点，在opencv中使用FLANN算法，其包含：

1：建树   2.查询

程序见下：

#include "kdtree.h"
#include <iostream>
#include <iomanip> 
#include "cv.h"
#include "highgui.h"  
#include <fstream>
#include "cv.h"
#include "highgui.h"  
#include <vector>    // <vector > STL头文件
#include <stdio.h>
#include "opencv2/objdetect/objdetect.hpp"  
#include "opencv2/features2d/features2d.hpp"  
#include "opencv2/highgui/highgui.hpp"  
#include "opencv2/calib3d/calib3d.hpp"  
#include "opencv2/imgproc/imgproc_c.h"
#include <iostream> 


#define EXAMPLAR_NUM 700
#define EXAMPLAR_DIM 3
/****系统可以搜索660个3维点云中的最近点****/
/************保存CvMat型矩阵****************************
输入参数：&VecString:n个点云容器
输出参数：*Matri:转换成对应的3*n矩阵
*************************************************************/
void SaveCvmatToTXT(const CvMat*Matri, const char* filename)
{
std::ofstream  file(filename);  //保存文件


for (int i = 0; i<Matri->rows; i++)
{
for (int j = 0; j<Matri->cols; j++)
{
file << "    " << cvmGet(Matri, i, j);
}
file << std::endl;
}
file.close();
}
/***********读取txt文档保存CvMat型矩阵****************************
输入参数：&VecString:n个点云容器
输出参数：*Matri:转换成对应的3*n矩阵
***************************************************************/
void ReadTxtToCvmat(CvMat*Matri, const char* filename)
{
std::ifstream  file(filename);  //保存文件
double temp;
for (int i = 0; i<Matri->rows; i++)
{
for (int j = 0; j<Matri->cols; j++)
{
file >> temp;
cvmSet(Matri, i, j,temp);
}
}
file.close();
}

int main(int argc, char *argv[])
{

cv::Mat target(10, 3, CV_32F);  //目标点云（用之构建k-d树

ReadTxtToCvmat(&(CvMat)target, "data.txt");
std::cout << "target"<<target<< std::endl;

//1）创建查询树 :（此处构建K-d树）


cv::flann::KDTreeIndexParams indexParams(4);//（此参数用来设置构建的数据结构，此处选择K-d树）
cv::flann::Index kdtree(target, indexParams);//此处用target构建k-d树


//2） 查找 :
/*cv::Point3f pt ;
std::cout << "Input target point:" << std::endl;
std::cin >> pt.x  >> pt.y >> pt.z;
std::vector<float> query;
query.push_back(pt.x);
query.push_back(pt.y);
    query.push_back(pt.z);*/  //可用来手动输入单个要搜索的原始点

cv::Mat source= target;    //原始点
cv::Mat neibours(source.rows, source.cols, CV_32F);//存储搜索到的点

int k =1; //number of nearest neighbors 

cv::Mat indices(source.rows, k, CV_32F);   //装载搜索到的对应点的索引（即neibours在target这个矩阵的行数）
cv::Mat dists(source.rows,k,CV_32F);         //搜索到的最近邻的距离
kdtree.knnSearch(source, indices, dists, k, cv::flann::SearchParams(32));


//std::cout << indices.at<int>(9, 0) << std::endl;
//std::cout << dists.at<float>(9,0) << std::endl;
for (int i = 0; i < neibours.rows; i++)
{
neibours.row(i) = target.row(indices.at<int>(i, 0)) + 0;
}

std::cout << "source=" << source << std::endl;
std::cout << "neibours" << neibours << std::endl;
std::cout << "indices=" << indices << std::endl;
std::cout << "dists=" << dists << std::endl;


system("PAUSE");
return 0;
}

实验结果：

OpenCV K-d树实现之FLANN (Fast Library for Approximate Nearest Neighbors) 算法实现及解析