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;
}
实验结果：