FastMarching.cpp

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#include "FastMarching.h"
 
// local
#include "DgmOctree.h"
 
using namespace cloudViewer;
 
FastMarching::FastMarching()
    : m_initialized(false),
      m_dx(0),
      m_dy(0),
      m_dz(0),
      m_rowSize(0),
      m_sliceSize(0),
      m_indexShift(0),
      m_gridSize(0),
      m_theGrid(nullptr),
      m_octree(nullptr),
      m_gridLevel(0),
      m_cellSize(1.0f),
      m_minFillIndexes(0, 0, 0),
      m_numberOfNeighbours(6) {
    memset(m_neighboursIndexShift, 0,
           sizeof(int) * CC_FM_MAX_NUMBER_OF_NEIGHBOURS);
    memset(m_neighboursDistance, 0,
           sizeof(float) * CC_FM_MAX_NUMBER_OF_NEIGHBOURS);
}
 
FastMarching::~FastMarching() {
    if (m_theGrid) {
        for (unsigned i = 0; i < m_gridSize; ++i) {
            if (m_theGrid[i]) {
                delete m_theGrid[i];
            }
        }
 
        delete[] m_theGrid;
        m_theGrid = nullptr;
    }
}
 
float FastMarching::getTime(Tuple3i& pos, bool absoluteCoordinates) const {
    unsigned index = 0;
 
    if (absoluteCoordinates) {
        index = pos2index(pos);
    } else {
        index = static_cast<unsigned>(pos.x + 1) +
                static_cast<unsigned>(pos.y + 1) * m_rowSize +
                static_cast<unsigned>(pos.z + 1) * m_sliceSize;
    }
 
    assert(m_theGrid[index]);
 
    return m_theGrid[index]->T;
}
 
int FastMarching::initGrid(float step, unsigned dim[3]) {
    m_octree = nullptr;
    m_gridLevel = 0;
    m_cellSize = step;
    m_minFillIndexes = Tuple3i(0, 0, 0);
 
    m_dx = dim[0];
    m_dy = dim[1];
    m_dz = dim[2];
 
    return initOther();
}
 
int FastMarching::initGridWithOctree(DgmOctree* octree,
                                     unsigned char gridLevel) {
    if (!octree || gridLevel > DgmOctree::MAX_OCTREE_LEVEL) return -2;
 
    const int* minFillIndexes = octree->getMinFillIndexes(gridLevel);
    const int* maxFillIndexes = octree->getMaxFillIndexes(gridLevel);
 
    m_octree = octree;
    m_gridLevel = gridLevel;
    m_cellSize = static_cast<float>(octree->getCellSize(gridLevel));
    m_minFillIndexes.x = minFillIndexes[0];
    m_minFillIndexes.y = minFillIndexes[1];
    m_minFillIndexes.z = minFillIndexes[2];
 
    m_dx = static_cast<unsigned>(maxFillIndexes[0] - minFillIndexes[0] + 1);
    m_dy = static_cast<unsigned>(maxFillIndexes[1] - minFillIndexes[1] + 1);
    m_dz = static_cast<unsigned>(maxFillIndexes[2] - minFillIndexes[2] + 1);
 
    return initOther();
}
 
