DgmOctree.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#pragma once
 
// Local
#include "CVPlatform.h"
#include "GenericOctree.h"
 
// system
#include <cassert>
#include <cstring>
#include <vector>
 
#ifdef CV_ENV_64
// enables 64 bits code octree (can go up to level 21, but take 50% more memory)
#define OCTREE_CODES_64_BITS
#endif
 
// DGM: tests in progress
// #define TEST_CELLS_FOR_SPHERICAL_NN
 
namespace cloudViewer {
 
class ReferenceCloud;
class GenericIndexedCloudPersist;
class GenericProgressCallback;
class NormalizedProgress;
 
 
class CV_CORE_LIB_API DgmOctree : public GenericOctree {
public:
 
    static unsigned char GET_BIT_SHIFT(unsigned char level);
 
 
    static int OCTREE_LENGTH(int level);
 
    /*******************************/
    /*******************************/
 
 
#ifdef OCTREE_CODES_64_BITS
    static const int MAX_OCTREE_LEVEL = 21;
#else
    static const int MAX_OCTREE_LEVEL = 10;
#endif
 
 
#ifdef OCTREE_CODES_64_BITS
    using CellCode =
            unsigned long long;  // max 21 levels (but twice more memory!)
#else
    using CellCode = unsigned;  // max 10 levels
#endif
 
 
    static const int MAX_OCTREE_LENGTH = (1 << MAX_OCTREE_LEVEL);
 
 
    static const CellCode INVALID_CELL_CODE = (~static_cast<CellCode>(0));
 
    using cellCodesContainer = std::vector<CellCode>;
 
    using cellIndexesContainer = std::vector<unsigned int>;
 
 
    struct PointDescriptor {
        const CCVector3* point;
        unsigned pointIndex;
        double squareDistd;
 
        PointDescriptor() : point(nullptr), pointIndex(0), squareDistd(-1.0) {}
 
        PointDescriptor(const CCVector3* P, unsigned index)
            : point(P), pointIndex(index), squareDistd(-1.0) {}
 
        PointDescriptor(const CCVector3* P, unsigned index, double d2)
            : point(P), pointIndex(index), squareDistd(d2) {}
 
 
        static bool distComp(const PointDescriptor& a,
                             const PointDescriptor& b) {
            return a.squareDistd < b.squareDistd;
        }
    };
 
    using NeighboursSet = std::vector<PointDescriptor>;
 
    struct CellDescriptor {
        CCVector3 center;
        unsigned index;
 
        CellDescriptor() = default;
 
        CellDescriptor(const CCVector3& C, unsigned i) : center(C), index(i) {}
    };
 
    using NeighbourCellsSet = std::vector<CellDescriptor>;
 
 
    struct NearestNeighboursSearchStruct {
        /*** Information to set before search ***/
 
 
        CCVector3 queryPoint;
 
        unsigned char level;
 
        unsigned minNumberOfNeighbors;
 
        Tuple3i cellPos;
 
        CCVector3 cellCenter;
 
 
        double maxSearchSquareDistd;
 
        /*** Information to set to 0 before search ***/
 
 
        cellIndexesContainer minimalCellsSetToVisit;
 
 
        NeighboursSet pointsInNeighbourhood;
 
 
        int alreadyVisitedNeighbourhoodSize;
 
        /*** Result ***/
 
 
        unsigned theNearestPointIndex;
 
        NearestNeighboursSearchStruct()
            : queryPoint(0, 0, 0),
              level(1),
              minNumberOfNeighbors(1),
              cellPos(0, 0, 0),
              cellCenter(0, 0, 0),
              maxSearchSquareDistd(0),
              alreadyVisitedNeighbourhoodSize(0),
              theNearestPointIndex(0) {}
    };
 
