CloudSamplingTools.cpp

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#include <CloudSamplingTools.h>
 
// local
#include <CVPointCloud.h>
#include <DgmOctreeReferenceCloud.h>
#include <DistanceComputationTools.h>
#include <GenericProgressCallback.h>
#include <Neighbourhood.h>
#include <ReferenceCloud.h>
#include <ScalarField.h>
#include <ScalarFieldTools.h>
#include <SimpleMesh.h>
 
// system
#include <algorithm>
#include <random>
 
using namespace cloudViewer;
 
GenericIndexedCloud* CloudSamplingTools::resampleCloudWithOctree(
        GenericIndexedCloudPersist* inputCloud,
        int newNumberOfPoints,
        RESAMPLING_CELL_METHOD resamplingMethod,
        GenericProgressCallback* progressCb,
        DgmOctree* inputOctree) {
    assert(inputCloud);
 
    DgmOctree* octree = inputOctree;
    if (!octree) {
        octree = new DgmOctree(inputCloud);
        if (octree->build(progressCb) < 1) return nullptr;
    }
 
    // look for the Octree level that gives the number of cells (= points)
    // closest to the desired value
    unsigned char bestLevel =
            octree->findBestLevelForAGivenCellNumber(newNumberOfPoints);
 
    GenericIndexedCloud* sampledCloud = resampleCloudWithOctreeAtLevel(
            inputCloud, bestLevel, resamplingMethod, progressCb, octree);
 
    if (!inputOctree) delete octree;
 
    return sampledCloud;
}
 
PointCloud* CloudSamplingTools::resampleCloudWithOctreeAtLevel(
        GenericIndexedCloudPersist* inputCloud,
        unsigned char octreeLevel,
        RESAMPLING_CELL_METHOD resamplingMethod,
        GenericProgressCallback* progressCb /*=0*/,
        DgmOctree* inputOctree /*=0*/) {
    assert(inputCloud);
 
    DgmOctree* octree = inputOctree;
    if (!octree) {
        octree = new DgmOctree(inputCloud);
        if (octree->build(progressCb) < 1) {
            delete octree;
            return nullptr;
        }
    }
 
    PointCloud* cloud = new PointCloud();
 
    unsigned nCells = octree->getCellNumber(octreeLevel);
    if (!cloud->reserve(nCells)) {
        if (!inputOctree) delete octree;
        delete cloud;
        return nullptr;
    }
 
    // structure contenant les parametres additionnels
    void* additionalParameters[2] = {
            reinterpret_cast<void*>(cloud),
            reinterpret_cast<void*>(&resamplingMethod)};
 
    if (octree->executeFunctionForAllCellsAtLevel(
                octreeLevel, &resampleCellAtLevel, additionalParameters,
                false,  // the process is so simple that MT is slower!
                progressCb, "Cloud Resampling") == 0) {
        // something went wrong
        delete cloud;
        cloud = nullptr;
    }
 
    if (!inputOctree) delete octree;
 
    return cloud;
}
 
ReferenceCloud* CloudSamplingTools::subsampleCloudWithOctree(
        GenericIndexedCloudPersist* inputCloud,
        int newNumberOfPoints,
        SUBSAMPLING_CELL_METHOD subsamplingMethod,
        GenericProgressCallback* progressCb /*=0*/,
        DgmOctree* inputOctree /*=0*/) {
    assert(inputCloud);
 
    DgmOctree* octree = inputOctree;
    if (!octree) {
        octree = new DgmOctree(inputCloud);
        if (octree->build(progressCb) < 1) return nullptr;
    }
 
    // on cherche le niveau qui donne le nombre de points le plus proche de la
    // consigne
    unsigned char bestLevel =
            octree->findBestLevelForAGivenCellNumber(newNumberOfPoints);
 
    ReferenceCloud* subsampledCloud = subsampleCloudWithOctreeAtLevel(
            inputCloud, bestLevel, subsamplingMethod, progressCb, octree);
 
    if (!inputOctree) delete octree;
 
    return subsampledCloud;
}
 
