// 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 ); }
TEST(KDTree, shouldSearchInTree) { KDTree tree; vector<RTShape*> shapes; RTSphere sphere0(Vector(2.5, 2.5, 2.5), 2.4); RTSphere sphere1(Vector(2.5, 2.5, 7.5), 2.4); RTSphere sphere2(Vector(2.5, 7.5, 7.5), 2.4); RTSphere sphere3(Vector(2.5, 7.5, 2.5), 2.4); RTSphere sphere4(Vector(7.5, 2.5, 2.5), 2.4); RTSphere sphere5(Vector(7.5, 2.5, 7.5), 2.4); RTSphere sphere6(Vector(7.5, 7.5, 7.5), 2.4); RTSphere sphere7(Vector(7.5, 7.5, 2.5), 2.4); shapes.push_back(&sphere0); shapes.push_back(&sphere1); shapes.push_back(&sphere2); shapes.push_back(&sphere3); shapes.push_back(&sphere4); shapes.push_back(&sphere5); shapes.push_back(&sphere6); shapes.push_back(&sphere7); BoundingBox box(Vector(0,0,0), Vector(10,10,10)); tree.setBoundingBox(box); tree.setTerminationCondition(1); tree.build(shapes, 0); Ray ray(Vector(7.5, 7.5, -2 ), Vector(0,0,1)); IntersectionPtr intersection = tree.intersect(ray); CHECK( intersection != nullptr ); CHECK( intersection->getShape() == &sphere7 ); }
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); }
TEST(KDTree, itSplitsXthenYthenZ) { 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); CHECK_EQUAL( 1, tree.getLeft()->getLeft()->size() ); CHECK_EQUAL( 1, tree.getLeft()->getRight()->size() ); CHECK_EQUAL( 1, tree.getRight()->getLeft()->size() ); CHECK_EQUAL( 1, tree.getRight()->getRight()->size() ); }
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(); }
void buildTestScene1() { float w,h,f; w = 9.0f; h = 6.0f; f = 6.0f; cam = PinholeCamera(Vec3(0.0f,0.0f,0.0f), Vec3(0.0f,0.0f,-5.0f), Vec3(0.0f,1.0f,0.0f),f,w,h); buffer = RayBuffer(); PointLight *l; l = new PointLight(Vec3(1.0f,0.0f,1.0f),Col(1.0f,1.0f,1.0f), 15, 1, 1, 1); lights.addLight(*l); l = new PointLight(Vec3(-3.0f,3.0f,-1.0f),Col(1.0f,1.0f,1.0f), 10, 1, 1, 1); lights.addLight(*l); l = new PointLight(Vec3(2.0f,0.0f,0.0f),Col(0.30f,0.30f,0.30f), 4, 1, 1, 1); lights.addLight(*l); std::vector<Collidable *> *vec = new std::vector<Collidable *>(); Collidable *c; c = new Sphere(Vec3(0.0f, .5f, -4.20f), 1.0f, Material(1)); c->precomputeBounds(); vec->push_back(c); c = new Sphere(Vec3(-0.0f, -1.0f, -3.0f), 0.5f, Material(2)); c->precomputeBounds(); vec->push_back(c); c = new Sphere(Vec3(-0.0f, 0.00f, -0.50f), 0.1f, Material(3)); c->precomputeBounds(); vec->push_back(c); c = new Sphere(Vec3(0.7f, 0.30f, -2.5f), 0.5f, Material(4)); c->precomputeBounds(); //vec->push_back(c); c = new Sphere(Vec3(02.0f, 0.00f, -5.0f), 0.70f, Material(4)); c->precomputeBounds(); vec->push_back(c); c = new Sphere(Vec3(0.9f, 0.00f, -02.50f), 0.50f, Material(7)); c->precomputeBounds(); vec->push_back(c); sceneTree.buildTreeStart(*vec); sceneTree.isLeaf = true; sceneTree.items = vec; sceneTree.xMin = -100; sceneTree.xMax = 100; sceneTree.yMin = -100; sceneTree.yMax = 100; sceneTree.zMin = -100; sceneTree.zMax = 100; sceneTree.axis = -1; sceneTree.depth = 0; sceneTree.id = 55; }
void testKDTree2() { KDTree<Point<int>, int, 2> kdtree; int N = 1500000; for(int i = 0; i < N; ++i) { kdtree.insert(Point<int>(GlobalRNG.next(), GlobalRNG.next()), i); } int M = 1500; for(int i = 0; i < M; ++i) { assert(kdtree.nearestNeighbor(Point<int>(GlobalRNG.next(), GlobalRNG.next()), EuclideanDistance<Point<int> >::DistanceIncremental())); int k = 2; assert(kdtree.kNN(Point<int>(GlobalRNG.next(), GlobalRNG.next()), k, EuclideanDistance<Point<int> >::DistanceIncremental()).getSize() == k); } }
TEST(KDTree, mixingTrianglesAndSphereIsNotAProblem) { KDTree tree; vector<RTShape*> shapes; RTTriangle triangle(Vector(0.1,0.1,0.1), Vector(1,1,1), Vector(0.1,2,0.1)); RTSphere sphere(Vector(-1,-1,-1), 1); shapes.push_back(&triangle); shapes.push_back(&sphere); BoundingBox box(Vector(-2,-2,-2), Vector(2,4,2)); tree.setBoundingBox(box); tree.build(shapes, 0); CHECK_EQUAL( 1, tree.getLeft()->size() ); CHECK_EQUAL( 1, tree.getRight()->size() ); }
TEST(KDTree, shouldSupportTriangles) { KDTree tree; vector<RTShape*> shapes; RTTriangle triangle0(Vector(0.1,0.1,0.1), Vector(1,1,1), Vector(0.1,2,0.1)); RTTriangle triangle1(Vector(-0.1,-0.1,-0.1), Vector(-1,-1,-1), Vector(-0.1,-2,-0.1)); shapes.push_back(&triangle0); shapes.push_back(&triangle1); BoundingBox box(Vector(-1,-2,-1), Vector(2,4,2)); tree.setBoundingBox(box); tree.build(shapes, 0); CHECK_EQUAL( 1, tree.getLeft()->size() ); CHECK_EQUAL( 1, tree.getRight()->size() ); }
int CV_KDTreeTest_CPP::checkFindBoxed() { vector<float> min( dims, minValue), max(dims, maxValue); vector<int> indices; tr->findOrthoRange( &min[0], &max[0], &indices ); // TODO check indices if( (int)indices.size() != featuresCount) return CvTS::FAIL_BAD_ACCURACY; return CvTS::OK; }
/* Basic test: Can we build a small tree and look up the elements it contains? */ void BasicKDTreeTest() try { #if BasicKDTreeTestEnabled PrintBanner("Basic KDTree Test"); /* Construct the KDTree. */ KDTree<3, size_t> kd; CheckCondition(true, "KDTree construction completed."); /* Check basic properties of the KDTree. */ CheckCondition(kd.dimension() == 3, "Dimension is three."); CheckCondition(kd.size() == 0, "New KD tree has no elements."); CheckCondition(kd.empty(), "New KD tree is empty."); /* Add some elements. */ const double dataPoints[3][3] = { {1, 0, 0}, {0, 1, 0}, {0, 0, 1} }; for (size_t i = 0; i < 3; ++i) kd.insert(PointFromRange<3>(dataPoints[i], dataPoints[i] + 3), i); /* Check basic properties again. */ CheckCondition(kd.size() == 3, "After adding three elements, KDTree has size 3."); CheckCondition(!kd.empty(), "After adding three elements, KDTree is not empty."); /* Make sure that the elements we built the tree out of are still there. */ CheckCondition(kd.contains(PointFromRange<3>(dataPoints[0], dataPoints[0] + 3)), "New KD tree has element zero."); CheckCondition(kd.contains(PointFromRange<3>(dataPoints[1], dataPoints[1] + 3)), "New KD tree has element one."); CheckCondition(kd.contains(PointFromRange<3>(dataPoints[2], dataPoints[2] + 3)), "New KD tree has element two."); /* Make sure that the values of these points are correct. */ for (size_t i = 0; i < 3; ++i) CheckCondition(kd.at(PointFromRange<3>(dataPoints[i], dataPoints[i] + 3)) == i, "New KD tree has correct values."); EndTest(); #else TestDisabled("BasicKDTreeTest"); #endif } catch (const exception& e) { FailTest(e); }
void testKDTree() { KDTree<Point<int>, int, 2> kdtree; int N = 1500000; for(int i = 0; i < N; ++i) { int p1 = (GlobalRNG.next() % 1000), p2 = (GlobalRNG.next() % 1000); kdtree.insert(Point<int>(min(p1, p2), max(p1, p2)), i); } bool dimensions[2]; dimensions[0] = true; dimensions[1] = true; for(int k = 0; k < 1; ++k) { Vector<KDTree<Point<int>, int, 2>::NodeType*> result; int point = 0; kdtree.rangeQuery(Point<int>(-999999999, point), Point<int>(point, 999999999), dimensions, result); DEBUG(result.getSize()); } }
TEST(KDTree, shouldSupportIntersectionSearchForChildNode) { KDTree tree; vector<RTShape*> shapes; RTTriangle t1(Vector(0,0,0), Vector(0,1,0), Vector(0,0,1)); RTTriangle t2(Vector(-1,0,0), Vector(-1,1,0), Vector(-1,0,1)); shapes.push_back(&t1); shapes.push_back(&t2); BoundingBox box(Vector(0,0,0), Vector(1,0,1)); tree.setBoundingBox(box); tree.setTerminationCondition(2); tree.build(shapes, 0); Ray ray(Vector(-5,0.1,0.1), Vector(1,0,0)); IntersectionPtr intersection = tree.intersect(ray); CHECK( intersection->getShape() == &t2 ); }
void NearestNeighborGraph(const vector<Vector3>& pc,int k,Graph::Graph<int,int>& G) { vector<Vector> copy(pc.size()); for(size_t i=0;i<copy.size();i++) { copy[i].resize(3); pc[i].get(copy[i]); } G.Resize(pc.size()); for(size_t i=0;i<pc.size();i++) G.nodes[i] = (int)i; KDTree* tree = KDTree::Create(copy,3,pc.size()); vector<Real> dist(k); vector<int> inds(k); for(size_t i=0;i<pc.size();i++) { tree->KClosestPoints(copy[i],k,&dist[0],&inds[0]); for(int j=0;j<k;j++) { if(inds[j] == (int)i) continue; G.AddEdge((int)i,inds[j],0); } } }
/* This test checks that the at() operators correctly throw * exceptions when elements are not found. This is not an * exhaustive test, and you should be sure to confirm on your * own that everything works correctly. */ void ThrowingKDTreeTest() try { #if MutatingKDTreeTestEnabled PrintBanner("Throwing KDTree Test"); { /* Create a non-const, empty KDTree and look things up. */ KDTree<3, size_t> empty; bool didThrow = false; try { empty.at(MakePoint(0, 0, 0)); } catch (const out_of_range&) { didThrow = true; } CheckCondition(didThrow, "Exception generated during non-const lookup."); } { /* Create a const, empty KDTree and look things up. */ KDTree<3, size_t> empty; bool didThrow = false; try { empty.at(MakePoint(0, 0, 0)); } catch (const out_of_range&) { didThrow = true; } CheckCondition(didThrow, "Exception generated during const lookup."); } EndTest(); #else TestDisabled("ThrowingKDTreeTest"); #endif } catch (const exception& e) { FailTest(e); }
int main(int argc, const char *argv[]) { typedef std::vector<int> Point; int dimensions = 2; Point p1 = {10, 3}; Point p2 = {3, 8}; Point p3 = {8, 3}; Point p4 = {1, 4}; Point p5 = {5, 1}; Node<Point> n1(p1); Node<Point> n2(p2); Node<Point> n3(p3); Node<Point> n4(p4); Node<Point> n5(p5); Node<Point> nodes[] = {n1, n2, n3, n4, n5}; std::cout << "Source:" << std::endl; for (int i = 0; i < sizeof(nodes) / sizeof(*nodes); ++i) { std::cout << "("; for (int j = 0; j < dimensions; ++j) { std::cout << nodes[i].value[j] << ","; } std::cout << "), "; } std::cout << std::endl; std::cout << "Tree:" << std::endl; KDTree<Point> tree; tree.make_tree(nodes, 5, 2); tree.visualise(); return 0; }
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 vector<double> query_data(tree->ndims(), 0); retrieve_point( prhs[1], query_data ); // printf("query size: %dx%d\n", npoints, ndims); // npoints x 1 indexes in output plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL); double* kk = mxGetPr(plhs[0]); // Compute kernel density estimate: kk[0] = tree->kde(query_data); }
std::vector<const Edge*> connection(const std::vector<Edge*> &edges, const KDTree &tree) { std::vector<const Edge*> solution; double best = std::numeric_limits<double>::infinity(); for(const auto edge : edges) { auto res = tree.kNearestWithin(edge, linkRadius); for(const auto e : res.elements) { Edge newEdge = agent.steer(edge->end, e->end, std::numeric_limits<double>::infinity()); if(workspace.safeEdge(agent, newEdge, collisionCheckDT)) { double newSolutionCost = edge->gCost() + e->gCost(); if(newSolutionCost >= best) { continue; } best = newSolutionCost; std::vector<Edge *> newSolution; newSolution.push_back(edge); unsigned int edgeCount = 1; while(newSolution.back()->parent != NULL) { edgeCount++; newSolution.push_back(newSolution.back()->parent); } std::reverse(newSolution.begin(), newSolution.end()); newEdge.updateParent(edge); auto curEdge = e; newSolution.push_back(curEdge); while(curEdge->parent != NULL) { auto prevEdge = newSolution.back(); curEdge = curEdge->parent; auto saved = new Edge(*curEdge); saved->updateParent(prevEdge); newSolution.push_back(saved); } solution.insert(solution.begin(), newSolution.begin(), newSolution.end()); } } } return solution; }