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 QScopedPointer<ccProgressDialog> progressDlg; if (parent) { progressDlg.reset(new ccProgressDialog(false, parent)); } Garbage<CCLib::GenericIndexedCloudPersist> cloudGarbage; //if the 'model' entity is a mesh, we need to sample points on it CCLib::GenericIndexedCloudPersist* modelCloud = nullptr; ccGenericMesh* modelMesh = nullptr; 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 = nullptr; if (data->isKindOf(CC_TYPES::MESH)) { dataCloud = CCLib::MeshSamplingTools::samplePointsOnMesh(ccHObjectCaster::ToGenericMesh(data), s_defaultSampledPointsOnDataMesh, progressDlg.data()); 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 = nullptr; 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, progressDlg.data()); } else { result = CCLib::DistanceComputationTools::computeApproxCloud2CloudDistance( dataCloud, modelCloud, gridLevel, -1, progressDlg.data()); } 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); } ParallelSort(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 = nullptr; CCLib::ScalarField* dataWeights = nullptr; { 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 cloud scalar fields can be used as weights!"); } } if (useDataSFAsWeights) { if (!dataDisplayedSF) { if (dataCloud == ccHObjectCaster::ToPointCloud(data)) ccLog::Warning("[ICP] 'useDataSFAsWeights' is true but data has no displayed scalar field!"); else ccLog::Warning("[ICP] 'useDataSFAsWeights' is true but only point cloud 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*>(progressDlg.data())); 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); } return (result < CCLib::ICPRegistrationTools::ICP_ERROR); }
bool ccRegistrationTools::ICP( ccHObject* data, ccHObject* model, ccGLMatrix& transMat, double &finalScale, double& finalError, double minErrorDecrease, unsigned maxIterationCount, unsigned randomSamplingLimit, bool removeFarthestPoints, ConvergenceMethod method, bool adjustScale, bool useDataSFAsWeights/*=false*/, bool useModelSFAsWeights/*=false*/, QWidget* parent/*=0*/) { //progress bar ccProgressDialog pDlg(false,parent); //if the 'model' entity is a mesh, we need to sample points on it CCLib::GenericIndexedCloudPersist* modelCloud = 0; if (model->isKindOf(CC_TYPES::MESH)) { modelCloud = CCLib::MeshSamplingTools::samplePointsOnMesh(ccHObjectCaster::ToGenericMesh(model),s_defaultSampledPointsOnModelMesh,&pDlg); if (!modelCloud) { ccLog::Error("[ICP] Failed to sample points on 'model' mesh!"); return false; } } 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; } } 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 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::Warning("[ICP] Couldn't create temporary scalar field! Not enough memory?"); } else { dataCloud->enableScalarField(); } //parameters CCLib::PointProjectionTools::Transformation transform; CCLib::ScalarField* modelWeights = 0; if (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!"); } } CCLib::ScalarField* dataWeights = 0; 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::CC_ICP_RESULT result; result = CCLib::ICPRegistrationTools::RegisterClouds( modelCloud, dataCloud, transform, method, minErrorDecrease, maxIterationCount, finalError, adjustScale, static_cast<CCLib::GenericProgressCallback*>(&pDlg), removeFarthestPoints, randomSamplingLimit, modelWeights, dataWeights); 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 = ccGLMatrix(transform.R, transform.T, transform.s); finalScale = transform.s; } //if we had to sample points an the data mesh if (!data->isKindOf(CC_TYPES::POINT_CLOUD)) { delete dataCloud; dataCloud = 0; } else { if (data->isA(CC_TYPES::POINT_CLOUD)) { ccPointCloud* pc = static_cast<ccPointCloud*>(data); pc->setCurrentScalarField(oldDataSfIdx); if (dataSfIdx >= 0) pc->deleteScalarField(dataSfIdx); dataSfIdx=-1; } } //if we had to sample points an the model mesh if (!model->isKindOf(CC_TYPES::POINT_CLOUD)) { delete modelCloud; modelCloud = 0; } return (result < CCLib::ICPRegistrationTools::ICP_ERROR); }