fastMarchingForFacetExtraction.cpp

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#include "fastMarchingForFacetExtraction.h"
 
// CV_DB_LIB
#include <ecvGenericPointCloud.h>
#include <ecvOctree.h>
#include <ecvPointCloud.h>
 
// Qt
#include <QApplication>
 
const int c_3dNeighboursPosShift[] = {
        -1, -1, -1, -1, -1, 0,  -1, -1, 1,  -1, 0,  -1, -1, 0,  0,  -1,
        0,  1,  -1, 1,  -1, -1, 1,  0,  -1, 1,  1,  0,  -1, -1, 0,  -1,
        0,  0,  -1, 1,  0,  0,  -1, 0,  0,  1,  0,  1,  -1, 0,  1,  0,
        0,  1,  1,  1,  -1, -1, 1,  -1, 0,  1,  -1, 1,  1,  0,  -1, 1,
        0,  0,  1,  0,  1,  1,  1,  -1, 1,  1,  0,  1,  1,  1};
 
FastMarchingForFacetExtraction::FastMarchingForFacetExtraction()
    : cloudViewer::FastMarching(),
      m_currentFacetPoints(nullptr),
      m_currentFacetError(0),
      m_maxError(0),
      m_errorMeasure(cloudViewer::DistanceComputationTools::RMS),
      m_useRetroProjectionError(false),
      m_propagateProgressCb(nullptr),
      m_propagateProgress(0) {}
 
FastMarchingForFacetExtraction::~FastMarchingForFacetExtraction() {
    if (m_currentFacetPoints) {
        delete m_currentFacetPoints;
    }
}
 
static bool ComputeCellStats(
        cloudViewer::ReferenceCloud* subset,
        CCVector3& N,
        CCVector3& C,
        ScalarType& error,
        cloudViewer::DistanceComputationTools::ERROR_MEASURES errorMeasure) {
    error = 0;
 
    if (!subset || subset->size() == 0) return false;
 
    // we compute the gravity center
    cloudViewer::Neighbourhood Yk(subset);
    C = *Yk.getGravityCenter();
 
    // we compute the (least square) best fit plane
    const PointCoordinateType* planeEquation = Yk.getLSPlane();
    if (planeEquation) {
        N = CCVector3(planeEquation);  // normal = first 3 components
        error = cloudViewer::DistanceComputationTools::
                ComputeCloud2PlaneDistance(subset, planeEquation, errorMeasure);
    } else {
        // not enough points?
        N = CCVector3(0, 0, 0);
    }
 
    return true;
}
 
int FastMarchingForFacetExtraction::init(
        ccGenericPointCloud* cloud,
        cloudViewer::DgmOctree* theOctree,
        unsigned char level,
        ScalarType maxError,
        cloudViewer::DistanceComputationTools::ERROR_MEASURES errorMeasure,
        bool useRetroProjectionError,
        cloudViewer::GenericProgressCallback* progressCb /*=0*/) {
    m_maxError = maxError;
    m_errorMeasure = errorMeasure;
    m_useRetroProjectionError = useRetroProjectionError;
 
    if (progressCb) {
        if (progressCb->textCanBeEdited()) {
            progressCb->setMethodTitle("Fast Marching grid initialization");
            progressCb->setInfo(qPrintable(QString("Level: %1").arg(level)));
        }
        progressCb->update(0);
        progressCb->start();
    }
 
    int result = initGridWithOctree(theOctree, level);
    if (result < 0) return result;
 
    // fill the grid with the octree
    cloudViewer::DgmOctree::cellCodesContainer cellCodes;
    theOctree->getCellCodes(level, cellCodes, true);
    size_t cellCount = cellCodes.size();
 
    cloudViewer::NormalizedProgress nProgress(progressCb,
                                              static_cast<unsigned>(cellCount));
    if (progressCb) {
        progressCb->setInfo(qPrintable(
                QString("Level: %1\nCells: %2").arg(level).arg(cellCount)));
    }
 
    cloudViewer::ReferenceCloud Yk(theOctree->associatedCloud());
    while (!cellCodes.empty()) {
        if (theOctree->getPointsInCell(cellCodes.back(), level, &Yk, true)) {
            // convert the octree cell code to grid position
            Tuple3i cellPos;
            theOctree->getCellPos(cellCodes.back(), level, cellPos, true);
 
            CCVector3 N, C;
            ScalarType error;
            if (ComputeCellStats(&Yk, N, C, error, m_errorMeasure)) {
                // convert octree cell pos to FM cell pos index
                unsigned gridPos = pos2index(cellPos);
 
                // create corresponding cell
                PlanarCell* aCell = new PlanarCell;
                aCell->cellCode = cellCodes.back();
                aCell->N = N;
                aCell->C = C;
                aCell->planarError = error;
                m_theGrid[gridPos] = aCell;
            } else {
                // an error occurred?!
                return -10;
            }
        }
 
        cellCodes.pop_back();
 
        if (progressCb && !nProgress.oneStep()) {
            // process cancelled by user
            progressCb->stop();
            return -1;
        }
    }
 
    if (progressCb) {
        progressCb->stop();
    }
 
    m_initialized = true;
 
    return 0;
}
 
