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;
}
void ccIndexedTransformationBuffer::sort()
{
SortAlgo(begin(), end(), IndexedSortOperator);
}
bool ccRegistrationTools::ICP( ccHObject* data,
ccHObject* model,
ccGLMatrix& transMat,
double &finalScale,
double& finalRMS,
unsigned& finalPointCount,
double minRMSDecrease,
unsigned maxIterationCount,
unsigned randomSamplingLimit,
bool removeFarthestPoints,
CCLib::ICPRegistrationTools::CONVERGENCE_TYPE method,
bool adjustScale,
double finalOverlapRatio/*=1.0*/,
bool useDataSFAsWeights/*=false*/,
bool useModelSFAsWeights/*=false*/,
int filters/*=CCLib::ICPRegistrationTools::SKIP_NONE*/,
int maxThreadCount/*=0*/,
QWidget* parent/*=0*/)
{
//progress bar
ccProgressDialog pDlg(false, parent);
Garbage<CCLib::GenericIndexedCloudPersist> cloudGarbage;
//if the 'model' entity is a mesh, we need to sample points on it
CCLib::GenericIndexedCloudPersist* modelCloud = 0;
ccGenericMesh* modelMesh = 0;
if (model->isKindOf(CC_TYPES::MESH))
{
modelMesh = ccHObjectCaster::ToGenericMesh(model);
modelCloud = modelMesh->getAssociatedCloud();
}
else
{
modelCloud = ccHObjectCaster::ToGenericPointCloud(model);
}
//if the 'data' entity is a mesh, we need to sample points on it
CCLib::GenericIndexedCloudPersist* dataCloud = 0;
if (data->isKindOf(CC_TYPES::MESH))
{
dataCloud = CCLib::MeshSamplingTools::samplePointsOnMesh(ccHObjectCaster::ToGenericMesh(data), s_defaultSampledPointsOnDataMesh, &pDlg);
if (!dataCloud)
{
ccLog::Error("[ICP] Failed to sample points on 'data' mesh!");
return false;
}
cloudGarbage.add(dataCloud);
}
else
{
dataCloud = ccHObjectCaster::ToGenericPointCloud(data);
}
//we activate a temporary scalar field for registration distances computation
CCLib::ScalarField* dataDisplayedSF = 0;
int oldDataSfIdx = -1, dataSfIdx = -1;
//if the 'data' entity is a real ccPointCloud, we can even create a proper temporary SF for registration distances
if (data->isA(CC_TYPES::POINT_CLOUD))
{
ccPointCloud* pc = static_cast<ccPointCloud*>(data);
dataDisplayedSF = pc->getCurrentDisplayedScalarField();
oldDataSfIdx = pc->getCurrentInScalarFieldIndex();
dataSfIdx = pc->getScalarFieldIndexByName(REGISTRATION_DISTS_SF);
if (dataSfIdx < 0)
dataSfIdx = pc->addScalarField(REGISTRATION_DISTS_SF);
if (dataSfIdx >= 0)
pc->setCurrentScalarField(dataSfIdx);
else
{
ccLog::Error("[ICP] Couldn't create temporary scalar field! Not enough memory?");
return false;
}
}
else
{
if (!dataCloud->enableScalarField())
{
ccLog::Error("[ICP] Couldn't create temporary scalar field! Not enough memory?");
return false;
}
}
//add a 'safety' margin to input ratio
static double s_overlapMarginRatio = 0.2;
finalOverlapRatio = std::max(finalOverlapRatio, 0.01); //1% minimum
//do we need to reduce the input point cloud (so as to be close
//to the theoretical number of overlapping points - but not too
//low so as we are not registered yet ;)
if (finalOverlapRatio < 1.0 - s_overlapMarginRatio)
{
//DGM we can now use 'approximate' distances as SAITO algorithm is exact (but with a coarse resolution)
//level = 7 if < 1.000.000
//level = 8 if < 10.000.000
//level = 9 if > 10.000.000
int gridLevel = static_cast<int>(floor(log10(static_cast<double>(std::max(dataCloud->size(), modelCloud->size()))))) + 2;
gridLevel = std::min(std::max(gridLevel,7),9);
int result = -1;
if (modelMesh)
{
CCLib::DistanceComputationTools::Cloud2MeshDistanceComputationParams c2mParams;
c2mParams.octreeLevel = gridLevel;
c2mParams.maxSearchDist = 0;
c2mParams.useDistanceMap = true,
c2mParams.signedDistances = false;
c2mParams.flipNormals = false;
c2mParams.multiThread = false;
result = CCLib::DistanceComputationTools::computeCloud2MeshDistance(dataCloud, modelMesh, c2mParams, &pDlg);
}
else
{
result = CCLib::DistanceComputationTools::computeApproxCloud2CloudDistance( dataCloud,
modelCloud,
gridLevel,
-1,
&pDlg);
}
if (result < 0)
{
ccLog::Error("Failed to determine the max (overlap) distance (not enough memory?)");
return false;
}
//determine the max distance that (roughly) corresponds to the input overlap ratio
ScalarType maxSearchDist = 0;
{
unsigned count = dataCloud->size();
std::vector<ScalarType> distances;
try
{
distances.resize(count);
}
catch (const std::bad_alloc&)
{
ccLog::Error("Not enough memory!");
return false;
}
for (unsigned i=0; i<count; ++i)
{
distances[i] = dataCloud->getPointScalarValue(i);
}
SortAlgo(distances.begin(), distances.end());
//now look for the max value at 'finalOverlapRatio+margin' percent
maxSearchDist = distances[static_cast<unsigned>(std::max(1.0,count*(finalOverlapRatio+s_overlapMarginRatio)))-1];
}
//evntually select the points with distance below 'maxSearchDist'
//(should roughly correspond to 'finalOverlapRatio + margin' percent)
{
CCLib::ReferenceCloud* refCloud = new CCLib::ReferenceCloud(dataCloud);
cloudGarbage.add(refCloud);
unsigned countBefore = dataCloud->size();
unsigned baseIncrement = static_cast<unsigned>(std::max(100.0,countBefore*finalOverlapRatio*0.05));
for (unsigned i=0; i<countBefore; ++i)
{
if (dataCloud->getPointScalarValue(i) <= maxSearchDist)
{
if ( refCloud->size() == refCloud->capacity()
&& !refCloud->reserve(refCloud->size() + baseIncrement) )
{
ccLog::Error("Not enough memory!");
return false;
}
refCloud->addPointIndex(i);
}
}
refCloud->resize(refCloud->size());
dataCloud = refCloud;
unsigned countAfter = dataCloud->size();
double keptRatio = static_cast<double>(countAfter)/countBefore;
ccLog::Print(QString("[ICP][Partial overlap] Selecting %1 points out of %2 (%3%) for registration").arg(countAfter).arg(countBefore).arg(static_cast<int>(100*keptRatio)));
//update the relative 'final overlap' ratio
finalOverlapRatio /= keptRatio;
}
}
//weights
CCLib::ScalarField* modelWeights = 0;
CCLib::ScalarField* dataWeights = 0;
{
if (!modelMesh && useModelSFAsWeights)
{
if (modelCloud == dynamic_cast<CCLib::GenericIndexedCloudPersist*>(model) && model->isA(CC_TYPES::POINT_CLOUD))
{
ccPointCloud* pc = static_cast<ccPointCloud*>(model);
modelWeights = pc->getCurrentDisplayedScalarField();
if (!modelWeights)
ccLog::Warning("[ICP] 'useDataSFAsWeights' is true but model has no displayed scalar field!");
}
else
{
ccLog::Warning("[ICP] 'useDataSFAsWeights' is true but only point clouds scalar fields can be used as weights!");
}
}
if (useDataSFAsWeights)
{
if (!dataDisplayedSF)
{
if (dataCloud == (CCLib::GenericIndexedCloudPersist*)data && data->isA(CC_TYPES::POINT_CLOUD))
ccLog::Warning("[ICP] 'useDataSFAsWeights' is true but data has no displayed scalar field!");
else
ccLog::Warning("[ICP] 'useDataSFAsWeights' is true but inly point clouds scalar fields can be used as weights!");
}
else
dataWeights = dataDisplayedSF;
}
}
CCLib::ICPRegistrationTools::RESULT_TYPE result;
CCLib::PointProjectionTools::Transformation transform;
CCLib::ICPRegistrationTools::Parameters params;
{
params.convType = method;
params.minRMSDecrease = minRMSDecrease;
params.nbMaxIterations = maxIterationCount;
params.adjustScale = adjustScale;
params.filterOutFarthestPoints = removeFarthestPoints;
params.samplingLimit = randomSamplingLimit;
params.finalOverlapRatio = finalOverlapRatio;
params.modelWeights = modelWeights;
params.dataWeights = dataWeights;
params.transformationFilters = filters;
params.maxThreadCount = maxThreadCount;
}
result = CCLib::ICPRegistrationTools::Register( modelCloud,
modelMesh,
dataCloud,
params,
transform,
finalRMS,
finalPointCount,
static_cast<CCLib::GenericProgressCallback*>(&pDlg));
if (result >= CCLib::ICPRegistrationTools::ICP_ERROR)
{
ccLog::Error("Registration failed: an error occurred (code %i)",result);
}
else if (result == CCLib::ICPRegistrationTools::ICP_APPLY_TRANSFO)
{
transMat = FromCCLibMatrix<PointCoordinateType,float>(transform.R, transform.T, transform.s);
finalScale = transform.s;
}
//remove temporary SF (if any)
if (dataSfIdx >= 0)
{
assert(data->isA(CC_TYPES::POINT_CLOUD));
ccPointCloud* pc = static_cast<ccPointCloud*>(data);
pc->setCurrentScalarField(oldDataSfIdx);
pc->deleteScalarField(dataSfIdx);
dataSfIdx = -1;
}
return (result < CCLib::ICPRegistrationTools::ICP_ERROR);
}
