// 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/.

#include "OctreeTracks.hpp"

#include <aliceVision/system/Logger.hpp>

#include <aliceVision/mvsData/geometry.hpp>

#include <aliceVision/mvsUtils/common.hpp>

#include <aliceVision/mvsUtils/fileIO.hpp>

#include <boost/accumulators/accumulators.hpp>

#include <boost/accumulators/statistics.hpp>

#include <iostream>

namespace aliceVision {

namespace fuseCut {

OctreeTracks::Node::Node(NodeType type)

{

type_ = type;

}

OctreeTracks::trackStruct* OctreeTracks::getTrack(int x, int y, int z)

{

if(!((x >= 0 && x < size_) && (y >= 0 && y < size_) && (z >= 0 && z < size_)))

{

return nullptr;

}

Node** n = &root_;

int size = size_;

while(size != 1)

{

if(*n == nullptr)

{

return nullptr;

}

size /= 2;

n = &reinterpret_cast<Branch*>(*n)->children[!((x & size) == 0)][!((y & size) == 0)][!((z & size) == 0)];

}

if(*n == nullptr)

{

return nullptr;

}

return reinterpret_cast<trackStruct*>(*n);

}

void OctreeTracks::addPoint(int x, int y, int z, float sim, float pixSize, Point3d& p, int rc)

{

assert(x >= 0 && x < size_);

assert(y >= 0 && y < size_);

assert(z >= 0 && z < size_);

Node** n = &root_;

int size = size_;

while(size != 1)

{

if(*n == nullptr)

{

*n = new Branch();

}

else

{

size /= 2;

n = &reinterpret_cast<Branch*>(*n)->children[!((x & size) == 0)][!((y & size) == 0)][!((z & size) == 0)];

}

}

if(*n == nullptr)

{

*n = new trackStruct(sim, pixSize, p, rc);

leafsNumber_++;

}

else

{

reinterpret_cast<trackStruct*>(*n)->addPoint(sim, pixSize, p, rc);

}

}

void OctreeTracks::addTrack(int x, int y, int z, OctreeTracks::trackStruct* t)

{

assert(x >= 0 && x < size_);

assert(y >= 0 && y < size_);

assert(z >= 0 && z < size_);

Node** n = &root_;

int size = size_;

while(size != 1)

{

if(*n == nullptr)

{

*n = new Branch();

}

else

{

size /= 2;

n = &reinterpret_cast<Branch*>(*n)->children[!((x & size) == 0)][!((y & size) == 0)][!((z & size) == 0)];

}

}

if(*n == nullptr)

{

*n = new trackStruct(t);

leafsNumber_++;

}

else

{

reinterpret_cast<trackStruct*>(*n)->addTrack(t);

}

}

StaticVector<OctreeTracks::trackStruct*>* OctreeTracks::getAllPoints()

{

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

out->reserve(leafsNumber_);

if(root_ != nullptr)

{

getAllPointsRecursive(out, root_);

}

return out;

}

void OctreeTracks::getAllPointsRecursive(StaticVector<trackStruct*>* out, Node* node)

{

assert(node);

switch(node->type_)

{

case BranchNode:

{

Branch* b = reinterpret_cast<Branch*>(node);

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

{

for(int j = 0; j < 2; ++j)

{

for(int k = 0; k < 2; ++k)

{

if(b->children[i][j][k] != nullptr)

{

getAllPointsRecursive(out, b->children[i][j][k]);

}

}

}

}

}

break;

case LeafNode:

out->push_back(reinterpret_cast<trackStruct*>(node));

break;

}

}

void OctreeTracks::getNPointsByLevelsRecursive(Node* node, int level, StaticVector<int>* nptsAtLevel)

{

assert(node);

switch(node->type_)

{

case BranchNode:

{

Branch* b = reinterpret_cast<Branch*>(node);

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

{

for(int j = 0; j < 2; ++j)

{

for(int k = 0; k < 2; ++k)

{

if(b->children[i][j][k] != nullptr)

{

(*nptsAtLevel)[level + 1] += 1;

getNPointsByLevelsRecursive(b->children[i][j][k], level + 1, nptsAtLevel);

}

}

}

}

}

break;

case LeafNode:

//

break;

}

}

OctreeTracks::Branch::Branch()

: Node(BranchNode)

{

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

{

for(int j = 0; j < 2; ++j)

{

for(int k = 0; k < 2; ++k)

{

children[i][j][k] = nullptr;

}

}

}

}

OctreeTracks::Branch::~Branch()

{

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

{

for(int j = 0; j < 2; ++j)

{

for(int k = 0; k < 2; ++k)

{

assert(children[i][j][k] != this);

/*

Node *node = children[i][j][k];

if (node!=NULL) {

if ( node->type_ == BranchNode ) {

delete reinterpret_cast<Branch*>(node);

}else {

if ( node->type_ == LeafNode ) {

delete reinterpret_cast<trackStruct*>(node);

}else{

printf("WARNING unknows node type\n");

};

}

node = NULL;

children[i][j][k] = NULL;

}

*/

if(children[i][j][k] != nullptr)

{

if(children[i][j][k]->type_ == BranchNode)

{

delete reinterpret_cast<Branch*>(children[i][j][k]);

}

else

{

if(children[i][j][k]->type_ == LeafNode)

{

delete reinterpret_cast<trackStruct*>(children[i][j][k]);

}

else

{

ALICEVISION_LOG_WARNING("Unknows node type.");

}

}

children[i][j][k] = nullptr;

}

}

}

}

}

OctreeTracks::trackStruct::trackStruct(float sim, float pixSize, const Point3d& p, int rc)

: Node(LeafNode)

{

npts = 1;

point = p;

cams.reserve(10);

cams.push_back(Pixel(rc, 1));

minPixSize = pixSize;

minSim = sim;

}

OctreeTracks::trackStruct::trackStruct(trackStruct* t)

: Node(LeafNode)

{

npts = t->npts;

point = t->point;

cams = t->cams;

minPixSize = t->minPixSize;

minSim = t->minSim;

}

OctreeTracks::trackStruct::~trackStruct()

{

}

void OctreeTracks::trackStruct::addPoint(float sim, float pixSize, const Point3d& p, int rc)

{

int index = indexOf(rc);

if(index == -1)

{

cams.push_back(Pixel(rc, 1));

if(cams.size() > 1)

{

qsort(&cams[0], cams.size(), sizeof(Pixel), qSortComparePixelByXAsc);

}

}

else

{

cams[index].y += 1;

}

// strategy 1: average all values

// point = (point * (float)npts + p) / (float)(npts + 1);

// minPixSize = std::min(minPixSize, pixSize);

// minSim = std::min(minSim, sim);

// npts++;

// strategy 2: keep best from nearest cam

// if (pixSize < minPixSize * 1.2f) // if equivalent or better than the previous value

//{

// if ((sim < minSim) || (pixSize * 1.2f <= minPixSize))

// {

// point = p;

// minSim = sim;

// };

// minPixSize = std::min(minPixSize,pixSize);

//};

// strategy 3: average values with good precision (precision is given by pixel size)

if(pixSize < minPixSize * 0.8f) // if strongly better => replace previous values

{

point = p;

minPixSize = pixSize;

minSim = sim;

npts = 1;

}

else if(pixSize < minPixSize * 1.2f) // if close to the previous value => average

{

// average with previous values of the same precision

point = (point * (float)npts + p) / (float)(npts + 1);

minPixSize = std::min(minPixSize, pixSize);

minSim = std::min(minSim, sim);

npts++;

}

// else don't use the position information as it is less accurate.

}

void OctreeTracks::trackStruct::addDistinctNonzeroCamsFromTrackAsZeroCams(trackStruct* t)

{

for(int i = 0; i < t->cams.size(); i++)

{

if(t->cams[i].y > 0)

{

int rc = t->cams[i].x;

int index = indexOf(rc);

if(index == -1)

{

cams.push_back(Pixel(rc, 0));

if(cams.size() > 1)

{

qsort(&cams[0], cams.size(), sizeof(Pixel), qSortComparePixelByXAsc);

}

}

}

}

}

void OctreeTracks::trackStruct::addTrack(OctreeTracks::trackStruct* t)

{

for(int i = 0; i < t->cams.size(); i++)

{

int rc = t->cams[i].x;

int index = indexOf(rc);

if(index == -1)

{

cams.push_back(t->cams[i]);

if(cams.size() > 1)

{

qsort(&cams[0], cams.size(), sizeof(Pixel), qSortComparePixelByXAsc);

}

}

else

{

cams[index].y += t->cams[i].y;

}

}

// // to keep CG

// point = (point * (float)npts + t->point * (float)t->npts) / (float)(npts + t->npts);

// minPixSize = std::min(minPixSize, t->minPixSize);

// minSim = std::min(minSim, t->minSim);

// keep best from nearest cam

// if (t->minPixSize<minPixSize*1.2f)

//{

// if ((t->minSim<minSim)||(minPixSize>t->minPixSize*1.2f))

// {

// point = t->point;

// minSim = t->minSim;

// };

// minPixSize = std::min(minPixSize,t->minPixSize);

//};

// strategy 3: average values with good precision (precision is given by pixel size)

if(t->minPixSize < minPixSize * 0.8f) // if strongly better => replace previous values

{

point = t->point;

minPixSize = t->minPixSize;

minSim = t->minSim;

npts = t->npts;

}

else if(t->minPixSize < minPixSize * 1.2f) // if close to the previous value => average

{

// average with previous values of the same precision

point = (point * (float)npts + t->point * (float)t->npts) / (float)(npts + t->npts);

minPixSize = std::min(minPixSize, t->minPixSize);

minSim = std::min(minSim, t->minSim);

npts += t->npts;

}

// else don't use the position information as it is less accurate.

}

int OctreeTracks::trackStruct::indexOf(int val)

{

if(cams.size() == 0)

{

return -1;

}

int lef = 0;

int rig = cams.size() - 1;

int mid = lef + (rig - lef) / 2;

while((rig - lef) > 1)

{

if((val >= cams[lef].x) && (val < cams[mid].x))

{

//lef = lef;

rig = mid;

mid = lef + (rig - lef) / 2;

}

if((val >= cams[mid].x) && (val <= cams[rig].x))

{

lef = mid;

//rig = rig;

mid = lef + (rig - lef) / 2;

}

if((val < cams[lef].x) || (val > cams[rig].x))

{

lef = 0;

rig = 0;

mid = 0;

}

}

int id = -1;

if(val == cams[lef].x)

{

id = lef;

}

if(val == cams[rig].x)

{

id = rig;

}

// printf("index of %f is %i\n", (float)val, id);

return id;

}

void OctreeTracks::trackStruct::doPrintf()

{

ALICEVISION_LOG_INFO("point: " << point.x << " " << point.y << " " << point.z);

ALICEVISION_LOG_INFO("ncams: " << cams.size());

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

ALICEVISION_LOG_INFO("\t- cam: " << i << ", rc: " << cams[i].x << ", val: " << cams[i].y);

}

OctreeTracks::OctreeTracks(const Point3d* _voxel, mvsUtils::MultiViewParams* _mp, Voxel dimensions)

: Fuser(_mp)

{

numSubVoxsX = dimensions.x;

numSubVoxsY = dimensions.y;

numSubVoxsZ = dimensions.z;

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

{

vox[i] = _voxel[i];

}

O = vox[0];

vx = vox[1] - vox[0];

vy = vox[3] - vox[0];

vz = vox[4] - vox[0];

svx = vx.size();

svy = vy.size();

svz = vz.size();

vx = vx.normalize();

vy = vy.normalize();

vz = vz.normalize();

sx = svx / (float)numSubVoxsX;

sy = svy / (float)numSubVoxsY;

sz = svz / (float)numSubVoxsZ;

doFilterOctreeTracks = mp->userParams.get<bool>("LargeScale.doFilterOctreeTracks", true);

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

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

minNumOfConsistentCams = mp->userParams.get<int>("filter.minNumOfConsistentCams", 2);

simWspThr = (float)mp->userParams.get<double>("LargeScale.simWspThr", -0.0f);

int maxNumSubVoxs = std::max(std::max(numSubVoxsX, numSubVoxsY), numSubVoxsZ);

size_ = 2;

while(size_ < maxNumSubVoxs)

{

size_ *= 2;

}

root_ = nullptr;

leafsNumber_ = 0;

}

OctreeTracks::~OctreeTracks()

{

// printf("deleting octree\n");

if(root_ != nullptr)

{

delete reinterpret_cast<Branch*>(root_);

}

// printf("deleted\n");

}

bool OctreeTracks::getVoxelOfOctreeFor3DPoint(Voxel& out, Point3d& tp)

{

out.x = (int)floor(orientedPointPlaneDistance(tp, O, vx) / sx);

out.y = (int)floor(orientedPointPlaneDistance(tp, O, vy) / sy);

out.z = (int)floor(orientedPointPlaneDistance(tp, O, vz) / sz);

return ((out.x >= 0) && (out.x < numSubVoxsX) && (out.y >= 0) && (out.y < numSubVoxsY) && (out.z >= 0) &&

(out.z < numSubVoxsZ));

}

void OctreeTracks::filterMinNumConsistentCams(StaticVector<trackStruct*>* tracks)

{

using namespace boost::accumulators;

StaticVector<trackStruct*> tracksOut;

tracksOut.reserve(tracks->size());

typedef accumulator_set<float,

stats<

tag::min,

tag::mean,

tag::max,

tag::median(with_p_square_quantile)> > Accumulator;

Accumulator accMinPixSize;

Accumulator accMinSim;

Accumulator accNbCamsA;

Accumulator accNbCamsB;

// long t1 = initEstimate();

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

{

trackStruct* t = (*tracks)[i];

accNbCamsA(t->cams.size());

if(t->cams.size() >= minNumOfConsistentCams)

{

tracksOut.push_back((*tracks)[i]);

accMinPixSize(t->minPixSize);

accMinSim(t->minSim);

accNbCamsB(t->cams.size());

} // ELSE DO NOT DELETE BECAUSE IT IS POINTER TO THE STRUCTURE

// printfEstimate(i, tracks->size(), t1);

}

// finishEstimate();

ALICEVISION_LOG_INFO("filterMinNumConsistentCams: " << std::endl

<< "\t- minPixelSize min: " << boost::accumulators::min(accMinPixSize) << ", max: " << boost::accumulators::max(accMinPixSize) << ", mean: " << boost::accumulators::mean(accMinPixSize) << ", median: " << boost::accumulators::median(accMinPixSize) << std::endl

<< "\t- minSim min: " << boost::accumulators::min(accMinSim) << ", max: " << boost::accumulators::max(accMinSim) << ", mean: " << boost::accumulators::mean(accMinSim) << ", median: " << boost::accumulators::median(accMinSim) << std::endl

<< "\t- accNbCamsA min: " << boost::accumulators::min(accNbCamsA) << ", max: " << boost::accumulators::max(accNbCamsA) << ", mean: " << boost::accumulators::mean(accNbCamsA) << ", median: " << boost::accumulators::median(accNbCamsA) << std::endl

<< "\t- accNbCamsB min: " << boost::accumulators::min(accNbCamsB) << ", max: " << boost::accumulators::max(accNbCamsB) << ", mean: " << boost::accumulators::mean(accNbCamsB) << ", median: " << boost::accumulators::median(accNbCamsB) << std::endl);

tracks->swap(tracksOut);

}

// TODO this is not working well ...

// if there are two neighbouring voxels and their representants are closer than

// 1.2 minpixsize of each of them then they must have the same set of cameras

// this should preserve the case when the density of points is smaller than sx

void OctreeTracks::updateOctreeTracksCams(StaticVector<trackStruct*>* tracks)

{

float clusterSizeThr = 1.2f;

// long t1 = initEstimate();

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

{

int n = (int)ceil(((*tracks)[i]->minPixSize * clusterSizeThr) / sx);

Voxel vox;

if((n > 1) && (getVoxelOfOctreeFor3DPoint(vox, (*tracks)[i]->point)))

{

// printf("n %i\n",n);

for(int xp = -n; xp <= n; xp++)

{

for(int yp = -n; yp <= n; yp++)

{

for(int zp = -n; zp <= n; zp++)

{

if((xp == 0) && (yp == 0) && (zp == 0))

{

assert(getTrack(vox.x + xp, vox.y + yp, vox.z + zp) == (*tracks)[i]);

continue;

}

trackStruct* neighborTrack = getTrack(vox.x + xp, vox.y + yp, vox.z + zp);

if(neighborTrack != nullptr)

{

trackStruct& currentTrack = *(*tracks)[i];

const float dist = (currentTrack.point - neighborTrack->point).size();

if((dist < currentTrack.minPixSize * 1.2f) || (dist < neighborTrack->minPixSize * 1.2f))

// (nt->minPixSize < (*tracks)[i]->minPixSize * 1.2f) && ((*tracks)[i]->minPixSize < nt->minPixSize * 1.2f)

{

// currentTrack.cams

currentTrack.addDistinctNonzeroCamsFromTrackAsZeroCams(neighborTrack);

neighborTrack->addDistinctNonzeroCamsFromTrackAsZeroCams((*tracks)[i]);

}

}

}

}

}

}

}

// finishEstimate();

}

/// if there is in near distance to actual track another track that has

/// smaller enough pix size then remove the actual track

void OctreeTracks::filterOctreeTracks2(StaticVector<trackStruct*>* tracks)

{

if(mp->verbose)

ALICEVISION_LOG_DEBUG("filterOctreeTracks2");

StaticVector<trackStruct*> tracksOut;

tracksOut.reserve(tracks->size());

float clusterSizeThr = mp->userParams.get<double>("OctreeTracks.clusterSizeThr", 2.0f);

// long t1 = initEstimate();

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

{

int n = (int)ceil(((*tracks)[i]->minPixSize * clusterSizeThr) / sx);

bool ok = true;

Voxel vox;

if((n > 1) && (getVoxelOfOctreeFor3DPoint(vox, (*tracks)[i]->point)))

{

// printf("n %i\n",n);

for(int xp = -n; xp <= n; xp++)

{

for(int yp = -n; yp <= n; yp++)

{

for(int zp = -n; zp <= n; zp++)

{

trackStruct* nt = getTrack(vox.x + xp, vox.y + yp, vox.z + zp);

if((xp == 0) && (yp == 0) && (zp == 0))

{

assert(nt == (*tracks)[i]);

}

else

{

if((nt != nullptr) && ((*tracks)[i]->minPixSize > nt->minPixSize * 1.2f))

{

ok = false;

}

}

}

}

}

}

if(ok)

{

tracksOut.push_back((*tracks)[i]);

} // ELSE DO NOT DELETE BECAUSE IT IS POINTER TO THE STRUCTURE

// printfEstimate(i, tracks->size(), t1);

}

// finishEstimate();

tracks->swap(tracksOut);

}

StaticVector<OctreeTracks::trackStruct*>* OctreeTracks::fillOctree(int maxPts, std::string depthMapsPtsSimsTmpDir)

{

long t1 = clock();

StaticVector<int> cams = mp->findCamsWhichIntersectsHexahedron(vox, depthMapsPtsSimsTmpDir + "minMaxDepths.bin");

if(mp->verbose)

mvsUtils::printfElapsedTime(t1, "findCamsWhichIntersectsHexahedron");

if(mp->verbose)

ALICEVISION_LOG_DEBUG("ncams: " << cams.size());

t1 = clock();

// long t1=initEstimate();

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

{

int rc = cams[camid];

StaticVector<Point3d>* pts =

loadArrayFromFile<Point3d>(depthMapsPtsSimsTmpDir + std::to_string(mp->getViewId(rc)) + "pts.bin");

StaticVector<float>* sims =

loadArrayFromFile<float>(depthMapsPtsSimsTmpDir + std::to_string(mp->getViewId(rc)) + "sims.bin");

// long tpts=initEstimate();

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

{

float sim = (*sims)[i];

Point3d p = (*pts)[i];

Voxel otVox;

if(((doUseWeaklySupportedPoints) || (sim < simWspThr)) && (getVoxelOfOctreeFor3DPoint(otVox, p))) // doUseWeaklySupportedPoints: false by default

{

if(doUseWeaklySupportedPointCam)

{

if(sim > 1.0f)

{

sim -= 2.0f;

}

}

float pixSize = mp->getCamPixelSize(p, rc);

addPoint(otVox.x, otVox.y, otVox.z, sim, pixSize, p, rc);

}

if(leafsNumber_ > 2 * maxPts)

{

return nullptr;

}

// printfEstimate(i, pts->size(), tpts);

} // for i

// finishEstimate();

delete pts;

delete sims;

// printfEstimate(camid, cams.size(), t1);

}

// finishEstimate();

StaticVector<trackStruct*>* tracks = getAllPoints();

if(mp->verbose)

mvsUtils::printfElapsedTime(t1, "fillOctree fill");

// TODO this is not working well ...

// updateOctreeTracksCams(tracks);

// if (mp->verbose) printfElapsedTime(t1,"updateOctreeTracksCams");

if(doFilterOctreeTracks)

{

if(mp->verbose)

ALICEVISION_LOG_DEBUG("# tracks before filtering: " << tracks->size());

long t2 = clock();

filterMinNumConsistentCams(tracks);

if(mp->verbose)

mvsUtils::printfElapsedTime(t2, "filterMinNumConsistentCams");

if(mp->verbose)

ALICEVISION_LOG_DEBUG("# tracks after filterMinNumConsistentCams: " << tracks->size());

t2 = clock();

// filter cameras observations that have a large pixelSize regarding the others

filterOctreeTracks2(tracks);

if(mp->verbose)

mvsUtils::printfElapsedTime(t2, "filterOctreeTracks2");

if(mp->verbose)

ALICEVISION_LOG_DEBUG("# tracks after filterOctreeTracks2: " << tracks->size());

if(mp->verbose)

ALICEVISION_LOG_DEBUG("# tracks after filtering: " << tracks->size());

}

if(tracks->size() > maxPts)

{

if(mp->verbose)

ALICEVISION_LOG_DEBUG("Too much tracks (" << tracks->size() << "), clear all.");

delete tracks; // DO NOT DELETE POINTER JUST DELETE THE ARRAY!!!

return nullptr;

}

if(mp->verbose)

ALICEVISION_LOG_DEBUG("number of tracks: " << tracks->size());

return tracks;

}

StaticVector<OctreeTracks::trackStruct*>*

OctreeTracks::fillOctreeFromTracks(StaticVector<OctreeTracks::trackStruct*>* tracksIn)

{

long t1 = clock();

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

{

trackStruct* t = (*tracksIn)[i];

Voxel otVox;

if(getVoxelOfOctreeFor3DPoint(otVox, t->point))

{

addTrack(otVox.x, otVox.y, otVox.z, t);

}

} // for i

StaticVector<trackStruct*>* tracks = getAllPoints();

if(mp->verbose)

mvsUtils::printfElapsedTime(t1, "fillOctreeFromTracks");

return tracks;

}

StaticVector<int>* OctreeTracks::getTracksCams(StaticVector<OctreeTracks::trackStruct*>* tracks)

{

StaticVectorBool* camsb = new StaticVectorBool();

camsb->reserve(mp->ncams);

camsb->resize_with(mp->ncams, false);

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

{

for(int c = 0; c < (*tracks)[i]->cams.size(); c++)

{

(*camsb)[(*tracks)[i]->cams[c].x] = true;

}

}

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

cams->reserve(mp->ncams);

for(int i = 0; i < mp->ncams; i++)

{

if((*camsb)[i])

{

cams->push_back(i);

}

}

delete camsb;

return cams;

}

} // namespace fuseCut

} // namespace aliceVision