// This file is part of the AliceVision project.

// Copyright (c) 2017 AliceVision contributors.

// This Source Code Form is subject to the terms of the Mozilla Public License,

// v. 2.0. If a copy of the MPL was not distributed with this file,

// You can obtain one at https://mozilla.org/MPL/2.0/.

// To analyse history of intersected geometries during vote

// #define ALICEVISION_DEBUG_VOTE

#include "DelaunayGraphCut.hpp"

// #include <aliceVision/fuseCut/MaxFlow_CSR.hpp>

#include <aliceVision/fuseCut/MaxFlow_AdjList.hpp>

#include <aliceVision/sfmData/SfMData.hpp>

#include <aliceVision/mvsData/geometry.hpp>

#include <aliceVision/mvsData/jetColorMap.hpp>

#include <aliceVision/mvsData/Pixel.hpp>

#include <aliceVision/mvsData/Point2d.hpp>

#include <aliceVision/mvsData/Universe.hpp>

#include <aliceVision/mvsUtils/fileIO.hpp>

#include <aliceVision/mvsData/imageIO.hpp>

#include <aliceVision/mvsData/imageAlgo.hpp>

#include <aliceVision/alicevision_omp.hpp>

#include "nanoflann.hpp"

#include <geogram/points/kd_tree.h>

#include <boost/filesystem.hpp>

#include <boost/filesystem/operations.hpp>

#include <random>

#include <stdexcept>

#include <boost/math/constants/constants.hpp>

#include <boost/accumulators/accumulators.hpp>

#include <boost/accumulators/statistics.hpp>

#include <boost/progress.hpp>

