Kd Tree Iterative implementation ( C++ )

Hello Does anybody has iterative implementation of Kd-Tree in C++. I tried but it is failing when the number of nodes are odd. Here is my code so far. I am referring http://ldots.org/kdtree/#buildingAkDTree site for details.

#include <stdio.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <stack>
#include <queue>
#include <iomanip>


struct Point {
    double pt[2];
    int id;
};

typedef std::vector<Point> TPointVector;

struct KdNode {
    double point[2];
    int id;
    double desc;
    bool leaf;

    KdNode *left;
    KdNode *right;
    KdNode *parent;
    KdNode(KdNode *parent_):parent(parent_),leaf(false){}
    KdNode(KdNode *parent_,TPointVector::iterator itr, int depth, TPointVector &pv);
    KdNode(KdNode *t, TPointVector &pv);

};

KdNode::KdNode(KdNode *parent_,TPointVector::iterator itr, int depth, TPointVector &pv) {
    parent = parent_ ;
    left   = 0;
    right  = 0;
    desc   = itr->pt[depth % 2 ];
    leaf   = false;
}

KdNode::KdNode(KdNode *t, TPointVector &pv) {
    id       = pv[0].id;
    point[0] = pv[0].pt[0];
    point[1] = pv[0].pt[1];
    left     = 0;
    right    = 0;
    parent   = t;
    leaf     = true;
}

KdNode *pRoot = 0;


struct ComparePoints {
    int cord;
    ComparePoints(int  cord_) : cord(cord_ % 2) { };
    bool operator()(const Point& lhs, const Point& rhs) const {
        return lhs.pt[cord] < rhs.pt[cord];
    }
};
void buildLeftTree(std::stack<TPointVector > &stackL) {
    KdNode *pCurrent = pRoot;
    KdNode **pNode  = &(pCurrent->left);
    int depth      = 0; 
    bool changeDirection = false;
    while (! stackL.empty()) {
        TPointVector pv = stackL.top(); 
        stackL.pop();
        if ( pv.size() != 1 ) { 
            std::sort(pv.begin(), pv.end(), ComparePoints(++depth));

            *pNode = new KdNode(pCurrent, pv.begin() + pv.size()/2, depth, pv);

            TPointVector lvp,rvp;
            std::size_t median = pv.size() / 2;
            std::copy(pv.begin(), pv.begin() + median, std::back_inserter(lvp));
            std::copy(pv.begin() + median, pv.end(), std::back_inserter(rvp));

            stackL.push(rvp); 
            stackL.push(lvp);

            if ( changeDirection ) {
                pCurrent = pCurrent->right;
                changeDirection = false;
            } else {
                pCurrent = pCurrent->left;
            }           
            pNode = &(pCurrent->left);

   开发者_运维问答     } else {
            KdNode **pNodeLeft   = &(pCurrent->left);
            *pNodeLeft  = new KdNode(pCurrent, pv);
            pv = stackL.top();
            stackL.pop();

            KdNode **pNodeRight   = &(pCurrent->right);
            *pNodeRight  = new KdNode(pCurrent,pv);

            pCurrent = pCurrent->parent;
            pNode  = &(pCurrent->right);
            changeDirection = true;
            depth--;
        }           
    }
}

void buildRightTree(std::stack<TPointVector > &stackR) {
    KdNode *pCurrent = pRoot;
    KdNode **pNode  = &(pCurrent->right);
    int depth      = 0; 
    bool changeDirection = true;
    while (! stackR.empty()) {
        TPointVector pv = stackR.top(); 
        stackR.pop();

        if ( pv.size() != 1 ) { 
            std::sort(pv.begin(), pv.end(), ComparePoints(++depth));
            *pNode = new KdNode(pCurrent, pv.begin() + pv.size()/2, depth, pv);

            TPointVector lvp,rvp;
            std::size_t median = pv.size() / 2;
            std::copy(pv.begin(), pv.begin() + median, std::back_inserter(lvp));
            std::copy(pv.begin() + median, pv.end(), std::back_inserter(rvp));

            stackR.push(rvp); 
            stackR.push(lvp);       

            if ( changeDirection ) {
                pCurrent = pCurrent->right;
                changeDirection = false;
            } else {
                pCurrent = pCurrent->left;
            }       
            pNode = &(pCurrent->left);

        } else {
            KdNode **pNodeLeft   = &(pCurrent->left);
            *pNodeLeft  = new KdNode(pCurrent, pv);
            pv = stackR.top();
            stackR.pop();

            KdNode **pNodeRight   = &(pCurrent->right);
            *pNodeRight  = new KdNode(pCurrent,pv);

            pCurrent = pCurrent->parent;
            pNode  = &(pCurrent->right);
            depth--;
            changeDirection = true;
        }           
    }
}


