[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单元似乎还是一件很复杂的事情啊！

1 2	`//! The coded octree structure` `cellsContainer m_thePointsAndTheirCellCodes;`

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

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

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

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

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

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

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 }