ReferenceCloud* CloudSamplingTools::subsampleCloudWithOctreeAtLevel(
        GenericIndexedCloudPersist* inputCloud,
        unsigned char octreeLevel,
        SUBSAMPLING_CELL_METHOD subsamplingMethod,
        GenericProgressCallback* progressCb /*=0*/,
        DgmOctree* inputOctree /*=0*/) {
    assert(inputCloud);
 
    DgmOctree* octree = inputOctree;
    if (!octree) {
        octree = new DgmOctree(inputCloud);
        if (octree->build(progressCb) < 1) {
            delete octree;
            return nullptr;
        }
    }
 
    ReferenceCloud* cloud = new ReferenceCloud(inputCloud);
 
    unsigned nCells = octree->getCellNumber(octreeLevel);
    if (!cloud->reserve(nCells)) {
        if (!inputOctree) delete octree;
        delete cloud;
        return nullptr;
    }
 
    // structure contenant les parametres additionnels
    void* additionalParameters[2] = {
            reinterpret_cast<void*>(cloud),
            reinterpret_cast<void*>(&subsamplingMethod)};
 
    if (octree->executeFunctionForAllCellsAtLevel(
                octreeLevel, &subsampleCellAtLevel, additionalParameters,
                false,  // the process is so simple that MT is slower!
                progressCb, "Cloud Subsampling") == 0) {
        // something went wrong
        delete cloud;
        cloud = nullptr;
    }
 
    if (!inputOctree) delete octree;
 
    return cloud;
}
 
ReferenceCloud* CloudSamplingTools::UniformDownSample(
        GenericIndexedCloudPersist* inputCloud, unsigned every_k_points) {
    assert(inputCloud);
 
    if (every_k_points <= 1) {
        return nullptr;
    }
 
    unsigned pointCount = inputCloud->size();
 
    unsigned newNumberOfPoints = pointCount / every_k_points + 2;
 
    // we put all input points in a ReferenceCloud
    ReferenceCloud* newCloud = new ReferenceCloud(inputCloud);
    if (!newCloud->reserve(newNumberOfPoints)) {
        // not enough memory
        delete newCloud;
        return nullptr;
    }
 
    int num = 0;
    for (unsigned i = 0; i < pointCount; i += every_k_points) {
        newCloud->addPointIndex(i);
        num += 1;
    }
 
    newCloud->resize(num);  // always smaller, so it should be ok!
 
    return newCloud;
}
 
ReferenceCloud* CloudSamplingTools::subsampleCloudRandomly(
        GenericIndexedCloudPersist* inputCloud,
        unsigned newNumberOfPoints,
        GenericProgressCallback* progressCb /*=0*/) {
    assert(inputCloud);
 
    unsigned theCloudSize = inputCloud->size();
 
    // we put all input points in a ReferenceCloud
    ReferenceCloud* newCloud = new ReferenceCloud(inputCloud);
    if (!newCloud->addPointIndex(0, theCloudSize)) {
        delete newCloud;
        return nullptr;
    }
 
    // we have less points than requested?!
    if (theCloudSize <= newNumberOfPoints) {
        return newCloud;
    }
 
    unsigned pointsToRemove = theCloudSize - newNumberOfPoints;
    std::random_device rd;   // non-deterministic generator
    std::mt19937 gen(rd());  // to seed mersenne twister.
 
    NormalizedProgress normProgress(progressCb, pointsToRemove);
    if (progressCb) {
        if (progressCb->textCanBeEdited()) {
            progressCb->setInfo("Random subsampling");
        }
        progressCb->update(0);
        progressCb->start();
    }
 
    // we randomly remove "inputCloud.size() - newNumberOfPoints" points (much
    // simpler)
    unsigned lastPointIndex = theCloudSize - 1;
    for (unsigned i = 0; i < pointsToRemove; ++i) {
        std::uniform_int_distribution<unsigned> dist(0, lastPointIndex);
        unsigned index = dist(gen);
        newCloud->swap(index, lastPointIndex);
        --lastPointIndex;
 
        if (progressCb && !normProgress.oneStep()) {
            // cancel process
            delete newCloud;
            return nullptr;
        }
    }
 
    newCloud->resize(newNumberOfPoints);  // always smaller, so it should be ok!
 
    return newCloud;
}
 
