C++ KDTree类代码示例

// C++ tests go here
int test1(){
	int N = 8;
	vector< Point > A(N, vector<double>(3,0));
	A[0][0] = 200; A[0][1] = 300; A[0][2] = 400;
	A[1][0] = 100; A[1][1] = 200; A[1][2] = 300;
	A[2][0] = 600; A[2][1] = 100; A[2][2] = 500;
	A[3][0] = 100; A[3][1] = 100; A[3][2] = 300;
	A[4][0] = 700; A[4][1] = 50;  A[4][2] = 300;
	A[5][0] = 700; A[5][1] = 110; A[5][2] = 500;
	A[6][0] = 699; A[6][1] = 51;  A[6][2] = 301;
	A[7][0] = 701; A[7][1] = 50;  A[7][2] = 305;
	KDTree* tree = new KDTree( A );

    vector<double> Xq( 3, 0 );
    Xq[0] = 201; Xq[1] = 301; Xq[2] = 400;

    cout << "In test\n";

    //return 0;

	// for (int i=0; i < N; i++)
	//	cout << "expected: " << i << " obtained: " << tree -> closest_point( A[i] ) << endl;

    cout << "%.2f\n" << tree->kde(Xq);

	return 0;
}

TEST(KDTree, shouldSupportIntersectionSearchForRegularNodes) {

  KDTree tree;
  
  vector<RTShape*> shapes;
  RTSphere sphere0(Vector(5,5,0), 1);
  RTSphere sphere1(Vector(5,-5,0), 1);
  RTSphere sphere2(Vector(-5,5,0), 1);
  RTSphere sphere3(Vector(-5,-5,0), 1);
  shapes.push_back(&sphere0);
  shapes.push_back(&sphere1);
  shapes.push_back(&sphere2);
  shapes.push_back(&sphere3);

  BoundingBox box(Vector(-6,-6,-6), Vector(12,12,12));
  tree.setBoundingBox(box);
  tree.setTerminationCondition(1);
  tree.build(shapes, 0);

  Ray ray(Vector(-10, 5, 0 ), Vector(1,0,0));
  IntersectionPtr intersection = tree.intersect(ray);
  
  CHECK( intersection != nullptr );
  CHECK( intersection->getShape() == &sphere2 );

}

int CV_KDTreeTest_CPP::checkGetPoins( const Mat& data )
{
    Mat res1( data.size(), data.type() ),
        res2( data.size(), data.type() ),
        res3( data.size(), data.type() );
    Mat idxs( 1, data.rows, CV_32SC1 );
    for( int pi = 0; pi < data.rows; pi++ )
    {
        idxs.at<int>(0, pi) = pi;
        // 1st way
        const float* point = tr->getPoint(pi);
        for( int di = 0; di < data.cols; di++ )
            res1.at<float>(pi, di) = point[di];
    }
    // 2nd way
    tr->getPoints( idxs.ptr<int>(0), data.rows, res2 );

    // 3d way
    tr->getPoints( idxs, res3 );

    if( norm( res1, data, NORM_L1) != 0 ||
        norm( res2, data, NORM_L1) != 0 ||
        norm( res3, data, NORM_L1) != 0)
        return CvTS::FAIL_BAD_ACCURACY;
    return CvTS::OK;
}

static void testBoxIntersect() {

	KDTree<Vector3> tree;

	// Make a tree containing a regular grid of points
	for (int x = -5; x <= 5; ++x) {
		for (int y = -5; y <= 5; ++y) {
			for (int z = -5; z <= 5; ++z) {
				tree.insert(Vector3(x, y, z));
			}
		}
	}
	tree.balance();

	AABox box(Vector3(-1.5, -1.5, -1.5), Vector3(1.5, 1.5, 1.5));

	KDTree<Vector3>::BoxIntersectionIterator it = tree.beginBoxIntersection(box);
	const KDTree<Vector3>::BoxIntersectionIterator end = tree.endBoxIntersection();

	int hits = 0;
	while (it != end) { 
		const Vector3& v = *it;

		debugAssert(box.contains(v));

		++hits;
		++it;
	}

	debugAssertM(hits == 3*3*3, "Wrong number of intersections found in testBoxIntersect for KDTree");
}

