KdTree（3d）C＃实现不能正常工作且非常慢

Question

我正在3D空间中实现Kd Tree，我得到了一个点云，我需要将它存储在kdTree结构中。我的问题是它不能正常工作，它添加与无限循环相同的数字，有人可以检查我的代码，谢谢。有没有什么可以提高性能，因为它对于实时应用来说有点慢。 这是代码：

//this is called from main, pcdList is a list of points cloud and pcdTree is the class representing the Tree structure

pcdTree = new KdTree(pcdList[0]);
pcdList.Remove(pcdList[0]);
pcdTree.fillTree(pcdTree, pcdTree, pcdList, 0, 3);

//This is the function to fill the tree

public void fillTree(KdTree root, KdTree actualNode, /*ICollection<Vector3>*/List<Vector3> pointCloud, int mod, int k)
{
  if (mod > 3)
  {
    mod = 0;
  }

  int depth = (mod % k) + 1;

  if (pointCloud.Count > 0)
  {
    // foreach (Vector3 point in pointCloud)
    //{
    Vector3 point = pointCloud[0];

    //Console.WriteLine("pt = pt[0]");

    if (actualNode == null)
    {
      Console.WriteLine("before count > 0 " + pointCloud.Count);
      actualNode = new KdTree(point);
      nodeNum++;
      //Console.WriteLine("x,y,z " + actualNode.value.X + " " + actualNode.value.Y + " " + actualNode.value.Z);
      pointCloud.Remove(point);
      Console.WriteLine("depth " + depth);
      fillTree(root, root, pointCloud, 0, k);
    }
    else
    {
      switch (depth)
      {
        case 1:
          if (actualNode.leftChild == null)
          {
            if (actualNode.value.X <= point.X)
            {
              /*actualNode.leftChild = new KdTree(point);
                                pointCloud.Remove(point);*/
              Console.WriteLine("left" + depth);
              mod++;
              fillTree(root, actualNode.leftChild, pointCloud, mod, k);
            }
          }
          else if (actualNode.rightChild == null)
          {
            if (actualNode.value.X > point.X)
            {
              /*actualNode.rightChild = new KdTree(point);
                                pointCloud.Remove(point);*/
              Console.WriteLine("right" + depth);
              mod++;
              fillTree(root, actualNode.rightChild, pointCloud, mod, k);
            }
          }
          else
          {
            if (actualNode.value.X > point.X)
            {
              Console.WriteLine("filled left" + depth);
              mod++;
              fillTree(root, actualNode.leftChild, pointCloud, mod, k);
            }
            else
            {
              Console.WriteLine("filled right" + depth);
              mod++;
              fillTree(root, actualNode.rightChild, pointCloud, mod, k);
            }

          }

          break;

        case 2:
          if (actualNode.leftChild == null)
          {
            if (actualNode.value.Y <= point.Y)
            {
              /* actualNode.leftChild = new KdTree(point);
              pointCloud.Remove(point);*/
              Console.WriteLine("left" + depth);
              mod++;
              fillTree(root, actualNode.leftChild, pointCloud, mod, k);
            }
          }
          else if (actualNode.rightChild == null)
          {
            if (actualNode.value.Y > point.Y)
            {
              /*actualNode.rightChild = new KdTree(point);
              pointCloud.Remove(point);*/
              Console.WriteLine("right" + depth);
              mod++;
              fillTree(root, actualNode.rightChild, pointCloud, mod, k);
            }
          }
          else
          {
            if (actualNode.value.Y > point.Y)
            {
              mod++;
              fillTree(root, actualNode.rightChild, pointCloud, mod, k);
            }
            else
            {
              mod++;
              fillTree(root, actualNode.leftChild, pointCloud, mod, k);
            }

          }
          break;
        case 3:
          if (actualNode.leftChild == null)
          {
            if (actualNode.value.Z <= point.Z)
            {
              /*actualNode.leftChild = new KdTree(point);
                                pointCloud.Remove(point);*/
              Console.WriteLine("left" + depth);
              mod++;
              fillTree(root, actualNode.leftChild, pointCloud, mod, k);
            }
          }
          else if (actualNode.rightChild == null)
          {
            if (actualNode.value.Z > point.Z)
            {
              /*actualNode.rightChild = new KdTree(point);
                                pointCloud.Remove(point);*/
              Console.WriteLine("right" + depth);
              mod++;
              fillTree(root, actualNode.rightChild, pointCloud, mod, k);
            }
          }
          else
          {
            if (actualNode.value.Z > point.Z)
            {
              mod++;
              fillTree(root, actualNode.rightChild, pointCloud, mod, k);
            }
            else
            {
              mod++;
              fillTree(root, actualNode.leftChild, pointCloud, mod, k);
            }

          }
          break;
      }
    }
    //}
  }
}