    struct NearestNeighboursSphericalSearchStruct
        : public NearestNeighboursSearchStruct {
#ifdef TEST_CELLS_FOR_SPHERICAL_NN
        NeighboursSet pointsInSphericalNeighbourhood;
 
        NeighbourCellsSet cellsInNeighbourhood;
 
        PointCoordinateType maxInD2;
 
        PointCoordinateType minOutD2;
#endif
        bool ready;
 
        inline void prepare(PointCoordinateType radius,
                            PointCoordinateType cellSize) {
#ifdef TEST_CELLS_FOR_SPHERICAL_NN
            PointCoordinateType cellDiag =
                    cellSize * static_cast<PointCoordinateType>(SQRT_3 / 2);
            minOutD2 = radius + cellDiag;
            minOutD2 *= minOutD2;
            maxInD2 = radius - cellDiag;
            if (maxInD2 <= 0)
                maxInD2 = 0;
            else
                maxInD2 *= maxInD2;
#endif
        }
 
        NearestNeighboursSphericalSearchStruct()
            : NearestNeighboursSearchStruct(),
              ready(false)
#ifdef TEST_CELLS_FOR_SPHERICAL_NN
              ,
              maxInD2(0.0),
              minOutD2(FLT_MAX)
#endif
        {
        }
    };
 
 
    struct IndexAndCode {
        unsigned theIndex;
        CellCode theCode;
 
        IndexAndCode() : theIndex(0), theCode(0) {}
 
        IndexAndCode(unsigned index, CellCode code)
            : theIndex(index), theCode(code) {}
 
        IndexAndCode(const IndexAndCode& ic)
            : theIndex(ic.theIndex), theCode(ic.theCode) {}
 
        inline bool operator<(const IndexAndCode& iac) const {
            return theCode < iac.theCode;
        }
 
        inline bool operator>(const IndexAndCode& iac) const {
            return theCode > iac.theCode;
        }
 
 
        static bool codeComp(const IndexAndCode& a,
                             const IndexAndCode& b) throw() {
            return a.theCode < b.theCode;
        }
 
 
        static bool indexComp(const IndexAndCode& a,
                              const IndexAndCode& b) throw() {
            return a.theIndex < b.theIndex;
        }
    };
 
    using cellsContainer = std::vector<IndexAndCode>;
 
    struct octreeCell {
        // Warning: put the non aligned members (< 4 bytes) at the end to avoid
        // too much alignment padding!
 
        const DgmOctree* parentOctree;  // 8 bytes
        CellCode truncatedCode;  // 8 bytes
        unsigned index;  // 4 bytes
        ReferenceCloud* points;  // 8 bytes
        unsigned char level;  // 1 byte (+ 3 for alignment)
 
        // Total
        // //32 bytes (for 64 bits arch.)
 
        explicit octreeCell(const DgmOctree* parentOctree);
 
        virtual ~octreeCell();
 
    private:
        octreeCell(const octreeCell& cell);
    };
 
 
    using octreeCellFunc = bool (*)(const octreeCell&,
                                    void**,
                                    NormalizedProgress*);
 
    /******************************/
    /******************************/
 
 
    explicit DgmOctree(GenericIndexedCloudPersist* cloud);
 
    ~DgmOctree() override = default;
 
    virtual void clear();
 
 
    int build(GenericProgressCallback* progressCb = nullptr);
 
 
    int build(const CCVector3& octreeMin,
              const CCVector3& octreeMax,
              const CCVector3* pointsMinFilter = nullptr,
              const CCVector3* pointsMaxFilter = nullptr,
              GenericProgressCallback* progressCb = nullptr);
 