void mexFunction(int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[]){
	// check number of arguments
	if( nrhs!=2 )
		mexErrMsgTxt("This function requires 2 arguments\n");
	if( !mxIsNumeric(prhs[0]) )
		mexErrMsgTxt("varargin{0} must be a valid kdtree pointer\n");
	if( !mxIsNumeric(prhs[1]) )
		mexErrMsgTxt("varargin{1} must be a query set of points\n");
		
    // retrieve the tree pointer
    KDTree* tree;
    retrieve_tree( prhs[0], tree ); 
    // retrieve the query data
    double* query_data;
    int npoints, ndims;
    retrieve_data( prhs[1], query_data, npoints, ndims );
    // printf("query size: %dx%d\n", npoints, ndims);
    
    // check dimensions
    if( ndims != tree->ndims() ) 
    	mexErrMsgTxt("vararg{1} must be a [Nxk] matrix of N points in k dimensions\n");   
    
    // npoints x 1 indexes in output
    plhs[0] = mxCreateDoubleMatrix(npoints, 1, mxREAL);
    double* indexes = mxGetPr(plhs[0]);
    // cout << "nindexes: " << mxGetM(plhs[0]) << "x" << mxGetN(plhs[0]) << endl;
    
    // execute the query FOR EVERY point storing the index
    vector< double > query(ndims,0);
    for(int i=0; i<npoints; i++)
    	for( int j=0; j<ndims; j++ ){
    		query[j]   = query_data[ i+j*npoints ];
    		indexes[i] = tree->closest_point(query)+1;
    	}
}

int CV_KDTreeTest_CPP::findNeighbors( Mat& points, Mat& neighbors )
{
    const int emax = 20;
    Mat neighbors2( neighbors.size(), CV_32SC1 );
    int j;
    vector<float> min(points.cols, minValue);
    vector<float> max(points.cols, maxValue);
    for( int pi = 0; pi < points.rows; pi++ )
    {
        // 1st way
        tr->findNearest( points.ptr<float>(pi), neighbors.cols, emax, neighbors.ptr<int>(pi) );

        // 2nd way
        vector<int> neighborsIdx2( neighbors2.cols, 0 );
        tr->findNearest( points.ptr<float>(pi), neighbors2.cols, emax, &neighborsIdx2 );
        vector<int>::const_iterator it2 = neighborsIdx2.begin();
        for( j = 0; it2 != neighborsIdx2.end(); ++it2, j++ )
            neighbors2.at<int>(pi,j) = *it2;
    }

    // compare results
    if( norm( neighbors, neighbors2, NORM_L1 ) != 0 )
        return CvTS::FAIL_BAD_ACCURACY;

    return CvTS::OK;
}

//=================================================================================================
//  TreeTest::Setup
//=================================================================================================
void TreeTest::SetUp(void)
{
  // Create all objects for test
  partdata = new SphParticle<3>[Npart];
  randnumb = new XorshiftRand(rseed);
  kdtree = new KDTree<3,SphParticle,KDTreeCell>(Nleafmax, thetamaxsqd, kernrange,
                                                macerror, gravity_mac, multipole);

  // Create some simple particle configuration (random population of a cube)
  for (int i=0; i<Npart; i++) {
    for (int k=0; k<3; k++) partdata[i].r[k] = 1.0 - 2.0*randnumb->floatrand();
    partdata[i].m = 1.0 / (FLOAT) Npart;
    partdata[i].h = 0.2*randnumb->floatrand();
  }
  for (int k=0; k<3; k++) {
    simbox.boxmin[k] = -1.0;
    simbox.boxmax[k] = 1.0;
    simbox.boxsize[k] = 2.0;
    simbox.boxhalf[k] = 1.0;
  }

  // Now build the tree using the particle configuration
  kdtree->Ntot       = Npart;
  kdtree->Ntotmaxold = 0;
  kdtree->Ntotmax    = Npart;
  kdtree->ifirst     = 0;
  kdtree->ilast      = Npart - 1;
  kdtree->BuildTree(0, Npart-1, Npart, Npart, partdata, 0.0);
  kdtree->StockTree(kdtree->celldata[0], partdata);

  return;
}

