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

最新推荐文章于 2023-03-19 13:24:06 发布

weixin_34242819

最新推荐文章于 2023-03-19 13:24:06 发布

阅读量307

点赞数

　　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

 1 //重要，获取每一八叉树层的Cell索引的集合
 2 bool DgmOctreeNDT::getCellIndexes(unsigned char level, cellIndexesContainer& vec) const
 3 {
 4     try
 5     {
 6         vec.resize(m_cellCount[level]);
 7     }
 8     catch (const std::bad_alloc&)
 9     {
10         //not enough memory
11         return false;
12     }
13 
14     //binary shift for cell code truncation
15     unsigned char bitDec = GET_NDT_BIT_SHIFT(level);
16 
17     cellsContainer::const_iterator p = m_thePointsAndTheirCellCodes.begin();
18 
19     OctreeCellCodeType predCode = (p->theCode >> bitDec)+1; //pred value must be different than the first element's
20 
21     for (unsigned i=0,j=0; i<m_numberOfProjectedPoints; ++i,++p)
22     {
23         OctreeCellCodeType currentCode = (p->theCode >> bitDec);
24 
25         if (predCode != currentCode)
26             vec[j++] = i;
27 
28         predCode = currentCode;
29     }
30 
31     return true;
32 }

DgmOctree::getCellIndexes

 1 bool DgmOctreeNDT::getPointsInCellByCellIndex(    ReferenceCloud* cloud,
 2                                             unsigned cellIndex,
 3                                             unsigned char level,
 4                                             bool clearOutputCloud/* = true*/) const
 5 {
 6     assert(cloud && cloud->getAssociatedCloud() == m_theAssociatedCloud);
 7 
 8     //binary shift for cell code truncation
 9     unsigned char bitDec = GET_NDT_BIT_SHIFT(level);
10 
11     //we look for the first index in 'm_thePointsAndTheirCellCodes' corresponding to this cell
12     cellsContainer::const_iterator p = m_thePointsAndTheirCellCodes.begin()+cellIndex;
13     OctreeCellCodeType searchCode = (p->theCode >> bitDec);
14 
15     if (clearOutputCloud)
16         cloud->clear(false);
17 
18     //while the (partial) cell code matches this cell
19     while ((p != m_thePointsAndTheirCellCodes.end()) && ((p->theCode >> bitDec) == searchCode))
20     {
21         if (!cloud->addPointIndex(p->theIndex))
22             return false;
23         ++p;
24     }
25 
26     return true;
27 }

DgmOctree::getPointsInCellByCellIndex

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

　　另外：

 1 bool qNDTRansacSD::computeCellEigenValueAtLevel(const CCNDTLib::DgmOctreeNDT::octreeCellNDT& cell,
 2     void** additionalParameters,NormalizedProgress* nProgress/*=0*/)
 3 {
 4     //parameters
 5     PointCoordinateType radius    = *static_cast<PointCoordinateType*>(additionalParameters[0]);
 6 
 7     structure for nearest neighbors search
 8     int n=cell.points->size();
 9     CCVector3d Psum(0,0,0);
10     for (unsigned i=0; i<n; ++i)
11     {
12         CCVector3 P;
13         cell.points->getPoint(i,P);
14         Psum.x += P.x;
15         Psum.y += P.y;
16         Psum.z += P.z;
17     }
18     ScalarType curv = NAN_VALUE;
19     CCVector3 G(static_cast<PointCoordinateType>(Psum.x / n),
20         static_cast<PointCoordinateType>(Psum.y / n),
21         static_cast<PointCoordinateType>(Psum.z / n) );
22 
23     double mXX = 0.0;
24     double mYY = 0.0;
25     double mZZ = 0.0;
26     double mXY = 0.0;
27     double mXZ = 0.0;
28     double mYZ = 0.0;
29     //for each point in the cell
30     for (unsigned i=0; i<n; ++i)
31     {
32         CCVector3 CellP;
33         cell.points->getPoint(i,CellP);
34         CCVector3 P = CellP - G;
35         
36         mXX += static_cast<double>(P.x)*P.x;
37         mYY += static_cast<double>(P.y)*P.y;
38         mZZ += static_cast<double>(P.z)*P.z;
39         mXY += static_cast<double>(P.x)*P.y;
40         mXZ += static_cast<double>(P.x)*P.z;
41         mYZ += static_cast<double>(P.y)*P.z;
42     }
43     //symmetry
44     CCLib::SquareMatrixd covMat(3);
45     covMat.m_values[0][0] = mXX/n;
46     covMat.m_values[1][1] = mYY/n;
47     covMat.m_values[2][2] = mZZ/n;
48     covMat.m_values[1][0] = covMat.m_values[0][1] = mXY/n;
49     covMat.m_values[2][0] = covMat.m_values[0][2] = mXZ/n;
50     covMat.m_values[2][1] = covMat.m_values[1][2] = mYZ/n;
51 
52     CCLib::SquareMatrixd eigVectors;
53     std::vector<double> eigValues;
54 
55     if (!Jacobi<double>::ComputeEigenValuesAndVectors(covMat, eigVectors, eigValues))
56     {
57         //failure
58         curv= NAN_VALUE;
59     }
60 
61     //compute curvature as the rate of change of the surface
62     double e0 = eigValues[0];
63     double e1 = eigValues[1];
64     double e2 = eigValues[2];
65     double sum = fabs(e0+e1+e2);
66     if (sum < ZERO_TOLERANCE)
67     {
68         curv= NAN_VALUE;
69     }
70 
71     double eMin = std::min(std::min(e0,e1),e2);
72     curv= static_cast<ScalarType>(fabs(eMin) / sum);
73 
74 
75     for (unsigned i=0; i<n; ++i)
76     {
77         //current point index
78         unsigned index = cell.points->getPointGlobalIndex(i);
79         //current point index in neighbourhood (to compute curvature at the right position!)
80         unsigned indexInNeighbourhood = 0;
81             
82         cell.points->setPointScalarValue(i,curv);            
83         if (nProgress && !nProgress->oneStep())
84         {
85             return false;
86         }
87     }
88     
89     return true;
90 }

qNDTRansacSD::computeCellEigenValueAtLevel

weixin_34242819

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
[CC]DgmOctree—执行Cell遍历和单元计算

　　DgmOctree类的executeFunctionForAllCellsAtLevel和executeFunctionForAllCellsStartingAtLevel方法通过回调函数octreeCellFunc，执行八叉树中每个单元的相关计算。unsigned DgmOctree::executeFunctionForAllCellsAtLevel( unsigned char ...
复制链接

扫一扫