| 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 |
} |