MosaicCanvas * mapTiles(SourceImage const & theSource, vector<TileImage> const & theTiles)
{
    /**
     * @todo Implement this function!
     */

     MosaicCanvas * mosaic_ptr = new MosaicCanvas(theSource.getRows(), theSource.getColumns()); //declare new pointer

     vector<Point<3>> tree_vector;//get the vector to become the KDTree

     map<Point<3>, TileImage> tiles;//declare a new map


	for(auto& iterator : theTiles) //make an autofor loop to go though the theTiles Vector
	{

		//create a temporary RBBAPixel
		RGBAPixel temp = iterator.getAverageColor(); //used built in function get average color

		Point<3> holder; //declare a Point<3> holder
		holder[0] = temp.red; //0th element is red  
		holder[1] = temp.green; //1th element is green
		holder[2] = temp.blue; //2th element is blue

		tree_vector.push_back(holder);//then push the holder onto the tree vector thats going to become the KDTree!
		tiles[holder] = iterator;//set tiles at the key equal to the iterator
		//key is average color so you set that equal to the point so you can find what the tile is
	}


		KDTree<3> myTree = KDTree<3>(tree_vector);//construct a KDTree using tree_vector


		for (int i = 0; i < theSource.getRows(); i++) //go through every row 
		{
			for (int j = 0; j < theSource.getColumns(); j++) //go through every column
			{
				
				RGBAPixel temp2 = theSource.getRegionColor(i,j); //used built in function to get redion color

				Point<3> holder2; //declare a Point<3> holder2
				holder2[0] = temp2.red; //0th element is red  
				holder2[1] = temp2.green; //1th element is green
				holder2[2] = temp2.blue; //2th element is blue

				Point<3> holder3 = myTree.findNearestNeighbor(holder2);//used build in get nearest neightbor function. use holder 2 which we just populated above

				mosaic_ptr->setTile(i,j,tiles.at(holder3));//used build in setTile function ..third parameter uses holder3


			}
		}


		return mosaic_ptr;//return pointer the mosaic

	
}

KDTree::Node::Node(const vector<int>& _triangleIndexes, int _depth, const KDTree& tree)
 :  depth(_depth),
    triangleIndexes(_triangleIndexes),
    boundingBox(tree.getMesh().getTriangles(), triangleIndexes, tree.getMesh().getVertices()),
    plan(),
    left_node(NULL),
    right_node(NULL)
{
}

