Kd树迭代实现(C++）

Hello有人在C++中迭代实现Kd树吗。我尝试过，但当节点数为奇数时，它失败了。这是我迄今为止的代码。我指的是http://ldots.org/kdtree/#buildingAkDTree网站了解详细信息。

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

失败的样本输入：

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

此操作的示例输入：

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