KD дерево, медленное построение дерева

Я пытаюсь построить дерево KD (статический случай). Мы предполагаем, что точки отсортированы как по координатам x, так и по y.

Для равномерной глубины рекурсии набор разбивается на два подмножества вертикальной линией, проходящей через медианную координату x.

Для нечетной глубины рекурсии набор разбивается на два подмножества горизонтальной линией, проходящей через медиану координаты y.

Медиану можно определить из отсортированного набора по координате x/y. Этот шаг я делаю перед каждым разбиением набора. И я думаю, что это вызывает медленное построение дерева.

1. Пожалуйста, не могли бы вы помочь мне проверить и оптимизировать код?
2. Я не могу найти k-го ближайшего соседа, может кто-нибудь помочь мне с кодом?

Большое спасибо за помощь и терпение...

Пожалуйста, посмотрите пример кода:

class KDNode
{
private:
Point2D *data;
KDNode *left;
KDNode *right;
    ....
};

void KDTree::createKDTree(Points2DList *pl)
{
//Create list
KDList kd_list;

//Create KD list (all input points)
for (unsigned int i = 0; i < pl->size(); i++)
{
kd_list.push_back((*pl)[i]);
}

//Sort points by x
std::sort(kd_list.begin(), kd_list.end(), sortPoints2DByY());

//Build KD Tree
root = buildKDTree(&kd_list, 1);
}


KDNode * KDTree::buildKDTree(KDList *kd_list, const unsigned int depth)
{
//Build KD tree
const unsigned int n = kd_list->size();

 //No leaf will be built
 if (n == 0)
 {
  return NULL;
 }

 //Only one point: create leaf of KD Tree
 else if (n == 1)
 {
  //Create one leaft
  return new KDNode(new Point2D ((*kd_list)[0]));
 }

 //At least 2 points: create one leaf, split tree into left and right subtree
 else
 {
  //New KD node
  KDNode *node = NULL;

  //Get median index
  const unsigned int median_index = n/2;

  //Create new KD Lists
  KDList kd_list1, kd_list2;

  //The depth is even, process by x coordinate
  if (depth%2 == 0)
  {
   //Create new median node
   node = new KDNode(new Point2D( (*kd_list)[median_index]));

   //Split list
   for (unsigned int i = 0; i < n; i++)
   {
    //Geta actual point
    Point2D *p = &(*kd_list)[i];

    //Add point to the first list: x < median.x
    if (p->getX() < (*kd_list)[median_index].getX())
    {
     kd_list1.push_back(*p);
    }

    //Add point to the second list: x > median.x
    else if (p->getX() > (*kd_list)[median_index].getX())
    {
     kd_list2.push_back(*p);
    }
   }

   //Sort points by y for the next recursion step: slow construction of the tree???
   std::sort(kd_list1.begin(), kd_list1.end(), sortPoints2DByY());
   std::sort(kd_list2.begin(), kd_list2.end(), sortPoints2DByY());

  }

  //The depth is odd, process by y coordinates
  else
  {

   //Create new median node
   node = new KDNode(new Point2D((*kd_list)[median_index]));

   //Split list
   for (unsigned int i = 0; i < n; i++)
   {
    //Geta actual point
    Point2D *p = &(*kd_list)[i];

    //Add point to the first list: y < median.y
    if (p->getY() < (*kd_list)[median_index].getY())
    {
     kd_list1.push_back(*p);
    }

    //Add point to the second list: y < median.y
    else if (p->getY() >(*kd_list)[median_index].getY())
    {
     kd_list2.push_back(*p);
    }
   }

   //Sort points by x for the next recursion step: slow construction of the tree???
   std::sort(kd_list1.begin(), kd_list1.end(), sortPoints2DByX());
   std::sort(kd_list2.begin(), kd_list2.end(), sortPoints2DByX());

  }

  //Build left subtree
  node->setLeft( buildKDTree(&kd_list1, depth +1 ) );

  //Build right subtree
  node->setRight( buildKDTree(&kd_list2, depth + 1 ) );

  //Return new node 
  return node; 
 }
}