C++ ReferenceCloud类代码示例

ReferenceCloud* ManualSegmentationTools::segment(GenericIndexedCloudPersist* aCloud, ScalarType minDist, ScalarType maxDist)
{
	if (!aCloud)
	{
		assert(false);
		return 0;
	}

	ReferenceCloud* Y = new ReferenceCloud(aCloud);

	//for each point
	for (unsigned i=0; i<aCloud->size(); ++i)
	{
		const ScalarType dist = aCloud->getPointScalarValue(i);
		//we test if its assocaited scalar value falls inside the specified intervale
		if (dist >= minDist && dist <= maxDist)
		{
			if (!Y->addPointIndex(i))
			{
				//not engouh memory
				delete Y;
				Y=0;
				break;
			}
		}
	}

	return Y;
}

bool CloudSamplingTools::subsampleCellAtLevel(const DgmOctree::octreeCell& cell, void** additionalParameters)
{
	ReferenceCloud* cloud					    = (ReferenceCloud*)additionalParameters[0];
	SUBSAMPLING_CELL_METHOD subsamplingMethod	= *((SUBSAMPLING_CELL_METHOD*)additionalParameters[1]);

	unsigned selectedPointIndex=0;
	unsigned pointsCount = cell.points->size();

	if (subsamplingMethod == RANDOM_POINT)
	{
	    selectedPointIndex = (static_cast<unsigned>(rand()) % pointsCount);
	}
	else // if (subsamplingMethod == NEAREST_POINT_TO_CELL_CENTER)
	{
		PointCoordinateType center[3];
		cell.parentOctree->computeCellCenter(cell.truncatedCode,cell.level,center,true);

		ScalarType dist,minDist;
		minDist = CCVector3::vdistance2(cell.points->getPoint(0)->u,center);

		for (unsigned i=1;i<pointsCount;++i)
		{
			dist = CCVector3::vdistance2(cell.points->getPoint(i)->u,center);
			if (dist<minDist)
			{
				selectedPointIndex = i;
                minDist=dist;
            }
        }
    }

	return cloud->addPointIndex(cell.points->getPointGlobalIndex(selectedPointIndex));
}

ReferenceCloud* FastMarchingForPropagation::extractPropagatedPoints()
{
	if (!m_initialized || !m_octree || m_gridLevel > DgmOctree::MAX_OCTREE_LEVEL)
		return 0;

	ReferenceCloud* Zk = new ReferenceCloud(m_octree->associatedCloud());

	for (unsigned i=0; i<m_activeCells.size(); ++i)
	{
		PropagationCell* aCell = (PropagationCell*)m_theGrid[m_activeCells[i]];
		ReferenceCloud* Yk = m_octree->getPointsInCell(aCell->cellCode,m_gridLevel,true);

		if (!Zk->reserve(Yk->size())) //not enough memory
		{
			delete Zk;
			return 0;
		}

		Yk->placeIteratorAtBegining();
		for (unsigned k=0;k<Yk->size();++k)
		{
			Zk->addPointIndex(Yk->getCurrentPointGlobalIndex()); //can't fail (see above)
			//raz de la norme du gradient du point, pour qu'il ne soit plus pris en compte par la suite !
			Yk->setCurrentPointScalarValue(NAN_VALUE);
			Yk->forwardIterator();
		}

		//Yk->clear(); //inutile
	}

	return Zk;
}