int FastMarchingForFacetExtraction::step() {
    if (!m_initialized) return -1;
 
    // get 'earliest' cell
    unsigned minTCellIndex = getNearestTrialCell();
    if (minTCellIndex == 0) return 0;
 
    cloudViewer::FastMarching::Cell* minTCell = m_theGrid[minTCellIndex];
    assert(minTCell && minTCell->state != PlanarCell::ACTIVE_CELL);
 
    if (minTCell->T < Cell::T_INF()) {
        assert(m_currentFacetPoints);
        unsigned sizeBefore = m_currentFacetPoints->size();
 
        // check if we can add the cell to the current "ACTIVE" set
        ScalarType error = addCellToCurrentFacet(minTCellIndex);
 
        if (error >= 0) {
            if (error > m_maxError) {
                // resulting error would be too high
                m_currentFacetPoints->resize(sizeBefore);
                // we leave the cell as is (in the EMPTY state)
                // so that we won't look at it again!
                addIgnoredCell(minTCellIndex);
            } else {
                m_currentFacetError = error;
 
                // add the cell to the "ACTIVE" set
                addActiveCell(minTCellIndex);
 
                // add its neighbors to the TRIAL set
                for (unsigned i = 0; i < m_numberOfNeighbours; ++i) {
                    // get neighbor cell
                    unsigned nIndex = minTCellIndex + m_neighboursIndexShift[i];
                    cloudViewer::FastMarching::Cell* nCell = m_theGrid[nIndex];
                    if (nCell) {
                        // if it' not yet a TRIAL cell
                        if (nCell->state == PlanarCell::FAR_CELL) {
                            nCell->T = computeT(nIndex);
                            addTrialCell(nIndex);
                        }
                        // otherwise we must update it's arrival time
                        else if (nCell->state == PlanarCell::TRIAL_CELL) {
                            const float& t_old = nCell->T;
                            float t_new = computeT(nIndex);
 
                            if (t_new < t_old) nCell->T = t_new;
                        }
                    }
                }
 
                m_propagateProgress +=
                        (m_currentFacetPoints->size() - sizeBefore);
                if (m_propagateProgressCb) {
                    m_propagateProgressCb->update(
                            (100.0f * m_propagateProgress) /
                            m_currentFacetPoints->getAssociatedCloud()->size());
                }
            }
        } else {
            // an error occurred
            return -1;
        }
    } else {
        addIgnoredCell(minTCellIndex);
    }
 
    return 1;
}
 
