Удаление точек из облака, находящихся ближе порогового расстояния

Я импортирую файл сетки, где каждая грань описывается координатами ее вершин. Большинство вершин являются общими для точек. Поэтому я хотел бы исключить точки, находящиеся ближе threshold к точке, уже добавленной в облако. Это означает, что мне нужно одновременно эффективно выполнять поиск ближайшей точки и вставку точки.

Я пытаюсь использовать vtkPointLocator, который я уже использовал для статического облака; но я в недоумении, как его можно использовать постепенно. документация довольно кратка, и примеры (например, этот) не распространяется на этот сценарий. Этот пост был несколько полезен, но у меня до сих пор нет рабочего решения - я получаю segfaults в InsertNextPoint ( случай ниже), бесконечная рекурсия в CheateChildNode (при использовании vtkIncrementalOctreePointLocator вместо vtkPointLocator) или какая-то ошибка VTK (например, no points to subdivide, когда есть нулевые точки).

Примерно так делаю:

// read from input file
std::vector<Vector3d> vertices;
// bounds of the data are known    
double bounds[6]={/*...*/}; 

const double threshold=1e-5;

auto locator=vtkSmartPointer<vtkIncrementalOctreePointLocator>::New();
auto polydata=vtkSmartPointer<vtkPolyData>::New();
auto points=vtkSmartPointer<vtkPoint>::New();

polydata->SetPoints(points);
locator->SetDataSet(polydata);
locator->InitPointInsertion(points,bounds);

for(size_t i=0; i< i<vertices.size(); i++){
    double* vertex=vertices[i].data(); // pointer to data
    double dist; // unused
    vtkIdType id;
    // don't search if there are no points yet
    // FindClosestPointWithinRadius calls BuildLocator internally,
    // which needs some points to be present already
    if(points->GetNumberOfPoints()>0) id=locator->FindClosestPointWithinRadius(threshold,vertex,dist);
    else id=-1;
    if(id<0){
        // point not found, insert it into the locator
        locator->InsertNextPoint(vertex);
    }
}

Если есть очевидная ошибка в том, как организована ошибка, я буду рад любому предложению. Если нет, я пытаюсь сделать MWE.

Из чтения источника кажется, что даже инкрементные классы вызывают BuildLocator при каждом поиске, если набор точек был изменен, что может быть дорого. Поэтому предложение по лучшему классу для комбинированной вставки/поиска также будет оценено.