ReferenceCloud* CloudSamplingTools::subsampleCloudRandomly(GenericIndexedCloudPersist* inputCloud, unsigned newNumberOfPoints, GenericProgressCallback* progressCb/*=0*/)
{
	assert(inputCloud);

	unsigned theCloudSize = inputCloud->size();

	//we put all input points in a ReferenceCloud
	ReferenceCloud* newCloud = new ReferenceCloud(inputCloud);
	if (!newCloud->addPointIndex(0,theCloudSize))
	{
		delete newCloud;
		return 0;
	}

	//we have less points than requested?!
	if (theCloudSize <= newNumberOfPoints)
	{
		return newCloud;
	}

	unsigned pointsToRemove = theCloudSize - newNumberOfPoints;
	std::random_device rd;   // non-deterministic generator
	std::mt19937 gen(rd());  // to seed mersenne twister.

	NormalizedProgress* normProgress = 0;
	if (progressCb)
	{
		progressCb->setInfo("Random subsampling");
		normProgress = new NormalizedProgress(progressCb, pointsToRemove);
		progressCb->reset();
		progressCb->start();
	}

	//we randomly remove "inputCloud.size() - newNumberOfPoints" points (much simpler)
	unsigned lastPointIndex = theCloudSize-1;
	for (unsigned i=0; i<pointsToRemove; ++i)
	{
		std::uniform_int_distribution<unsigned> dist(0, lastPointIndex);
		unsigned index = dist(gen);
		newCloud->swap(index,lastPointIndex);
		--lastPointIndex;

		if (normProgress && !normProgress->oneStep())
		{
			//cancel process
			delete normProgress;
			delete newCloud;
			return 0;
		}
	}

	newCloud->resize(newNumberOfPoints); //always smaller, so it should be ok!

	if (normProgress)
		delete normProgress;

	return newCloud;
}

ReferenceCloud* CloudSamplingTools::subsampleCloudRandomly(GenericIndexedCloudPersist* theCloud, unsigned newNumberOfPoints, GenericProgressCallback* progressCb/*=0*/)
{
	assert(theCloud);

	unsigned theCloudSize = theCloud->size();

	//we put all input points in a ReferenceCloud
	ReferenceCloud* newCloud = new ReferenceCloud(theCloud);
	if (!newCloud->addPointIndex(0,theCloudSize))
	{
		delete newCloud;
		return 0;
	}

	//we have less points than requested?!
	if (theCloudSize <= newNumberOfPoints)
	{
		return newCloud;
	}

	unsigned pointsToRemove = theCloudSize-newNumberOfPoints;

	NormalizedProgress* normProgress=0;
    if (progressCb)
    {
        progressCb->setInfo("Random subsampling");
		normProgress = new NormalizedProgress(progressCb,pointsToRemove);
        progressCb->reset();
        progressCb->start();
    }

	//we randomly remove "theCloud.size() - newNumberOfPoints" points (much simpler)
	unsigned lastPointIndex = theCloudSize-1;
	for (unsigned i=0; i<pointsToRemove; ++i)
	{
		unsigned index = (unsigned)floor((float)rand()/(float)RAND_MAX * (float)lastPointIndex);
		newCloud->swap(index,lastPointIndex);
		--lastPointIndex;

		if (normProgress && !normProgress->oneStep())
		{
			//cancel process
			delete normProgress;
			delete newCloud;
			return 0;
		}
	}

	newCloud->resize(newNumberOfPoints); //always smaller, so it should be ok!

	if (normProgress)
		delete normProgress;

	return newCloud;
}

bool TrueKdTree::build(	double maxError,
						DistanceComputationTools::ERROR_MEASURES errorMeasure/*=DistanceComputationTools::RMS*/,
						unsigned maxPointCountPerCell/*=0*/,
						GenericProgressCallback* progressCb/*=0*/)
{
	if (!m_associatedCloud)
		return false;

	//tree already computed! (call clear before)
	if (m_root)
		return false;

	unsigned count = m_associatedCloud->size();
	if (count == 0) //no point, no node!
	{
		return false;
	}

	//structures used to sort the points along the 3 dimensions
	try
	{
		s_sortedCoordsForSplit.resize(count);
	}
	catch(std::bad_alloc)
	{
		//not enough memory!
		return false;
	}

	//initial 'subset' to start recursion
	ReferenceCloud* subset = new ReferenceCloud(m_associatedCloud);
	if (!subset->addPointIndex(0,count))
	{
		//not enough memory
		delete subset;
		return false;
	}

	InitProgress(progressCb,count);

	//launch recursive process
	m_maxError = maxError;
	m_maxPointCountPerCell = maxPointCountPerCell;
	m_errorMeasure = errorMeasure;
	m_root = split(subset);

	//clear static structure
	s_sortedCoordsForSplit.clear();

	return (m_root != 0);
}