bool KDTree::Node::intersectRay(const Ray& ray, const KDTree& tree, float& intersectionDistance, Vec3Df& intersectionPoint, Triangle& intersectionTriangle) const
{
    // Check if ray intersect bouding box
    if(!ray.intersect(boundingBox, intersectionPoint)){
        return false;
    }
    
    // If there are no sons, check with node triangles
    if(left_node == NULL && right_node == NULL){
        
        bool intersection = false;
        
        const std::vector<Triangle>& triangles = tree.getMesh().getTriangles();
        const std::vector<Vertex>& vertices = tree.getMesh().getVertices();
    
        intersectionDistance = std::numeric_limits<float>::max();
        
        float triangleIntersectionDistance;
        Vec3Df triangleIntersectionPoint;
        
        for (int index : triangleIndexes) {

            if (ray.intersect(triangles[index], vertices, triangleIntersectionDistance, triangleIntersectionPoint)
                && triangleIntersectionDistance < intersectionDistance) {
                intersectionDistance = triangleIntersectionDistance;
                intersectionPoint = triangleIntersectionPoint;
                intersectionTriangle = triangles[index];
                intersection = true;
            }
        }
        
        return intersection;

    }
    
    // Otherwise, check with the 2 sons
    float intersectionDistanceRight;
    Vec3Df intersectionPointRight;
    Triangle intersectionTriangleRight;

    bool intersectionLeft = left_node && left_node->intersectRay(ray, tree, intersectionDistance, intersectionPoint, intersectionTriangle);
    bool intersectionRight = right_node && right_node->intersectRay(ray, tree, intersectionDistanceRight, intersectionPointRight, intersectionTriangleRight);
    
    if ((intersectionRight && !intersectionLeft)
        || (intersectionRight && intersectionLeft && intersectionDistanceRight < intersectionDistance)){
        
        intersectionDistance = intersectionDistanceRight;
        intersectionPoint = intersectionPointRight;
        intersectionTriangle = intersectionTriangleRight;
        
    }
    
    return intersectionLeft || intersectionRight;
}

int main(int argc, char** argv) {
        assert(argc==4);
        unsigned n_Locations    =       atoi(argv[1]);  //      Number of points.
        unsigned n_Dimension    =       atoi(argv[2]);  //      Dimension.
        unsigned n_Properties   =       atoi(argv[3]);  //      Number of properties.

        double* locations       =       new double [n_Locations*n_Dimension];   //      Stores all the locations.
        double* properties      =       new double [n_Locations*n_Properties];  //      Stores all the properties.
        
        //      Generates random locations and random values of property.
        unsigned count_Location =       0;
        unsigned count_Property =       0;
        for (unsigned j=0; j<n_Locations; ++j) {
                for (unsigned k=0; k<n_Dimension; ++k) {
                        locations[count_Location]       =       double(rand())/double(RAND_MAX);
                        ++count_Location;
                }
                for (unsigned k=0; k<n_Properties; ++k) {
                        properties[count_Property]      =       double(rand())/double(RAND_MAX);
                        ++count_Property;
                }
        }

        std::cout << std::scientific;

        //      Display the initial contents.
//        display("Initial contents: ", n_Locations, n_Dimension, locations, n_Properties, properties);

        //      Creates a KDTree given the locations.
        KDTree* A                       =       new KDTree(n_Locations, n_Dimension, locations, n_Properties, properties);
        delete locations;
        delete properties;

        double* sorted_Locations        =       new double[n_Locations*n_Dimension];
        double* sorted_Properties       =       new double[n_Locations*n_Properties];
        
        clock_t start, end;
        //      Sorts the locations based on KDTree.
        start   =       clock();
        A->sort_KDTree();
        end     =       clock();
        std::cout << "Time taken is: " << double(end-start)/double(CLOCKS_PER_SEC) << std::endl;

        //      Obtains the sorted location.
        A->get_Location_Properties(sorted_Locations, sorted_Properties);

        //      Display the sorted contents.
//        display("Sorted contents: ", n_Locations, n_Dimension, sorted_Locations, n_Properties, sorted_Properties);

        delete sorted_Locations;
        delete sorted_Properties;
        delete A;
}