ReferenceCloud* CloudSamplingTools::resampleCloudSpatially(
        GenericIndexedCloudPersist* inputCloud,
        PointCoordinateType minDistance,
        const SFModulationParams& modParams,
        DgmOctree* inputOctree /*=0*/,
        GenericProgressCallback* progressCb /*=0*/) {
    assert(inputCloud);
    unsigned cloudSize = inputCloud->size();
 
    DgmOctree* octree = inputOctree;
    if (!octree) {
        octree = new DgmOctree(inputCloud);
        if (octree->build() < static_cast<int>(cloudSize)) {
            delete octree;
            return nullptr;
        }
    }
    assert(octree && octree->associatedCloud() == inputCloud);
 
    // output cloud
    ReferenceCloud* sampledCloud = new ReferenceCloud(inputCloud);
    const unsigned c_reserveStep = 65536;
    if (!sampledCloud->reserve(std::min(cloudSize, c_reserveStep))) {
        if (!inputOctree) delete octree;
        return nullptr;
    }
 
    std::vector<char>
            markers;  // DGM: upgraded from vector, as this can be quite huge!
    try {
        markers.resize(cloudSize, 1);  // true by default
    } catch (const std::bad_alloc&) {
        if (!inputOctree) delete octree;
        delete sampledCloud;
        return nullptr;
    }
 
    // best octree level (there may be several of them if we use parameter
    // modulation)
    std::vector<unsigned char> bestOctreeLevel;
    bool modParamsEnabled = modParams.enabled;
    ScalarType sfMin = 0, sfMax = 0;
    try {
        if (modParams.enabled) {
            // compute min and max sf values
            ScalarFieldTools::computeScalarFieldExtremas(inputCloud, sfMin,
                                                         sfMax);
 
            if (!ScalarField::ValidValue(sfMin)) {
                // all SF values are NAN?!
                modParamsEnabled = false;
            } else {
                // compute min and max 'best' levels
                PointCoordinateType dist0 = static_cast<PointCoordinateType>(
                        sfMin * modParams.a + modParams.b);
                PointCoordinateType dist1 = static_cast<PointCoordinateType>(
                        sfMax * modParams.a + modParams.b);
                unsigned char level0 =
                        octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(
                                dist0);
                unsigned char level1 =
                        octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(
                                dist1);
 
                bestOctreeLevel.push_back(level0);
                if (level1 != level0) {
                    // add intermediate levels if necessary
                    std::size_t levelCount =
                            (level1 < level0 ? level0 - level1
                                             : level1 - level0) +
                            1;
                    assert(levelCount != 0);
 
                    for (std::size_t i = 1; i < levelCount - 1;
                         ++i)  // we already know level0 and level1!
                    {
                        ScalarType sfVal =
                                sfMin + i * ((sfMax - sfMin) / levelCount);
                        PointCoordinateType dist =
                                static_cast<PointCoordinateType>(
                                        sfVal * modParams.a + modParams.b);
                        unsigned char level =
                                octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(
                                        dist);
                        bestOctreeLevel.push_back(level);
                    }
                }
                bestOctreeLevel.push_back(level1);
            }
        } else {
            unsigned char defaultLevel =
                    octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(
                            minDistance);
            bestOctreeLevel.push_back(defaultLevel);
        }
    } catch (const std::bad_alloc&) {
        // not enough memory
        if (!inputOctree) {
            delete octree;
        }
        delete sampledCloud;
        return nullptr;
    }
 
    // progress notification
    NormalizedProgress normProgress(progressCb, cloudSize);
    if (progressCb) {
        if (progressCb->textCanBeEdited()) {
            progressCb->setMethodTitle("Spatial resampling");
            char buffer[256];
            sprintf(buffer, "Points: %u\nMin dist.: %f", cloudSize,
                    minDistance);
            progressCb->setInfo(buffer);
        }
        progressCb->update(0);
        progressCb->start();
    }
 
    // for each point in the cloud that is still 'marked', we look
    // for its neighbors and remove their own marks
    bool error = false;
    // default octree level
    assert(!bestOctreeLevel.empty());
    unsigned char octreeLevel = bestOctreeLevel.front();
    // default distance between points
    PointCoordinateType minDistBetweenPoints = minDistance;
    for (unsigned i = 0; i < cloudSize; i++) {
        // no mark? we skip this point
        if (markers[i] != 0) {
            // init neighbor search structure
            const CCVector3* P = inputCloud->getPoint(i);
 
            // parameters modulation
            if (modParamsEnabled) {
                ScalarType sfVal = inputCloud->getPointScalarValue(i);
                if (ScalarField::ValidValue(sfVal)) {
                    // modulate minDistance
                    minDistBetweenPoints = static_cast<PointCoordinateType>(
                            sfVal * modParams.a + modParams.b);
                    // get (approximate) best level
                    std::size_t levelIndex = static_cast<std::size_t>(
                            bestOctreeLevel.size() *
                            ((sfVal - sfMin) / (sfMax - sfMin)));
                    if (levelIndex == bestOctreeLevel.size()) --levelIndex;
                    octreeLevel = bestOctreeLevel[levelIndex];
                } else {
                    minDistBetweenPoints = minDistance;
                    octreeLevel = bestOctreeLevel.front();
                }
            }
 
            // look for neighbors and 'de-mark' them
            {
                DgmOctree::NeighboursSet neighbours;
                octree->getPointsInSphericalNeighbourhood(
                        *P, minDistBetweenPoints, neighbours, octreeLevel);
                for (DgmOctree::NeighboursSet::iterator it = neighbours.begin();
                     it != neighbours.end(); ++it)
                    if (it->pointIndex != i) markers[it->pointIndex] = 0;
            }
 
            // At this stage, the ith point is the only one marked in a radius
            // of <minDistance>. Therefore it will necessarily be in the final
            // cloud!
            if (sampledCloud->size() == sampledCloud->capacity() &&
                !sampledCloud->reserve(sampledCloud->capacity() +
                                       c_reserveStep)) {
                // not enough memory
                error = true;
                break;
            }
            if (!sampledCloud->addPointIndex(i)) {
                // not enough memory
                error = true;
                break;
            }
        }
 
        // progress indicator
        if (progressCb && !normProgress.oneStep()) {
            // cancel process
            error = true;
            break;
        }
    }
 
    // remove unnecessarily allocated memory
    if (!error) {
        if (sampledCloud->capacity() > sampledCloud->size())
            sampledCloud->resize(sampledCloud->size());
    } else {
        delete sampledCloud;
        sampledCloud = nullptr;
    }
 
    if (progressCb) {
        progressCb->stop();
    }
 
    if (!inputOctree) {
        // locally computed octree
        delete octree;
        octree = nullptr;
    }
 
    return sampledCloud;
}
 
