opencv空间最近点搜索

#include "opencv2//flann/flann.hpp"
#include "opencv2//flann/miniflann.hpp"
#include "opencv2//flann/kdtree_index.h"
#include "opencv2//flann/matrix.h"
#include "opencv2//flann/miniflann.hpp"


using namespace cv;
using namespace std;

//其中的vSourcePoints和vTargetPoints分别是需要查找的点云和被查找的点云，将其中的每一个顶点的
//x、y、z值依次放入数组中即可，vIndices和vDists分别是查找得到的source中每个点在target点云中最近的下标及距离。
//被注释的两行是根据自己理解设置的参数，但是结果不正确；修改的参数是参考PCL中设置的，结果正确。
void searchNearestPoints(const float * vSourcePoints, int iSourceNum,
	const float * vTargetPoints, int iTargetNum,
	int * vIndices, float * vDists)
{
	// 建立索引
	cvflann::Matrix data(new float[iTargetNum * 3], iTargetNum, 3);
	for (int i = 0; i < iTargetNum; i++)
	{
		data[i][0] = vTargetPoints[i * 3 + 0];
		data[i][1] = vTargetPoints[i * 3 + 1];
		data[i][2] = vTargetPoints[i * 3 + 2];
	}
	//flann::Index > index(data, flann::LinearIndexParams());
	cvflann::Index > index(data, cvflann::KDTreeSingleIndexParams(15));
	index.buildIndex();

	// 查找最近邻
	int knn = 1;
	cvflann::Matrix p(new float[iSourceNum * 3], iSourceNum, 3);
	for (int i = 0; i < iSourceNum; i++)
	{
		p[i][0] = vSourcePoints[i * 3 + 0];
		p[i][1] = vSourcePoints[i * 3 + 1];
		p[i][2] = vSourcePoints[i * 3 + 2];
	}
	cvflann::Matrix indices(new int[iSourceNum*knn], iSourceNum, knn);
	cvflann::Matrix dists(new float[iSourceNum*knn], iSourceNum, knn);
	//index.knnSearch(p, indices, dists, knn, flann::SearchParams(512));
	index.knnSearch(p, indices, dists, knn, cvflann::SearchParams());

	// 返回结果
	for (int i = 0; i < iSourceNum; ++i)
	{
		vIndices[i] = indices[i][0];
		vDists[i] = dists[i][0];
	}

	// 释放内存
	data.free();
	p.free();
	indices.free();
	dists.free();
}