KDNode.h

Go to the documentation of this file.

/* Copyright (C) 2004
   K-Dimension Tree Node Data Structure Implementation

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.

   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef _KDNODE_H_
#define _KDNODE_H_

#include "Object.h"
#include <stdio.h>
#include <iostream>
#include <math.h>
#include "KDPoint.h"
#include "KDPQueue.h"

namespace RobotFlow {

#define KDN_LOW 0
#define KDN_HIGH 1

// Template class for node values
// IMPORTANT: class requires =, -, >, < operators
//            it should usually be either double, float, int, ...
template <class KDData>
class KDNode : public FD::Object
{
public:
        /*
                KDNode

                Default constructor
        */
        KDNode()
        : m_nPts(0), m_dimSize(0), m_bucket(NULL), m_bucketIdx(NULL)
        {
                m_children[KDN_LOW] = NULL;
                m_children[KDN_HIGH] = NULL;
        }

        /*
                KDNode

                Internal Tree Node Constructor
        */
        KDNode(int i_cutDim, KDData i_cutVal, KDData i_lowVal,
                KDData i_highVal, KDNode *i_lowChild, KDNode *i_highChild)
        : m_cutDim(i_cutDim), m_cutVal(i_cutVal), m_nPts(0), m_dimSize(0),
        m_bucket(NULL), m_bucketIdx(NULL)
        {
                m_children[KDN_LOW] = i_lowChild;
                m_children[KDN_HIGH] = i_highChild;
                m_cutBnds[KDN_LOW] = i_lowVal;
                m_cutBnds[KDN_HIGH] = i_highVal;
        }

        /*
                KDNode

                Leaf Node Constructor
        */
        KDNode(KDPoint<KDData> *i_dataPts, int *i_ptsIdx, int i_numPts, int i_dimSize)
        : m_nPts(i_numPts), m_dimSize(i_dimSize)
        {
                m_children[KDN_LOW] = NULL;
                m_children[KDN_HIGH] = NULL;

                if (m_nPts == 0) {
                        m_bucket = NULL;
                        m_bucketIdx = NULL;
                        return;
                }

                m_bucket = new KDPoint<KDData>[m_nPts];
                m_bucketIdx = new int[m_nPts];

                for (int i=0; i<m_nPts; ++i) {
                        m_bucket[i] = i_dataPts[i_ptsIdx[i]];
                        m_bucketIdx[i] = i_ptsIdx[i];
                }
        }

        /*
                KDNode

                Other Leaf Node Constructor
        */
        KDNode(KDData **i_dataPts, int *i_ptsIdx, int i_numPts, int i_dimSize)
        : m_nPts(i_numPts), m_dimSize(i_dimSize)
        {
                m_children[KDN_LOW] = NULL;
                m_children[KDN_HIGH] = NULL;

                if (m_nPts == 0) {
                        m_bucket = NULL;
                        m_bucketIdx = NULL;
                        return;
                }

                m_bucket = new KDPoint<KDData>[m_nPts](m_dimSize);
                m_bucketIdx = new int[m_nPts];

                for (int i=0; i<m_nPts; ++i) {
                        m_bucket[i].SetPoint(i_dataPts[i_ptsIdx[i]]);
                        m_bucketIdx[i] = i_ptsIdx[i];
                }
        }

        /*
                ~KDNode

                Destructor
        */
        ~KDNode()
        {
                delete [] m_bucket;
                delete [] m_bucketIdx;
                delete m_children[KDN_LOW];
                delete m_children[KDN_HIGH];
        }

        /*
                operator =

                Equal operator

                PARAMETERS
                i_ref : reference to node to copy

                RETURNS
                A copy of the given reference node
        */
        KDNode<KDData> & operator =(const KDNode<KDData> &i_ref)
        {
                // Avoid self assignment
                if (&i_ref == this) {
                        return *this;
                }

                m_cutDim = i_ref.m_cutDim;
                m_cutVal = i_ref.m_cutVal;
                m_cutBnds[0] = i_ref.m_cutBnds[0];
                m_cutBnds[1] = i_ref.m_cutBnds[1];
                m_nPts = i_ref.m_nPts;
                m_dimSize = i_ref.m_dimSize;

                if (m_bucket) {
                        delete [] m_bucket;
                }
                m_bucket = new KDPoint<KDData>[m_nPts];

                if (m_bucketIdx) {
                        delete [] m_bucketIdx;
                }
                m_bucketIdx = new int[m_nPts];

                for (int i=0; i<m_nPts; ++i) {
                        m_bucket[i] = i_ref.m_bucket[i];
                        m_bucketIdx[i] = i_ref.m_bucketIdx[i];
                }

                m_children[0] = i_ref.m_children[0];
                m_children[1] = i_ref.m_children[1];

                return *this;
        }

        /*
                printOn

                Object routine required to print its content

                PARAMETERS
                out : output stream to print the object content
        */
        void printOn(std::ostream &out) const
        {
                out << "<KDNode " << std::endl;
                if (m_children[KDN_LOW] == NULL &&
                        m_children[KDN_HIGH] == NULL &&
                        m_bucket != NULL) {
                        out << "<Type Leaf " << std::endl;
                        out << "<NumPts " << m_nPts << " >" << std::endl;
                        out << "<DimSize " << m_dimSize << " >" << std::endl;

                        out << "<Bucket " << std::endl;
                        for (int i=0; i<m_nPts; ++i) {
                                m_bucket[i].printOn(out);
                        }
                        out << " >" << std::endl;

                        out << "<BucketIdx ";
                        for (int i=0; i<m_nPts; ++i) {
                                out << m_bucketIdx[i] << " ";
                        }
                        out << " >" << std::endl;

                        out << " >" << std::endl;
                        out << " >" << std::endl;
                }
                else {
                        out << "<Type Internal " << std::endl;
                        out << "<CutDim " << m_cutDim << " >" << std::endl;
                        out << "<CutVal " << m_cutVal << " >" << std::endl;
                        out << "<CutBounds " << m_cutBnds[0] << " " << m_cutBnds[1]
                                << " >" << std::endl;

                        if (m_children[KDN_LOW] != NULL) {
                                out << "<LowChild " << std::endl;
                                m_children[KDN_LOW]->printOn(out);
                                out << " >" << std::endl;
                        }

                        if (m_children[KDN_HIGH] != NULL) {
                                out << "<HighChild " << std::endl;
                                m_children[KDN_HIGH]->printOn(out);
                                out << " >" << std::endl;
                        }

                        out << " >" << std::endl;
                        out << " >" << std::endl;
                }

                out << " >" << std::endl;
        }

        /*
                readFrom

                Object routine required to read an Object content

                PARAMETERS
                in : input stream to read the object content
        */
        void readFrom(std::istream &in)
        {
                std::string tag;

                while (1) {
                        char ch;
                        in >> ch;

                        if (ch == '>') {
                                break;
                        }
                        else if (ch != '<') {
                                throw new FD::GeneralException ("KDNode::readFrom : Parse error: '<' expected",__FILE__,__LINE__);
                        }

                        in >> tag;

                        if (tag == "KDNode") {
                                continue;
                        }
                        else if (tag == "Type") {
                                in >> tag;
                                if (tag == "Internal") {
                                        try {
                                                readFromInternal(in);
                                        }
                                        catch (FD::GeneralException *e) {
                                                throw e;
                                        }
                                }
                                else if (tag == "Leaf") {
                                        try {
                                                readFromLeaf(in);
                                        }
                                        catch (FD::GeneralException *e) {
                                                throw e;
                                        }
                                }
                                else {
                                        throw new FD::GeneralException ("KDNode::readFrom : unknown KDNode type",__FILE__,__LINE__);
                                }
                        }
                        else {
                                throw new FD::GeneralException ("KDNode::readFrom : Unknown argument: " + tag,__FILE__,__LINE__);
                        }

                        if (!in) {
                                throw new FD::GeneralException ("KDNode::readFrom : Parse error trying to build " + tag,__FILE__,__LINE__);
                        }

                        in >> tag;
                        if (tag != ">") {
                                throw new FD::GeneralException ("KDNode::readFrom : Parse error: '>' expected ",__FILE__,__LINE__);
                        }
                }
        }

        /*
                NNSearch

                Nearest neighbor recursive search routine. At this level, we only determine
                if we need to make a search in a leaf node (search its bucket) or in an
                internal node (find the next node to search).

                PARAMETERS
                i_qp : query point given to find its nearest neighbors
                i_maxNumNodes : maximum number of leaf nodes to visit before to stop the
                        search. If it is set to 0, than all the tree can be
                        searched.
                io_numVisited : number of leaf nodes visited so far (modified)
                i_maxErr : error bound to determine if we should visit a child node.
                io_bbDist : distance from query point to the tree points bounding box.
                io_nnQueue : priority queue containing the current NN information (modified).
                        In this priority queue, the NN distances are used as keys and the
                        NN point indexes, as elements.

        */
        void NNSearch(KDPoint<KDData> *i_qp, int i_maxNumNodes, int &io_numVisited,
                double i_maxErr, double io_bbDist, KDPQueue<double, int> *io_nnQueue)
        {
                if (m_children[KDN_LOW] == NULL &&
                        m_children[KDN_HIGH] == NULL &&
                        m_bucket != NULL) {
                        NNBucketSearch(i_qp, io_numVisited, io_nnQueue);
                }
                else if (m_children[KDN_LOW] != NULL || m_children[KDN_HIGH] != NULL) {
                        NNNodeSearch(i_qp, i_maxNumNodes, io_numVisited, i_maxErr, io_bbDist,
                                io_nnQueue);
                }
        }

        /*
                NNSearch

                Nearest neighbor search recursive routine. At this level, we only determine
                if we need to make a search in a leaf node (search its bucket) or in an
                internal node (find the next node to search).

                PARAMETERS
                i_qp : query point given to find its nearest neighbors
                i_maxNumNodes : maximum number of leaf nodes to visit before to stop the
                        search. If it is set to 0, than all the tree can be
                        searched.
                io_numVisited : number of leaf nodes visited so far (modified)
                i_maxErr : error bound to determine if we should visit a child node.
                io_bbDist : distance from query point to the tree points bounding box.
                io_nnQueue : priority queue containing the current NN information (modified).
                        In this priority queue, the NN distances are used as keys and the
                        NN points, as elements. (modified)

        */
        void NNSearch(KDPoint<KDData> *i_qp, int i_maxNumNodes, int &io_numVisited,
                double i_maxErr, double io_bbDist, KDPQueue<double, KDPoint<KDData> > *io_nnQueue)
        {
                if (m_children[KDN_LOW] == NULL &&
                        m_children[KDN_HIGH] == NULL &&
                        m_bucket != NULL) {
                        NNBucketSearch(i_qp, io_numVisited, io_nnQueue);
                }
                else if (m_children[KDN_LOW] != NULL || m_children[KDN_HIGH] != NULL) {
                        NNNodeSearch(i_qp, i_maxNumNodes, io_numVisited, i_maxErr, io_bbDist,
                                io_nnQueue);
                }
        }

