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);
}
Beispiel #2
0
CC_FILE_ERROR SavePolyline(ccPolyline* poly, QFile& file, int32_t& bytesWritten, ESRI_SHAPE_TYPE outputShapeType)
{
	bytesWritten = 0;

	if (!poly)
	{
		assert(false);
		return CC_FERR_BAD_ENTITY_TYPE;
	}

	CCLib::GenericIndexedCloudPersist* vertices = poly->getAssociatedCloud();
	if (!vertices)
		return CC_FERR_BAD_ENTITY_TYPE;

	int32_t realNumPoints = poly->size();
	if (realNumPoints < 3)
		return CC_FERR_BAD_ENTITY_TYPE;

	bool is2D = poly->is2DMode();
	bool isClosed = poly->isClosed();

	ccBBox box = poly->getBB();
	assert(box.isValid());

	//Shape Type
	{
		//Byte 0: Shape Type
		int32_t shapeTypeInt = qToLittleEndian<int32_t>(outputShapeType);
		file.write((const char*)&shapeTypeInt,4);
		bytesWritten += 4;
	}

	//Byte 4: Box
	{
		double xMin = qToLittleEndian<double>(box.minCorner().x);
		double xMax = qToLittleEndian<double>(box.maxCorner().x);
		double yMin = qToLittleEndian<double>(box.minCorner().y);
		double yMax = qToLittleEndian<double>(box.maxCorner().y);
		//The Bounding Box for the PolyLine stored in the order Xmin, Ymin, Xmax, Ymax
		/*Byte  4*/file.write((const char*)&xMin,8);
		/*Byte 12*/file.write((const char*)&yMin,8);
		/*Byte 20*/file.write((const char*)&xMax,8);
		/*Byte 28*/file.write((const char*)&yMax,8);
		bytesWritten += 32;
	}

	//Byte 36: NumParts (The number of parts in the PolyLine)
	{
		int32_t numParts = qToLittleEndian<int32_t>(1);
		file.write((const char*)&numParts,4);
		bytesWritten += 4;
	}

	//Byte 40: NumPoints (The total number of points for all parts)
	int32_t numPoints = realNumPoints;
	if (isClosed)
		numPoints++;
	{
		int32_t numPointsLE = qToLittleEndian<int32_t>(numPoints);
		file.write((const char*)&numPointsLE,4);
		bytesWritten += 4;
	}

	//Byte 44: Parts (An array of length NumParts)
	{
		//for each part, the index of its first point in the points array
		int32_t startIndex = qToLittleEndian<int32_t>(0);
		file.write((const char*)&startIndex,4);
		bytesWritten += 4;
	}

	//for polygons we must list the vertices in the right order:
	//"The neighborhood to the right of an observer walking along
	//the ring in vertex order is the inside of the polygon"
	bool inverseOrder = false;
	if (outputShapeType == SHP_POLYGON || outputShapeType == SHP_POLYGON_Z)
	{
		assert(isClosed);
		assert(numPoints > 2);
		//get bounding box
		ccBBox box = poly->getBB();
		assert(box.isValid());
		//get the two largest (ordered) dimensions (dim1, dim2)
		CCVector3 diag = box.getDiagVec();
		unsigned char minDim = diag.y < diag.x ? 1 : 0;
		if (diag.z < diag.u[minDim])
			minDim = 2;
		unsigned char dim1 = ((minDim+1) % 3);
		unsigned char dim2 = ((minDim+2) % 3);

		if (diag.u[dim1] > 0) //if the polyline is flat, no need to bother ;)
		{
			//look for the top-left-most point in this 'plane'
			int32_t leftMostPointIndex = 0;
			{
				const CCVector3* leftMostPoint = vertices->getPoint(0);
				for (int32_t i=1; i<realNumPoints; ++i)
				{
					const CCVector3* P = vertices->getPoint(i);
					if (P->u[dim1] < leftMostPoint->u[dim1] || (P->u[dim1] == leftMostPoint->u[dim1] && P->u[dim2] < leftMostPoint->u[dim2]))
					{
						leftMostPoint = P;
						leftMostPointIndex = i;
					}
				}
			}

			//we simply compare the angles betwween the two edges that have the top-left-most vertex in common
			{
				const CCVector3* B = vertices->getPoint(leftMostPointIndex > 0 ? leftMostPointIndex-1 : realNumPoints-1);
				const CCVector3* P = vertices->getPoint(leftMostPointIndex);
				const CCVector3* A = vertices->getPoint(leftMostPointIndex+1 < realNumPoints ? leftMostPointIndex+1 : 0);
			
				CCVector3 PA = *A-*P;
				CCVector3 PB = *B-*P;
				PointCoordinateType anglePA = atan2(PA.u[dim2],PA.u[dim1]); //forward
				PointCoordinateType anglePB = atan2(PB.u[dim2],PB.u[dim1]); //backward
				//angles should all be in [-PI/2;0]
				if (anglePA < anglePB)
					inverseOrder = true;
			}
		}
	}

	//Points (An array of length NumPoints)
	{
		for (int32_t i=0; i<numPoints; ++i)
		{
			int32_t ii = (inverseOrder ? numPoints-1-i : i);
			const CCVector3* P = vertices->getPoint(ii % realNumPoints); //warning: handle loop if polyline is closed

			double x = qToLittleEndian<double>(P->x);
			double y = qToLittleEndian<double>(P->y);
			/*Byte 0*/file.write((const char*)&x,8);
			/*Byte 8*/file.write((const char*)&y,8);
			bytesWritten += 16;
		}
	}

	//3D polylines
	if (!is2D)
	{
		//Z boundaries
		{
			double zMin = qToLittleEndian<double>(box.minCorner().z);
			double zMax = qToLittleEndian<double>(box.maxCorner().z);
			file.write((const char*)&zMin,8);
			file.write((const char*)&zMax,8);
			bytesWritten += 16;
		}

		//Z coordinates (for each part - just one here)
		{
			for (int32_t i=0; i<numPoints; ++i)
			{
				int32_t ii = (inverseOrder ? numPoints-1-i : i);
				const CCVector3* P = vertices->getPoint(ii % realNumPoints); //warning: handle loop if polyline is closed
				double z = qToLittleEndian<double>(P->z);
				file.write((const char*)&z,8);
				bytesWritten += 8;
			}
		}

		//M boundaries
		bool hasSF = vertices->isScalarFieldEnabled();
		{
			double mMin = ESRI_NO_DATA;
			double mMax = ESRI_NO_DATA;
			if (hasSF)
			{
				for (int32_t i=0; i<realNumPoints; ++i)
				{
					ScalarType scalar = vertices->getPointScalarValue(i);
					if (i != 0)
					{
						if (mMin > scalar)
							mMin = static_cast<double>(scalar);
						else if (mMax < scalar)
							mMax = static_cast<double>(scalar);
					}
					else
					{
						mMin = mMax = static_cast<double>(scalar);
					}
				}
			}
			mMin = qToLittleEndian<double>(mMin);
			mMax = qToLittleEndian<double>(mMax);
			file.write((const char*)&mMin,8);
			file.write((const char*)&mMax,8);
			bytesWritten += 16;
		}

		//M values (for each part - just one here)
		{
			double scalar = qToLittleEndian<double>(ESRI_NO_DATA);
			for (int32_t i=0; i<numPoints; ++i)
			{
				if (hasSF)
				{
					scalar = static_cast<double>(vertices->getPointScalarValue(i % realNumPoints)); //warning: handle loop if polyline is closed
					scalar = qToLittleEndian<double>(scalar);
				}
				file.write((const char*)&scalar,8);
				bytesWritten += 8;
			}
		}
	}

	return CC_FERR_NO_ERROR;
}