    /**** GETTERS ****/
 
 
    inline unsigned getNumberOfProjectedPoints() const {
        return m_numberOfProjectedPoints;
    }
 
 
    inline const CCVector3& getOctreeMins() const { return m_dimMin; }
 
 
    inline const CCVector3& getOctreeMaxs() const { return m_dimMax; }
 
 
    void getBoundingBox(CCVector3& bbMin, CCVector3& bbMax) const;
 
 
    inline const int* getMinFillIndexes(unsigned char level) const {
        return m_fillIndexes + 6 * level;
    }
 
 
    inline const int* getMaxFillIndexes(unsigned char level) const {
        return m_fillIndexes + 6 * level + 3;
    }
 
 
    inline const PointCoordinateType& getCellSize(unsigned char level) const {
        return m_cellSize[level];
    }
 
 
    void getCellDistanceFromBorders(const Tuple3i& cellPos,
                                    unsigned char level,
                                    int* cellDists) const;
 
 
    void getCellDistanceFromBorders(const Tuple3i& cellPos,
                                    unsigned char level,
                                    int neighbourhoodLength,
                                    int* cellDists) const;
 
 
    bool getPointsInCellByCellIndex(ReferenceCloud* cloud,
                                    unsigned cellIndex,
                                    unsigned char level,
                                    bool clearOutputCloud = true) const;
 
 
    bool getPointsInCell(CellCode cellCode,
                         unsigned char level,
                         ReferenceCloud* subset,
                         bool isCodeTruncated = false,
                         bool clearOutputCloud = true) const;
 
 
    ReferenceCloud* getPointsInCellsWithSortedCellCodes(
            cellCodesContainer& cellCodes,
            unsigned char level,
            ReferenceCloud* subset,
            bool areCodesTruncated = false) const;
 
    /**** NEIGHBOURHOOD SEARCH ****/
 
 
    unsigned findPointNeighbourhood(
            const CCVector3* _queryPoint,
            ReferenceCloud* Yk,
            unsigned maxNumberOfNeighbors,
            unsigned char level,
            double& maxSquareDist,
            double maxSearchDist = 0,
            int* finalNeighbourhoodSize = nullptr) const;
 
 
    double findTheNearestNeighborStartingFromCell(
            NearestNeighboursSearchStruct& nNSS) const;
 
 
    unsigned findNearestNeighborsStartingFromCell(
            NearestNeighboursSearchStruct& nNSS,
            bool getOnlyPointsWithValidScalar = false) const;
 
 
    int findNeighborsInASphereStartingFromCell(
            NearestNeighboursSearchStruct& nNSS,
            double radius,
            bool sortValues = true) const;
 
public:  // extraction of points inside geometrical volumes (sphere, cylinder,
         // box, etc.)
    // deprecated
    // int getPointsInSphericalNeighbourhood(const CCVector3& sphereCenter,
    // PointCoordinateType radius, NeighboursSet& neighbours) const;
 
 
    int getPointsInSphericalNeighbourhood(const CCVector3& sphereCenter,
                                          PointCoordinateType radius,
                                          NeighboursSet& neighbours,
                                          unsigned char level) const;
 
    struct CylindricalNeighbourhood {
        CCVector3 center;
        CCVector3 dir;
        PointCoordinateType radius;
        PointCoordinateType maxHalfLength;
        NeighboursSet neighbours;
        unsigned char level;
        bool onlyPositiveDir;
 
        CylindricalNeighbourhood()
            : center(0, 0, 0),
              dir(0, 0, 1),
              radius(0),
              maxHalfLength(0),
              level(0),
              onlyPositiveDir(false) {}
    };
 
 
    std::size_t getPointsInCylindricalNeighbourhood(
            CylindricalNeighbourhood& params) const;
 
    struct ProgressiveCylindricalNeighbourhood : CylindricalNeighbourhood {
        PointCoordinateType currentHalfLength;
 
        NeighboursSet potentialCandidates;
        Tuple3i prevMinCornerPos;
        Tuple3i prevMaxCornerPos;
 
        ProgressiveCylindricalNeighbourhood()
            : CylindricalNeighbourhood(),
              currentHalfLength(0),
              prevMinCornerPos(-1, -1, -1),
              prevMaxCornerPos(0, 0, 0) {}
    };
 
 
    std::size_t getPointsInCylindricalNeighbourhoodProgressive(
            ProgressiveCylindricalNeighbourhood& params) const;
 