ReferenceCloud* CloudSamplingTools::subsampleCloudWithOctreeAtLevel(GenericIndexedCloudPersist* inputCloud,
																	unsigned char octreeLevel,
																	SUBSAMPLING_CELL_METHOD subsamplingMethod,
																	GenericProgressCallback* progressCb/*=0*/,
																	DgmOctree* inputOctree/*=0*/)
{
	assert(inputCloud);

	DgmOctree* octree = inputOctree;
	if (!octree)
	{
		octree = new DgmOctree(inputCloud);
		if (octree->build(progressCb) < 1)
		{
			delete octree;
			return 0;
		}
	}

	ReferenceCloud* cloud = new ReferenceCloud(inputCloud);

	unsigned nCells = octree->getCellNumber(octreeLevel);
	if (!cloud->reserve(nCells))
	{
		if (!inputOctree)
			delete octree;
		delete cloud;
		return 0;
	}

	//structure contenant les parametres additionnels
	void* additionalParameters[2] = {	reinterpret_cast<void*>(cloud),
										reinterpret_cast<void*>(&subsamplingMethod) };

	if (octree->executeFunctionForAllCellsAtLevel(	octreeLevel,
														&subsampleCellAtLevel,
														additionalParameters,
														false, //the process is so simple that MT is slower!
														progressCb,
														"Cloud Subsampling") == 0)
	{
		//something went wrong
		delete cloud;
		cloud=0;
	}

	if (!inputOctree)
		delete octree;

	return cloud;
}

bool CloudSamplingTools::subsampleCellAtLevel(	const DgmOctree::octreeCell& cell,
												void** additionalParameters,
												NormalizedProgress* nProgress/*=0*/)
{
	ReferenceCloud* cloud						= static_cast<ReferenceCloud*>(additionalParameters[0]);
	SUBSAMPLING_CELL_METHOD subsamplingMethod	= *static_cast<SUBSAMPLING_CELL_METHOD*>(additionalParameters[1]);

	unsigned selectedPointIndex = 0;
	unsigned pointsCount = cell.points->size();

	if (subsamplingMethod == RANDOM_POINT)
	{
		selectedPointIndex = (static_cast<unsigned>(rand()) % pointsCount);

		if (nProgress && !nProgress->steps(pointsCount))
		{
			return false;
		}
	}
	else // if (subsamplingMethod == NEAREST_POINT_TO_CELL_CENTER)
	{
		CCVector3 center;
		cell.parentOctree->computeCellCenter(cell.truncatedCode,cell.level,center,true);

		PointCoordinateType minSquareDist = (*cell.points->getPoint(0) - center).norm2();

		for (unsigned i=1; i<pointsCount; ++i)
		{
			PointCoordinateType squareDist = (*cell.points->getPoint(i) - center).norm2();
			if (squareDist < minSquareDist)
			{
				selectedPointIndex = i;
				minSquareDist = squareDist;
			}

			if (nProgress && !nProgress->oneStep())
			{
				return false;
			}
		}
	}

	return cloud->addPointIndex(cell.points->getPointGlobalIndex(selectedPointIndex));
}

bool FastMarchingForPropagation::setPropagationTimingsAsDistances()
{
	if (!m_initialized || !m_octree || m_gridLevel > DgmOctree::MAX_OCTREE_LEVEL)
		return false;

	for (unsigned i=0;i<m_activeCells.size();++i)
	{
		PropagationCell* aCell = (PropagationCell*)m_theGrid[m_activeCells[i]];
		ReferenceCloud* Yk = m_octree->getPointsInCell(aCell->cellCode,m_gridLevel,true);

		Yk->placeIteratorAtBegining();
		for (unsigned k=0;k<Yk->size();++k)
		{
			Yk->setCurrentPointScalarValue(aCell->T);
			Yk->forwardIterator();
		}
		//Yk->clear(); //inutile
	}

	return true;
}

