Октри: Я делаю это неправильно? Получение очень медленной вставки

Я пишу контейнер на основе Octree от 10 до 1 миллиарда точек в память. Из-за объема загружаемых данных я должен следить за потреблением памяти.

Кажется, что все работает должным образом и сегментируется по мере необходимости, однако время вставки невероятно медленно. Вероятно, из-за перераспределения данных между родителями для детей.
Что я могу сделать, чтобы оптимизировать это? Я правильно это реализовал?
Я мог бы зарезервировать вектор в каждом узле для включения максимального количества точек, но это значительно увеличило бы требуемую память.

Используя простой контейнер типа R-tree, я загружаю 468 миллионов точек за 48 секунд.
используя октри ниже, я загружаюсь за 245 секунд.

    class OctreeNode {
public:
std::vector<std::shared_ptr<OctreeNode>>    Children;
std::vector<TPoint> Data;
BoundingBox         Bounds;

OctreeNode(){}

OctreeNode(BoundingBox bounds) : Bounds(bounds){
}

~OctreeNode(void){}

void Split();

};

typedef std::shared_ptr<OctreeNode> OctreeNodePtr;void OctreeNode::Split()
{
Point box[8];
Bounds.Get8Corners(box);
Point center = Bounds.Center;

Children.reserve(8);
Children.push_back(OctreeNodePtr(new OctreeNode(BoundingBox::From(box[0], center))));
Children.push_back(OctreeNodePtr(new OctreeNode(BoundingBox::From(box[1], center))));
Children.push_back(OctreeNodePtr(new OctreeNode(BoundingBox::From(box[3], center))));
Children.push_back(OctreeNodePtr(new OctreeNode(BoundingBox::From(box[2], center))));Children.push_back(OctreeNodePtr(new OctreeNode(BoundingBox::From(box[5], center))));
Children.push_back(OctreeNodePtr(new OctreeNode(BoundingBox::From(box[4], center))));
Children.push_back(OctreeNodePtr(new OctreeNode(BoundingBox::From(box[6], center))));
Children.push_back(OctreeNodePtr(new OctreeNode(BoundingBox::From(box[7], center))));
}Octree::Octree(BoundingBox bounds) : Bounds(bounds)
{
_root = OctreeNodePtr(new OctreeNode(bounds));
_root->Split();
}Octree::~Octree()
{
}bool Octree::InsertPoint(TPoint &p)
{
return InsertPoint(p, _root);
}

bool Octree::InsertPoint(TPoint &p, const OctreeNodePtr &parent)
{
if (parent->Children.size() != 0){
for (size_t i = 0; i < parent->Children.size(); i++){
OctreeNodePtr &currentNode = parent->Children[i];
if (currentNode->Bounds.IsContained(p.ToPoint3d())){
return InsertPoint(p, currentNode);
}
}

// Was not able to insert a point.
return false;
}

BoundingBox newBounds = parent->Bounds;
newBounds.Extend(p.ToPoint3d());// Check for split condition...
if (parent->Data.size() == MaxPerNode && newBounds.XLength() > 0.01){

// Split it...thus generating children nodes
parent->Split();// Resize the children arrays so that we don't have to keep allocating when redistributing points..
for (size_t i = 0; i < parent->Children.size(); i++){
parent->Children[i]->Data.reserve(parent->Data.size());
}// Distribute the points that were in the parent to its children..
for (size_t i = 0; i < parent->Data.size(); i++){
TPoint originalPoint = parent->Data[i];
if (!InsertPoint(originalPoint, parent)){
printf("Failed to insert point\n");
break;
}
}

// Insert the current point.
if (!InsertPoint(p, parent)){
printf("Failed to insert point\n");
}// Resize the arrays back so it fits the size of the data.....
for (size_t i = 0; i < parent->Children.size(); i++){
parent->Children[i]->Data.shrink_to_fit();
}

// clear out the parent information
parent->Data.clear();
parent->Data.shrink_to_fit();
return true;
} else {
// Node is valid so insert the data..
if (parent->Data.size() <= 100000){
parent->Data.push_back(p);
} else {
printf("Too much data in tiny node... Stop adding\n");
}

return true;
}}void Octree::Compress(){
Compress(_root);
}

void Octree::Compress(const OctreeNodePtr &parent){if (parent->Children.size() > 0){

// Look for and remove useless cells who do not contain children or point cloud data.
size_t j = 0;
bool removed = false;
while (j < parent->Children.size()){
if (parent->Children[j]->Children.size() == 0 && parent->Children[j]->Data.size() == 0){
parent->Children.erase(parent->Children.begin() + j);
removed = true;
} else {
Compress(parent->Children[j]);
++j;
}
}

if (removed)
parent->Children.shrink_to_fit();

return;
}

parent->Data.shrink_to_fit();
}