ReferenceCloud* CloudSamplingTools::sorFilter(
        GenericIndexedCloudPersist* inputCloud,
        int knn /*=6*/,
        double nSigma /*=1.0*/,
        DgmOctree* inputOctree /*=0*/,
        GenericProgressCallback* progressCb /*=0*/) {
    if (!inputCloud || knn <= 0 ||
        inputCloud->size() <= static_cast<unsigned>(knn)) {
        // invalid input
        assert(false);
        return nullptr;
    }
 
    DgmOctree* octree = inputOctree;
    if (!octree) {
        // compute the octree if necessary
        octree = new DgmOctree(inputCloud);
        if (octree->build(progressCb) < 1) {
            delete octree;
            return nullptr;
        }
    }
 
    // output
    ReferenceCloud* filteredCloud = nullptr;
    for (unsigned step = 0; step < 1; ++step)  // fake loop for easy break
    {
        unsigned pointCount = inputCloud->size();
 
        std::vector<PointCoordinateType> meanDistances;
        try {
            meanDistances.resize(pointCount, 0);
        } catch (const std::bad_alloc&) {
            // not enough memory
            break;
        }
        double avgDist = 0, stdDev = 0;
 
        // 1st step: compute the average distance to the neighbors
        {
            // additional parameters
            void* additionalParameters[] = {
                    reinterpret_cast<void*>(&knn),
                    reinterpret_cast<void*>(&meanDistances)};
 
            unsigned char octreeLevel =
                    octree->findBestLevelForAGivenPopulationPerCell(knn);
 
            if (octree->executeFunctionForAllCellsAtLevel(
                        octreeLevel, &applySORFilterAtLevel,
                        additionalParameters, true, progressCb,
                        "SOR filter") == 0) {
                // something went wrong
                break;
            }
 
            // deduce the average distance and std. dev.
            double sumDist = 0;
            double sumSquareDist = 0;
            for (unsigned i = 0; i < pointCount; ++i) {
                sumDist += meanDistances[i];
                sumSquareDist += meanDistances[i] * meanDistances[i];
            }
            avgDist = sumDist / pointCount;
            stdDev = sqrt(
                    std::abs(sumSquareDist / pointCount - avgDist * avgDist));
        }
 
        // 2nd step: remove the farthest points
        {
            // deduce the max distance
            double maxDist = avgDist + nSigma * stdDev;
 
            filteredCloud = new ReferenceCloud(inputCloud);
            if (!filteredCloud->reserve(pointCount)) {
                // not enough memory
                delete filteredCloud;
                filteredCloud = nullptr;
                break;
            }
 
            for (unsigned i = 0; i < pointCount; ++i) {
                if (meanDistances[i] <= maxDist) {
                    filteredCloud->addPointIndex(i);
                }
            }
 
            filteredCloud->resize(filteredCloud->size());
        }
    }
 
    if (!inputOctree) {
        delete octree;
        octree = 0;
    }
 
    return filteredCloud;
}
 