int FastMarching::initOther() {
    m_rowSize = m_dx + 2;
    m_sliceSize = m_rowSize * (m_dy + 2);
    m_gridSize = m_sliceSize * (m_dz + 2);
    m_indexShift = 1 + m_rowSize + m_sliceSize;
 
    for (unsigned i = 0; i < CC_FM_MAX_NUMBER_OF_NEIGHBOURS; ++i) {
        m_neighboursIndexShift[i] = c_FastMarchingNeighbourPosShift[i * 3] +
                                    c_FastMarchingNeighbourPosShift[i * 3 + 1] *
                                            static_cast<int>(m_rowSize) +
                                    c_FastMarchingNeighbourPosShift[i * 3 + 2] *
                                            static_cast<int>(m_sliceSize);
 
        m_neighboursDistance[i] =
                sqrt(static_cast<float>(
                        c_FastMarchingNeighbourPosShift[i * 3] *
                                c_FastMarchingNeighbourPosShift[i * 3] +
                        c_FastMarchingNeighbourPosShift[i * 3 + 1] *
                                c_FastMarchingNeighbourPosShift[i * 3 + 1] +
                        c_FastMarchingNeighbourPosShift[i * 3 + 2] *
                                c_FastMarchingNeighbourPosShift[i * 3 + 2])) *
                m_cellSize;
    }
 
    m_activeCells.resize(0);
    m_trialCells.resize(0);
    m_ignoredCells.resize(0);
 
    if (!instantiateGrid(m_gridSize)) return -3;
 
    return 0;
}
 
void FastMarching::resetCells(std::vector<unsigned>& list) {
    for (std::vector<unsigned>::const_iterator it = list.begin();
         it != list.end(); ++it) {
        if (m_theGrid[*it]) {
            m_theGrid[*it]->state = Cell::FAR_CELL;
            m_theGrid[*it]->T = Cell::T_INF();
        }
    }
    list.clear();
}
 
void FastMarching::cleanLastPropagation() {
    // reset all cells state
    resetCells(m_activeCells);
    resetCells(m_trialCells);
    resetCells(m_ignoredCells);
}
 
bool FastMarching::setSeedCell(const Tuple3i& pos) {
    unsigned index = pos2index(pos);
 
    assert(index < m_gridSize);
 
    Cell* aCell = m_theGrid[index];
    assert(aCell);
 
    if (aCell && aCell->state != Cell::ACTIVE_CELL) {
        // we add the cell to the "ACTIVE" set
        aCell->T = 0;
        addActiveCell(index);
        return true;
    } else {
        return false;
    }
}
 
void FastMarching::initTrialCells() {
    for (std::size_t j = 0; j < m_activeCells.size(); ++j) {
        const unsigned& index = m_activeCells[j];
        Cell* aCell = m_theGrid[index];
 
        assert(aCell != nullptr);
 
        for (unsigned i = 0; i < m_numberOfNeighbours; ++i) {
            unsigned nIndex = index + m_neighboursIndexShift[i];
            Cell* nCell = m_theGrid[nIndex];
            // if the neighbor exists
            if (nCell) {
                // and if it's not already in the TRIAL set
                if (nCell->state == Cell::FAR_CELL) {
                    nCell->T = m_neighboursDistance[i] *
                               computeTCoefApprox(aCell, nCell);
                    addTrialCell(nIndex);
                }
            }
        }
    }
}
 
void FastMarching::addTrialCell(unsigned index) {
    m_theGrid[index]->state = Cell::TRIAL_CELL;
    m_trialCells.push_back(index);
}
 
void FastMarching::addActiveCell(unsigned index) {
    m_theGrid[index]->state = Cell::ACTIVE_CELL;
    m_activeCells.push_back(index);
}
 
void FastMarching::addIgnoredCell(unsigned index) {
    m_theGrid[index]->state = Cell::EMPTY_CELL;
    m_ignoredCells.push_back(index);
}
 
unsigned FastMarching::getNearestTrialCell() {
    if (m_trialCells.empty()) return 0;  // 0 = error
 
    // we look for the "TRIAL" cell with the minimum time (T)
    std::size_t minTCellIndexPos = 0;
    unsigned minTCellIndex = m_trialCells[minTCellIndexPos];
    cloudViewer::FastMarching::Cell* minTCell = m_theGrid[minTCellIndex];
    assert(minTCell != nullptr);
 
    for (std::size_t i = 1; i < m_trialCells.size(); ++i) {
        unsigned cellIndex = m_trialCells[i];
        cloudViewer::FastMarching::Cell* cell = m_theGrid[cellIndex];
        assert(cell != nullptr);
 
        if (cell->T < minTCell->T) {
            minTCellIndexPos = i;
            minTCellIndex = cellIndex;
            minTCell = cell;
        }
    }
 
    // we remove this cell from the TRIAL set
    m_trialCells[minTCellIndexPos] = m_trialCells.back();
    m_trialCells.pop_back();
 
    return minTCellIndex;
}
 