void constructKD(TPointVector &pv) {
    int depth = 0;
    std::sort(pv.begin(), pv.end(), ComparePoints(depth));

    pRoot        = new KdNode(0);
    pRoot->desc  = ( pv.begin() + pv.size()/2)->pt[0];
    pRoot->left  = 0;
    pRoot->right = 0;

    TPointVector lvp, rvp;
    std::copy(pv.begin(), pv.begin() + pv.size()/2, std::back_inserter(lvp));
    std::copy(pv.begin() + pv.size()/2, pv.end(), std::back_inserter(rvp));

    std::stack<TPointVector > stackL, stackR;
    stackL.push(lvp);
    stackR.push(rvp);

    buildLeftTree(stackL);
    buildRightTree(stackR);

}
void readPoints(const char* fileName, TPointVector& points) {
    std::ifstream input(fileName);

    if ( input.peek() != EOF ) {
        while(!input.eof()) {
            int id = 0;
            double x_cord, y_cord;
            input >> id >> x_cord >> y_cord;

            Point t ;
            t.pt[0] = x_cord;
            t.pt[1] = y_cord;
            t.id    = id;

            points.push_back(t);
        }
        input.close();
    }   
}
void _printLevelWise(KdNode *node, std::queue<KdNode *> Q) {
    int depth = 0;
    while ( ! Q.empty()) {
        KdNode *qNode = Q.front();Q.pop();
        if ( qNode->leaf ) {
            std::cout << "[" << qNode->id << "]" << std::setprecision (25) << "(" << qNode->point[0] << "," << qNode->point[1] << ")" << std::endl;
        } else {
            std::cout << std::setprecision (25) << qNode->desc << std::endl;
        }       
        if (qNode->left != 0)
            Q.push(qNode->left);
        if (qNode->right != 0)
            Q.push(qNode->right);
    }
}
void PrintLevelWise(KdNode *node) {
    std::queue<KdNode *> Q;
    Q.push(node);
    _printLevelWise(node, Q);
}
int main ( int argc, char **argv ) {
    if ( argc <= 1 ) {
        return 0;
    }
    TPointVector points;
    readPoints(argv[1], points);
    for ( TPointVector::iterator itr = points.begin(); itr != points.end(); ++itr) {
        std::cout << "(" << itr->pt[0] << "," << itr->pt[1] << ")" << std::endl;
    }
    if ( points.size() == 0 )
        return 0;
    constructKD(points);
    PrintLevelWise(pRoot);
    std::cout << "Construction of KD Tree Done " << std::endl;
}

Sample Input for which this is failing :

1 6 1 
2 5 5 
3 9 6 
4 3 6 
5 4 9

Sample Input for which this is working :

1 6 1 
2 5 5 
3 9 6 
4 3 6 
5 4 9 
6 4 0 
7 7 9 
8 2 9

Your else in buildLeftTree and buildRightTree does not handle the case where there is an odd number of nodes on the right subtree. In your 5 point example, the else case in buildRightTree ends up with three points on stackR, the first it uses for the left node, the second two it silently assigns to the right node as if it were the only node.

This is due to your median selection, which uses a different criteria than the one listed on the site you reference.

std::size_t median = pv.size() / 2; // degenerates in cases where size() is odd

Your selection criteria is supposed to be based on the median x or y value and use sublists based on that criteria (which does not assume any given size).

In else of buildLeftTree and buildRightTree.

add

if (pv.size() > 1)
{
    pNode = &(pCurrent->right);
    continue;
}

between stack's top and pop function. It'll be OK.

I have another question, when the number of points is million, it'll be very slow to build the tree, how to improve the performance?