ReferenceCloud* CloudSamplingTools::noiseFilter(
        GenericIndexedCloudPersist* inputCloud,
        PointCoordinateType kernelRadius,
        double nSigma,
        bool removeIsolatedPoints /*=false*/,
        bool useKnn /*=false*/,
        int knn /*=6*/,
        bool useAbsoluteError /*=true*/,
        double absoluteError /*=0.0*/,
        DgmOctree* inputOctree /*=0*/,
        GenericProgressCallback* progressCb /*=0*/) {
    if (!inputCloud || inputCloud->size() < 2 || (useKnn && knn <= 0) ||
        (!useKnn && kernelRadius <= 0)) {
        // invalid input
        assert(false);
        return nullptr;
    }
 
    DgmOctree* octree = inputOctree;
    if (!octree) {
        octree = new DgmOctree(inputCloud);
        if (octree->build(progressCb) < 1) {
            delete octree;
            return nullptr;
        }
    }
 
    ReferenceCloud* filteredCloud = new ReferenceCloud(inputCloud);
 
    unsigned pointCount = inputCloud->size();
    if (!filteredCloud->reserve(pointCount)) {
        // not enough memory
        if (!inputOctree) delete octree;
        delete filteredCloud;
        return nullptr;
    }
 
    // additional parameters
    void* additionalParameters[] = {
            reinterpret_cast<void*>(filteredCloud),
            reinterpret_cast<void*>(&kernelRadius),
            reinterpret_cast<void*>(&nSigma),
            reinterpret_cast<void*>(&removeIsolatedPoints),
            reinterpret_cast<void*>(&useKnn),
            reinterpret_cast<void*>(&knn),
            reinterpret_cast<void*>(&useAbsoluteError),
            reinterpret_cast<void*>(&absoluteError)};
 
    unsigned char octreeLevel = 0;
    if (useKnn)
        octreeLevel = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(
                kernelRadius);
    else
        octreeLevel = octree->findBestLevelForAGivenPopulationPerCell(knn);
 
    if (octree->executeFunctionForAllCellsAtLevel(
                octreeLevel, &applyNoiseFilterAtLevel, additionalParameters,
                true, progressCb, "Noise filter") == 0) {
        // something went wrong
        delete filteredCloud;
        filteredCloud = nullptr;
    }
 
    if (!inputOctree) {
        delete octree;
        octree = nullptr;
    }
 
    if (filteredCloud) {
        filteredCloud->resize(filteredCloud->size());
    }
 
    return filteredCloud;
}
 
bool CloudSamplingTools::resampleCellAtLevel(
        const DgmOctree::octreeCell& cell,
        void** additionalParameters,
        NormalizedProgress* nProgress /*=0*/) {
    PointCloud* cloud = static_cast<PointCloud*>(additionalParameters[0]);
    RESAMPLING_CELL_METHOD resamplingMethod =
            *static_cast<RESAMPLING_CELL_METHOD*>(additionalParameters[1]);
 
    if (resamplingMethod == CELL_GRAVITY_CENTER) {
        const CCVector3* P = Neighbourhood(cell.points).getGravityCenter();
        if (!P) return false;
        cloud->addPoint(*P);
    } else  // if (resamplingMethod == CELL_CENTER)
    {
        CCVector3 center;
        cell.parentOctree->computeCellCenter(cell.truncatedCode, cell.level,
                                             center, true);
        cloud->addPoint(center);
    }
 
    if (nProgress && !nProgress->steps(cell.points->size())) {
        return false;
    }
 
    return true;
}
 