float FastMarching::computeT(unsigned index) {
    Cell* theCell = m_theGrid[index];
    if (!theCell) return Cell::T_INF();
 
    // arrival time FROM the neighbors
    double T[CC_FM_MAX_NUMBER_OF_NEIGHBOURS] = {0};
    {
        for (unsigned n = 0; n < m_numberOfNeighbours; ++n) {
            int nIndex = static_cast<int>(index) + m_neighboursIndexShift[n];
            Cell* nCell = m_theGrid[nIndex];
            if (nCell && (nCell->state == Cell::TRIAL_CELL ||
                          nCell->state == Cell::ACTIVE_CELL)) {
                // compute front arrival time
                T[n] = static_cast<double>(nCell->T) +
                       static_cast<double>(m_neighboursDistance[n]) *
                               static_cast<double>(
                                       computeTCoefApprox(nCell, theCell));
            } else {
                // no front yet
                T[n] = static_cast<double>(Cell::T_INF());
            }
        }
    }
 
    double A = 0, B = 0, C = 0;
    double Tij = static_cast<double>(theCell->T /*Cell::T_INF()*/);
 
    // Quadratic eq. along X
    {
        // look for the minimum arrival time from +/-X
        double Tmin = static_cast<double>(Cell::T_INF());
        for (unsigned n = 0; n < m_numberOfNeighbours; ++n)
            if (cloudViewer::c_FastMarchingNeighbourPosShift[n * 3] != 0)
                if (T[n] < Tmin) Tmin = T[n];
        if (Tij > Tmin) {
            A += 1.0;
            B += -2.0 * Tmin;
            C += Tmin * Tmin;
        }
    }
 
    // Quadratic eq. along Y
    {
        // look for the minimum arrival time from +/-Y
        double Tmin = static_cast<double>(Cell::T_INF());
        for (unsigned n = 0; n < m_numberOfNeighbours; ++n)
            if (cloudViewer::c_FastMarchingNeighbourPosShift[n * 3 + 1] != 0)
                if (T[n] < Tmin) Tmin = T[n];
        if (Tij > Tmin) {
            A += 1.0;
            B += -2.0 * Tmin;
            C += Tmin * Tmin;
        }
    }
 
    // Quadratic eq. along Z
    {
        // look for the minimum arrival time from +/-Z
        double Tmin = static_cast<double>(Cell::T_INF());
        for (unsigned n = 0; n < m_numberOfNeighbours; ++n)
            if (cloudViewer::c_FastMarchingNeighbourPosShift[n * 3 + 2] != 0)
                if (T[n] < Tmin) Tmin = T[n];
        if (Tij > Tmin) {
            A += 1.0;
            B += -2.0 * Tmin;
            C += Tmin * Tmin;
        }
    }
 
    // DGM: why?
    // C -=  static_cast<double>(m_cellSize*m_cellSize);
 
    // solve the quadratic equation
    double delta = B * B - 4.0 * A * C;
 
    // cases when the quadratic equation is singular
    if (A == 0 || delta < 0) {
        // take the 'earliest' neighbour
        Tij = T[0];
        for (unsigned n = 1; n < m_numberOfNeighbours; n++)
            if (T[n] < Tij) Tij = T[n];
    } else {
        // Note that the new crossing must be GREATER than the average
        // of the active neighbors, since only EARLIER elements are active.
        // Therefore the plus sign is appropriate.
        Tij = (-B + sqrt(delta)) / (2.0 * A);
    }
 
    return static_cast<float>(Tij);
}