unsigned FastMarchingForFacetExtraction::updateFlagsTable( ccGenericPointCloud* theCloud,
GenericChunkedArray<1,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.setValue(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!
CCLib::FastMarching::Cell* cell = m_theGrid[m_activeCells[i]];
m_theGrid[m_activeCells[i]] = 0;
if (cell)
delete cell;
}
//*/
//unsigned pointCount = 0;
CCLib::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.getValue(index) == 1);
//flags.setValue(index,1);
//++pointCount;
}
m_theGrid[m_activeCells[i]] = 0;
delete aCell;
}
return pointCount;
}
unsigned ccFastMarchingForNormsDirection::updateResolvedTable( ccGenericPointCloud* cloud,
GenericChunkedArray<1,unsigned char> &resolved,
NormsIndexesTableType* theNorms)
{
if (!m_initialized || !m_octree || m_gridLevel > CCLib::DgmOctree::MAX_OCTREE_LEVEL)
return 0;
CCLib::ReferenceCloud Yk(m_octree->associatedCloud());
unsigned count = 0;
for (size_t i=0; i<m_activeCells.size(); ++i)
{
DirectionCell* aCell = static_cast<DirectionCell*>(m_theGrid[m_activeCells[i]]);
if (!m_octree->getPointsInCell(aCell->cellCode,m_gridLevel,&Yk,true))
{
//not enough memory
return 0;
}
for (unsigned k=0; k<Yk.size(); ++k)
{
unsigned index = Yk.getPointGlobalIndex(k);
resolved.setValue(index,1);
const CompressedNormType& norm = theNorms->getValue(index);
const CCVector3& N = ccNormalVectors::GetNormal(norm);
//inverse point normal if necessary
if (N.dot(aCell->N) < 0)
{
theNorms->setValue(index,ccNormalVectors::GetNormIndex(-N));
}
#ifdef QT_DEBUG
cloud->setPointScalarValue(index,aCell->T);
//cloud->setPointScalarValue(index,aCell->signConfidence);
//cloud->setPointScalarValue(index,aCell->scalar);
#endif
++count;
}
}
return count;
}
int ccFastMarchingForNormsDirection::updateResolvedTable(ccGenericPointCloud* theCloud,
GenericChunkedArray<1,uchar> &resolved,
NormsIndexesTableType* theNorms)
{
if (!initialized)
return -1;
int count=0;
for (unsigned i=0;i<activeCells.size();++i)
{
DirectionCell* aCell = (DirectionCell*)theGrid[activeCells[i]];
CCLib::ReferenceCloud* Yk = theOctree->getPointsInCell(aCell->cellCode,gridLevel,true);
if (!Yk)
continue;
Yk->placeIteratorAtBegining();
for (unsigned k=0;k<Yk->size();++k)
{
unsigned index = Yk->getCurrentPointGlobalIndex();
resolved.setValue(index,1); //resolvedValue=1
const normsType& norm = theNorms->getValue(index);
if (CCVector3::vdot(ccNormalVectors::GetNormal(norm),aCell->N)<0.0)
{
PointCoordinateType newN[3];
const PointCoordinateType* N = ccNormalVectors::GetNormal(norm);
newN[0]=-N[0];
newN[1]=-N[1];
newN[2]=-N[2];
theNorms->setValue(index,ccNormalVectors::GetNormIndex(newN));
}
//norm = NormalVectors::getNormIndex(aCell->N);
//theNorms->setValue(index,&norm);
theCloud->setPointScalarValue(index,aCell->T);
//theCloud->setPointScalarValue(index,aCell->v);
Yk->forwardIterator();
++count;
}
}
return count;
}
ReferenceCloud* CloudSamplingTools::resampleCloudSpatially(GenericIndexedCloudPersist* theCloud,
PointCoordinateType minDistance,
DgmOctree* theOctree/*=0*/,
GenericProgressCallback* progressCb/*=0*/)
{
assert(theCloud);
unsigned cloudSize = theCloud->size();
DgmOctree *_theOctree=theOctree;
if (!_theOctree)
{
_theOctree = new DgmOctree(theCloud);
if (_theOctree->build()<(int)cloudSize)
{
delete _theOctree;
return 0;
}
}
ReferenceCloud* sampledCloud = new ReferenceCloud(theCloud);
if (!sampledCloud->reserve(cloudSize))
{
if (!theOctree)
delete _theOctree;
return 0;
}
GenericChunkedArray<1,bool>* markers = new GenericChunkedArray<1,bool>(); //DGM: upgraded from vector, as this can be quite huge!
if (!markers->resize(cloudSize,true,true))
{
markers->release();
if (!theOctree)
delete _theOctree;
delete sampledCloud;
return 0;
}
NormalizedProgress* normProgress=0;
if (progressCb)
{
progressCb->setInfo("Spatial resampling");
normProgress = new NormalizedProgress(progressCb,cloudSize);
progressCb->reset();
progressCb->start();
}
//for each point in the cloud that is still 'marked', we look
//for its neighbors and remove their own marks
DgmOctree::NearestNeighboursSphericalSearchStruct nss;
nss.level = _theOctree->findBestLevelForAGivenNeighbourhoodSizeExtraction(minDistance);
markers->placeIteratorAtBegining();
for (unsigned i=0; i<cloudSize; i++, markers->forwardIterator())
{
//progress indicator
if (normProgress && !normProgress->oneStep())
{
//cancel process
delete sampledCloud;
sampledCloud = 0;
break;
}
//no mark? we skip this point
if (!markers->getCurrentValue())
continue;
//init neighbor search structure
theCloud->getPoint(i,nss.queryPoint);
bool inbounds = false;
_theOctree->getTheCellPosWhichIncludesThePoint(&nss.queryPoint, nss.cellPos, nss.level, inbounds);
nss.truncatedCellCode = (inbounds ? _theOctree->generateTruncatedCellCode(nss.cellPos, nss.level) : DgmOctree::INVALID_CELL_CODE);
_theOctree->computeCellCenter(nss.cellPos, nss.level, nss.cellCenter);
//add the points that lie in the same cell (faster)
{
ReferenceCloud* Y = _theOctree->getPointsInCell(nss.truncatedCellCode, nss.level, true);
unsigned count = Y->size();
try
{
nss.pointsInNeighbourhood.resize(count);
}
catch (std::bad_alloc) //out of memory
{
//stop process
delete sampledCloud;
sampledCloud = 0;
break;
}
unsigned realCount = 0;
DgmOctree::NeighboursSet::iterator it = nss.pointsInNeighbourhood.begin();
for (unsigned j=0; j<count; ++j)
{
unsigned index = Y->getPointGlobalIndex(j);
if (index != i && markers->getValue(index)) //no need to add the point itself and those already flagged off
{
it->point = Y->getPointPersistentPtr(j);
it->pointIndex = index;
++it;
++realCount;
}
}
nss.pointsInNeighbourhood.resize(realCount); //should be ok as realCount<=count
nss.alreadyVisitedNeighbourhoodSize = 1;
}
#ifdef TEST_CELLS_FOR_SPHERICAL_NN
nss.pointsInSphericalNeighbourhood.clear();
#endif
nss.prepare(minDistance,_theOctree->getCellSize(nss.level));
//look for neighbors and 'de-mark' them
{
unsigned nbNeighbors = _theOctree->findNeighborsInASphereStartingFromCell(nss, minDistance, false);
DgmOctree::NeighboursSet::iterator it = nss.pointsInNeighbourhood.begin();
for (unsigned j=0; j<nbNeighbors; ++j, ++it)
if (it->pointIndex != i)
markers->setValue(it->pointIndex,false);
}
//At this stage, the ith point is the only one marked in a radius of <minDistance>.
//Therefore it will necessarily be in the final cloud!
if (!sampledCloud->addPointIndex(i)) //not enough memory
{
//stop process
delete sampledCloud;
sampledCloud = 0;
break;
}
}
if(normProgress)
{
delete normProgress;
normProgress = 0;
}
if (!theOctree)
delete _theOctree;
markers->release();
return sampledCloud;
}
ReferenceCloud* CloudSamplingTools::resampleCloudSpatially(GenericIndexedCloudPersist* inputCloud,
PointCoordinateType minDistance,
const SFModulationParams& modParams,
DgmOctree* inputOctree/*=0*/,
GenericProgressCallback* progressCb/*=0*/)
{
assert(inputCloud);
unsigned cloudSize = inputCloud->size();
DgmOctree* octree = inputOctree;
if (!octree)
{
octree = new DgmOctree(inputCloud);
if (octree->build() < static_cast<int>(cloudSize))
{
delete octree;
return 0;
}
}
assert(octree && octree->associatedCloud() == inputCloud);
//output cloud
ReferenceCloud* sampledCloud = new ReferenceCloud(inputCloud);
const unsigned c_reserveStep = 65536;
if (!sampledCloud->reserve(std::min(cloudSize,c_reserveStep)))
{
if (!inputOctree)
delete octree;
return 0;
}
GenericChunkedArray<1,char>* markers = new GenericChunkedArray<1,char>(); //DGM: upgraded from vector, as this can be quite huge!
if (!markers->resize(cloudSize,true,1)) //true by default
{
markers->release();
if (!inputOctree)
delete octree;
delete sampledCloud;
return 0;
}
//best octree level (there may be several of them if we use parameter modulation)
std::vector<unsigned char> bestOctreeLevel;
bool modParamsEnabled = modParams.enabled;
ScalarType sfMin = 0, sfMax = 0;
try
{
if (modParams.enabled)
{
//compute min and max sf values
ScalarFieldTools::computeScalarFieldExtremas(inputCloud,sfMin,sfMax);
if (!ScalarField::ValidValue(sfMin))
{
//all SF values are NAN?!
modParamsEnabled = false;
}
else
{
//compute min and max 'best' levels
PointCoordinateType dist0 = static_cast<PointCoordinateType>(sfMin * modParams.a + modParams.b);
PointCoordinateType dist1 = static_cast<PointCoordinateType>(sfMax * modParams.a + modParams.b);
unsigned char level0 = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(dist0);
unsigned char level1 = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(dist1);
bestOctreeLevel.push_back(level0);
if (level1 != level0)
{
//add intermediate levels if necessary
size_t levelCount = (level1 < level0 ? level0-level1 : level1-level0) + 1;
assert(levelCount != 0);
for (size_t i=1; i<levelCount-1; ++i) //we already know level0 and level1!
{
ScalarType sfVal = sfMin + i*((sfMax-sfMin)/levelCount);
PointCoordinateType dist = static_cast<PointCoordinateType>(sfVal * modParams.a + modParams.b);
unsigned char level = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(dist);
bestOctreeLevel.push_back(level);
}
}
bestOctreeLevel.push_back(level1);
}
}
else
{
unsigned char defaultLevel = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(minDistance);
bestOctreeLevel.push_back(defaultLevel);
}
}
catch (const std::bad_alloc&)
{
//not enough memory
markers->release();
if (!inputOctree)
{
delete octree;
}
delete sampledCloud;
return 0;
}
//progress notification
NormalizedProgress normProgress(progressCb, cloudSize);
if (progressCb)
{
if (progressCb->textCanBeEdited())
{
progressCb->setMethodTitle("Spatial resampling");
char buffer[256];
sprintf(buffer, "Points: %u\nMin dist.: %f", cloudSize, minDistance);
progressCb->setInfo(buffer);
}
progressCb->update(0);
progressCb->start();
}
//for each point in the cloud that is still 'marked', we look
//for its neighbors and remove their own marks
markers->placeIteratorAtBegining();
bool error = false;
//default octree level
assert(!bestOctreeLevel.empty());
unsigned char octreeLevel = bestOctreeLevel.front();
//default distance between points
PointCoordinateType minDistBetweenPoints = minDistance;
for (unsigned i=0; i<cloudSize; i++, markers->forwardIterator())
{
//no mark? we skip this point
if (markers->getCurrentValue() != 0)
{
//init neighbor search structure
const CCVector3* P = inputCloud->getPoint(i);
//parameters modulation
if (modParamsEnabled)
{
ScalarType sfVal = inputCloud->getPointScalarValue(i);
if (ScalarField::ValidValue(sfVal))
{
//modulate minDistance
minDistBetweenPoints = static_cast<PointCoordinateType>(sfVal * modParams.a + modParams.b);
//get (approximate) best level
size_t levelIndex = static_cast<size_t>(bestOctreeLevel.size() * (sfVal / (sfMax-sfMin)));
if (levelIndex == bestOctreeLevel.size())
--levelIndex;
octreeLevel = bestOctreeLevel[levelIndex];
}
else
{
minDistBetweenPoints = minDistance;
octreeLevel = bestOctreeLevel.front();
}
}
//look for neighbors and 'de-mark' them
{
DgmOctree::NeighboursSet neighbours;
octree->getPointsInSphericalNeighbourhood(*P,minDistBetweenPoints,neighbours,octreeLevel);
for (DgmOctree::NeighboursSet::iterator it = neighbours.begin(); it != neighbours.end(); ++it)
if (it->pointIndex != i)
markers->setValue(it->pointIndex,0);
}
//At this stage, the ith point is the only one marked in a radius of <minDistance>.
//Therefore it will necessarily be in the final cloud!
if (sampledCloud->size() == sampledCloud->capacity() && !sampledCloud->reserve(sampledCloud->capacity() + c_reserveStep))
{
//not enough memory
error = true;
break;
}
if (!sampledCloud->addPointIndex(i))
{
//not enough memory
error = true;
break;
}
}
//progress indicator
if (progressCb && !normProgress.oneStep())
{
//cancel process
error = true;
break;
}
}
//remove unnecessarily allocated memory
if (!error)
{
if (sampledCloud->capacity() > sampledCloud->size())
sampledCloud->resize(sampledCloud->size());
}
else
{
delete sampledCloud;
sampledCloud = 0;
}
if (progressCb)
{
progressCb->stop();
}
if (!inputOctree)
{
//locally computed octree
delete octree;
octree = 0;
}
markers->release();
markers = 0;
return sampledCloud;
}
