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 }
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 }
涉及的类包括：近邻点类、近邻点查询结构体
另外：
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 }