[CC]DgmOctree—执行Cell遍历和单元计算

　　DgmOctree类的executeFunctionForAllCellsAtLevel和executeFunctionForAllCellsStartingAtLevel方法通过回调函数octreeCellFunc，执行八叉树中每个单元的相关计算。

unsigned DgmOctree::executeFunctionForAllCellsAtLevel(	unsigned char level,
														octreeCellFunc func,
														void** additionalParameters,
														bool multiThread/*=false*/,
														GenericProgressCallback* progressCb/*=0*/,
														const char* functionTitle/*=0*/,
														int maxThreadCount/*=0*/)

　　

unsigned DgmOctree::executeFunctionForAllCellsStartingAtLevel(unsigned char startingLevel,
    octreeCellFunc func,
    void** additionalParameters,
    unsigned minNumberOfPointsPerCell,
    unsigned maxNumberOfPointsPerCell,
    bool multiThread/* = true*/,
    GenericProgressCallback* progressCb/*=0*/,
    const char* functionTitle/*=0*/,
    int maxThreadCount/*=0*/)

　　通过DgmOctree遍历获取每一个Cell单元似乎还是一件很复杂的事情啊！

//! The coded octree structure
	cellsContainer m_thePointsAndTheirCellCodes;

　　通过编码集合实现？m_thePointsAndTheirCellCodes根据CellCodes对所有的点进行了排序。注意，所以知道每个Cell中的起始点的Index就可以确定Cell中的其他点。

	const cellsContainer& pointsAndTheirCellCodes() const
	{
		return m_thePointsAndTheirCellCodes;
	}

　　注意octreeCellFunc 回调函数的参数，第一个是const引用传递参数，值不可以修改。

typedef bool (*octreeCellFunc)(const octreeCell& cell, void**, NormalizedProgress*);

　　因此尝试了使用下面的代码，应该是可行的：