private:
        /*
                readFromInternal

                Specific routine to read the content of an internal node

                PARAMETERS
                in : input stream to read the object content
        */
        void readFromInternal(std::istream &in)
        {
                std::string tag;

                while (1) {
                        char ch;
                        in >> ch;

                        if (ch == '>') {
                                break;
                        }
                        else if (ch != '<') {
                                throw new FD::GeneralException ("KDNode::readFromInternal : Parse error: '<' expected",__FILE__,__LINE__);
                        }

                        in >> tag;

                        if (tag == "CutDim") {
                                in >> m_cutDim;
                        }
                        else if (tag == "CutVal") {
                                in >> m_cutVal;
                        }
                        else if (tag == "CutBounds") {
                                in >> m_cutBnds[0];
                                in >> m_cutBnds[1];
                        }
                        else if (tag == "LowChild") {
                                try {
                                        // Allocate low child node
                                        m_children[KDN_LOW] = new KDNode<KDData>();
                                        m_children[KDN_LOW]->readFrom(in);
                                }
                                catch (FD::GeneralException *e) {
                                        throw e;
                                }
                        }
                        else if (tag == "HighChild") {
                                try {
                                        // Allocate high child node
                                        m_children[KDN_HIGH] = new KDNode<KDData>();
                                        m_children[KDN_HIGH]->readFrom(in);
                                }
                                catch (FD::GeneralException *e) {
                                        throw e;
                                }
                        }
                        else {
                                throw new FD::GeneralException ("KDNode::readFromInternal : Unknown argument: " + tag,__FILE__,__LINE__);
                        }

                        if (!in) {
                                throw new FD::GeneralException ("KDNode::readFromInternal : Parse error trying to build " + tag,__FILE__,__LINE__);
                        }

                        in >> tag;
                        if (tag != ">") {
                                throw new FD::GeneralException ("KDNode::readFromInternal : Parse error: '>' expected ",__FILE__,__LINE__);
                        }
                }
        }

        /*
                readFromLeaf

                Specific routine to read the content of a leaf node

                PARAMETERS
                in : input stream to read the object content
        */
        void readFromLeaf(std::istream &in)
        {
                std::string tag;

                while (1) {
                        char ch;
                        in >> ch;

                        if (ch == '>') {
                                break;
                        }
                        else if (ch != '<') {
                                throw new FD::GeneralException ("KDNode::readFromLeaf : Parse error: '<' expected",__FILE__,__LINE__);
                        }

                        in >> tag;

                        if (tag == "NumPts") {
                                in >> m_nPts;
                        }
                        else if (tag == "DimSize") {
                                in >> m_dimSize;
                        }
                        else if (tag == "BucketIdx") {
                                // Allocate index bucket
                                m_bucketIdx = new int[m_nPts];

                                for (int i=0; i<m_nPts; ++i) {
                                        in >> m_bucketIdx[i];
                                }
                        }
                        else if (tag == "Bucket") {
                                try {
                                        // Allocate bucket
                                        m_bucket = new KDPoint<KDData>[m_nPts];

                                        for (int i=0; i<m_nPts; ++i) {
                                                m_bucket[i].readFrom(in);
                                        }
                                }
                                catch (FD::GeneralException *e) {
                                        throw e->add(new FD::GeneralException("In KDNode::readFromLeaf : ",__FILE__,__LINE__));
                                }
                        }
                        else {
                                throw new FD::GeneralException ("KDNode::readFromLeaf : Unknown argument: " + tag,__FILE__,__LINE__);
                        }

                        if (!in) {
                                throw new FD::GeneralException ("KDNode::readFromLeaf : Parse error trying to build " + tag,__FILE__,__LINE__);
                        }

                        in >> tag;
                        if (tag != ">") {
                                throw new FD::GeneralException ("KDNode::readFromLeaf : Parse error: '>' expected ",__FILE__,__LINE__);
                        }
                }
        }

        /*
                NNBucketSearch

                Nearest neighbor search routine to search the content of a leaf node bucket.

                PARAMETERS
                i_qp : query point given to find its nearest neighbors
                io_numVisited : number of leaf nodes visited so far (modified)
                io_nnQueue : priority queue containing the current NN information (modified).
                        In this priority queue, the NN distances are used as keys and the
                        NN point indexes, as elements. (modified)

        */
        void NNBucketSearch(KDPoint<KDData> *i_qp, int &io_numVisited,
                KDPQueue<double, int> *io_nnQueue)
        {
                int d;
                double dist;
                double tmp;
                double minDist = io_nnQueue->GetMaxKey();

                //Search through all points in bucket
                for (int i=0; i<m_nPts; ++i) {
                        // Use pointers on coordinates of bucket and query points
                        KDData *bktDataPtr = m_bucket[i].GetPoint();
                        KDData *queryPtPtr = i_qp->GetPoint();

                        dist = 0.0;

                        for (d=0; d<m_dimSize; ++d) {
                                tmp = (double)(*(bktDataPtr++)) - (double)(*(queryPtPtr++));
                                dist += tmp*tmp;

                                // Check if current distance exceeds min distance so far
                                if (dist >= minDist) {
                                        break;
                                }
                        }

                        // Check if current point is the current nearest neighbor
                        if (d >= m_dimSize && dist < minDist) {
                                io_nnQueue->Insert(dist, &(m_bucketIdx[i]));
                                minDist = io_nnQueue->GetMaxKey();
                        }
                }

                // Increment number of points visited
                io_numVisited += m_nPts;
        }

        /*
                NNBucketSearch

                Nearest neighbor search routine to search the content of a leaf node bucket.

                PARAMETERS
                i_qp : query point given to find its nearest neighbors
                io_numVisited : number of leaf nodes visited so far (modified)
                io_nnQueue : priority queue containing the current NN information (modified).
                        In this priority queue, the NN distances are used as keys and the
                        NN points, as elements. (modified)

        */
        void NNBucketSearch(KDPoint<KDData> *i_qp, int &io_numVisited,
                KDPQueue<double, KDPoint<KDData> > *io_nnQueue)
        {
                int d;
                double dist;
                double tmp;
                double minDist = io_nnQueue->GetMaxKey();

                //Search through all points in bucket
                for (int i=0; i<m_nPts; ++i) {
                        // Use pointers on coordinates of bucket and query points
                        KDData *bktDataPtr = m_bucket[i].GetPoint();
                        KDData *queryPtPtr = i_qp->GetPoint();

                        dist = 0.0;

                        for (d=0; d<m_dimSize; ++d) {
                                tmp = (double)(*(bktDataPtr++)) - (double)(*(queryPtPtr++));
                                dist += tmp*tmp;

                                // Check if current distance exceeds min distance so far
                                if (dist >= minDist) {
                                        break;
                                }
                        }

                        // Check if current point is the current nearest neighbor
                        if (d >= m_dimSize && dist < minDist) {
                                io_nnQueue->Insert(dist, &(m_bucket[i]));
                                minDist = io_nnQueue->GetMaxKey();
                        }
                }

                // Increment number of points visited
                io_numVisited += m_nPts;
        }

        /*
                NNNodeSearch

                Nearest neighbor search recursive routine. At this level, we only determine
                if we need to make a search in a leaf node (search its bucket) or in an
                internal node (find the next node to search).

                PARAMETERS
                i_qp : query point given to find its nearest neighbors
                i_maxNumNodes : maximum number of leaf nodes to visit before to stop the
                        search. If it is set to 0, than all the tree can be
                        searched.
                io_numVisited : number of leaf nodes visited so far (modified)
                i_maxErr : error bound to determine if we should visit a child node.
                io_bbDist : distance from query point to the tree points bounding box.
                io_nnQueue : priority queue containing the current NN information (modified).
                        In this priority queue, the NN distances are used as keys and the
                        NN points, as elements. (modified)

        */
        void NNNodeSearch(KDPoint<KDData> *i_qp, int i_maxNumNodes, int &io_numVisited,
                double i_maxErr, double io_bbDist, KDPQueue<double, int> *io_nnQueue)
        {
                // Verify if we visited enough leaf nodes. If i_maxNumNodes = 0, we make a
                // complete search
                if (i_maxNumNodes && io_numVisited >= i_maxNumNodes) {
                        return;
                }

                double cutDist = (double)(i_qp->GetCoord(m_cutDim)) - (double)m_cutVal;

                if (cutDist < 0.0) {
                        // Left side of cutting plane
                        if (m_children[KDN_LOW] != NULL) {
                                // Visit closer child first
                                m_children[KDN_LOW]->NNSearch(i_qp, i_maxNumNodes, io_numVisited,
                                        i_maxErr, io_bbDist, io_nnQueue);

                                double bboxDiff = (double)(m_cutBnds[KDN_LOW]) - (double)(i_qp->GetCoord(m_cutDim));

                                // Ignore if within bounds
                                if (bboxDiff < 0.0) {
                                        bboxDiff = 0.0;
                                }

                                // Increase distance to include child bounding box
                                io_bbDist += cutDist*cutDist - bboxDiff*bboxDiff;

                                if (io_bbDist*i_maxErr < io_nnQueue->GetMaxKey()) {
                                        // Other child is close enough
                                        m_children[KDN_HIGH]->NNSearch(i_qp, i_maxNumNodes, io_numVisited,
                                                i_maxErr, io_bbDist, io_nnQueue);
                                }
                        }
                }
                else {
                        // Right side of cutting plane
                        if (m_children[KDN_HIGH] != NULL) {
                                // Visit closer child first
                                m_children[KDN_HIGH]->NNSearch(i_qp, i_maxNumNodes, io_numVisited,
                                        i_maxErr, io_bbDist, io_nnQueue);

                                double bboxDiff = (double)(i_qp->GetCoord(m_cutDim)) - (double)(m_cutBnds[KDN_HIGH]);

                                // Ignore if within bounds
                                if (bboxDiff < 0.0) {
                                        bboxDiff = 0.0;
                                }

                                // Increase distance to include child bounding box
                                io_bbDist += cutDist*cutDist - bboxDiff*bboxDiff;

                                if (io_bbDist*i_maxErr < io_nnQueue->GetMaxKey()) {
                                        // Other child is close enough
                                        m_children[KDN_LOW]->NNSearch(i_qp, i_maxNumNodes, io_numVisited,
                                                i_maxErr, io_bbDist, io_nnQueue);
                                }
                        }
                }
        }

        /*
                NNNodeSearch

                Nearest neighbor search recursive routine. At this level, we only determine
                if we need to make a search in a leaf node (search its bucket) or in an
                internal node (find the next node to search).

                PARAMETERS
                i_qp : query point given to find its nearest neighbors
                i_maxNumNodes : maximum number of leaf nodes to visit before to stop the
                        search. If it is set to 0, than all the tree can be
                        searched.
                io_numVisited : number of leaf nodes visited so far (modified)
                i_maxErr : error bound to determine if we should visit a child node.
                io_bbDist : distance from query point to the tree points bounding box.
                io_nnQueue : priority queue containing the current NN information (modified).
                        In this priority queue, the NN distances are used as keys and the
                        NN point indexes, as elements. (modified)

        */
        void NNNodeSearch(KDPoint<KDData> *i_qp, int i_maxNumNodes, int &io_numVisited,
                double i_maxErr, double io_bbDist, KDPQueue<double, KDPoint<KDData> > *io_nnQueue)
        {
                // Verify if we visited enough leaf nodes. If i_maxNumNodes = 0, we make a
                // complete search
                if (i_maxNumNodes && io_numVisited >= i_maxNumNodes) {
                        return;
                }

                double cutDist = (double)(i_qp->GetCoord(m_cutDim)) - (double)m_cutVal;

                if (cutDist < 0.0) {
                        // Left side of cutting plane
                        if (m_children[KDN_LOW] != NULL) {
                                // Visit closer child first
                                m_children[KDN_LOW]->NNSearch(i_qp, i_maxNumNodes, io_numVisited,
                                        i_maxErr, io_bbDist, io_nnQueue);

                                double bboxDiff = (double)(m_cutBnds[KDN_LOW]) - (double)(i_qp->GetCoord(m_cutDim));

                                // Ignore if within bounds
                                if (bboxDiff < 0.0) {
                                        bboxDiff = 0.0;
                                }

                                // Increase distance to include child bounding box
                                io_bbDist += cutDist*cutDist - bboxDiff*bboxDiff;

                                if (io_bbDist*i_maxErr < io_nnQueue->GetMaxKey()) {
                                        // Other child is close enough
                                        m_children[KDN_HIGH]->NNSearch(i_qp, i_maxNumNodes, io_numVisited,
                                                i_maxErr, io_bbDist, io_nnQueue);
                                }
                        }
                }
                else {
                        // Right side of cutting plane
                        if (m_children[KDN_HIGH] != NULL) {
                                // Visit closer child first
                                m_children[KDN_HIGH]->NNSearch(i_qp, i_maxNumNodes, io_numVisited,
                                        i_maxErr, io_bbDist, io_nnQueue);

                                double bboxDiff = (double)(i_qp->GetCoord(m_cutDim)) - (double)(m_cutBnds[KDN_HIGH]);

                                // Ignore if within bounds
                                if (bboxDiff < 0.0) {
                                        bboxDiff = 0.0;
                                }

                                // Increase distance to include child bounding box
                                io_bbDist += cutDist*cutDist - bboxDiff*bboxDiff;

                                if (io_bbDist*i_maxErr < io_nnQueue->GetMaxKey()) {
                                        // Other child is close enough
                                        m_children[KDN_LOW]->NNSearch(i_qp, i_maxNumNodes, io_numVisited,
                                                i_maxErr, io_bbDist, io_nnQueue);
                                }
                        }
                }
        }

private:
        // Orthogonal dimension to cutting plane
        int m_cutDim;
        // Location of cutting plane
        KDData m_cutVal;
        // Bounds along orthogonal dimension to cutting plane
        KDData m_cutBnds[2];
        // Current node children
        KDNode *m_children[2];

        // Number of points in bucket
        int m_nPts;
        // Dimension size of points in bucket
        int m_dimSize;
        // Bucket of points
        KDPoint<KDData> *m_bucket;
        // Bucket of indexes of the points
        int *m_bucketIdx;
};

}//namespace RobotFlow

#endif

---