float FastMarchingForFacetExtraction::computeTCoefApprox(
        cloudViewer::FastMarching::Cell* originCell,
        cloudViewer::FastMarching::Cell* destCell) const {
    PlanarCell* oCell = static_cast<PlanarCell*>(originCell);
    PlanarCell* dCell = static_cast<PlanarCell*>(destCell);
 
    // compute the 'confidence' relatively to the neighbor cell
    // 1) it depends on the angle between the current cell's orientation
    //  and its neighbor's orientation (symmetric)
    // 2) it depends on whether the neighbor's relative position is
    //  compatible with the current cell orientation (symmetric)
    float orientationConfidence = 0;
    {
        CCVector3 AB = dCell->C - oCell->C;
        AB.normalize();
 
        float psOri = fabs(
                static_cast<float>(AB.dot(oCell->N)));  // ideal: 90 degrees
        float psDest = fabs(
                static_cast<float>(AB.dot(dCell->N)));  // ideal: 90 degrees
        orientationConfidence =
                (psOri + psDest) / 2;  // between 0 and 1 (ideal: 0)
    }
 
    // add reprojection error into balance
    if (m_useRetroProjectionError && m_octree && oCell->N.norm2() != 0) {
        PointCoordinateType theLSQPlaneEquation[4];
        theLSQPlaneEquation[0] = oCell->N.x;
        theLSQPlaneEquation[1] = oCell->N.y;
        theLSQPlaneEquation[2] = oCell->N.z;
        theLSQPlaneEquation[3] = oCell->C.dot(oCell->N);
 
        cloudViewer::ReferenceCloud Yk(m_octree->associatedCloud());
        if (m_octree->getPointsInCell(oCell->cellCode, m_gridLevel, &Yk,
                                      true)) {
            ScalarType reprojError = cloudViewer::DistanceComputationTools::
                    ComputeCloud2PlaneDistance(&Yk, theLSQPlaneEquation,
                                               m_errorMeasure);
            if (reprojError >= 0)
                return (1.0f - orientationConfidence) *
                       static_cast<float>(reprojError);
        }
    }
 
    return (1.0f - orientationConfidence) ;
}
 
int FastMarchingForFacetExtraction::propagate() {
    // init "TRIAL" set with seed's neighbors
    initTrialCells();
 
    int result = 1;
    while (result > 0) {
        result = step();
 
        if (result > 0 && m_propagateProgressCb &&
            m_propagateProgressCb->isCancelRequested()) {
            return -1;
        }
    }
 
    return result;
}
 
unsigned FastMarchingForFacetExtraction::updateFlagsTable(
        ccGenericPointCloud* theCloud,
        std::vector<unsigned char>& flags,
        unsigned facetIndex) {
    if (!m_initialized || !m_currentFacetPoints) return 0;
 
    unsigned pointCount = m_currentFacetPoints->size();
    for (unsigned k = 0; k < pointCount; ++k) {
        unsigned index = m_currentFacetPoints->getPointGlobalIndex(k);
        flags[index] = 1;
 
        theCloud->setPointScalarValue(index,
                                      static_cast<ScalarType>(facetIndex));
    }
 
    if (m_currentFacetPoints) {
        m_currentFacetPoints->clear(false);
    }
 
    // for (size_t i = 0; i < m_activeCells.size(); ++i)
    //{
    //  //we remove the processed cell so as to be sure not to consider them
    // again!   cloudViewer::FastMarching::Cell* cell =
    // m_theGrid[m_activeCells[i]];     m_theGrid[m_activeCells[i]] = 0;
    // if (cell)        delete cell;
    // }
 
    // unsigned pointCount = 0;
    cloudViewer::ReferenceCloud Yk(m_octree->associatedCloud());
    for (size_t i = 0; i < m_activeCells.size(); ++i) {
        PlanarCell* aCell =
                static_cast<PlanarCell*>(m_theGrid[m_activeCells[i]]);
        if (!m_octree->getPointsInCell(aCell->cellCode, m_gridLevel, &Yk, true))
            continue;
 
        for (unsigned k = 0; k < Yk.size(); ++k) {
            unsigned index = Yk.getPointGlobalIndex(k);
            assert(flags[index] == 1);
            // flags.setValue(index,1);
            //++pointCount;
        }
 
        m_theGrid[m_activeCells[i]] = 0;
        delete aCell;
    }
 
    return pointCount;
}
 
