Debian Codegraph @

plugins/core/Standard/qFacets/src/kdTreeForFacetExtraction.cpp

raw
1 //##########################################################################
2 //# #
3 //# CLOUDCOMPARE PLUGIN: qFacets #
4 //# #
5 //# This program is free software; you can redistribute it and/or modify #
6 //# it under the terms of the GNU General Public License as published by #
7 //# the Free Software Foundation; version 2 or later of the License. #
8 //# #
9 //# This program is distributed in the hope that it will be useful, #
10 //# but WITHOUT ANY WARRANTY; without even the implied warranty of #
11 //# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the #
12 //# GNU General Public License for more details. #
13 //# #
14 //# COPYRIGHT: Thomas Dewez, BRGM #
15 //# #
16 //##########################################################################
17
18 #include "kdTreeForFacetExtraction.h"
19
20 //CCCoreLib
21 #include <GenericProgressCallback.h>
22 #include <Neighbourhood.h>
23 #include <ParallelSort.h>
24
25 //qCC_db
26 #include <ccPointCloud.h>
27
28 //Qt
29 #include <QApplication>
30
31 //static bool AscendingLeafErrorComparison(const ccKdTree::Leaf* a, const ccKdTree::Leaf* b)
32 //{
33 // return a->error < b->error;
34 //}
35
36 static bool DescendingLeafSizeComparison(const ccKdTree::Leaf* a, const ccKdTree::Leaf* b)
37 {
38 return a->points->size() > b->points->size();
39 }
40
41 struct Candidate
42 {
43 ccKdTree::Leaf* leaf;
44 PointCoordinateType dist;
45 PointCoordinateType radius;
46 CCVector3 centroid;
47
48 Candidate() : leaf(nullptr), dist(CCCoreLib::PC_NAN), radius(0) {}
49 Candidate(ccKdTree::Leaf* l) : leaf(l), dist(CCCoreLib::PC_NAN), radius(0)
50 {
51 if (leaf && leaf->points)
52 {
53 CCCoreLib::Neighbourhood N(leaf->points);
54 centroid = *N.getGravityCenter();
55 radius = N.computeLargestRadius();
56 }
57 }
58 };
59
60 static bool CandidateDistAscendingComparison(const Candidate& a, const Candidate& b)
61 {
62 return a.dist < b.dist;
63 }
64
65 bool ccKdTreeForFacetExtraction::FuseCells( ccKdTree* kdTree,
66 double maxError,
67 CCCoreLib::DistanceComputationTools::ERROR_MEASURES errorMeasure,
68 double maxAngle_deg,
69 PointCoordinateType overlapCoef/*=1*/,
70 bool closestFirst/*=true*/,
71 CCCoreLib::GenericProgressCallback* progressCb/*=nullptr*/)
72 {
73 if (!kdTree)
74 return false;
75
76 ccGenericPointCloud* associatedGenericCloud = kdTree->associatedGenericCloud();
77 if (!associatedGenericCloud || !associatedGenericCloud->isA(CC_TYPES::POINT_CLOUD) || maxError < 0.0)
78 return false;
79
80 //get leaves
81 std::vector<ccKdTree::Leaf*> leaves;
82 if (!kdTree->getLeaves(leaves) || leaves.empty())
83 return false;
84
85 //progress notification
86 CCCoreLib::NormalizedProgress nProgress(progressCb, static_cast<unsigned>(leaves.size()));
87 if (progressCb)
88 {
89 progressCb->update(0);
90 if (progressCb->textCanBeEdited())
91 {
92 progressCb->setMethodTitle("Fuse Kd-tree cells");
93 progressCb->setInfo(qPrintable(QString("Cells: %1\nMax error: %2").arg(leaves.size()).arg(maxError)));
94 }
95 progressCb->start();
96 }
97
98 ccPointCloud* pc = static_cast<ccPointCloud*>(associatedGenericCloud);
99
100 //sort cells based on their population size (we start by the biggest ones)
101 ParallelSort(leaves.begin(), leaves.end(), DescendingLeafSizeComparison);
102
103 //set all 'userData' to -1 (i.e. unfused cells)
104 {
105 for (auto& leaf : leaves)
106 {
107 leaf->userData = -1;
108 //check by the way that the plane normal is unit!
109 assert(static_cast<double>(std::abs(CCVector3(leaf->planeEq).norm2()) - 1.0) < 1.0e-6);
110 }
111 }
112
113 // cosine of the max angle between fused 'planes'
114 const double c_minCosNormAngle = cos( CCCoreLib::DegreesToRadians( maxAngle_deg ) );
115
116 //fuse all cells, starting from the ones with the best error
117 const int unvisitedNeighborValue = -1;
118 bool cancelled = false;
119 int macroIndex = 1; //starts at 1 (0 is reserved for cells already above the max error)
120 {
121 for (auto& currentCell : leaves)
122 {
123 if (currentCell->error >= maxError)
124 currentCell->userData = 0; //0 = special group for cells already above the user defined threshold!
125
126 //already fused?
127 if (currentCell->userData != -1)
128 {
129 if (progressCb && !nProgress.oneStep()) //process canceled by user
130 {
131 cancelled = true;
132 break;
133 }
134 continue;
135 }
136
137 //we create a new "macro cell" index
138 currentCell->userData = macroIndex++;
139
140 //we init the current set of 'fused' points with the cell's points
141 CCCoreLib::ReferenceCloud* currentPointSet = currentCell->points;
142 //get current fused set centroid and normal
143 CCVector3 currentCentroid = *CCCoreLib::Neighbourhood(currentPointSet).getGravityCenter();
144 CCVector3 currentNormal(currentCell->planeEq);
145
146 //visited neighbors
147 ccKdTree::LeafSet visitedNeighbors;
148 //set of candidates
149 std::list<Candidate> candidates;
150
151 //we are going to iteratively look for neighbor cells that could be fused to this one
152 ccKdTree::LeafVector cellsToTest;
153 cellsToTest.push_back(currentCell);
154
155 if (progressCb && !nProgress.oneStep()) //process canceled by user
156 {
157 cancelled = true;
158 break;
159 }
160
161 while (!cellsToTest.empty() || !candidates.empty())
162 {
163 //get all neighbors around the 'waiting' cell(s)
164 if (!cellsToTest.empty())
165 {
166 ccKdTree::LeafSet neighbors;
167 while (!cellsToTest.empty())
168 {
169 if (!kdTree->getNeighborLeaves(cellsToTest.back(), neighbors, &unvisitedNeighborValue)) //we only consider unvisited cells!
170 {
171 //an error occurred
172 return false;
173 }
174 cellsToTest.pop_back();
175 }
176
177 //add those (new) neighbors to the 'visitedNeighbors' set
178 //and to the candidates set by the way if they are not yet there
179 for (ccKdTree::LeafSet::iterator it = neighbors.begin(); it != neighbors.end(); ++it)
180 {
181 ccKdTree::Leaf* neighbor = *it;
182 std::pair<ccKdTree::LeafSet::iterator,bool> ret = visitedNeighbors.insert(neighbor);
183 //neighbour not already in the set?
184 if (ret.second)
185 {
186 //we create the corresponding candidate
187 try
188 {
189 candidates.push_back(Candidate(neighbor));
190 }
191 catch (const std::bad_alloc&)
192 {
193 //not enough memory!
194 ccLog::Warning("[ccKdTreeForFacetExtraction] Not enough memory!");
195 return false;
196 }
197 }
198 }
199 }
200
201 //is there remaining candidates?
202 if (!candidates.empty())
203 {
204 //update the set of candidates
205 if (closestFirst && candidates.size() > 1)
206 {
207 for (std::list<Candidate>::iterator it = candidates.begin(); it !=candidates.end(); ++it)
208 it->dist = (it->centroid-currentCentroid).norm2();
209
210 //sort candidates by their distance
211 candidates.sort(CandidateDistAscendingComparison);
212 }
213
214 //we will keep track of the best fused 'couple' at each pass
215 std::list<Candidate>::iterator bestIt = candidates.end();
216 CCCoreLib::ReferenceCloud* bestFused = nullptr;
217 CCVector3 bestNormal(0, 0, 0);
218 double bestError = -1.0;
219
220 unsigned skipCount = 0;
221 for (std::list<Candidate>::iterator it = candidates.begin(); it != candidates.end(); /*++it*/)
222 {
223 assert(it->leaf && it->leaf->points);
224 assert(currentPointSet->getAssociatedCloud() == it->leaf->points->getAssociatedCloud());
225
226 //if the leaf orientation is too different
227 if (std::abs(CCVector3(it->leaf->planeEq).dot(currentNormal)) < c_minCosNormAngle)
228 {
229 it = candidates.erase(it);
230 //++it;
231 //++skipCount;
232 continue;
233 }
234
235 //compute the minimum distance between the candidate centroid and the 'currentPointSet'
236 PointCoordinateType minDistToMainSet = 0.0;
237 {
238 for (unsigned j = 0; j < currentPointSet->size(); ++j)
239 {
240 const CCVector3* P = currentPointSet->getPoint(j);
241 PointCoordinateType d2 = (*P - it->centroid).norm2();
242 if (d2 < minDistToMainSet || j == 0)
243 minDistToMainSet = d2;
244 }
245 minDistToMainSet = sqrt(minDistToMainSet);
246 }
247
248 //if the leaf is too far
249 if (it->radius < minDistToMainSet / overlapCoef)
250 {
251 ++it;
252 ++skipCount;
253 continue;
254 }
255
256 //fuse the main set with the current candidate
257 CCCoreLib::ReferenceCloud* fused = new CCCoreLib::ReferenceCloud(*currentPointSet);
258 if (!fused->add(*(it->leaf->points)))
259 {
260 //not enough memory!
261 ccLog::Warning("[ccKdTreeForFacetExtraction] Not enough memory!");
262 delete fused;
263 if (currentPointSet != currentCell->points)
264 delete currentPointSet;
265 return false;
266 }
267
268 //fit a plane and estimate the resulting error
269 double error = -1.0;
270 const PointCoordinateType* planeEquation = CCCoreLib::Neighbourhood(fused).getLSPlane();
271 if (planeEquation)
272 error = CCCoreLib::DistanceComputationTools::ComputeCloud2PlaneDistance(fused, planeEquation, errorMeasure);
273
274 if (error < 0.0 || error > maxError)
275 {
276 //candidate is rejected
277 it = candidates.erase(it);
278 }
279 else
280 {
281 //otherwise we keep track of the best one!
282 if (bestError < 0.0 || error < bestError)
283 {
284 bestIt = it;
285 bestError = error;
286 if (bestFused)
287 delete bestFused;
288 bestFused = fused;
289 bestNormal = CCVector3(planeEquation);
290 fused = nullptr;
291
292 if (closestFirst)
293 break; //if we have found a good candidate, we stop here (closest first ;)
294 }
295 ++it;
296 }
297
298 if (fused)
299 {
300 delete fused;
301 fused = nullptr;
302 }
303 }
304
305 //we have a (best) candidate for this pass?
306 if (bestIt != candidates.end())
307 {
308 assert(bestFused && bestError >= 0.0);
309 if (currentPointSet != currentCell->points)
310 delete currentPointSet;
311 currentPointSet = bestFused;
312 {
313 //update infos
314 CCCoreLib::Neighbourhood N(currentPointSet);
315 //currentCentroid = *N.getGravityCenter(); //if we update it, the search will naturally shift along one dimension!
316 //currentNormal = bestNormal; //same thing here for normals
317 }
318
319 bestIt->leaf->userData = currentCell->userData;
320
321 //we will test this cell's neighbors as well
322 cellsToTest.push_back(bestIt->leaf);
323
324 if (progressCb && !nProgress.oneStep()) //process canceled by user
325 {
326 //premature end!
327 candidates.clear();
328 cellsToTest.clear();
329 cancelled = true;
330 break;
331 }
332 QApplication::processEvents();
333
334 //we also remove it from the candidates list
335 candidates.erase(bestIt);
336 }
337
338 if (skipCount == candidates.size() && cellsToTest.empty())
339 {
340 //only far leaves remain...
341 candidates.clear();
342 }
343
344 }
345
346 } //no more candidates or cells to test
347
348 //end of the fusion process for the current leaf
349 if (currentPointSet != currentCell->points)
350 delete currentPointSet;
351 currentPointSet = nullptr;
352
353 if (cancelled)
354 break;
355 }
356 }
357
358 //convert fused indexes to SF
359 if (!cancelled)
360 {
361 if (!pc->enableScalarField())
362 {
363 ccLog::Error("Not enough memory");
364 return false;
365 }
366
367 for (size_t i = 0; i < leaves.size(); ++i)
368 {
369 CCCoreLib::ReferenceCloud* subset = leaves[i]->points;
370 if (subset)
371 {
372 ScalarType scalar = static_cast<ScalarType>(leaves[i]->userData);
373 if (leaves[i]->userData <= 0) //for unfused cells, we create new individual groups
374 {
375 scalar = static_cast<ScalarType>(macroIndex++);
376 //scalar = NAN_VALUE; //FIXME TEST
377 }
378 for (unsigned j = 0; j < subset->size(); ++j)
379 {
380 subset->setPointScalarValue(j, scalar);
381 }
382 }
383 }
384
385 //pc->setCurrentDisplayedScalarField(sfIdx);
386 }
387
388 return !cancelled;
389 }