    struct BoxNeighbourhood {
        CCVector3 center;
        CCVector3* axes;
        CCVector3 dimensions;
        NeighboursSet neighbours;
        unsigned char level;
 
        BoxNeighbourhood()
            : center(0, 0, 0), axes(nullptr), dimensions(0, 0, 0), level(0) {}
    };
 
 
    std::size_t getPointsInBoxNeighbourhood(BoxNeighbourhood& params) const;
 
public: /***** CELLS POSITION HANDLING *****/
 
    static CellCode GenerateTruncatedCellCode(const Tuple3i& cellPos,
                                              unsigned char level);
 
#ifndef OCTREE_CODES_64_BITS
    static CellCode GenerateTruncatedCellCode(const Tuple3s& pos,
                                              unsigned char level);
#endif
 
 
    inline void getTheCellPosWhichIncludesThePoint(const CCVector3* thePoint,
                                                   Tuple3i& cellPos) const {
        const PointCoordinateType& cs = getCellSize(MAX_OCTREE_LEVEL);
        // DGM: if we admit that cs >= 0, then the 'floor' operator is useless
        // (int cast = truncation)
        cellPos.x = static_cast<int>(/*floor*/ (thePoint->x - m_dimMin.x) / cs);
        cellPos.y = static_cast<int>(/*floor*/ (thePoint->y - m_dimMin.y) / cs);
        cellPos.z = static_cast<int>(/*floor*/ (thePoint->z - m_dimMin.z) / cs);
    }
 
 
    inline void getTheCellPosWhichIncludesThePoint(const CCVector3* thePoint,
                                                   Tuple3i& cellPos,
                                                   unsigned char level) const {
        assert(level <= MAX_OCTREE_LEVEL);
 
        getTheCellPosWhichIncludesThePoint(thePoint, cellPos);
 
        const unsigned char dec = MAX_OCTREE_LEVEL - level;
        cellPos.x >>= dec;
        cellPos.y >>= dec;
        cellPos.z >>= dec;
    }
 
 
    inline void getTheCellPosWhichIncludesThePoint(const CCVector3* thePoint,
                                                   Tuple3i& cellPos,
                                                   unsigned char level,
                                                   bool& inBounds) const {
        assert(level <= MAX_OCTREE_LEVEL);
 
        getTheCellPosWhichIncludesThePoint(thePoint, cellPos);
 
        inBounds = (cellPos.x >= 0 && cellPos.x < MAX_OCTREE_LENGTH &&
                    cellPos.y >= 0 && cellPos.y < MAX_OCTREE_LENGTH &&
                    cellPos.z >= 0 && cellPos.z < MAX_OCTREE_LENGTH);
 
        const unsigned char dec = MAX_OCTREE_LEVEL - level;
        cellPos.x >>= dec;
        cellPos.y >>= dec;
        cellPos.z >>= dec;
    }
 
 
    void getCellPos(CellCode code,
                    unsigned char level,
                    Tuple3i& cellPos,
                    bool isCodeTruncated) const;
 
 
    inline void computeCellCenter(CellCode code,
                                  unsigned char level,
                                  CCVector3& center,
                                  bool isCodeTruncated = false) const {
        Tuple3i cellPos;
        getCellPos(code, level, cellPos, isCodeTruncated);
 
        return computeCellCenter(cellPos, level, center);
    }
 
 
    inline void computeCellCenter(const Tuple3i& cellPos,
                                  unsigned char level,
                                  CCVector3& center) const {
        const PointCoordinateType& cs = getCellSize(level);
        center.x =
                m_dimMin.x + cs * (static_cast<PointCoordinateType>(0.5) +
                                   static_cast<PointCoordinateType>(cellPos.x));
        center.y =
                m_dimMin.y + cs * (static_cast<PointCoordinateType>(0.5) +
                                   static_cast<PointCoordinateType>(cellPos.y));
        center.z =
                m_dimMin.z + cs * (static_cast<PointCoordinateType>(0.5) +
                                   static_cast<PointCoordinateType>(cellPos.z));
    }
 
#ifndef OCTREE_CODES_64_BITS
    inline void computeCellCenter(const Tuple3s& cellPos,
                                  unsigned char level,
                                  CCVector3& center) const {
        const PointCoordinateType& cs = getCellSize(level);
        center.x =
                m_dimMin.x + cs * (static_cast<PointCoordinateType>(0.5) +
                                   static_cast<PointCoordinateType>(cellPos.x));
        center.y =
                m_dimMin.y + cs * (static_cast<PointCoordinateType>(0.5) +
                                   static_cast<PointCoordinateType>(cellPos.y));
        center.z =
                m_dimMin.z + cs * (static_cast<PointCoordinateType>(0.5) +
                                   static_cast<PointCoordinateType>(cellPos.z));
    }
#endif
 
 
    void computeCellLimits(CellCode code,
                           unsigned char level,
                           CCVector3& cellMin,
                           CCVector3& cellMax,
                           bool isCodeTruncated = false) const;
 