bool FastMarchingForFacetExtraction::setSeedCell(const Tuple3i& pos) {
    if (!cloudViewer::FastMarching::setSeedCell(pos)) {
        return false;
    }
 
    if (m_octree) {
        if (!m_currentFacetPoints) {
            m_currentFacetPoints = new cloudViewer::ReferenceCloud(
                    m_octree->associatedCloud());
        }
        assert(m_currentFacetPoints->size() == 0);
 
        unsigned index = pos2index(pos);
        m_currentFacetError = addCellToCurrentFacet(index);
        if (m_currentFacetError < 0)  // invalid error?
            return false;
 
        m_propagateProgress += m_currentFacetPoints->size();
    }
 
    return true;
}
 
ScalarType FastMarchingForFacetExtraction::addCellToCurrentFacet(
        unsigned index) {
    if (!m_currentFacetPoints || !m_initialized || !m_octree ||
        m_gridLevel > cloudViewer::DgmOctree::MAX_OCTREE_LEVEL)
        return -1;
 
    PlanarCell* cell = static_cast<PlanarCell*>(m_theGrid[index]);
    if (!cell) return -1;
 
    cloudViewer::ReferenceCloud Yk(m_octree->associatedCloud());
    if (!m_octree->getPointsInCell(cell->cellCode, m_gridLevel, &Yk, true))
        return -1;
 
    if (!m_currentFacetPoints->add(Yk)) {
        // not enough memory?
        return -1;
    }
 
    // update error
    CCVector3 N, C;
    ScalarType error;
    ComputeCellStats(m_currentFacetPoints, N, C, error, m_errorMeasure);
 
    return error;
}
 
void FastMarchingForFacetExtraction::initTrialCells() {
    // we expect at most one 'ACTIVE' cell (i.e. the current seed)
    size_t seedCount = m_activeCells.size();
    assert(seedCount <= 1);
 
    if (seedCount == 1 && m_currentFacetError <= m_maxError) {
        unsigned index = m_activeCells.front();
        PlanarCell* seedCell = static_cast<PlanarCell*>(m_theGrid[index]);
 
        assert(seedCell != nullptr);
        assert(seedCell->T == 0);
 
        // add all its neighbour cells to the TRIAL set
        for (unsigned i = 0; i < m_numberOfNeighbours; ++i) {
            unsigned nIndex = index + m_neighboursIndexShift[i];
            PlanarCell* nCell = (PlanarCell*)m_theGrid[nIndex];
            // if the neighbor exists (it shouldn't be in the TRIAL or ACTIVE
            // sets)
            if (nCell /* && nCell->state == PlanarCell::FAR_CELL*/) {
                assert(nCell->state == PlanarCell::FAR_CELL);
                addTrialCell(nIndex);
 
                // compute its approximate arrival time
                nCell->T = seedCell->T +
                           m_neighboursDistance[i] *
                                   computeTCoefApprox(seedCell, nCell);
            }
        }
    }
}
 
