Быстрая реализация диапазона поиска в C ++

Question

Я собираюсь реализовать ту же функцию, что и rangesearch в matlab. Проблема в том, что данные действительно большие (6,7 миллиона 3D-точек). Я читаю Попарное вычисление расстояния в c ++ и улучшаю свой код, но он еще недостаточно быстр. Поскольку для этого количества данных матрица вычисления расстояний потребляет ОЗУ, она неприменима. Я положил свой код здесь. Дайте мне знать, если есть способ сделать это быстрее. Я не уверен, помогает ли распараллеливание здесь или нет. Данные отсортированы по 1-му измерению, и я хочу убедиться, что для каждой точки первым соседом является сама точка.

std::vector<std::vector<long int>> rangesearch(std::vector<std::vector<float>> &data, float radius) {
    float distmat = 0;
    float xdist = 0;
    std::vector<std::vector<long int>> indices(data.size());

    //This make sure that the first neighbour of each point is itself.
    for (unsigned long int i = 0; i < data.size(); i++) {
        indices[i].push_back(i);
    }

    // instead of computing sqrt() of distance, compute the 2nd power of radius once and compare it again and again which is faster
    radius = std::pow(radius, 2);

    for (unsigned long int i = 0; i < data.size(); i++) {
        for (long int j = i + 1; j < data.size(); j++) {

            xdist = std::pow(data[i][0] - data[j][0], 2);
            distmat = xdist + std::pow(data[i][1] - data[j][1], 2) + std::pow(data[i][2] - data[j][2], 2);

            if (distmat <= radius) {
                indices[i].push_back(j);
                indices[j].push_back(i);
            }

            //This is just to make the preprocessing faster. Data should be sorted based on X cordinates.
            //Then if the distance for x cordinate is bigger than radius it means that it will be even bigger
            // for the rest of the point so there is no need to check all of them and skip the rest!
            if (xdist > radius)
                break;
        }
    }
    return indices;
}