/* A more merciless test of copy behavior.. */
void ModerateCopyTest() try {
#if BasicCopyTestEnabled
  PrintBanner("Moderate Copy Test");

  /* For simplicity, we'll use one-dimensional KDTrees in this step. */
  KDTree<1, size_t> one;
  for (size_t i = 0; i < 10; ++i)
    one[MakePoint(2 * i)] = i; // Load with 0, 2, 4, ..., 18

  {
    /* Create a clone of one and confirm that everything copied correctly. 
     * This uses the copy constructor.
     */
    KDTree<1, size_t> clone = one;

    /* Add odd numbers to the clone. */
    for (size_t i = 0; i < 10; ++i)
      clone[MakePoint(2 * i + 1)] = i;

    /* Confirm that they didn't appear in one. */
    CheckCondition(one.size() == 10, "Adding to clone change original size.");
    for (size_t i = 0; i < 10; ++i)
      CheckCondition(!one.contains(MakePoint(2 * i + 1)), "Modifying copy doesn't modify original.");
  }

  /* Check the integrity of the original out here as well to see that the dtor didn't hose things. */
  CheckCondition(one.size() == 10, "After dtor, original is still good.");
  for (size_t i = 0; i < 10; ++i) {
    CheckCondition(!one.contains(MakePoint(2 * i + 1)), "After dtor, missing elements still missing.");
    CheckCondition(one[MakePoint(2 * i)] == i, "After dtor, original elements are still there.");
  }

  {
    /* Create a clone of one and confirm that everything copied correctly. 
     * This uses the assignment operator.
     */
    KDTree<1, size_t> clone;
    clone = one;

    /* Do awful, awful things to the copy. */
    clone = clone = (clone = clone);
    (clone = one) = clone;
    clone = clone = clone = clone = clone;
  }

  EndTest();
#else
  TestDisabled("ModerateCopyTest");
#endif
} catch (const exception& e) {
  FailTest(e);
}

void Panorama::create_matchs()
{
	KDTree kd;
	Node *nodes = new Node[features.size()];
	kd.KDTree_build(nodes, &(featureData[0]), features.size(), 128);

	int *numMatchOf_i = new int[images.size()]();

#define _K 4
	int max_match = min(_K, (int)images.size()-1);
	for (int i = 0; i < features.size(); ++i) {
		int *target = &featureData[128*i];
		int indice[_K];
		kd.find_kNN(indice, target, max_match, i);
		for (int j = 0; j < max_match; ++j) {
			int iMatch = indice[j];
			Feature f1 = features[i];
			Feature f2 = features[iMatch];
			if (f1.iid != f2.iid) {
				MatchPair match = {f1, f2};
				matchs.push_back(match);
				++(numMatchOf_i[f1.iid]);
			}
		}
#undef _K
	}

	vector<MatchPair>::iterator op = matchs.begin();
	vector<MatchPair>::iterator ed = matchs.begin();
	for (int i = 0; i < imageNames.size(); ++i) {
		op = ed;
		ed += numMatchOf_i[i];
		sort(op, ed, compare_match);
	}
	create_match_info();

	FILE *fp = fopen("matchs_0.txt", "w");
	print_matchs(fp, matchs);
	fclose(fp);

	fp = fopen("imatchs_0.txt", "w");
	print_imatchs(fp, imageMatchInfos);
	fclose(fp);
	delete[] numMatchOf_i;

	// featureData is not needed anymore, it is very large, so free its memory
	// Note: clear() will not free the memory
	vector<int> emptyVec(0);
	emptyVec.swap(featureData);
	delete[] nodes;
}

void testRay()
{

    KDTree *tr = new KDTree(-5, 5, -3, 3, -2, 0,    0,0, true);
    Vec3 ori = Vec3(1,1,1);
    Vec3 dir = Vec3(0,2,-1);
    dir.normalizeLoc();
    Vec3 out;
    bool fin;
    fin = tr->findEnterPoint(ori, dir, out);
    printf("en %f %f %f %d\n", out.x, out.y, out.z, (fin)?1:0 );
    fin = tr->findExitPoint(ori, dir, out);
    printf("ex %f %f %f %d\n", out.x, out.y, out.z, (fin)?1:0);
}

static void testSerialize() {
    KDTree<Vector3> tree;
    int N = 1000;

    for (int i = 0; i < N; ++i) {
        tree.insert(Vector3::random());
    }
    tree.balance();

    // Save the struture
    BinaryOutput b("test-bsp.dat", G3D_LITTLE_ENDIAN);
    tree.serializeStructure(b);
    b.commit();

}