int FastMarchingForFacetExtraction::ExtractPlanarFacets(
        ccPointCloud* theCloud,
        unsigned char octreeLevel,
        ScalarType maxError,
        cloudViewer::DistanceComputationTools::ERROR_MEASURES errorMeasure,
        bool useRetroProjectionError /*=true*/,
        cloudViewer::GenericProgressCallback* progressCb /*=0*/,
        cloudViewer::DgmOctree* _theOctree /*=0*/) {
    assert(theCloud);
 
    unsigned numberOfPoints = theCloud->size();
    if (numberOfPoints == 0) return -1;
 
    if (!theCloud->getCurrentOutScalarField()) return -2;
 
    if (progressCb) {
        // just spawn the dialog so that we can see the
        // octree computation (in case the CPU charge prevents
        // the dialog from being shown)
        progressCb->start();
        QApplication::processEvents();
    }
 
    // we compute the octree if none is provided
    cloudViewer::DgmOctree* theOctree = _theOctree;
    QScopedPointer<cloudViewer::DgmOctree> tempOctree;
    if (!theOctree) {
        theOctree = new cloudViewer::DgmOctree(theCloud);
        tempOctree.reset(theOctree);
        if (theOctree->build(progressCb) < 1) {
            return -3;
        }
    }
 
    if (progressCb) {
        if (progressCb->textCanBeEdited()) {
            progressCb->setMethodTitle("Fast Marching for facets extraction");
            progressCb->setInfo("Initializing...");
        }
        progressCb->start();
        QApplication::processEvents();
    }
    if (!theCloud->enableScalarField()) {
        CVLog::Warning(
                "[FastMarchingForFacetExtraction] Couldn't enable scalar "
                "field! Not enough memory?");
        return -4;
    }
 
    // raz SF values
    {
        for (unsigned i = 0; i != theCloud->size(); ++i)
            theCloud->setPointScalarValue(i, 0);
    }
 
    // flags indicating if each point has been processed or not
    std::vector<unsigned char> flags;
    try {
        flags.resize(numberOfPoints, 0);  // defaultFlagValue = 0
    } catch (const std::bad_alloc&) {
        CVLog::Warning("[FastMarchingForFacetExtraction] Not enough memory!");
        return -5;
    }
 
    // Fast Marching propagation
    FastMarchingForFacetExtraction fm;
 
    int result = fm.init(theCloud, theOctree, octreeLevel, maxError,
                         errorMeasure, useRetroProjectionError, progressCb);
    if (result < 0) {
        CVLog::Error(
                "[FastMarchingForFacetExtraction] Something went wrong during "
                "initialization...");
        return -6;
    }
 
    // progress notification
    if (progressCb) {
        progressCb->update(0);
        if (progressCb->textCanBeEdited()) {
            progressCb->setMethodTitle("Facets extraction");
            progressCb->setInfo(
                    qPrintable(QString("Octree level: %1\nPoints: %2")
                                       .arg(octreeLevel)
                                       .arg(numberOfPoints)));
        }
        progressCb->start();
        QApplication::processEvents();
 
        fm.setPropagateCallback(progressCb);
    }
 
    const int octreeWidth = (1 << octreeLevel) - 1;
 
    // enable 26-connectivity mode
    // fm.setExtendedConnectivity(true);
 
    // while non-processed points remain...
    unsigned resolvedPoints = 0;
    int lastProcessedPoint = -1;
    unsigned facetIndex = 0;
    while (true) {
        // find the next non-processed point
        do {
            ++lastProcessedPoint;
        } while (lastProcessedPoint < static_cast<int>(numberOfPoints) &&
                 flags[lastProcessedPoint] != 0);
 
        // all points have been processed? Then we can stop.
        if (lastProcessedPoint == static_cast<int>(numberOfPoints)) {
            break;
        }
 
        // we start the propagation from this point
        //(from its corresponding cell in fact ;)
        const CCVector3* thePoint = theCloud->getPoint(lastProcessedPoint);
        Tuple3i pos;
        theOctree->getTheCellPosWhichIncludesThePoint(thePoint, pos,
                                                      octreeLevel);
 
        // clipping (in case the octree is not 'complete')
        pos.x = std::min(octreeWidth, pos.x);
        pos.y = std::min(octreeWidth, pos.y);
        pos.z = std::min(octreeWidth, pos.z);
 
        // set corresponding FM cell as 'seed'
        if (!fm.setSeedCell(pos)) {
            // an error occurred?!
            // result = -7;
            // break;
            continue;
        }
 
        // launch propagation
        int propagationResult = fm.propagate();
        if (propagationResult < 0) {
            // an error occurred or the process was cancelled by the user
            result = -7;
            break;
        }
 
        // compute the number of points processed during this pass
        unsigned count = fm.updateFlagsTable(theCloud, flags, ++facetIndex);
 
        if (count != 0) {
            resolvedPoints += count;
            // if (progressCb)
            //{
            //  if (progressCb->isCancelRequested())
            //  {
            //      result = -7;
            //      break;
            //  }
            //  progressCb->update((resolvedPoints * 100.0f) / numberOfPoints);
            // }
        }
 
        fm.cleanLastPropagation();
    }
 
    fm.setPropagateCallback(nullptr);
 
    if (progressCb) {
        progressCb->stop();
    }
 
    return result;
}