bool ccKdTree::convertCellIndexToSF()
{
if (!m_associatedGenericCloud || !m_associatedGenericCloud->isA(CC_TYPES::POINT_CLOUD))
return false;
//get leaves
std::vector<Leaf*> leaves;
if (!getLeaves(leaves) || leaves.empty())
return false;
ccPointCloud* pc = static_cast<ccPointCloud*>(m_associatedGenericCloud);
const char c_defaultSFName[] = "Kd-tree indexes";
int sfIdx = pc->getScalarFieldIndexByName(c_defaultSFName);
if (sfIdx < 0)
sfIdx = pc->addScalarField(c_defaultSFName);
if (sfIdx < 0)
{
ccLog::Error("Not enough memory!");
return false;
}
pc->setCurrentScalarField(sfIdx);
//for each cell
for (size_t i=0; i<leaves.size(); ++i)
{
CCLib::ReferenceCloud* subset = leaves[i]->points;
if (subset)
{
for (unsigned j=0; j<subset->size(); ++j)
subset->setPointScalarValue(j,(ScalarType)i);
}
}
pc->getScalarField(sfIdx)->computeMinAndMax();
pc->setCurrentDisplayedScalarField(sfIdx);
pc->showSF(true);
return true;
}
bool ccKdTreeForFacetExtraction::FuseCells( ccKdTree* kdTree,
double maxError,
CCLib::DistanceComputationTools::ERROR_MEASURES errorMeasure,
double maxAngle_deg,
PointCoordinateType overlapCoef/*=1*/,
bool closestFirst/*=true*/,
CCLib::GenericProgressCallback* progressCb/*=0*/)
{
if (!kdTree)
return false;
ccGenericPointCloud* associatedGenericCloud = kdTree->associatedGenericCloud();
if (!associatedGenericCloud || !associatedGenericCloud->isA(CC_TYPES::POINT_CLOUD) || maxError < 0.0)
return false;
//get leaves
std::vector<ccKdTree::Leaf*> leaves;
if (!kdTree->getLeaves(leaves) || leaves.empty())
return false;
//progress notification
CCLib::NormalizedProgress nProgress(progressCb, static_cast<unsigned>(leaves.size()));
if (progressCb)
{
progressCb->update(0);
if (progressCb->textCanBeEdited())
{
progressCb->setMethodTitle("Fuse Kd-tree cells");
progressCb->setInfo(qPrintable(QString("Cells: %1\nMax error: %2").arg(leaves.size()).arg(maxError)));
}
progressCb->start();
}
ccPointCloud* pc = static_cast<ccPointCloud*>(associatedGenericCloud);
//sort cells based on their population size (we start by the biggest ones)
SortAlgo(leaves.begin(), leaves.end(), DescendingLeafSizeComparison);
//set all 'userData' to -1 (i.e. unfused cells)
{
for (size_t i=0; i<leaves.size(); ++i)
{
leaves[i]->userData = -1;
//check by the way that the plane normal is unit!
assert(static_cast<double>(fabs(CCVector3(leaves[i]->planeEq).norm2()) - 1.0) < 1.0e-6);
}
}
// cosine of the max angle between fused 'planes'
const double c_minCosNormAngle = cos(maxAngle_deg * CC_DEG_TO_RAD);
//fuse all cells, starting from the ones with the best error
const int unvisitedNeighborValue = -1;
bool cancelled = false;
int macroIndex = 1; //starts at 1 (0 is reserved for cells already above the max error)
{
for (size_t i=0; i<leaves.size(); ++i)
{
ccKdTree::Leaf* currentCell = leaves[i];
if (currentCell->error >= maxError)
currentCell->userData = 0; //0 = special group for cells already above the user defined threshold!
//already fused?
if (currentCell->userData != -1)
{
if (progressCb && !nProgress.oneStep()) //process canceled by user
{
cancelled = true;
break;
}
continue;
}
//we create a new "macro cell" index
currentCell->userData = macroIndex++;
//we init the current set of 'fused' points with the cell's points
CCLib::ReferenceCloud* currentPointSet = currentCell->points;
//get current fused set centroid and normal
CCVector3 currentCentroid = *CCLib::Neighbourhood(currentPointSet).getGravityCenter();
CCVector3 currentNormal(currentCell->planeEq);
//visited neighbors
ccKdTree::LeafSet visitedNeighbors;
//set of candidates
std::list<Candidate> candidates;
//we are going to iteratively look for neighbor cells that could be fused to this one
ccKdTree::LeafVector cellsToTest;
cellsToTest.push_back(currentCell);
if (progressCb && !nProgress.oneStep()) //process canceled by user
{
cancelled = true;
break;
}
while (!cellsToTest.empty() || !candidates.empty())
{
//get all neighbors around the 'waiting' cell(s)
if (!cellsToTest.empty())
{
ccKdTree::LeafSet neighbors;
while (!cellsToTest.empty())
{
if (!kdTree->getNeighborLeaves(cellsToTest.back(), neighbors, &unvisitedNeighborValue)) //we only consider unvisited cells!
{
//an error occurred
return false;
}
cellsToTest.pop_back();
}
//add those (new) neighbors to the 'visitedNeighbors' set
//and to the candidates set by the way if they are not yet there
for (ccKdTree::LeafSet::iterator it=neighbors.begin(); it != neighbors.end(); ++it)
{
ccKdTree::Leaf* neighbor = *it;
std::pair<ccKdTree::LeafSet::iterator,bool> ret = visitedNeighbors.insert(neighbor);
//neighbour not already in the set?
if (ret.second)
{
//we create the corresponding candidate
try
{
candidates.push_back(Candidate(neighbor));
}
catch (const std::bad_alloc&)
{
//not enough memory!
ccLog::Warning("[ccKdTreeForFacetExtraction] Not enough memory!");
return false;
}
}
}
}
//is there remaining candidates?
if (!candidates.empty())
{
//update the set of candidates
if (closestFirst && candidates.size() > 1)
{
for (std::list<Candidate>::iterator it = candidates.begin(); it !=candidates.end(); ++it)
it->dist = (it->centroid-currentCentroid).norm2();
//sort candidates by their distance
candidates.sort(CandidateDistAscendingComparison);
}
//we will keep track of the best fused 'couple' at each pass
std::list<Candidate>::iterator bestIt = candidates.end();
CCLib::ReferenceCloud* bestFused = 0;
CCVector3 bestNormal(0,0,0);
double bestError = -1.0;
unsigned skipCount = 0;
for (std::list<Candidate>::iterator it = candidates.begin(); it != candidates.end(); /*++it*/)
{
assert(it->leaf && it->leaf->points);
assert(currentPointSet->getAssociatedCloud() == it->leaf->points->getAssociatedCloud());
//if the leaf orientation is too different
if (fabs(CCVector3(it->leaf->planeEq).dot(currentNormal)) < c_minCosNormAngle)
{
it = candidates.erase(it);
//++it;
//++skipCount;
continue;
}
//compute the minimum distance between the candidate centroid and the 'currentPointSet'
PointCoordinateType minDistToMainSet = 0.0;
{
for (unsigned j=0; j<currentPointSet->size(); ++j)
{
const CCVector3* P = currentPointSet->getPoint(j);
PointCoordinateType d2 = (*P-it->centroid).norm2();
if (d2 < minDistToMainSet || j == 0)
minDistToMainSet = d2;
}
minDistToMainSet = sqrt(minDistToMainSet);
}
//if the leaf is too far
if (it->radius < minDistToMainSet / overlapCoef)
{
++it;
++skipCount;
continue;
}
//fuse the main set with the current candidate
CCLib::ReferenceCloud* fused = new CCLib::ReferenceCloud(*currentPointSet);
if (!fused->add(*(it->leaf->points)))
{
//not enough memory!
ccLog::Warning("[ccKdTreeForFacetExtraction] Not enough memory!");
delete fused;
if (currentPointSet != currentCell->points)
delete currentPointSet;
return false;
}
//fit a plane and estimate the resulting error
double error = -1.0;
const PointCoordinateType* planeEquation = CCLib::Neighbourhood(fused).getLSPlane();
if (planeEquation)
error = CCLib::DistanceComputationTools::ComputeCloud2PlaneDistance(fused, planeEquation, errorMeasure);
if (error < 0.0 || error > maxError)
{
//candidate is rejected
it = candidates.erase(it);
}
else
{
//otherwise we keep track of the best one!
if (bestError < 0.0 || error < bestError)
{
bestIt = it;
bestError = error;
if (bestFused)
delete bestFused;
bestFused = fused;
bestNormal = CCVector3(planeEquation);
fused = 0;
if (closestFirst)
break; //if we have found a good candidate, we stop here (closest first ;)
}
++it;
}
if (fused)
{
delete fused;
fused = 0;
}
}
//we have a (best) candidate for this pass?
if (bestIt != candidates.end())
{
assert(bestFused && bestError >= 0.0);
if (currentPointSet != currentCell->points)
delete currentPointSet;
currentPointSet = bestFused;
{
//update infos
CCLib::Neighbourhood N(currentPointSet);
//currentCentroid = *N.getGravityCenter(); //if we update it, the search will naturally shift along one dimension!
//currentNormal = bestNormal; //same thing here for normals
}
bestIt->leaf->userData = currentCell->userData;
//bestIt->leaf->userData = macroIndex++; //FIXME TEST
//we will test this cell's neighbors as well
cellsToTest.push_back(bestIt->leaf);
if (progressCb && !nProgress.oneStep()) //process canceled by user
{
//premature end!
candidates.clear();
cellsToTest.clear();
cancelled = true;
break;
}
QApplication::processEvents();
//we also remove it from the candidates list
candidates.erase(bestIt);
}
if (skipCount == candidates.size() && cellsToTest.empty())
{
//only far leaves remain...
candidates.clear();
}
}
} //no more candidates or cells to test
//end of the fusion process for the current leaf
if (currentPointSet != currentCell->points)
delete currentPointSet;
currentPointSet = 0;
if (cancelled)
break;
}
}
//convert fused indexes to SF
if (!cancelled)
{
pc->enableScalarField();
for (size_t i=0; i<leaves.size(); ++i)
{
CCLib::ReferenceCloud* subset = leaves[i]->points;
if (subset)
{
ScalarType scalar = (ScalarType)leaves[i]->userData;
if (leaves[i]->userData <= 0) //for unfused cells, we create new individual groups
scalar = static_cast<ScalarType>(macroIndex++);
//scalar = NAN_VALUE; //FIXME TEST
for (unsigned j=0; j<subset->size(); ++j)
subset->setPointScalarValue(j,scalar);
}
}
//pc->setCurrentDisplayedScalarField(sfIdx);
}
return !cancelled;
}