if (!m_app)
		return;

	const ccHObject::Container& selectedEntities = m_app->getSelectedEntities();
	size_t selNum = selectedEntities.size();
	if (selNum!=1)
	{
		m_app->dispToConsole("Select only one cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
		return;
	}

	ccHObject* ent = selectedEntities[0];
	assert(ent);
	if (!ent || !ent->isA(CC_TYPES::POINT_CLOUD))
	{
		m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
		return;
	}

	ccPointCloud* theCloud = static_cast<ccPointCloud*>(ent);
	if (!theCloud)
		return;
	//输入参数，半径大小
	float kernelRadius=1;
	unsigned count = theCloud->size();
	bool hasNorms = theCloud->hasNormals();
	CCVector3 bbMin, bbMax;
	theCloud->getBoundingBox(bbMin,bbMax);
	const CCVector3d& globalShift = theCloud->getGlobalShift();
	double globalScale = theCloud->getGlobalScale();
	//进度条
	ccProgressDialog pDlg(true,m_app->getMainWindow());
	unsigned numberOfPoints = theCloud->size();
	if (numberOfPoints < 5)
		return ;
	//构建八叉树
	CCLib::DgmOctree* theOctree = NULL;
	if (!theOctree)
	{
		theOctree = new CCLib::DgmOctree(theCloud);
		if (theOctree->build(&pDlg) < 1)
		{
			delete theOctree;
			return ;
		}
	}
	int result = 0;
	QString sfName="Eigen_Value";
	int sfIdx = -1;
	ccPointCloud* pc = 0;
	//注意先添加ScalarField，并设置为当前的
	if (theCloud->isA(CC_TYPES::POINT_CLOUD))
	{
		pc = static_cast<ccPointCloud*>(theCloud);

		sfIdx = pc->getScalarFieldIndexByName(qPrintable(sfName));
		if (sfIdx < 0)
			sfIdx = pc->addScalarField(qPrintable(sfName));
		if (sfIdx >= 0)
			pc->setCurrentInScalarField(sfIdx);
		else
		{
			ccConsole::Error(QString("Failed to create scalar field on cloud '%1' (not enough memory?)").arg(pc->getName()));
		}
	}
	//开启ScalarField模式
	theCloud->enableScalarField();
	//给定的半径，寻找最佳的Level
	unsigned char level = theOctree->findBestLevelForAGivenNeighbourhoodSizeExtraction(kernelRadius);

	//parameters
	void* additionalParameters[1] = {static_cast<void*>(&kernelRadius) };

	if (theOctree->executeFunctionForAllCellsAtLevel(level,
		&computeCellEigenValueAtLevel,
		additionalParameters,
		true,
		&pDlg,
		"Eigen value Computation") == 0)
	{
		//something went wrong
		result = -4;
	}

	//number of cells for this level
	unsigned cellCount = theOctree->getCellNumber(level);
	unsigned maxCellPopulation =theOctree->getmaxCellPopulation(level);

	//cell descriptor (initialize it with first cell/point)
	CCLib::DgmOctree::octreeCell cell(theOctree);
	if (!cell.points->reserve(maxCellPopulation)) //not enough memory
		return;
	cell.level = level;
	cell.index = 0;

	CCLib::DgmOctree::cellIndexesContainer vec;
	try
	{
		vec.resize(cellCount);
	}
	catch (const std::bad_alloc&)
	{
		//not enough memory
		return;
	}

	//binary shift for cell code truncation
	unsigned char bitDec = GET_NDT_BIT_SHIFT(level);

	CCLib::DgmOctree::cellsContainer::const_iterator p =theOctree->pointsAndTheirCellCodes().begin();

	CCLib::DgmOctree::OctreeCellCodeType predCode = (p->theCode >> bitDec)+1; //pred value must be different than the first element's

	for (unsigned i=0,j=0; i<theOctree->getNumberOfProjectedPoints(); ++i,++p)
	{
		CCLib::DgmOctree::OctreeCellCodeType currentCode = (p->theCode >> bitDec);

		if (predCode != currentCode)
		{
			vec[j++] = i;//存储索引

			int n=cell.points->size();
			if(n>=5)
			{
				CCVector3d Psum(0,0,0);
				for (unsigned i=0; i<n; ++i)
				{
					CCVector3 P;
					cell.points->getPoint(i,P);
					Psum.x += P.x;
					Psum.y += P.y;
					Psum.z += P.z;
				}
				ScalarType curv = NAN_VALUE;
				CCVector3 G(static_cast<PointCoordinateType>(Psum.x / n),
					static_cast<PointCoordinateType>(Psum.y / n),
					static_cast<PointCoordinateType>(Psum.z / n) );

				double mXX = 0.0;
				double mYY = 0.0;
				double mZZ = 0.0;
				double mXY = 0.0;
				double mXZ = 0.0;
				double mYZ = 0.0;
				//for each point in the cell
				for (unsigned i=0; i<n; ++i)
				{
					CCVector3 CellP;
					cell.points->getPoint(i,CellP);
					CCVector3 P = CellP - G;

					mXX += static_cast<double>(P.x)*P.x;
					mYY += static_cast<double>(P.y)*P.y;
					mZZ += static_cast<double>(P.z)*P.z;
					mXY += static_cast<double>(P.x)*P.y;
					mXZ += static_cast<double>(P.x)*P.z;
					mYZ += static_cast<double>(P.y)*P.z;
				}
				//symmetry
				CCLib::SquareMatrixd covMat(3);
				covMat.m_values[0][0] = mXX/n;
				covMat.m_values[1][1] = mYY/n;
				covMat.m_values[2][2] = mZZ/n;
				covMat.m_values[1][0] = covMat.m_values[0][1] = mXY/n;
				covMat.m_values[2][0] = covMat.m_values[0][2] = mXZ/n;
				covMat.m_values[2][1] = covMat.m_values[1][2] = mYZ/n;

				CCLib::SquareMatrixd eigVectors;
				std::vector<double> eigValues;

				if (!Jacobi<double>::ComputeEigenValuesAndVectors(covMat, eigVectors, eigValues))
				{
					//failure
					curv= NAN_VALUE;
				}
				//compute eigen value
				double e0 = eigValues[0];
				double e1 = eigValues[1];
				double e2 = eigValues[2];
				double sum = fabs(e0+e1+e2);
				if (sum < ZERO_TOLERANCE)
				{
					curv= NAN_VALUE;
				}

				double eMin = std::min(std::min(e0,e1),e2);
				curv= static_cast<ScalarType>(fabs(eMin) / sum);

				for (unsigned i=0; i<n; ++i)
				{
					//current point index
					unsigned index = cell.points->getPointGlobalIndex(i);
					//current point index in neighbourhood (to compute curvature at the right position!)
					unsigned indexInNeighbourhood = 0;

					cell.points->setPointScalarValue(i,curv);
				}
			}

			//and we start a new cell开始新的Cell
			cell.index += cell.points->size();
			cell.points->clear(false);
			cell.truncatedCode = currentCode;
			/*cell.mean_=G;
			cell.cov_=covMat;
			CCVector3 eigvalues1(e0,e1,e2);
			cell.evals_=eigvalues1;*/
		}

		cell.points->addPointIndex(p->theIndex); //can't fail (see above)
		predCode = currentCode;
	}

	if (result == 0)
	{
		if (pc && sfIdx >= 0)
		{
			//设置当前显示的ScalarField
			pc->setCurrentDisplayedScalarField(sfIdx);
			pc->showSF(sfIdx >= 0);
			pc->getCurrentInScalarField()->computeMinAndMax();
		}
		theCloud->prepareDisplayForRefresh();
	}
	else
	{
		ccConsole::Warning(QString("Failed to apply processing to cloud '%1'").arg(theCloud->getName()));
		if (pc && sfIdx >= 0)
		{
			pc->deleteScalarField(sfIdx);
			sfIdx = -1;
		}
	}

　　参考DgmOctree::getCellIndexes   DgmOctree::getPointsInCellByCellIndex

 //重要，获取每一八叉树层的Cell索引的集合
 bool DgmOctreeNDT::getCellIndexes(unsigned char level, cellIndexesContainer& vec) const
 {
     try
     {
         vec.resize(m_cellCount[level]);
     }
     catch (const std::bad_alloc&)
     {
         //not enough memory
         return false;
     }

     //binary shift for cell code truncation
     unsigned char bitDec = GET_NDT_BIT_SHIFT(level);

     cellsContainer::const_iterator p = m_thePointsAndTheirCellCodes.begin();

     OctreeCellCodeType predCode = (p->theCode >> bitDec)+; //pred value must be different than the first element's

     for (unsigned i=,j=; i<m_numberOfProjectedPoints; ++i,++p)
     {
         OctreeCellCodeType currentCode = (p->theCode >> bitDec);

         if (predCode != currentCode)
             vec[j++] = i;

         predCode = currentCode;
     }

     return true;
 }

DgmOctree::getCellIndexes

 bool DgmOctreeNDT::getPointsInCellByCellIndex(    ReferenceCloud* cloud,
                                             unsigned cellIndex,
                                             unsigned char level,
                                             bool clearOutputCloud/* = true*/) const
 {
     assert(cloud && cloud->getAssociatedCloud() == m_theAssociatedCloud);

     //binary shift for cell code truncation
     unsigned char bitDec = GET_NDT_BIT_SHIFT(level);

     //we look for the first index in 'm_thePointsAndTheirCellCodes' corresponding to this cell
     cellsContainer::const_iterator p = m_thePointsAndTheirCellCodes.begin()+cellIndex;
     OctreeCellCodeType searchCode = (p->theCode >> bitDec);

     if (clearOutputCloud)
         cloud->clear(false);

     //while the (partial) cell code matches this cell
     while ((p != m_thePointsAndTheirCellCodes.end()) && ((p->theCode >> bitDec) == searchCode))
     {
         if (!cloud->addPointIndex(p->theIndex))
             return false;
         ++p;
     }

     return true;
 }

DgmOctree::getPointsInCellByCellIndex

　　涉及的类包括：近邻点类、近邻点查询结构体

　　另外：

 bool qNDTRansacSD::computeCellEigenValueAtLevel(const CCNDTLib::DgmOctreeNDT::octreeCellNDT& cell,
     void** additionalParameters,NormalizedProgress* nProgress/*=0*/)
 {
     //parameters
     PointCoordinateType radius    = *static_cast<PointCoordinateType*>(additionalParameters[]);

     structure for nearest neighbors search
     int n=cell.points->size();
     CCVector3d Psum(,,);
     for (unsigned i=; i<n; ++i)
     {
         CCVector3 P;
         cell.points->getPoint(i,P);
         Psum.x += P.x;
         Psum.y += P.y;
         Psum.z += P.z;
     }
     ScalarType curv = NAN_VALUE;
     CCVector3 G(static_cast<PointCoordinateType>(Psum.x / n),
         static_cast<PointCoordinateType>(Psum.y / n),
         static_cast<PointCoordinateType>(Psum.z / n) );

     double mXX = 0.0;
     double mYY = 0.0;
     double mZZ = 0.0;
     double mXY = 0.0;
     double mXZ = 0.0;
     double mYZ = 0.0;
     //for each point in the cell
     for (unsigned i=; i<n; ++i)
     {
         CCVector3 CellP;
         cell.points->getPoint(i,CellP);
         CCVector3 P = CellP - G;

         mXX += static_cast<double>(P.x)*P.x;
         mYY += static_cast<double>(P.y)*P.y;
         mZZ += static_cast<double>(P.z)*P.z;
         mXY += static_cast<double>(P.x)*P.y;
         mXZ += static_cast<double>(P.x)*P.z;
         mYZ += static_cast<double>(P.y)*P.z;
     }
     //symmetry
     CCLib::SquareMatrixd covMat();
     covMat.m_values[][] = mXX/n;
     covMat.m_values[][] = mYY/n;
     covMat.m_values[][] = mZZ/n;
     covMat.m_values[][] = covMat.m_values[][] = mXY/n;
     covMat.m_values[][] = covMat.m_values[][] = mXZ/n;
     covMat.m_values[][] = covMat.m_values[][] = mYZ/n;

     CCLib::SquareMatrixd eigVectors;
     std::vector<double> eigValues;

     if (!Jacobi<double>::ComputeEigenValuesAndVectors(covMat, eigVectors, eigValues))
     {
         //failure
         curv= NAN_VALUE;
     }

     //compute curvature as the rate of change of the surface
     double e0 = eigValues[];
     double e1 = eigValues[];
     double e2 = eigValues[];
     double sum = fabs(e0+e1+e2);
     if (sum < ZERO_TOLERANCE)
     {
         curv= NAN_VALUE;
     }

     double eMin = std::min(std::min(e0,e1),e2);
     curv= static_cast<ScalarType>(fabs(eMin) / sum);

     for (unsigned i=; i<n; ++i)
     {
         //current point index
         unsigned index = cell.points->getPointGlobalIndex(i);
         //current point index in neighbourhood (to compute curvature at the right position!)
         unsigned indexInNeighbourhood = ;

         cell.points->setPointScalarValue(i,curv);
         if (nProgress && !nProgress->oneStep())
         {
             return false;
         }
     }

     return true;
 }

qNDTRansacSD::computeCellEigenValueAtLevel