    /**** OCTREE DIAGNOSIS ****/
 
 
    unsigned char findBestLevelForAGivenNeighbourhoodSizeExtraction(
            PointCoordinateType radius) const;
 
 
    unsigned char findBestLevelForComparisonWithOctree(
            const DgmOctree* theOtherOctree) const;
 
 
    unsigned char findBestLevelForAGivenPopulationPerCell(
            unsigned indicativeNumberOfPointsPerCell) const;
 
 
    unsigned char findBestLevelForAGivenCellNumber(
            unsigned indicativeNumberOfCells) const;
 
    inline const CellCode& getCellCode(unsigned index) const {
        return m_thePointsAndTheirCellCodes[index].theCode;
    }
 
 
    bool getCellCodes(unsigned char level,
                      cellCodesContainer& vec,
                      bool truncatedCodes = false) const;
 
 
    bool getCellIndexes(unsigned char level, cellIndexesContainer& vec) const;
 
 
    bool getCellCodesAndIndexes(unsigned char level,
                                cellsContainer& vec,
                                bool truncatedCodes = false) const;
 
 
    void diff(const cellCodesContainer& codesA,
              const cellCodesContainer& codesB,
              cellCodesContainer& diffA,
              cellCodesContainer& diffB) const;
 
 
    bool diff(unsigned char octreeLevel,
              const cellsContainer& codesA,
              const cellsContainer& codesB,
              int& diffA,
              int& diffB,
              int& cellsA,
              int& cellsB) const;
 
    inline const unsigned& getCellNumber(unsigned char level) const {
        assert(level <= MAX_OCTREE_LEVEL);
        return m_cellCount[level];
    }
 
 
    double computeMeanOctreeDensity(unsigned char level) const;
 
 
    static inline PointCoordinateType ComputeMinDistanceToCellBorder(
            const CCVector3& queryPoint,
            PointCoordinateType cs,
            const CCVector3& cellCenter) {
        PointCoordinateType d1 = std::abs(cellCenter.x - queryPoint.x);
        PointCoordinateType d2 = std::abs(cellCenter.y - queryPoint.y);
        if (d2 > d1) d1 = d2;
 
        d2 = std::abs(cellCenter.z - queryPoint.z);
        return cs / 2 - (d2 > d1 ? d2 : d1);
    }
 
    /**** ADVANCED METHODS ****/
 
 
    int extractCCs(const cellCodesContainer& cellCodes,
                   unsigned char level,
                   bool sixConnexity,
                   GenericProgressCallback* progressCb = nullptr) const;
 
 
    int extractCCs(unsigned char level,
                   bool sixConnexity,
                   GenericProgressCallback* progressCb = nullptr) const;
 