bool CloudSamplingTools::applyNoiseFilterAtLevel(	const DgmOctree::octreeCell& cell,
													void** additionalParameters,
													NormalizedProgress* nProgress/*=0*/)
{
	ReferenceCloud* cloud				=  static_cast<ReferenceCloud*>(additionalParameters[0]);
	PointCoordinateType kernelRadius	= *static_cast<PointCoordinateType*>(additionalParameters[1]);
	double nSigma						= *static_cast<double*>(additionalParameters[2]);
	bool removeIsolatedPoints			= *static_cast<bool*>(additionalParameters[3]);
	bool useKnn							= *static_cast<bool*>(additionalParameters[4]);
	int knn								= *static_cast<int*>(additionalParameters[5]);
	bool useAbsoluteError				= *static_cast<bool*>(additionalParameters[6]);
	double absoluteError				= *static_cast<double*>(additionalParameters[7]);

	//structure for nearest neighbors search
	DgmOctree::NearestNeighboursSphericalSearchStruct nNSS;
	nNSS.level = cell.level;
	nNSS.prepare(kernelRadius,cell.parentOctree->getCellSize(nNSS.level));
	if (useKnn)
	{
		nNSS.minNumberOfNeighbors = knn;
	}
	cell.parentOctree->getCellPos(cell.truncatedCode,cell.level,nNSS.cellPos,true);
	cell.parentOctree->computeCellCenter(nNSS.cellPos,cell.level,nNSS.cellCenter);

	unsigned n = cell.points->size(); //number of points in the current cell

	//for each point in the cell
	for (unsigned i=0; i<n; ++i)
	{
		cell.points->getPoint(i,nNSS.queryPoint);

		//look for neighbors (either inside a sphere or the k nearest ones)
		//warning: there may be more points at the end of nNSS.pointsInNeighbourhood than the actual nearest neighbors (neighborCount)!
		unsigned neighborCount = 0;

		if (useKnn)
			neighborCount = cell.parentOctree->findNearestNeighborsStartingFromCell(nNSS);
		else
			neighborCount = cell.parentOctree->findNeighborsInASphereStartingFromCell(nNSS,kernelRadius,false);

		if (neighborCount > 3) //we want 3 points or more (other than the point itself!)
		{
			//find the query point in the nearest neighbors set and place it at the end
			const unsigned globalIndex = cell.points->getPointGlobalIndex(i);
			unsigned localIndex = 0;
			while (localIndex < neighborCount && nNSS.pointsInNeighbourhood[localIndex].pointIndex != globalIndex)
				++localIndex;
			//the query point should be in the nearest neighbors set!
			assert(localIndex < neighborCount);
			if (localIndex+1 < neighborCount) //no need to swap with another point if it's already at the end!
			{
				std::swap(nNSS.pointsInNeighbourhood[localIndex],nNSS.pointsInNeighbourhood[neighborCount-1]);
			}

			unsigned realNeighborCount = neighborCount-1;
			DgmOctreeReferenceCloud neighboursCloud(&nNSS.pointsInNeighbourhood,realNeighborCount); //we don't take the query point into account!
			Neighbourhood Z(&neighboursCloud);

			const PointCoordinateType* lsPlane = Z.getLSPlane();
			if (lsPlane)
			{
				double maxD = absoluteError;
				if (!useAbsoluteError)
				{
					//compute the std. dev. to this plane
					double sum_d = 0;
					double sum_d2 = 0;
					for (unsigned j=0; j<realNeighborCount; ++j)
					{
						const CCVector3* P = neighboursCloud.getPoint(j);
						double d = CCLib::DistanceComputationTools::computePoint2PlaneDistance(P,lsPlane);
						sum_d += d;
						sum_d2 += d*d;
					}

					double stddev = sqrt(fabs(sum_d2*realNeighborCount - sum_d*sum_d))/realNeighborCount;
					maxD = stddev * nSigma;
				}

				//distance from the query point to the plane
				double d = fabs(CCLib::DistanceComputationTools::computePoint2PlaneDistance(&nNSS.queryPoint,lsPlane));

				if (d <= maxD)
					cloud->addPointIndex(globalIndex);
			}
			else
			{
				//TODO: ???
			}
		}
		else
		{
			//not enough points to fit a plane AND compute distances to it
			if (!removeIsolatedPoints)
			{
				//we keep the point
				unsigned globalIndex = cell.points->getPointGlobalIndex(i);
				cloud->addPointIndex(globalIndex);
			}
		}
//.........这里部分代码省略.........

bool AutoSegmentationTools::frontPropagationBasedSegmentation(GenericIndexedCloudPersist* theCloud,
        ScalarType minSeedDist,
        uchar octreeLevel,
        ReferenceCloudContainer& theSegmentedLists,
        GenericProgressCallback* progressCb,
        DgmOctree* inputOctree,
        bool applyGaussianFilter,
        float alpha)
{
    unsigned numberOfPoints = (theCloud ? theCloud->size() : 0);
    if (numberOfPoints == 0)
        return false;

    //compute octree if none was provided
    DgmOctree* theOctree = inputOctree;
    if (!theOctree)
    {
        theOctree = new DgmOctree(theCloud);
        if (theOctree->build(progressCb)<1)
        {
            delete theOctree;
            return false;
        }
    }

    //on calcule le gradient (va ecraser le champ des distances)
    if (ScalarFieldTools::computeScalarFieldGradient(theCloud,true,true,progressCb,theOctree) < 0)
    {
        if (!inputOctree)
            delete theOctree;
        return false;
    }

    //et on lisse le resultat
    if (applyGaussianFilter)
    {
        uchar level = theOctree->findBestLevelForAGivenPopulationPerCell(NUMBER_OF_POINTS_FOR_GRADIENT_COMPUTATION);
        PointCoordinateType cellSize = theOctree->getCellSize(level);
        ScalarFieldTools::applyScalarFieldGaussianFilter(static_cast<float>(cellSize/3),theCloud,-1,progressCb,theOctree);
    }

    unsigned seedPoints = 0;
    unsigned numberOfSegmentedLists = 0;

    //on va faire la propagation avec le FastMarching();
    FastMarchingForPropagation* fm = new FastMarchingForPropagation();

    fm->setJumpCoef(50.0);
    fm->setDetectionThreshold(alpha);

    int result = fm->init(theCloud,theOctree,octreeLevel);
    int octreeLength = OCTREE_LENGTH(octreeLevel)-1;

    if (result<0)
    {
        if (!inputOctree)
            delete theOctree;
        delete fm;
        return false;
    }

    if (progressCb)
    {
        progressCb->reset();
        progressCb->setMethodTitle("FM Propagation");
        char buffer[256];
        sprintf(buffer,"Octree level: %i\nNumber of points: %u",octreeLevel,numberOfPoints);
        progressCb->setInfo(buffer);
        progressCb->start();
    }

    ScalarField* theDists = new ScalarField("distances");
    {
        ScalarType d = theCloud->getPointScalarValue(0);
        if (!theDists->resize(numberOfPoints,true,d))
        {
            if (!inputOctree)
                delete theOctree;
            return false;

        }
    }

    unsigned maxDistIndex = 0, begin = 0;
    CCVector3 startPoint;

    while (true)
    {
        ScalarType maxDist = NAN_VALUE;

        //on cherche la premiere distance superieure ou egale a "minSeedDist"
        while (begin<numberOfPoints)
        {
            const CCVector3 *thePoint = theCloud->getPoint(begin);
            const ScalarType& theDistance = theDists->getValue(begin);
            ++begin;

            //FIXME DGM: what happens if SF is negative?!
            if (theCloud->getPointScalarValue(begin) >= 0 && theDistance >= minSeedDist)
            {
//.........这里部分代码省略.........

TrueKdTree::BaseNode* TrueKdTree::split(ReferenceCloud* subset)
{
	assert(subset); //subset will always be taken care of by this method
	
	unsigned count = subset->size();

	const PointCoordinateType* planeEquation = Neighbourhood(subset).getLSQPlane();
	if (!planeEquation)
	{
		//an error occurred during LS plane computation?!
		delete subset;
		return 0;
	}

	//we always split sets larger the a given size (but we can't skip cells with less than 6 points!)
	ScalarType error = -1;
	if (count < m_maxPointCountPerCell || m_maxPointCountPerCell < 6)
	{
		assert(fabs(CCVector3(planeEquation).norm2() - 1.0) < 1.0e-6);
		error = (count != 3 ? DistanceComputationTools::ComputeCloud2PlaneDistance(subset, planeEquation, m_errorMeasure) : 0);
	
		//if we have less than 6 points, then the subdivision would produce a subset with less than 3 points
		//(and we can't fit a plane on less than 3 points!)
		bool isLeaf = (count < 6 || error <= m_maxError);
		if (isLeaf)
		{
			UpdateProgress(count);
			//the Leaf class takes ownership of the subset!
			return new Leaf(subset,planeEquation,error);
		}
	}

	/*** proceed with a 'standard' binary partition ***/

	//cell limits (dimensions)
	CCVector3 dims;
	{
		CCVector3 bbMin,bbMax;
		subset->getBoundingBox(bbMin.u,bbMax.u);
		dims = bbMax - bbMin;
	}

	//find the largest dimension
	uint8_t splitDim = X_DIM;
	if (dims.y > dims.x)
		splitDim = Y_DIM;
	if (dims.z > dims.u[splitDim])
		splitDim = Z_DIM;

	//find the median by sorting the points coordinates
	assert(s_sortedCoordsForSplit.size() >= static_cast<size_t>(count));
	for (unsigned i=0; i<count; ++i)
	{
		const CCVector3* P = subset->getPoint(i);
		s_sortedCoordsForSplit[i] = P->u[splitDim];
	}
	std::sort(s_sortedCoordsForSplit.begin(),s_sortedCoordsForSplit.begin()+count);

	unsigned splitCount = count/2;
	assert(splitCount >= 3); //count >= 6 (see above)
	
	//we must check that the split value is the 'first one'
	if (s_sortedCoordsForSplit[splitCount-1] == s_sortedCoordsForSplit[splitCount])
	{
		if (s_sortedCoordsForSplit[2] != s_sortedCoordsForSplit[splitCount]) //can we go backward?
		{
			while (/*splitCount>0 &&*/ s_sortedCoordsForSplit[splitCount-1] == s_sortedCoordsForSplit[splitCount])
			{
				assert(splitCount > 3);
				--splitCount;
			}
		}
		else if (s_sortedCoordsForSplit[count-3] != s_sortedCoordsForSplit[splitCount]) //can we go forward?
		{
			do
			{
				++splitCount;
				assert(splitCount < count-3);
			}
			while (/*splitCount+1<count &&*/ s_sortedCoordsForSplit[splitCount] == s_sortedCoordsForSplit[splitCount-1]);
		}
		else //in fact we can't split this cell!
		{
			UpdateProgress(count);
			if (error < 0)
				error = (count != 3 ? DistanceComputationTools::ComputeCloud2PlaneDistance(subset, planeEquation, m_errorMeasure) : 0);
			//the Leaf class takes ownership of the subset!
			return new Leaf(subset,planeEquation,error);
		}
	}

	PointCoordinateType splitCoord = s_sortedCoordsForSplit[splitCount]; //count > 3 --> splitCount >= 2

	ReferenceCloud* leftSubset = new ReferenceCloud(subset->getAssociatedCloud());
	ReferenceCloud* rightSubset = new ReferenceCloud(subset->getAssociatedCloud());
	if (!leftSubset->reserve(splitCount) || !rightSubset->reserve(count-splitCount))
	{
		//not enough memory!
		delete leftSubset;
		delete rightSubset;
//.........这里部分代码省略.........

ICPRegistrationTools::CC_ICP_RESULT ICPRegistrationTools::RegisterClouds(GenericIndexedCloudPersist* _modelCloud,
																			GenericIndexedCloudPersist* _dataCloud,
																			PointProjectionTools::Transformation& transform,
																			CC_ICP_CONVERGENCE_TYPE convType,
																			double minErrorDecrease,
																			unsigned nbMaxIterations,
																			double& finalError,
																			GenericProgressCallback* progressCb/*=0*/,
																			bool filterOutFarthestPoints/*=false*/,
																			unsigned samplingLimit/*=20000*/,
																			ScalarField* modelWeights/*=0*/,
																			ScalarField* dataWeights/*=0*/)
{
    assert(_modelCloud && _dataCloud);

    finalError = -1.0;

	//MODEL CLOUD (reference, won't move)
	GenericIndexedCloudPersist* modelCloud=_modelCloud;
	ScalarField* _modelWeights=modelWeights;
	{
		if (_modelCloud->size()>samplingLimit) //shall we resample the clouds? (speed increase)
		{
			ReferenceCloud* subModelCloud = CloudSamplingTools::subsampleCloudRandomly(_modelCloud,samplingLimit);
			if (subModelCloud && modelWeights)
			{
				_modelWeights = new ScalarField("ResampledModelWeights",modelWeights->isPositive());
				unsigned realCount = subModelCloud->size();
				if (_modelWeights->reserve(realCount))
				{
					for (unsigned i=0;i<realCount;++i)
						_modelWeights->addElement(modelWeights->getValue(subModelCloud->getPointGlobalIndex(i)));
					_modelWeights->computeMinAndMax();
				}
				else
				{
					//not enough memory
					delete subModelCloud;
					subModelCloud=0;
				}
			}
			modelCloud = subModelCloud;
		}
		if (!modelCloud) //something bad happened
			return ICP_ERROR_NOT_ENOUGH_MEMORY;
	}

	//DATA CLOUD (will move)
	ReferenceCloud* dataCloud=0;
	ScalarField* _dataWeights=dataWeights;
	SimpleCloud* rotatedDataCloud=0; //temporary structure (rotated vertices)
	{
		if (_dataCloud->size()>samplingLimit) //shall we resample the clouds? (speed increase)
		{
			dataCloud = CloudSamplingTools::subsampleCloudRandomly(_dataCloud,samplingLimit);
			if (dataCloud && dataWeights)
			{
				_dataWeights = new ScalarField("ResampledDataWeights",dataWeights->isPositive());
				unsigned realCount = dataCloud->size();
				if (_dataWeights->reserve(realCount))
				{
					for (unsigned i=0;i<realCount;++i)
						_dataWeights->addElement(dataWeights->getValue(dataCloud->getPointGlobalIndex(i)));
					_dataWeights->computeMinAndMax();
				}
				else
				{
					//not enough memory
					delete dataCloud;
					dataCloud=0;
				}
			}
		}
		else
		{
			//create a 'fake' reference cloud with all points
			dataCloud = new ReferenceCloud(_dataCloud);
			if (dataCloud->reserve(_dataCloud->size()))
			{
				dataCloud->addPointIndex(0,_dataCloud->size());
			}
			else //not enough memory
			{
				delete dataCloud;
				dataCloud=0;
			}
		}

		if (!dataCloud || !dataCloud->enableScalarField()) //something bad happened
		{
			if (dataCloud)
				delete dataCloud;
			if (modelCloud && modelCloud != _modelCloud)
				delete modelCloud;
			if (_modelWeights && _modelWeights!=modelWeights)
				_modelWeights->release();
			return ICP_ERROR_NOT_ENOUGH_MEMORY;
		}
	}

//.........这里部分代码省略.........