FastMarchingForPropagation.cpp

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#include "FastMarchingForPropagation.h"
 
// local
#include "DgmOctree.h"
#include "ReferenceCloud.h"
#include "ScalarFieldTools.h"
 
using namespace cloudViewer;
 
FastMarchingForPropagation::FastMarchingForPropagation()
    : FastMarching(),
      m_jumpCoef(0)  // resistance a l'avancement du front, en fonction de
                     // Cell->f (ici, pas de resistance)
      ,
      m_detectionThreshold(
              Cell::T_INF())  // saut relatif de la valeur d'arrivee qui arrete
                              // la propagation (ici, "desactive")
{}
 
int FastMarchingForPropagation::init(GenericCloud* theCloud,
                                     DgmOctree* theOctree,
                                     unsigned char level,
                                     bool constantAcceleration /*=false*/) {
    assert(theCloud && theOctree);
 
    int result = initGridWithOctree(theOctree, level);
    if (result < 0) return result;
 
    // on remplit la grille
    DgmOctree::cellCodesContainer cellCodes;
    theOctree->getCellCodes(level, cellCodes, true);
 
    ReferenceCloud Yk(theOctree->associatedCloud());
 
    while (!cellCodes.empty()) {
        if (!theOctree->getPointsInCell(cellCodes.back(), level, &Yk, true)) {
            // not enough memory?
            return -1;
        }
 
        // on transforme le code de cellule en position
        Tuple3i cellPos;
        theOctree->getCellPos(cellCodes.back(), level, cellPos, true);
 
        // on renseigne la grille
        unsigned gridPos = pos2index(cellPos);
 
        PropagationCell* aCell = new PropagationCell;
        aCell->cellCode = cellCodes.back();
        aCell->f = (constantAcceleration
                            ? 1.0f
                            : static_cast<float>(
                                      ScalarFieldTools::computeMeanScalarValue(
                                              &Yk)));
 
        m_theGrid[gridPos] = aCell;
 
        cellCodes.pop_back();
    }
 
    m_initialized = true;
 
    return 0;
}
 
int FastMarchingForPropagation::step() {
    if (!m_initialized) return -1;
 
    unsigned minTCellIndex = getNearestTrialCell();
    if (minTCellIndex == 0) {
        // fl_alert("No more trial cells !");
        return 0;
    }
 
    Cell* minTCell = m_theGrid[minTCellIndex];
    assert(minTCell != nullptr);
 
    // last arrival time
    float lastT =
            (m_activeCells.empty() ? 0 : m_theGrid[m_activeCells.back()]->T);
 
    if (minTCell->T - lastT > m_detectionThreshold * m_cellSize) {
        // reset();
        return 0;
    }
 
    assert(minTCell->state != Cell::ACTIVE_CELL);
 
    if (minTCell->T < Cell::T_INF()) {
        // we add this cell to the "ACTIVE" set
        addActiveCell(minTCellIndex);
 
        assert(minTCell->T >= lastT);
 
        // add its neighbors to the TRIAL set
        Cell* nCell;
        for (unsigned i = 0; i < m_numberOfNeighbours; ++i) {
            // get neighbor cell
            unsigned nIndex = minTCellIndex + m_neighboursIndexShift[i];
            nCell = m_theGrid[nIndex];
            if (nCell) {
                // if it' not yet a TRIAL cell
                if (nCell->state == Cell::FAR_CELL) {
                    nCell->T = computeT(nIndex);
                    addTrialCell(nIndex);
                } else if (nCell->state == Cell::TRIAL_CELL)
                // otherwise we must update it's arrival time
                {
                    float t_old = nCell->T;
                    float t_new = computeT(nIndex);
 
                    if (t_new < t_old) nCell->T = t_new;
                }
            }
        }
    } else {
        addIgnoredCell(minTCellIndex);
    }
 
    return 1;
}
 
int FastMarchingForPropagation::propagate() {
    initTrialCells();
 
    int result = 1;
    while (result > 0) {
        result = step();
    }
 
    return result;
}
 
