// 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;
}