namespace aliceVision {

namespace fuseCut {

namespace bfs = boost::filesystem;

// #define USE_GEOGRAM_KDTREE 1

#ifdef USE_GEOGRAM_KDTREE

#else

// static const std::size_t MAX_LEAF_ELEMENTS = 64;

static const std::size_t MAX_LEAF_ELEMENTS = 10;

struct PointVectorAdaptator

{

using Derived = PointVectorAdaptator; //!< In this case the dataset class is myself.

using T = double;

const std::vector<Point3d>& _data;

PointVectorAdaptator(const std::vector<Point3d>& data)

: _data(data)

{}

/// CRTP helper method

inline const Derived& derived() const { return *static_cast<const Derived*>(this); }

/// CRTP helper method

inline Derived& derived() { return *static_cast<Derived*>(this); }

// Must return the number of data points

inline size_t kdtree_get_point_count() const { return _data.size(); }

// Returns the dim'th component of the idx'th point in the class:

// Since this is inlined and the "dim" argument is typically an immediate value, the

// "if/else's" are actually solved at compile time.

inline T kdtree_get_pt(const size_t idx, int dim) const

{

return _data.at(idx).m[dim];

}

// Optional bounding-box computation: return false to default to a standard bbox computation loop.

// Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.

// Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)

template <class BBOX>

bool kdtree_get_bbox(BBOX &bb) const { return false; }

};

typedef nanoflann::KDTreeSingleIndexAdaptor<

nanoflann::L2_Simple_Adaptor<double, PointVectorAdaptator>,

PointVectorAdaptator,

3 /* dim */

> KdTree;

/**

* A result-set class used when performing a radius based search.

*/

template <typename DistanceType, typename IndexType = size_t>

class SmallerPixSizeInRadius

{

public:

const DistanceType radius;

const std::vector<double>& m_pixSizePrepare;

const std::vector<float>& m_simScorePrepare;

size_t m_result = 0;

const int m_i;

bool found = false;

inline SmallerPixSizeInRadius(DistanceType radius_,

const std::vector<double>& pixSizePrepare,

const std::vector<float>& simScorePrepare,

int i)

: radius(radius_)

, m_pixSizePrepare(pixSizePrepare)

, m_simScorePrepare(simScorePrepare)

, m_i(i)

{

init();

}

inline void init() { clear(); }

inline void clear() { m_result = 0; }

inline size_t size() const { return m_result; }

inline bool full() const { return found; }

/**

* Called during search to add an element matching the criteria.

* @return true if the search should be continued, false if the results are sufficient

*/

inline bool addPoint(DistanceType dist, IndexType index)

{

if(dist < radius)

{

++m_result;

if(m_simScorePrepare[index] * m_pixSizePrepare[index] * m_pixSizePrepare[index] < m_simScorePrepare[m_i] * m_pixSizePrepare[m_i] * m_pixSizePrepare[m_i])

{

found = true;

return false;

}

}

return true;

}

inline DistanceType worstDist() const { return radius; }

};

#endif

/// Filter by pixSize

void filterByPixSize(const std::vector<Point3d>& verticesCoordsPrepare, std::vector<double>& pixSizePrepare, double pixSizeMarginCoef, std::vector<float>& simScorePrepare)

{

#ifdef USE_GEOGRAM_KDTREE

ALICEVISION_LOG_INFO("Build geogram KdTree index.");

GEO::AdaptiveKdTree kdTree(3);

kdTree.set_exact(false);

kdTree.set_points(verticesCoordsPrepare.size(), verticesCoordsPrepare[0].m);

#else

ALICEVISION_LOG_INFO("Build nanoflann KdTree index.");

PointVectorAdaptator pointCloudRef(verticesCoordsPrepare);

KdTree kdTree(3 /*dim*/, pointCloudRef, nanoflann::KDTreeSingleIndexAdaptorParams(MAX_LEAF_ELEMENTS));

kdTree.buildIndex();

#endif

ALICEVISION_LOG_INFO("KdTree created for " << verticesCoordsPrepare.size() << " points.");

#pragma omp parallel for

for(int vIndex = 0; vIndex < verticesCoordsPrepare.size(); ++vIndex)

{

if(pixSizePrepare[vIndex] == -1.0)

{

continue;

}

const double pixSizeScore = pixSizeMarginCoef * simScorePrepare[vIndex] * pixSizePrepare[vIndex] * pixSizePrepare[vIndex];

if(pixSizeScore < std::numeric_limits<double>::epsilon())

{

pixSizePrepare[vIndex] = -1.0;

continue;

}

#ifdef USE_GEOGRAM_KDTREE

static const std::size_t nbNeighbors = 20;

static const double nbNeighborsInv = 1.0 / (double)nbNeighbors;

std::array<GEO::index_t, nbNeighbors> nnIndex;

std::array<double, nbNeighbors> sqDist;

// kdTree.get_nearest_neighbors(nbNeighbors, verticesCoordsPrepare[i].m, &nnIndex.front(), &sqDist.front());

kdTree.get_nearest_neighbors(nbNeighbors, vIndex, &nnIndex.front(), &sqDist.front());

for(std::size_t n = 0; n < nbNeighbors; ++n)

{

// NOTE: we don't need to test the distance regarding pixSizePrepare[nnIndex[vIndex]]

// as we kill ourself only if our pixSize is bigger

if(sqDist[n] < pixSizeScore)

{

if(pixSizePrepare[nnIndex[n]] < pixSizePrepare[vIndex] ||

(pixSizePrepare[nnIndex[n]] == pixSizePrepare[vIndex] && nnIndex[n] < vIndex)

)

{

// Kill itself if inside our volume (defined by marginCoef*pixSize) there is another point with a smaller pixSize

pixSizePrepare[vIndex] = -1.0;

break;

}

}

// else

// {

// break;

// }

}

#else

static const nanoflann::SearchParams searchParams(32, 0, false); // false: dont need to sort

SmallerPixSizeInRadius<double, std::size_t> resultSet(pixSizeScore, pixSizePrepare, simScorePrepare, vIndex);

kdTree.findNeighbors(resultSet, verticesCoordsPrepare[vIndex].m, searchParams);

if(resultSet.found)

pixSizePrepare[vIndex] = -1.0;

#endif

}

ALICEVISION_LOG_INFO("Filtering done.");

}

/// Remove invalid points based on invalid pixSize

void removeInvalidPoints(std::vector<Point3d>& verticesCoordsPrepare, std::vector<double>& pixSizePrepare, std::vector<float>& simScorePrepare)

{

std::vector<Point3d> verticesCoordsTmp;

verticesCoordsTmp.reserve(verticesCoordsPrepare.size());

std::vector<double> pixSizeTmp;

pixSizeTmp.reserve(pixSizePrepare.size());

std::vector<float> simScoreTmp;

simScoreTmp.reserve(simScorePrepare.size());

for(int i = 0; i < verticesCoordsPrepare.size(); ++i)

{

if(pixSizePrepare[i] != -1.0)

{

verticesCoordsTmp.push_back(verticesCoordsPrepare[i]);

pixSizeTmp.push_back(pixSizePrepare[i]);

simScoreTmp.push_back(simScorePrepare[i]);

}

}

ALICEVISION_LOG_INFO((verticesCoordsPrepare.size() - verticesCoordsTmp.size()) << " invalid points removed.");

verticesCoordsPrepare.swap(verticesCoordsTmp);

pixSizePrepare.swap(pixSizeTmp);

simScorePrepare.swap(simScoreTmp);

}

void removeInvalidPoints(std::vector<Point3d>& verticesCoordsPrepare, std::vector<double>& pixSizePrepare, std::vector<float>& simScorePrepare, std::vector<GC_vertexInfo>& verticesAttrPrepare)

{

std::vector<Point3d> verticesCoordsTmp;

verticesCoordsTmp.reserve(verticesCoordsPrepare.size());

std::vector<double> pixSizeTmp;

pixSizeTmp.reserve(pixSizePrepare.size());

std::vector<float> simScoreTmp;

simScoreTmp.reserve(simScorePrepare.size());

std::vector<GC_vertexInfo> verticesAttrTmp;

verticesAttrTmp.reserve(verticesAttrPrepare.size());

for(int i = 0; i < verticesCoordsPrepare.size(); ++i)

{

if(pixSizePrepare[i] != -1.0)

{

verticesCoordsTmp.push_back(verticesCoordsPrepare[i]);

pixSizeTmp.push_back(pixSizePrepare[i]);

simScoreTmp.push_back(simScorePrepare[i]);

verticesAttrTmp.push_back(verticesAttrPrepare[i]);

}

}

ALICEVISION_LOG_INFO((verticesCoordsPrepare.size() - verticesCoordsTmp.size()) << " invalid points removed.");

verticesCoordsPrepare.swap(verticesCoordsTmp);

pixSizePrepare.swap(pixSizeTmp);

simScorePrepare.swap(simScoreTmp);

verticesAttrPrepare.swap(verticesAttrTmp);

}

void createVerticesWithVisibilities(const StaticVector<int>& cams, std::vector<Point3d>& verticesCoordsPrepare, std::vector<double>& pixSizePrepare, std::vector<float>& simScorePrepare,

std::vector<GC_vertexInfo>& verticesAttrPrepare, mvsUtils::MultiViewParams* mp, float simFactor, float voteMarginFactor, float contributeMarginFactor, float simGaussianSize)

{

#ifdef USE_GEOGRAM_KDTREE

GEO::AdaptiveKdTree kdTree(3);

kdTree.set_points(verticesCoordsPrepare.size(), verticesCoordsPrepare[0].m);

ALICEVISION_LOG_INFO("GEOGRAM: KdTree created");

#else

PointVectorAdaptator pointCloudRef(verticesCoordsPrepare);

KdTree kdTree(3 /*dim*/, pointCloudRef, nanoflann::KDTreeSingleIndexAdaptorParams(MAX_LEAF_ELEMENTS));

kdTree.buildIndex();

ALICEVISION_LOG_INFO("NANOFLANN: KdTree created.");

#endif

// TODO FACA: update into new data structures

// std::vector<Point3d> newVerticesCoordsPrepare(verticesCoordsPrepare.size());

// std::vector<float> newSimScorePrepare(simScorePrepare.size());

// std::vector<double> newPixSizePrepare(pixSizePrepare.size());

std::vector<omp_lock_t> locks(verticesCoordsPrepare.size());

for (auto& lock: locks)

omp_init_lock(&lock);

omp_set_nested(1);

#pragma omp parallel for num_threads(3)

for(int c = 0; c < cams.size(); ++c)

{

ALICEVISION_LOG_INFO("Create visibilities (" << c << "/" << cams.size() << ")");

std::vector<float> depthMap;

std::vector<float> simMap;

int width, height;

{

const std::string depthMapFilepath = getFileNameFromIndex(mp, c, mvsUtils::EFileType::depthMap, 0);

imageIO::readImage(depthMapFilepath, width, height, depthMap, imageIO::EImageColorSpace::NO_CONVERSION);

if(depthMap.empty())

{

ALICEVISION_LOG_WARNING("Empty depth map: " << depthMapFilepath);

continue;

}

int wTmp, hTmp;

const std::string simMapFilepath = getFileNameFromIndex(mp, c, mvsUtils::EFileType::simMap, 0);

// If we have a simMap in input use it,

// else init with a constant value.

if(boost::filesystem::exists(simMapFilepath))

{

imageIO::readImage(simMapFilepath, wTmp, hTmp, simMap, imageIO::EImageColorSpace::NO_CONVERSION);

if(wTmp != width || hTmp != height)

throw std::runtime_error("Similarity map size doesn't match the depth map size: " + simMapFilepath +

", " + depthMapFilepath);

{

std::vector<float> simMapTmp(simMap.size());

imageAlgo::convolveImage(width, height, simMap, simMapTmp, "gaussian", simGaussianSize,

simGaussianSize);

simMap.swap(simMapTmp);

}

}

else

{

ALICEVISION_LOG_WARNING("simMap file can't be found.");

simMap.resize(width * height, -1);

}

}

// Add visibility

#pragma omp parallel for

for(int y = 0; y < height; ++y)

{

for(int x = 0; x < width; ++x)

{

const std::size_t index = y * width + x;

const float depth = depthMap[index];

if(depth <= 0.0f)

continue;

const Point3d p = mp->backproject(c, Point2d(x, y), depth);

const double pixSize = mp->getCamPixelSize(p, c);

#ifdef USE_GEOGRAM_KDTREE

const std::size_t nearestVertexIndex = kdTree.get_nearest_neighbor(p.m);

// NOTE: Could compute the distance between the line (camera to pixel) and the nearestVertex OR

// the distance between the back-projected point and the nearestVertex

const double dist = (p - verticesCoordsPrepare[nearestVertexIndex]).size2();

#else

nanoflann::KNNResultSet<double, std::size_t> resultSet(1);

std::size_t nearestVertexIndex = std::numeric_limits<std::size_t>::max();

double dist = std::numeric_limits<double>::max();

resultSet.init(&nearestVertexIndex, &dist);

if(!kdTree.findNeighbors(resultSet, p.m, nanoflann::SearchParams()))

{

ALICEVISION_LOG_TRACE("Failed to find Neighbors.");

continue;

}

#endif

const float pixSizeScoreI = simScorePrepare[nearestVertexIndex] * pixSize * pixSize;

const float pixSizeScoreV = simScorePrepare[nearestVertexIndex] * pixSizePrepare[nearestVertexIndex] * pixSizePrepare[nearestVertexIndex];

if(dist < voteMarginFactor * std::max(pixSizeScoreI, pixSizeScoreV))

{

GC_vertexInfo& va = verticesAttrPrepare[nearestVertexIndex];

Point3d& vc = verticesCoordsPrepare[nearestVertexIndex];

const float simValue = simMap[index];

// remap similarity values from [-1;+1] to [+1;+simFactor]

// interpretation is [goodSimilarity;badSimilarity]

const float simScore = simValue < -1.0f ? 1.0f : 1.0f + (1.0f + simValue) * simFactor;

// Custom locks to limit it to the index: nearestVertexIndex

// to avoid using "omp critical"

omp_lock_t* lock = &locks[nearestVertexIndex];

omp_set_lock(lock);

{

va.cams.push_back_distinct(c);

if(dist < contributeMarginFactor * pixSizeScoreV)

{

vc = (vc * (double)va.nrc + p) / double(va.nrc + 1);

// newVerticesCoordsPrepare[nearestVertexIndex] = (newVerticesCoordsPrepare[nearestVertexIndex] * double(va.nrc) + p) / double(va.nrc + 1);

// newSimScorePrepare[nearestVertexIndex] = (newSimScorePrepare[nearestVertexIndex] * float(va.nrc) + simScore) / float(va.nrc + 1);

// newPixSizePrepare[nearestVertexIndex] = (newPixSizePrepare[nearestVertexIndex] * double(va.nrc) + pixSize) / double(va.nrc + 1);

va.nrc += 1;

}

}

omp_unset_lock(lock);

}

}

}

}

omp_set_nested(0);

for(auto& lock: locks)

omp_destroy_lock(&lock);

// compute pixSize

#pragma omp parallel for

for(int vi = 0; vi < verticesAttrPrepare.size(); ++vi)

{

GC_vertexInfo& v = verticesAttrPrepare[vi];

v.pixSize = mp->getCamsMinPixelSize(verticesCoordsPrepare[vi], v.cams);

}

// verticesCoordsPrepare.swap(newVerticesCoordsPrepare);

// simScorePrepare.swap(newSimScorePrepare);

// pixSizePrepare.swap(newPixSizePrepare);

ALICEVISION_LOG_INFO("Visibilities created.");

}

void DelaunayGraphCut::IntersectionHistory::append(const GeometryIntersection& geom, const Point3d& intersectPt)

{

++steps;

geometries.push_back(geom);

intersectPts.push_back(intersectPt);

const Point3d toCam = cam - intersectPt;

vecToCam.push_back(toCam);

distToCam.push_back(toCam.size());

angleToCam.push_back(angleBetwV1andV2(dirVect, intersectPt - originPt));

}

DelaunayGraphCut::DelaunayGraphCut(mvsUtils::MultiViewParams* _mp)

{

mp = _mp;

_camsVertexes.resize(mp->ncams, -1);

saveTemporaryBinFiles = mp->userParams.get<bool>("LargeScale.saveTemporaryBinFiles", false);

GEO::initialize();

_tetrahedralization = GEO::Delaunay::create(3, "BDEL");

// _tetrahedralization->set_keeps_infinite(true);

_tetrahedralization->set_stores_neighbors(true);

// _tetrahedralization->set_stores_cicl(true);

}

DelaunayGraphCut::~DelaunayGraphCut()

{

}

void DelaunayGraphCut::saveDhInfo(const std::string& fileNameInfo)

{

FILE* f = fopen(fileNameInfo.c_str(), "wb");

int npts = getNbVertices();

fwrite(&npts, sizeof(int), 1, f);

for(const GC_vertexInfo& v: _verticesAttr)

{

v.fwriteinfo(f);

}

int ncells = _cellsAttr.size();

fwrite(&ncells, sizeof(int), 1, f);

for(const GC_cellInfo& c: _cellsAttr)

{

c.fwriteinfo(f);

}

fclose(f);

}

void DelaunayGraphCut::saveDh(const std::string& fileNameDh, const std::string& fileNameInfo)

{

ALICEVISION_LOG_DEBUG("Saving triangulation.");

saveDhInfo(fileNameInfo);

long t1 = clock();

// std::ofstream oFileT(fileNameDh.c_str());

// oFileT << *_tetrahedralization; // TODO GEOGRAM

mvsUtils::printfElapsedTime(t1);

}

std::vector<DelaunayGraphCut::CellIndex> DelaunayGraphCut::getNeighboringCellsByGeometry(const GeometryIntersection& g) const

{

switch (g.type)

{

case EGeometryType::Edge:

return getNeighboringCellsByEdge(g.edge);

case EGeometryType::Vertex:

return getNeighboringCellsByVertexIndex(g.vertexIndex);

case EGeometryType::Facet:

return getNeighboringCellsByFacet(g.facet);

case EGeometryType::None:

break;

}

throw std::runtime_error("[error] getNeighboringCellsByGeometry: an undefined/None geometry has no neighboring cells.");

}

std::vector<DelaunayGraphCut::CellIndex> DelaunayGraphCut::getNeighboringCellsByFacet(const Facet& f) const

{

std::vector<CellIndex> neighboringCells;

neighboringCells.push_back(f.cellIndex);

const Facet mFacet = mirrorFacet(f);

if(!isInvalidOrInfiniteCell(mFacet.cellIndex))

neighboringCells.push_back(mFacet.cellIndex);

return neighboringCells;

}

std::vector<DelaunayGraphCut::CellIndex> DelaunayGraphCut::getNeighboringCellsByEdge(const Edge& e) const

{

const std::vector<CellIndex>& v0ci = getNeighboringCellsByVertexIndex(e.v0);

const std::vector<CellIndex>& v1ci = getNeighboringCellsByVertexIndex(e.v1);

std::vector<CellIndex> neighboringCells;

std::set_intersection(v0ci.begin(), v0ci.end(), v1ci.begin(), v1ci.end(), std::back_inserter(neighboringCells));

return neighboringCells;

}

void DelaunayGraphCut::computeDelaunay()

{

ALICEVISION_LOG_DEBUG("computeDelaunay GEOGRAM ...\n");

assert(_verticesCoords.size() == _verticesAttr.size());

long tall = clock();

_tetrahedralization->set_vertices(_verticesCoords.size(), _verticesCoords.front().m);

mvsUtils::printfElapsedTime(tall, "GEOGRAM Delaunay tetrahedralization ");

initCells();

updateVertexToCellsCache();

ALICEVISION_LOG_DEBUG("computeDelaunay done\n");

}

void DelaunayGraphCut::initCells()

{

_cellsAttr.resize(_tetrahedralization->nb_cells()); // or nb_finite_cells() if keeps_infinite()

ALICEVISION_LOG_INFO(_cellsAttr.size() << " cells created by tetrahedralization.");

for(int i = 0; i < _cellsAttr.size(); ++i)

{

GC_cellInfo& c = _cellsAttr[i];

c.cellSWeight = 0.0f;

c.cellTWeight = 0.0f;

c.on = 0.0f;

c.fullnessScore = 0.0f;

c.emptinessScore = 0.0f;

for(int s = 0; s < 4; ++s)

{

c.gEdgeVisWeight[s] = 0.0f; // weights for the 4 faces of the tetrahedron

}

}

ALICEVISION_LOG_DEBUG("initCells [" << _tetrahedralization->nb_cells() << "] done");

}

void DelaunayGraphCut::displayStatistics()

{

// Display some statistics

StaticVector<int>* ptsCamsHist = getPtsCamsHist();

ALICEVISION_LOG_TRACE("Histogram of number of cams per point:");

for(int i = 0; i < ptsCamsHist->size(); ++i)

ALICEVISION_LOG_TRACE(" " << i << ": " << mvsUtils::num2str((*ptsCamsHist)[i]));

delete ptsCamsHist;

/*

StaticVector<int>* ptsNrcsHist = getPtsNrcHist();

ALICEVISION_LOG_TRACE("Histogram of Nrc per point:");

for(int i = 0; i < ptsNrcsHist->size(); ++i)

ALICEVISION_LOG_TRACE(" " << i << ": " << mvsUtils::num2str((*ptsNrcsHist)[i]));

delete ptsNrcsHist;

*/

}

StaticVector<StaticVector<int>*>* DelaunayGraphCut::createPtsCams()

{

long t = std::clock();

ALICEVISION_LOG_INFO("Extract visibilities.");

int npts = getNbVertices();

StaticVector<StaticVector<int>*>* out = new StaticVector<StaticVector<int>*>();

out->reserve(npts);

for(const GC_vertexInfo& v: _verticesAttr)

{

StaticVector<int>* cams = new StaticVector<int>();

cams->reserve(v.getNbCameras());

for(int c = 0; c < v.getNbCameras(); c++)

{

cams->push_back(v.cams[c]);

}

out->push_back(cams);

} // for i

ALICEVISION_LOG_INFO("Extract visibilities done.");

mvsUtils::printfElapsedTime(t, "Extract visibilities ");

return out;

}

void DelaunayGraphCut::createPtsCams(StaticVector<StaticVector<int>>& out_ptsCams)

{

long t = std::clock();

ALICEVISION_LOG_INFO("Extract visibilities.");

int npts = getNbVertices();

out_ptsCams.reserve(npts);

for(const GC_vertexInfo& v: _verticesAttr)

{

StaticVector<int> cams;

cams.reserve(v.getNbCameras());

for(int c = 0; c < v.getNbCameras(); c++)

{

cams.push_back(v.cams[c]);

}

out_ptsCams.push_back(cams);

} // for i

ALICEVISION_LOG_INFO("Extract visibilities done.");

mvsUtils::printfElapsedTime(t, "Extract visibilities ");

}

StaticVector<int>* DelaunayGraphCut::getPtsCamsHist()

{

int maxnCams = 0;

for(const GC_vertexInfo& v: _verticesAttr)

{

maxnCams = std::max(maxnCams, (int)v.getNbCameras());

}

maxnCams++;

ALICEVISION_LOG_DEBUG("maxnCams: " << maxnCams);

StaticVector<int>* ncamsHist = new StaticVector<int>();

ncamsHist->reserve(maxnCams);

ncamsHist->resize_with(maxnCams, 0);

for(const GC_vertexInfo& v: _verticesAttr)

{

(*ncamsHist)[v.getNbCameras()] += 1;

}

return ncamsHist;

}

StaticVector<int>* DelaunayGraphCut::getPtsNrcHist()

{

int maxnnrcs = 0;

for(const GC_vertexInfo& v: _verticesAttr)

{

maxnnrcs = std::max(maxnnrcs, v.nrc);

}

maxnnrcs++;

ALICEVISION_LOG_DEBUG("maxnnrcs before clamp: " << maxnnrcs);

maxnnrcs = std::min(1000, maxnnrcs);

ALICEVISION_LOG_DEBUG("maxnnrcs: " << maxnnrcs);

StaticVector<int>* nnrcsHist = new StaticVector<int>();

nnrcsHist->reserve(maxnnrcs);

nnrcsHist->resize_with(maxnnrcs, 0);

for(const GC_vertexInfo& v: _verticesAttr)

{

if(v.nrc < nnrcsHist->size())

{

(*nnrcsHist)[v.nrc] += 1;

}

}

return nnrcsHist;

}

StaticVector<int> DelaunayGraphCut::getIsUsedPerCamera() const

{

long timer = std::clock();

StaticVector<int> cams;

cams.resize_with(mp->getNbCameras(), 0);

//#pragma omp parallel for

for(int vi = 0; vi < _verticesAttr.size(); ++vi)

{

const GC_vertexInfo& v = _verticesAttr[vi];

for(int c = 0; c < v.cams.size(); ++c)

{

const int obsCam = v.cams[c];

//#pragma OMP_ATOMIC_WRITE

{

cams[obsCam] = 1;

}

}

}

mvsUtils::printfElapsedTime(timer, "getIsUsedPerCamera ");

return cams;

}

StaticVector<int> DelaunayGraphCut::getSortedUsedCams() const

{

const StaticVector<int> isUsed = getIsUsedPerCamera();

StaticVector<int> out;

out.reserve(isUsed.size());

for(int cameraIndex = 0; cameraIndex < isUsed.size(); ++cameraIndex)

{

if(isUsed[cameraIndex] != 0)

out.push_back(cameraIndex);

}

return out;

}

void DelaunayGraphCut::addPointsFromSfM(const Point3d hexah[8], const StaticVector<int>& cams, const sfmData::SfMData& sfmData)

{

const std::size_t nbPoints = sfmData.getLandmarks().size();

const std::size_t verticesOffset = _verticesCoords.size();

_verticesCoords.resize(verticesOffset + nbPoints);

_verticesAttr.resize(verticesOffset + nbPoints);

sfmData:: Landmarks::const_iterator landmarkIt = sfmData.getLandmarks().begin();

std::vector<Point3d>::iterator vCoordsIt = _verticesCoords.begin();

std::vector<GC_vertexInfo>::iterator vAttrIt = _verticesAttr.begin();

std::advance(vCoordsIt, verticesOffset);

std::advance(vAttrIt, verticesOffset);

std::size_t addedPoints = 0;

for(std::size_t i = 0; i < nbPoints; ++i)

{

const sfmData::Landmark& landmark = landmarkIt->second;

const Point3d p(landmark.X(0), landmark.X(1), landmark.X(2));

if(mvsUtils::isPointInHexahedron(p, hexah))

{

*vCoordsIt = p;

vAttrIt->nrc = landmark.observations.size();

vAttrIt->cams.reserve(vAttrIt->nrc);

for(const auto& observationPair : landmark.observations)

vAttrIt->cams.push_back(mp->getIndexFromViewId(observationPair.first));

vAttrIt->pixSize = mp->getCamsMinPixelSize(p, vAttrIt->cams);

++vCoordsIt;

++vAttrIt;

++addedPoints;

}

++landmarkIt;

}

if(addedPoints != nbPoints)

{

_verticesCoords.resize(verticesOffset + addedPoints);

_verticesAttr.resize(verticesOffset + addedPoints);

}

ALICEVISION_LOG_WARNING("Add " << addedPoints << " new points for the SfM.");

}

void DelaunayGraphCut::addPointsFromCameraCenters(const StaticVector<int>& cams, float minDist)

{

int addedPoints = 0;

for(int camid = 0; camid < cams.size(); camid++)

{

int rc = cams[camid];

{

const Point3d p(mp->CArr[rc].x, mp->CArr[rc].y, mp->CArr[rc].z);

const GEO::index_t vi = locateNearestVertex(p);

if((vi == GEO::NO_VERTEX) || ((_verticesCoords[vi] - mp->CArr[rc]).size() > minDist))

{

const GEO::index_t nvi = _verticesCoords.size();

_verticesCoords.push_back(p);

GC_vertexInfo newv;

newv.nrc = 0;

_camsVertexes[rc] = nvi;

_verticesAttr.push_back(newv);

++addedPoints;

}

else

{

_camsVertexes[rc] = vi;

}

}

}

ALICEVISION_LOG_WARNING("Add " << addedPoints << " new points for the " << cams.size() << " cameras centers.");

}

void DelaunayGraphCut::addPointsToPreventSingularities(const Point3d voxel[8], float minDist)

{

Point3d vcg = (voxel[0] + voxel[1] + voxel[2] + voxel[3] + voxel[4] + voxel[5] + voxel[6] + voxel[7]) / 8.0f;

Point3d extrPts[6];

Point3d fcg;

fcg = (voxel[0] + voxel[1] + voxel[2] + voxel[3]) / 4.0f;

extrPts[0] = fcg + (fcg - vcg) / 10.0f;

fcg = (voxel[0] + voxel[4] + voxel[7] + voxel[3]) / 4.0f;

extrPts[1] = fcg + (fcg - vcg) / 10.0f;

fcg = (voxel[0] + voxel[1] + voxel[5] + voxel[4]) / 4.0f;

extrPts[2] = fcg + (fcg - vcg) / 10.0f;

fcg = (voxel[4] + voxel[5] + voxel[6] + voxel[7]) / 4.0f;

extrPts[3] = fcg + (fcg - vcg) / 10.0f;

fcg = (voxel[1] + voxel[5] + voxel[6] + voxel[2]) / 4.0f;

extrPts[4] = fcg + (fcg - vcg) / 10.0f;

fcg = (voxel[3] + voxel[2] + voxel[6] + voxel[7]) / 4.0f;

extrPts[5] = fcg + (fcg - vcg) / 10.0f;

int addedPoints = 0;

for(int i = 0; i < 6; i++)

{

const Point3d p(extrPts[i].x, extrPts[i].y, extrPts[i].z);

const GEO::index_t vi = locateNearestVertex(p);

if((vi == GEO::NO_VERTEX) || ((_verticesCoords[vi] - extrPts[i]).size() > minDist))

{

_verticesCoords.push_back(p);

GC_vertexInfo newv;

newv.nrc = 0;

_verticesAttr.push_back(newv);

++addedPoints;

}

}

ALICEVISION_LOG_WARNING("Add " << addedPoints << " points to prevent singularities");

}

void DelaunayGraphCut::densifyWithHelperPoints(int nbFront, int nbBack, double scale)

{

if(nbFront <= 0 && nbBack <= 0)

return;

const std::size_t nbInputVertices = _verticesCoords.size();

std::vector<Point3d> newHelperPoints;

newHelperPoints.reserve((nbFront + nbBack) * nbInputVertices);

for(std::size_t vi = 0; vi < nbInputVertices; ++vi)

{

const Point3d& v = _verticesCoords[vi];

const GC_vertexInfo& vAttr = _verticesAttr[vi];

if(vAttr.cams.empty() || vAttr.pixSize <= std::numeric_limits<float>::epsilon())

continue;

Point3d mainCamDir;

for(int camId: vAttr.cams)

{

const Point3d& cam = mp->CArr[camId];

const Point3d d = (cam - v).normalize();

mainCamDir += d;

}

mainCamDir /= double(vAttr.cams.size());

mainCamDir = mainCamDir.normalize() * vAttr.pixSize;

for(int iFront = 1; iFront < nbFront + 1; ++iFront)

newHelperPoints.push_back(v + mainCamDir * iFront * scale);

for(int iBack = 1; iBack < nbBack + 1; ++iBack)

newHelperPoints.push_back(v - mainCamDir * iBack * scale);

}

_verticesCoords.resize(nbInputVertices + newHelperPoints.size());

_verticesAttr.resize(nbInputVertices + newHelperPoints.size());

for(std::size_t vi = 0; vi < newHelperPoints.size(); ++vi)

{

_verticesCoords[nbInputVertices + vi] = newHelperPoints[vi];

// GC_vertexInfo& vAttr = _verticesAttr[nbInputVertices + vi];

// Keep vertexInfo with default/empty values, so they will be removed at the end as other helper points

// vAttr.nrc = 0;

}

ALICEVISION_LOG_WARNING("Densify the " << nbInputVertices << " vertices with " << newHelperPoints.size()

<< " new helper points.");

}

void DelaunayGraphCut::addGridHelperPoints(int helperPointsGridSize, const Point3d voxel[8], float minDist)

{

if(helperPointsGridSize <= 0)

return;

int ns = helperPointsGridSize;

float md = 1.0f / 500.0f;

Point3d vx = (voxel[1] - voxel[0]);

Point3d vy = (voxel[3] - voxel[0]);

Point3d vz = (voxel[4] - voxel[0]);

Point3d O = voxel[0] + vx * md + vy * md + vz * md;

vx = vx - vx * 2.0f * md;

vy = vy - vy * 2.0f * md;

vz = vz - vz * 2.0f * md;

float maxSize = 2.0f * (O - voxel[0]).size();

Point3d CG = (voxel[0] + voxel[1] + voxel[2] + voxel[3] + voxel[4] + voxel[5] + voxel[6] + voxel[7]) / 8.0f;

const unsigned int seed = (unsigned int)mp->userParams.get<unsigned int>("delaunaycut.seed", 0);

std::mt19937 generator(seed != 0 ? seed : std::random_device{}());

auto rand = std::bind(std::uniform_real_distribution<float>{0.0, 1.0}, generator);

int addedPoints = 0;

for(int x = 0; x <= ns; ++x)

{

for(int y = 0; y <= ns; ++y)

{

for(int z = 0; z <= ns; ++z)

{

Point3d pt = voxel[0] + vx * ((float)x / (float)ns) + vy * ((float)y / (float)ns) +

vz * ((float)z / (float)ns);

pt = pt + (CG - pt).normalize() * (maxSize * rand());

const Point3d p(pt.x, pt.y, pt.z);

const GEO::index_t vi = locateNearestVertex(p);

// if there is no nearest vertex or the nearest vertex is not too close

if((vi == GEO::NO_VERTEX) || ((_verticesCoords[vi] - pt).size() > minDist))

{

_verticesCoords.push_back(p);

GC_vertexInfo newv;

newv.nrc = 0;

_verticesAttr.push_back(newv);

++addedPoints;

}

}

}

}

ALICEVISION_LOG_WARNING("Add " << addedPoints << " new helper points for a 3D grid of " << ns << "x" << ns << "x" << ns <<".");

}

void DelaunayGraphCut::addMaskHelperPoints(const Point3d voxel[8], const StaticVector<int>& cams, const FuseParams& params)

{

if(params.maskHelperPointsWeight <= 0.0)

return;

ALICEVISION_LOG_INFO("Add Mask Helper Points.");

Point3d inflatedVoxel[8];

mvsUtils::inflateHexahedron(voxel, inflatedVoxel, 1.01f);

std::size_t nbPixels = 0;

for(const auto& imgParams : mp->getImagesParams())

{

nbPixels += imgParams.size;

}

// int step = std::floor(std::sqrt(double(nbPixels) / double(params.maxInputPoints)));

// step = std::max(step, params.minStep);

const int step = 1;

int nbAddedPoints = 0;

ALICEVISION_LOG_INFO("Load depth maps and add points.");

{

for(int c = 0; c < cams.size(); c++)

{

std::vector<float> depthMap;

int width, height;

{

const std::string depthMapFilepath = getFileNameFromIndex(mp, c, mvsUtils::EFileType::depthMap, 0);

imageIO::readImage(depthMapFilepath, width, height, depthMap, imageIO::EImageColorSpace::NO_CONVERSION);

if(depthMap.empty())

{

ALICEVISION_LOG_WARNING("Empty depth map: " << depthMapFilepath);

continue;

}

}

int syMax = std::ceil(height / step);

int sxMax = std::ceil(width / step);

for(int sy = 0; sy < syMax; ++sy)

{

for(int sx = 0; sx < sxMax; ++sx)

{

float bestScore = 0;

int bestX = 0;

int bestY = 0;

for(int y = sy * step, ymax = std::min((sy + 1) * step, height); y < ymax; ++y)

{

for(int x = sx * step, xmax = std::min((sx + 1) * step, width); x < xmax; ++x)

{

const std::size_t index = y * width + x;

const float depth = depthMap[index];

// -2 means that the pixels should be masked-out with mask helper points

if(depth > -1.5f)

continue;

int nbValidDepth = 0;

const int kernelSize = params.maskBorderSize;

for(int ly = std::max(y - kernelSize, 0), lyMax = std::min(y + kernelSize, height - 1);

ly < lyMax; ++ly)

{

for(int lx = std::max(x - kernelSize, 0), lxMax = std::min(x + kernelSize, width - 1);

lx < lxMax; ++lx)

{

if(depthMap[ly * width + lx] > 0.0f)

++nbValidDepth;

}

}

const float score = nbValidDepth; // TODO: best score based on nbValidDepth and kernel size ?

if(score > bestScore)

{