bool CloudSamplingTools::subsampleCellAtLevel(
        const DgmOctree::octreeCell& cell,
        void** additionalParameters,
        NormalizedProgress* nProgress /*=0*/) {
    ReferenceCloud* cloud =
            static_cast<ReferenceCloud*>(additionalParameters[0]);
    SUBSAMPLING_CELL_METHOD subsamplingMethod =
            *static_cast<SUBSAMPLING_CELL_METHOD*>(additionalParameters[1]);
 
    unsigned selectedPointIndex = 0;
    unsigned pointsCount = cell.points->size();
 
    if (subsamplingMethod == RANDOM_POINT) {
        selectedPointIndex = (static_cast<unsigned>(rand()) % pointsCount);
 
        if (nProgress && !nProgress->steps(pointsCount)) {
            return false;
        }
    } else  // if (subsamplingMethod == NEAREST_POINT_TO_CELL_CENTER)
    {
        CCVector3 center;
        cell.parentOctree->computeCellCenter(cell.truncatedCode, cell.level,
                                             center, true);
 
        PointCoordinateType minSquareDist =
                (*cell.points->getPoint(0) - center).norm2();
 
        for (unsigned i = 1; i < pointsCount; ++i) {
            PointCoordinateType squareDist =
                    (*cell.points->getPoint(i) - center).norm2();
            if (squareDist < minSquareDist) {
                selectedPointIndex = i;
                minSquareDist = squareDist;
            }
 
            if (nProgress && !nProgress->oneStep()) {
                return false;
            }
        }
    }
 
    return cloud->addPointIndex(
            cell.points->getPointGlobalIndex(selectedPointIndex));
}
 
bool CloudSamplingTools::applyNoiseFilterAtLevel(
        const DgmOctree::octreeCell& cell,
        void** additionalParameters,
        NormalizedProgress* nProgress /*=0*/) {
    ReferenceCloud* cloud =
            static_cast<ReferenceCloud*>(additionalParameters[0]);
    PointCoordinateType kernelRadius =
            *static_cast<PointCoordinateType*>(additionalParameters[1]);
    double nSigma = *static_cast<double*>(additionalParameters[2]);
    bool removeIsolatedPoints = *static_cast<bool*>(additionalParameters[3]);
    bool useKnn = *static_cast<bool*>(additionalParameters[4]);
    int knn = *static_cast<int*>(additionalParameters[5]);
    bool useAbsoluteError = *static_cast<bool*>(additionalParameters[6]);
    double absoluteError = *static_cast<double*>(additionalParameters[7]);
 
    // structure for nearest neighbors search
    DgmOctree::NearestNeighboursSphericalSearchStruct nNSS;
    nNSS.level = cell.level;
    nNSS.prepare(kernelRadius, cell.parentOctree->getCellSize(nNSS.level));
    if (useKnn) {
        nNSS.minNumberOfNeighbors = knn;
    }
    cell.parentOctree->getCellPos(cell.truncatedCode, cell.level, nNSS.cellPos,
                                  true);
    cell.parentOctree->computeCellCenter(nNSS.cellPos, cell.level,
                                         nNSS.cellCenter);
 
    unsigned n = cell.points->size();  // number of points in the current cell
 
    // for each point in the cell
    for (unsigned i = 0; i < n; ++i) {
        cell.points->getPoint(i, nNSS.queryPoint);
 
        // look for neighbors (either inside a sphere or the k nearest ones)
        // warning: there may be more points at the end of
        // nNSS.pointsInNeighbourhood than the actual nearest neighbors
        // (neighborCount)!
        unsigned neighborCount = 0;
 
        if (useKnn)
            neighborCount =
                    cell.parentOctree->findNearestNeighborsStartingFromCell(
                            nNSS);
        else
            neighborCount =
                    cell.parentOctree->findNeighborsInASphereStartingFromCell(
                            nNSS, kernelRadius, false);
 
        if (neighborCount >
            3)  // we want 3 points or more (other than the point itself!)
        {
            // find the query point in the nearest neighbors set and place it at
            // the end
            const unsigned globalIndex = cell.points->getPointGlobalIndex(i);
            unsigned localIndex = 0;
            while (localIndex < neighborCount &&
                   nNSS.pointsInNeighbourhood[localIndex].pointIndex !=
                           globalIndex)
                ++localIndex;
            // the query point should be in the nearest neighbors set!
            assert(localIndex < neighborCount);
            if (localIndex + 1 <
                neighborCount)  // no need to swap with another point if it's
                                // already at the end!
            {
                std::swap(nNSS.pointsInNeighbourhood[localIndex],
                          nNSS.pointsInNeighbourhood[neighborCount - 1]);
            }
 
            unsigned realNeighborCount = neighborCount - 1;
            DgmOctreeReferenceCloud neighboursCloud(
                    &nNSS.pointsInNeighbourhood,
                    realNeighborCount);  // we don't take the query point into
                                         // account!
            Neighbourhood Z(&neighboursCloud);
 
            const PointCoordinateType* lsPlane = Z.getLSPlane();
            if (lsPlane) {
                double maxD = absoluteError;
                if (!useAbsoluteError) {
                    // compute the std. dev. to this plane
                    double sum_d = 0;
                    double sum_d2 = 0;
                    for (unsigned j = 0; j < realNeighborCount; ++j) {
                        const CCVector3* P = neighboursCloud.getPoint(j);
                        double d = cloudViewer::DistanceComputationTools::
                                computePoint2PlaneDistance(P, lsPlane);
                        sum_d += d;
                        sum_d2 += d * d;
                    }
 
                    double stddev = sqrt(std::abs(sum_d2 * realNeighborCount -
                                                  sum_d * sum_d)) /
                                    realNeighborCount;
                    maxD = stddev * nSigma;
                }
 
                // distance from the query point to the plane
                double d = std::abs(cloudViewer::DistanceComputationTools::
                                            computePoint2PlaneDistance(
                                                    &nNSS.queryPoint, lsPlane));
 
                if (d <= maxD) {
                    cloud->addPointIndex(globalIndex);
                }
            } else {
                // TODO: ???
            }
        } else {
            // not enough points to fit a plane AND compute distances to it
            if (!removeIsolatedPoints) {
                // we keep the point
                unsigned globalIndex = cell.points->getPointGlobalIndex(i);
                cloud->addPointIndex(globalIndex);
            }
        }
 
        if (nProgress && !nProgress->oneStep()) {
            return false;
        }
    }
 
    return true;
}
 