    /**** OCTREE VISITOR ****/
 
 
    unsigned executeFunctionForAllCellsStartingAtLevel(
            unsigned char startingLevel,
            octreeCellFunc func,
            void** additionalParameters,
            unsigned minNumberOfPointsPerCell,
            unsigned maxNumberOfPointsPerCell,
            bool multiThread = true,
            GenericProgressCallback* progressCb = nullptr,
            const char* functionTitle = nullptr,
            int maxThreadCount = 0);
 
 
    unsigned executeFunctionForAllCellsAtLevel(
            unsigned char level,
            octreeCellFunc func,
            void** additionalParameters,
            bool multiThread = false,
            GenericProgressCallback* progressCb = nullptr,
            const char* functionTitle = nullptr,
            int maxThreadCount = 0);
 
    enum RayCastProcess { RC_NEAREST_POINT, RC_CLOSE_POINTS };
 
    bool rayCast(const CCVector3& rayAxis,
                 const CCVector3& rayOrigin,
                 double maxRadiusOrFov,
                 bool isFOV,  // whether the previous parameter is a radius
                              // (distance) or a FOV (in radians)
                 RayCastProcess process,
                 std::vector<PointDescriptor>& output) const;
 
    inline GenericIndexedCloudPersist* associatedCloud() const {
        return m_theAssociatedCloud;
    }
 
    const cellsContainer& pointsAndTheirCellCodes() const {
        return m_thePointsAndTheirCellCodes;
    }
 
    static bool MultiThreadSupport();
 
protected:
    /*******************************/
    /*******************************/
 
    struct octreeTopDownScanStruct {
        // Warning: put the non aligned members (< 4 bytes) at the end to avoid
        // too much alignment padding!
 
        unsigned pos;  // 4 bytes
        unsigned elements;  // 4 bytes
        unsigned char level;  // 1 byte (+ 3 for alignment)
 
        // Total
        // //12 bytes
    };
 
    /********************************/
    /********************************/
 
    cellsContainer m_thePointsAndTheirCellCodes;
 
    GenericIndexedCloudPersist* m_theAssociatedCloud;
 
    unsigned m_numberOfProjectedPoints;
 
    unsigned m_nearestPow2;
 
    CCVector3 m_dimMin;
    CCVector3 m_dimMax;
 
    CCVector3 m_pointsMin;
    CCVector3 m_pointsMax;
 
    PointCoordinateType m_cellSize[MAX_OCTREE_LEVEL + 2];
    int m_fillIndexes[(MAX_OCTREE_LEVEL + 1) * 6];
    unsigned m_cellCount[MAX_OCTREE_LEVEL + 1];
    unsigned m_maxCellPopulation[MAX_OCTREE_LEVEL + 1];
    double m_averageCellPopulation[MAX_OCTREE_LEVEL + 1];
    double m_stdDevCellPopulation[MAX_OCTREE_LEVEL + 1];
 
    /******************************/
    /******************************/
 
 
    int genericBuild(GenericProgressCallback* progressCb = nullptr);
 
    void updateMinAndMaxTables();
 
    void updateCellSizeTable();
 
    void updateCellCountTable();
 
 
    void computeCellsStatistics(unsigned char level);
 
 
    void getNeighborCellsAround(const Tuple3i& cellPos,
                                cellIndexesContainer& neighborCellsIndexes,
                                int neighbourhoodLength,
                                unsigned char level) const;
 
 
    void getPointsInNeighbourCellsAround(
            NearestNeighboursSearchStruct& nNSS,
            int neighbourhoodLength,
            bool getOnlyPointsWithValidScalar = false) const;
 
#ifdef TEST_CELLS_FOR_SPHERICAL_NN
    void getPointsInNeighbourCellsAround(
            NearestNeighboursSphericalSearchStruct& nNSS,
            int minNeighbourhoodLength,
            int maxNeighbourhoodLength) const;
#endif
 
 
    unsigned getCellIndex(CellCode truncatedCellCode,
                          unsigned char bitDec) const;
 
 
    unsigned getCellIndex(CellCode truncatedCellCode,
                          unsigned char bitDec,
                          unsigned begin,
                          unsigned end) const;
};
 
}  // namespace cloudViewer