Debian Codegraph @

plugins/core/Standard/qFacets/src/fastMarchingForFacetExtraction.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 "fastMarchingForFacetExtraction.h"
19
20 //CCCoreLib
21 #include <Neighbourhood.h>
22
23 //qCC_db
24 #include <ccGenericPointCloud.h>
25 #include <ccPointCloud.h>
26 #include <ccOctree.h>
27
28 //Qt
29 #include <QApplication>
30
31
32 //! 26-connexity neighbouring cells positions (common edges)
33 const int c_3dNeighboursPosShift[] {-1,-1,-1,
34 -1,-1, 0,
35 -1,-1, 1,
36 -1, 0,-1,
37 -1, 0, 0,
38 -1, 0, 1,
39 -1, 1,-1,
40 -1, 1, 0,
41 -1, 1, 1,
42 0,-1,-1,
43 0,-1, 0,
44 0,-1, 1,
45 0, 0,-1,
46 0, 0, 1,
47 0, 1,-1,
48 0, 1, 0,
49 0, 1, 1,
50 1,-1,-1,
51 1,-1, 0,
52 1,-1, 1,
53 1, 0,-1,
54 1, 0, 0,
55 1, 0, 1,
56 1, 1,-1,
57 1, 1, 0,
58 1, 1, 1 };
59
60 FastMarchingForFacetExtraction::FastMarchingForFacetExtraction()
61 : CCCoreLib::FastMarching()
62 , m_currentFacetPoints(nullptr)
63 , m_currentFacetError(0)
64 , m_maxError(0)
65 , m_errorMeasure(CCCoreLib::DistanceComputationTools::RMS)
66 , m_useRetroProjectionError(false)
67 , m_propagateProgressCb(nullptr)
68 , m_propagateProgress(0)
69 {
70 }
71
72 FastMarchingForFacetExtraction::~FastMarchingForFacetExtraction()
73 {
74 if (m_currentFacetPoints)
75 {
76 delete m_currentFacetPoints;
77 }
78 }
79
80 static bool ComputeCellStats( CCCoreLib::ReferenceCloud* subset,
81 CCVector3& N,
82 CCVector3& C,
83 ScalarType& error,
84 CCCoreLib::DistanceComputationTools::ERROR_MEASURES errorMeasure)
85 {
86 error = 0;
87
88 if (!subset || subset->size() == 0)
89 return false;
90
91 //we compute the gravity center
92 CCCoreLib::Neighbourhood Yk(subset);
93 C = *Yk.getGravityCenter();
94
95 //we compute the (least square) best fit plane
96 const PointCoordinateType* planeEquation = Yk.getLSPlane();
97 if (planeEquation)
98 {
99 N = CCVector3(planeEquation); //normal = first 3 components
100 error = CCCoreLib::DistanceComputationTools::ComputeCloud2PlaneDistance(subset, planeEquation, errorMeasure);
101 }
102 else
103 {
104 //not enough points?
105 N = CCVector3(0,0,0);
106 }
107
108 return true;
109 }
110
111 int FastMarchingForFacetExtraction::init( CCCoreLib::DgmOctree* theOctree,
112 unsigned char level,
113 ScalarType maxError,
114 CCCoreLib::DistanceComputationTools::ERROR_MEASURES errorMeasure,
115 bool useRetroProjectionError,
116 CCCoreLib::GenericProgressCallback* progressCb/*=nullptr*/)
117 {
118 m_maxError = maxError;
119 m_errorMeasure = errorMeasure;
120 m_useRetroProjectionError = useRetroProjectionError;
121
122 if (progressCb)
123 {
124 if (progressCb->textCanBeEdited())
125 {
126 progressCb->setMethodTitle("Fast Marching grid initialization");
127 progressCb->setInfo(qPrintable(QString("Level: %1").arg(level)));
128 }
129 progressCb->update(0);
130 progressCb->start();
131 }
132
133 int result = initGridWithOctree(theOctree, level);
134 if (result < 0)
135 return result;
136
137 //fill the grid with the octree
138 CCCoreLib::DgmOctree::cellCodesContainer cellCodes;
139 theOctree->getCellCodes(level, cellCodes, true);
140 size_t cellCount = cellCodes.size();
141
142 CCCoreLib::NormalizedProgress nProgress(progressCb, static_cast<unsigned>(cellCount));
143 if (progressCb)
144 {
145 progressCb->setInfo(qPrintable(QString("Level: %1\nCells: %2").arg(level).arg(cellCount)));
146 }
147
148 CCCoreLib::ReferenceCloud Yk(theOctree->associatedCloud());
149 while (!cellCodes.empty())
150 {
151 if (theOctree->getPointsInCell(cellCodes.back(), level, &Yk, true))
152 {
153 //convert the octree cell code to grid position
154 Tuple3i cellPos;
155 theOctree->getCellPos(cellCodes.back(), level, cellPos, true);
156
157 CCVector3 N;
158 CCVector3 C;
159 ScalarType error;
160 if (ComputeCellStats(&Yk, N, C, error, m_errorMeasure))
161 {
162 //convert octree cell pos to FM cell pos index
163 unsigned gridPos = pos2index(cellPos);
164
165 //create corresponding cell
166 PlanarCell* aCell = new PlanarCell;
167 aCell->cellCode = cellCodes.back();
168 aCell->N = N;
169 aCell->C = C;
170 aCell->planarError = error;
171 m_theGrid[gridPos] = aCell;
172 }
173 else
174 {
175 //an error occurred?!
176 return -10;
177 }
178 }
179
180 cellCodes.pop_back();
181
182 if (progressCb && !nProgress.oneStep())
183 {
184 //process cancelled by user
185 progressCb->stop();
186 return -1;
187 }
188 }
189
190 if (progressCb)
191 {
192 progressCb->stop();
193 }
194
195 m_initialized = true;
196
197 return 0;
198 }
199
200 int FastMarchingForFacetExtraction::step()
201 {
202 if (!m_initialized)
203 return -1;
204
205 //get 'earliest' cell
206 unsigned minTCellIndex = getNearestTrialCell();
207 if (minTCellIndex == 0)
208 return 0;
209
210 CCCoreLib::FastMarching::Cell* minTCell = m_theGrid[minTCellIndex];
211 assert(minTCell && minTCell->state != PlanarCell::ACTIVE_CELL);
212
213 if (minTCell->T < Cell::T_INF())
214 {
215 assert(m_currentFacetPoints);
216 unsigned sizeBefore = m_currentFacetPoints->size();
217
218 //check if we can add the cell to the current "ACTIVE" set
219 ScalarType error = addCellToCurrentFacet(minTCellIndex);
220
221 if (error >= 0)
222 {
223 if (error > m_maxError)
224 {
225 //resulting error would be too high
226 m_currentFacetPoints->resize(sizeBefore);
227 //we leave the cell as is (in the EMPTY state)
228 //so that we won't look at it again!
229 addIgnoredCell(minTCellIndex);
230 }
231 else
232 {
233 m_currentFacetError = error;
234
235 //add the cell to the "ACTIVE" set
236 addActiveCell(minTCellIndex);
237
238 //add its neighbors to the TRIAL set
239 for (unsigned i = 0; i < m_numberOfNeighbours; ++i)
240 {
241 //get neighbor cell
242 unsigned nIndex = minTCellIndex + m_neighboursIndexShift[i];
243 CCCoreLib::FastMarching::Cell* nCell = m_theGrid[nIndex];
244 if (nCell)
245 {
246 //if it' not yet a TRIAL cell
247 if (nCell->state == PlanarCell::FAR_CELL)
248 {
249 nCell->T = computeT(nIndex);
250 addTrialCell(nIndex);
251 }
252 //otherwise we must update it's arrival time
253 else if (nCell->state == PlanarCell::TRIAL_CELL)
254 {
255 const float& t_old = nCell->T;
256 float t_new = computeT(nIndex);
257
258 if (t_new < t_old)
259 nCell->T = t_new;
260 }
261 }
262 }
263
264 m_propagateProgress += (m_currentFacetPoints->size() - sizeBefore);
265 if (m_propagateProgressCb)
266 {
267 m_propagateProgressCb->update((100.0f * m_propagateProgress) / m_currentFacetPoints->getAssociatedCloud()->size());
268 }
269 }
270 }
271 else
272 {
273 //an error occurred
274 return -1;
275 }
276 }
277 else
278 {
279 addIgnoredCell(minTCellIndex);
280 }
281
282 return 1;
283 }
284
285 float FastMarchingForFacetExtraction::computeTCoefApprox(CCCoreLib::FastMarching::Cell* originCell, CCCoreLib::FastMarching::Cell* destCell) const
286 {
287 PlanarCell* oCell = static_cast<PlanarCell*>(originCell);
288 PlanarCell* dCell = static_cast<PlanarCell*>(destCell);
289
290 //compute the 'confidence' relatively to the neighbor cell
291 //1) it depends on the angle between the current cell's orientation
292 // and its neighbor's orientation (symmetric)
293 //2) it depends on whether the neighbor's relative position is
294 // compatible with the current cell orientation (symmetric)
295 float orientationConfidence = 0;
296 {
297 CCVector3 AB = dCell->C - oCell->C;
298 AB.normalize();
299
300 PointCoordinateType psOri = std::abs(AB.dot(oCell->N)); //ideal: 90 degrees
301 PointCoordinateType psDest = std::abs(AB.dot(dCell->N)); //ideal: 90 degrees
302 orientationConfidence = static_cast<float>((psOri + psDest) / 2); //between 0 and 1 (ideal: 0)
303 }
304
305 //add reprojection error into balance
306 if (m_useRetroProjectionError && m_octree && oCell->N.norm2() != 0)
307 {
308 PointCoordinateType theLSQPlaneEquation[4];
309 theLSQPlaneEquation[0] = oCell->N.x;
310 theLSQPlaneEquation[1] = oCell->N.y;
311 theLSQPlaneEquation[2] = oCell->N.z;
312 theLSQPlaneEquation[3] = oCell->C.dot(oCell->N);
313
314 CCCoreLib::ReferenceCloud Yk(m_octree->associatedCloud());
315 if (m_octree->getPointsInCell(oCell->cellCode, m_gridLevel, &Yk, true))
316 {
317 ScalarType reprojError = CCCoreLib::DistanceComputationTools::ComputeCloud2PlaneDistance(&Yk, theLSQPlaneEquation, m_errorMeasure);
318 if (reprojError >= 0)
319 return (1.0f - orientationConfidence) * static_cast<float>(reprojError);
320 }
321 }
322
323 return (1.0f - orientationConfidence) /** oCell->planarError*/;
324 }
325
326 int FastMarchingForFacetExtraction::propagate()
327 {
328 //init "TRIAL" set with seed's neighbors
329 initTrialCells();
330
331 int result = 1;
332 while (result > 0)
333 {
334 result = step();
335
336 if (result > 0 && m_propagateProgressCb && m_propagateProgressCb->isCancelRequested())
337 {
338 return -1;
339 }
340 }
341
342 return result;
343 }
344
345 unsigned FastMarchingForFacetExtraction::updateFlagsTable( ccGenericPointCloud* theCloud,
346 std::vector<unsigned char>& flags,
347 unsigned facetIndex)
348 {
349 if (!m_initialized || !m_currentFacetPoints)
350 return 0;
351
352 unsigned pointCount = m_currentFacetPoints->size();
353 for (unsigned k = 0; k < pointCount; ++k)
354 {
355 unsigned index = m_currentFacetPoints->getPointGlobalIndex(k);
356 flags[index] = 1;
357
358 theCloud->setPointScalarValue(index, static_cast<ScalarType>(facetIndex));
359 }
360
361 if (m_currentFacetPoints)
362 {
363 m_currentFacetPoints->clear(false);
364 }
365
366 //for (size_t i = 0; i < m_activeCells.size(); ++i)
367 //{
368 // //we remove the processed cell so as to be sure not to consider them again!
369 // CCCoreLib::FastMarching::Cell* cell = m_theGrid[m_activeCells[i]];
370 // m_theGrid[m_activeCells[i]] = 0;
371 // if (cell)
372 // delete cell;
373 //}
374
375 //unsigned pointCount = 0;
376 CCCoreLib::ReferenceCloud Yk(m_octree->associatedCloud());
377 for (size_t i = 0; i < m_activeCells.size(); ++i)
378 {
379 PlanarCell* aCell = static_cast<PlanarCell*>(m_theGrid[m_activeCells[i]]);
380 if (!m_octree->getPointsInCell(aCell->cellCode, m_gridLevel, &Yk, true))
381 continue;
382
383 for (unsigned k = 0; k < Yk.size(); ++k)
384 {
385 unsigned index = Yk.getPointGlobalIndex(k);
386 assert(flags[index] == 1);
387 //flags.setValue(index,1);
388 //++pointCount;
389 }
390
391 m_theGrid[m_activeCells[i]] = nullptr;
392 delete aCell;
393 }
394
395 return pointCount;
396 }
397
398 bool FastMarchingForFacetExtraction::setSeedCell(const Tuple3i& pos)
399 {
400 if (!CCCoreLib::FastMarching::setSeedCell(pos))
401 {
402 return false;
403 }
404
405 if (m_octree)
406 {
407 if (!m_currentFacetPoints)
408 {
409 m_currentFacetPoints = new CCCoreLib::ReferenceCloud(m_octree->associatedCloud());
410 }
411 assert(m_currentFacetPoints->size() == 0);
412
413 unsigned index = pos2index(pos);
414 m_currentFacetError = addCellToCurrentFacet(index);
415 if (m_currentFacetError < 0) //invalid error?
416 return false;
417
418 m_propagateProgress += m_currentFacetPoints->size();
419 }
420
421 return true;
422 }
423
424 ScalarType FastMarchingForFacetExtraction::addCellToCurrentFacet(unsigned index)
425 {
426 if (!m_currentFacetPoints || !m_initialized || !m_octree || m_gridLevel > CCCoreLib::DgmOctree::MAX_OCTREE_LEVEL)
427 return -1;
428
429 PlanarCell* cell = static_cast<PlanarCell*>(m_theGrid[index]);
430 if (!cell)
431 return -1;
432
433 CCCoreLib::ReferenceCloud Yk(m_octree->associatedCloud());
434 if (!m_octree->getPointsInCell(cell->cellCode, m_gridLevel, &Yk, true))
435 return -1;
436
437 if (!m_currentFacetPoints->add(Yk))
438 {
439 //not enough memory?
440 return -1;
441 }
442
443 //update error
444 CCVector3 N;
445 CCVector3 C;
446 ScalarType error;
447 ComputeCellStats(m_currentFacetPoints, N, C, error, m_errorMeasure);
448
449 return error;
450 }
451
452 void FastMarchingForFacetExtraction::initTrialCells()
453 {
454 //we expect at most one 'ACTIVE' cell (i.e. the current seed)
455 size_t seedCount = m_activeCells.size();
456 assert(seedCount <= 1);
457
458 if (seedCount == 1 && m_currentFacetError <= m_maxError)
459 {
460 unsigned index = m_activeCells.front();
461 PlanarCell* seedCell = static_cast<PlanarCell*>(m_theGrid[index]);
462
463 assert(seedCell != nullptr);
464 assert(seedCell->T == 0);
465
466 //add all its neighbour cells to the TRIAL set
467 for (unsigned i = 0; i < m_numberOfNeighbours; ++i)
468 {
469 unsigned nIndex = index + m_neighboursIndexShift[i];
470 PlanarCell* nCell = (PlanarCell*)m_theGrid[nIndex];
471 //if the neighbor exists (it shouldn't be in the TRIAL or ACTIVE sets)
472 if (nCell/* && nCell->state == PlanarCell::FAR_CELL*/)
473 {
474 assert(nCell->state == PlanarCell::FAR_CELL);
475 addTrialCell(nIndex);
476
477 //compute its approximate arrival time
478 nCell->T = seedCell->T + m_neighboursDistance[i] * computeTCoefApprox(seedCell,nCell);
479 }
480 }
481 }
482 }
483
484 int FastMarchingForFacetExtraction::ExtractPlanarFacets( ccPointCloud* theCloud,
485 unsigned char octreeLevel,
486 ScalarType maxError,
487 CCCoreLib::DistanceComputationTools::ERROR_MEASURES errorMeasure,
488 bool useRetroProjectionError/*=true*/,
489 CCCoreLib::GenericProgressCallback* progressCb/*=nullptr*/,
490 CCCoreLib::DgmOctree* _theOctree/*=nullptr*/)
491 {
492 assert(theCloud);
493
494 unsigned numberOfPoints = theCloud->size();
495 if (numberOfPoints == 0)
496 return -1;
497
498 if (!theCloud->getCurrentOutScalarField())
499 return -2;
500
501 if (progressCb)
502 {
503 //just spawn the dialog so that we can see the
504 //octree computation (in case the CPU charge prevents
505 //the dialog from being shown)
506 progressCb->start();
507 QApplication::processEvents();
508 }
509
510 //we compute the octree if none is provided
511 CCCoreLib::DgmOctree* theOctree = _theOctree;
512 QScopedPointer<CCCoreLib::DgmOctree> tempOctree;
513 if (!theOctree)
514 {
515 theOctree = new CCCoreLib::DgmOctree(theCloud);
516 tempOctree.reset(theOctree);
517 if (theOctree->build(progressCb) < 1)
518 {
519 return -3;
520 }
521 }
522
523 if (progressCb)
524 {
525 if (progressCb->textCanBeEdited())
526 {
527 progressCb->setMethodTitle("Fast Marching for facets extraction");
528 progressCb->setInfo("Initializing...");
529 }
530 progressCb->start();
531 QApplication::processEvents();
532 }
533 if (!theCloud->enableScalarField())
534 {
535 ccLog::Warning("[FastMarchingForFacetExtraction] Couldn't enable scalar field! Not enough memory?");
536 return -4;
537 }
538
539 //raz SF values
540 {
541 for (unsigned i = 0; i != theCloud->size(); ++i)
542 theCloud->setPointScalarValue(i, 0);
543 }
544
545 //flags indicating if each point has been processed or not
546 std::vector<unsigned char> flags;
547 try
548 {
549 flags.resize(numberOfPoints, 0); //defaultFlagValue = 0
550 }
551 catch (const std::bad_alloc&)
552 {
553 ccLog::Warning("[FastMarchingForFacetExtraction] Not enough memory!");
554 return -5;
555 }
556
557 //Fast Marching propagation
558 FastMarchingForFacetExtraction fm;
559
560 int result = fm.init( theOctree,
561 octreeLevel,
562 maxError,
563 errorMeasure,
564 useRetroProjectionError,
565 progressCb);
566 if (result < 0)
567 {
568 ccLog::Error("[FastMarchingForFacetExtraction] Something went wrong during initialization...");
569 return -6;
570 }
571
572 //progress notification
573 if (progressCb)
574 {
575 progressCb->update(0);
576 if (progressCb->textCanBeEdited())
577 {
578 progressCb->setMethodTitle("Facets extraction");
579 progressCb->setInfo(qPrintable(QString("Octree level: %1\nPoints: %2").arg(octreeLevel).arg(numberOfPoints)));
580 }
581 progressCb->start();
582 QApplication::processEvents();
583
584 fm.setPropagateCallback(progressCb);
585 }
586
587 const int octreeWidth = (1 << octreeLevel) - 1;
588
589 //enable 26-connectivity mode
590 //fm.setExtendedConnectivity(true);
591
592 //while non-processed points remain...
593 unsigned resolvedPoints = 0;
594 int lastProcessedPoint = -1;
595 unsigned facetIndex = 0;
596 while (true)
597 {
598 //find the next non-processed point
599 do
600 {
601 ++lastProcessedPoint;
602 }
603 while (lastProcessedPoint < static_cast<int>(numberOfPoints) && flags[lastProcessedPoint] != 0);
604
605 //all points have been processed? Then we can stop.
606 if (lastProcessedPoint == static_cast<int>(numberOfPoints))
607 {
608 break;
609 }
610
611 //we start the propagation from this point
612 //(from its corresponding cell in fact ;)
613 const CCVector3* thePoint = theCloud->getPoint(lastProcessedPoint);
614 Tuple3i pos;
615 theOctree->getTheCellPosWhichIncludesThePoint(thePoint, pos, octreeLevel);
616
617 //clipping (in case the octree is not 'complete')
618 pos.x = std::min(octreeWidth, pos.x);
619 pos.y = std::min(octreeWidth, pos.y);
620 pos.z = std::min(octreeWidth, pos.z);
621
622 //set corresponding FM cell as 'seed'
623 if (!fm.setSeedCell(pos))
624 {
625 //an error occurred?!
626 //result = -7;
627 //break;
628 continue;
629 }
630
631 //launch propagation
632 int propagationResult = fm.propagate();
633 if (propagationResult < 0)
634 {
635 //an error occurred or the process was cancelled by the user
636 result = -7;
637 break;
638 }
639
640 //compute the number of points processed during this pass
641 unsigned count = fm.updateFlagsTable(theCloud, flags, ++facetIndex);
642
643 if (count != 0)
644 {
645 resolvedPoints += count;
646 //if (progressCb)
647 //{
648 // if (progressCb->isCancelRequested())
649 // {
650 // result = -7;
651 // break;
652 // }
653 // progressCb->update((resolvedPoints * 100.0f) / numberOfPoints);
654 //}
655 }
656
657 fm.cleanLastPropagation();
658 }
659
660 fm.setPropagateCallback(nullptr);
661
662 if (progressCb)
663 {
664 progressCb->stop();
665 }
666
667 return result;
668 }