bool CloudSamplingTools::applySORFilterAtLevel(
        const DgmOctree::octreeCell& cell,
        void** additionalParameters,
        NormalizedProgress* nProgress /*=0*/) {
    int knn = *static_cast<int*>(additionalParameters[0]);
    std::vector<PointCoordinateType>& meanDistances =
            *static_cast<std::vector<PointCoordinateType>*>(
                    additionalParameters[1]);
 
    // structure for nearest neighbors search
    DgmOctree::NearestNeighboursSphericalSearchStruct nNSS;
    nNSS.level = cell.level;
    nNSS.minNumberOfNeighbors =
            knn;  // DGM: I woud have put knn+1 (as the point itself will be
                  // ignored) but in this case we won't get the same result as
                  // PCL!
    cell.parentOctree->getCellPos(cell.truncatedCode, cell.level, nNSS.cellPos,
                                  true);
    cell.parentOctree->computeCellCenter(nNSS.cellPos, cell.level,
                                         nNSS.cellCenter);
 
    unsigned n = cell.points->size();  // number of points in the current cell
 
    // for each point in the cell
    for (unsigned i = 0; i < n; ++i) {
        cell.points->getPoint(i, nNSS.queryPoint);
        const unsigned globalIndex = cell.points->getPointGlobalIndex(i);
 
        // look for the k nearest neighbors
        cell.parentOctree->findNearestNeighborsStartingFromCell(nNSS);
        double sumDist = 0;
        unsigned count = 0;
        for (int j = 0; j < knn; ++j) {
            if (nNSS.pointsInNeighbourhood[j].pointIndex != globalIndex) {
                sumDist += sqrt(nNSS.pointsInNeighbourhood[j].squareDistd);
                ++count;
            }
        }
 
        if (count) {
            meanDistances[globalIndex] =
                    static_cast<PointCoordinateType>(sumDist / count);
        } else {
            // shouldn't happen
            assert(false);
        }
 
        if (nProgress && !nProgress->oneStep()) {
            return false;
        }
    }
 
    return true;
}