bool FastMarchingForPropagation::extractPropagatedPoints(
        ReferenceCloud* points) {
    if (!m_initialized || !m_octree ||
        m_gridLevel > DgmOctree::MAX_OCTREE_LEVEL || !points)
        return false;
 
    points->clear();
 
    for (unsigned i = 0; i < m_activeCells.size(); ++i) {
        PropagationCell* aCell =
                static_cast<PropagationCell*>(m_theGrid[m_activeCells[i]]);
        if (!m_octree->getPointsInCell(aCell->cellCode, m_gridLevel, points,
                                       true, false))
            return false;
    }
 
    // raz de la norme du gradient du point, pour qu'il ne soit plus pris en
    // compte par la suite !
    points->placeIteratorAtBeginning();
    for (unsigned k = 0; k < points->size(); ++k) {
        points->setCurrentPointScalarValue(NAN_VALUE);
        points->forwardIterator();
    }
 
    return true;
}
 
bool FastMarchingForPropagation::setPropagationTimingsAsDistances() {
    if (!m_initialized || !m_octree ||
        m_gridLevel > DgmOctree::MAX_OCTREE_LEVEL)
        return false;
 
    ReferenceCloud Yk(m_octree->associatedCloud());
 
    for (unsigned i = 0; i < m_activeCells.size(); ++i) {
        PropagationCell* aCell =
                static_cast<PropagationCell*>(m_theGrid[m_activeCells[i]]);
 
        if (!m_octree->getPointsInCell(aCell->cellCode, m_gridLevel, &Yk,
                                       true)) {
            // not enough memory?
            return false;
        }
 
        Yk.placeIteratorAtBeginning();
        for (unsigned k = 0; k < Yk.size(); ++k) {
            Yk.setCurrentPointScalarValue(aCell->T);
            Yk.forwardIterator();
        }
        // Yk.clear(); //useless
    }
 
    return true;
}
 
#ifdef _MSC_VER
// Visual 2012 (and previous versions) don't know expm1
#if _MSC_VER <= 1700
 
// Compute exp(x) - 1 without loss of precision for small values of x.
template <typename T>
T expm1(T x) {
    if (std::abs(x) < 1e-5)
        return x + (x * x) / 2;
    else
        return exp(x) - 1;
}
 
#endif
#endif
 
float FastMarchingForPropagation::computeTCoefApprox(
        Cell* currentCell, Cell* neighbourCell) const {
    PropagationCell* cCell = static_cast<PropagationCell*>(currentCell);
    PropagationCell* nCell = static_cast<PropagationCell*>(neighbourCell);
    return expm1(m_jumpCoef * (cCell->f - nCell->f));
}
 
void FastMarchingForPropagation::findPeaks() {
    if (!m_initialized) return;
 
    // on fait bien attention a ne pas initialiser les cellules sur les bords
    for (unsigned k = 0; k < m_dz; ++k) {
        int pos[3] = {0, 0, static_cast<int>(k)};
 
        for (unsigned j = 0; j < m_dy; ++j) {
            pos[1] = static_cast<int>(j);
 
            for (unsigned i = 0; i < m_dx; ++i) {
                pos[0] = static_cast<int>(i);
 
                unsigned index =
                        static_cast<unsigned>(pos[0] + 1) +
                        static_cast<unsigned>(pos[1] + 1) * m_rowSize +
                        static_cast<unsigned>(pos[2] + 1) * m_sliceSize;
 
                PropagationCell* theCell =
                        reinterpret_cast<PropagationCell*>(m_theGrid[index]);
 
                if (theCell) {
                    bool isMin = true;
                    bool isMax = true;
 
                    // theCell->state = ACTIVE_CELL;
 
                    for (unsigned n = 0; n < CC_FM_MAX_NUMBER_OF_NEIGHBOURS;
                         ++n) {
                        const PropagationCell* nCell =
                                reinterpret_cast<const PropagationCell*>(
                                        m_theGrid[index +
                                                  m_neighboursIndexShift[n]]);
                        if (nCell) {
                            if (nCell->f > theCell->f)
                                isMax = false;
                            else if (nCell->f < theCell->f)
                                isMin = false;
                        }
                    }
 
                    if (isMin != isMax) {
                        // if (isMin)
                        //  theCell->T = 1.0;
                        // else
                        //  theCell->T = 2.0;
 
                        if (isMax) {
                            theCell->T = 0;
                            addActiveCell(index);
                        }
                    }
                    // else theCell->T = 0;
                }
            }
        }
    }
}