Debian Codegraph @

plugins/core/Standard/qRANSAC_SD/src/qRANSAC_SD.cpp

raw
1 //##########################################################################
2 //# #
3 //# CLOUDCOMPARE PLUGIN: qRANSAC_SD #
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: Daniel Girardeau-Montaut #
15 //# #
16 //##########################################################################
17
18 #include "qRANSAC_SD.h"
19 #include "qRANSAC_SD_Commands.h"
20
21 //PrimitiveShapes/MiscLib
22 #include <RansacShapeDetector.h>
23 #include <PlanePrimitiveShapeConstructor.h>
24 #include <CylinderPrimitiveShapeConstructor.h>
25 #include <SpherePrimitiveShapeConstructor.h>
26 #include <ConePrimitiveShapeConstructor.h>
27 #include <TorusPrimitiveShapeConstructor.h>
28 #include <PlanePrimitiveShape.h>
29 #include <SpherePrimitiveShape.h>
30 #include <CylinderPrimitiveShape.h>
31 #include <ConePrimitiveShape.h>
32 #include <TorusPrimitiveShape.h>
33
34 //Dialog
35 #include "ccRansacSDDlg.h"
36
37 //Qt
38 #include <QtGui>
39 #include <QApplication>
40 #include <QtConcurrentRun>
41 #include <QApplication>
42 #include <QMainWindow>
43
44 //qCC_db
45 #include <ccGenericPointCloud.h>
46 #include <ccPointCloud.h>
47 #include <ccGenericMesh.h>
48 #include <ccPlane.h>
49 #include <ccSphere.h>
50 #include <ccCylinder.h>
51 #include <ccCone.h>
52 #include <ccTorus.h>
53 #include <ccProgressDialog.h>
54
55 //CCCoreLib
56 #include <ScalarField.h>
57 #include <CCPlatform.h>
58
59 //System
60 #include <algorithm>
61 #if defined(CC_WINDOWS)
62 #include "windows.h"
63 #else
64 #include <time.h>
65 #endif
66
67 static ccMainAppInterface* s_app = nullptr;
68
69 qRansacSD::qRansacSD(QObject* parent/*=nullptr*/)
70 : QObject(parent)
71 , ccStdPluginInterface(":/CC/plugin/qRANSAC_SD/info.json")
72 , m_action(nullptr)
73 {
74 s_app = m_app;
75 }
76
77 void qRansacSD::onNewSelection(const ccHObject::Container& selectedEntities)
78 {
79 if (m_action)
80 m_action->setEnabled(selectedEntities.size() == 1 && selectedEntities[0]->isA(CC_TYPES::POINT_CLOUD));
81 }
82
83 QList<QAction*> qRansacSD::getActions()
84 {
85 //default action
86 if (!m_action)
87 {
88 m_action = new QAction(getName(), this);
89 m_action->setToolTip(getDescription());
90 m_action->setIcon(getIcon());
91 //connect signal
92 connect(m_action, &QAction::triggered, this, &qRansacSD::doAction);
93 }
94
95 return QList<QAction*>{ m_action };
96 }
97
98 void qRansacSD::registerCommands(ccCommandLineInterface* cmd)
99 {
100 if (!cmd)
101 {
102 assert(false);
103 return;
104 }
105 cmd->registerCommand(ccCommandLineInterface::Command::Shared(new CommandRANSAC));
106 }
107
108 static MiscLib::Vector< std::pair< MiscLib::RefCountPtr< PrimitiveShape >, size_t > >* s_shapes; // stores the detected shapes
109 static size_t s_remainingPoints = 0;
110 static RansacShapeDetector* s_detector = 0;
111 static PointCloud* s_cloud = 0;
112 void doDetection()
113 {
114 if (!s_detector || !s_cloud || !s_shapes)
115 return;
116
117 s_remainingPoints = s_detector->Detect(*s_cloud, 0, s_cloud->size(), s_shapes);
118 }
119
120 //for parameters persistence
121 static unsigned s_supportPoints = 500; // this is the minimal numer of points required for a primitive
122 static double s_maxNormalDev_deg = 25.0; // maximal normal deviation from ideal shape (in degrees)
123 static double s_proba = 0.01; // probability that no better candidate was overlooked during sampling
124 static bool s_primEnabled[5] = { true,true,true,false,false };
125 static bool s_allowSimplification = true;
126 static bool s_allowFitting = true;
127 static bool s_createCloudFromLeftOverPoints = true;
128 static bool s_randomColor = true;
129 static bool s_minSphereRadiusEnabled = false;
130 static bool s_maxSphereRadiusEnabled = false;
131 static bool s_minCylinderRadiusEnabled = false;
132 static bool s_maxCylinderRadiusEnabled = false;
133 static bool s_maxConeRadiusEnabled = false;
134 static bool s_maxConeLengthEnabled = false;
135 static bool s_maxConeAngleEnabled = false;
136 static bool s_maxCylinderLengthEnabled = false;
137 static bool s_minTorusMinorRadiusEnabled = false;
138 static bool s_minTorusMajorRadiusEnabled = false;
139 static bool s_maxTorusMinorRadiusEnabled = false;
140 static bool s_maxTorusMajorRadiusEnabled = false;
141 static double s_minSphereRadius = 1;
142 static double s_maxSphereRadius = 1;
143 static double s_minCylinderRadius = 1;
144 static double s_maxCylinderRadius = 1;
145 static double s_maxConeRadius = 1;
146 static double s_maxCylinderLength = 1;
147 static double s_maxConeLength = 1;
148 static double s_maxConeAngle_deg = 90;
149 static double s_minTorusMinorRadius = 1;
150 static double s_minTorusMajorRadius = 1;
151 static double s_maxTorusMinorRadius = 1;
152 static double s_maxTorusMajorRadius = 1;
153
154 void qRansacSD::doAction()
155 {
156 assert(m_app);
157 if (!m_app)
158 return;
159
160 const ccHObject::Container& selectedEntities = m_app->getSelectedEntities();
161 size_t selNum = selectedEntities.size();
162 if (selNum != 1)
163 {
164 ccLog::Error("[qRansacSD] Select only one cloud!");
165 return;
166 }
167
168 ccHObject* ent = selectedEntities[0];
169 assert(ent);
170 if (!ent || !ent->isA(CC_TYPES::POINT_CLOUD))
171 {
172 ccLog::Error("[qRansacSD] Select a real point cloud!");
173 return;
174 }
175
176 ccPointCloud* pc = static_cast<ccPointCloud*>(ent);
177
178 //input cloud
179 CCVector3 bbMin, bbMax;
180 pc->getBoundingBox(bbMin, bbMax);
181 CCVector3 diff = bbMax - bbMin;
182 float scale = std::max(std::max(diff[0], diff[1]), diff[2]);
183
184
185 //init dialog with default values
186 ccRansacSDDlg rsdDlg(m_app->getMainWindow());
187 rsdDlg.advancedConeGroupBox->setVisible(false);
188 rsdDlg.epsilonDoubleSpinBox->setValue(.005 * scale); // set distance threshold to 0.5% of bounding box width
189 rsdDlg.bitmapEpsilonDoubleSpinBox->setValue(.01 * scale); // set bitmap resolution (= sampling resolution) to 1% of bounding box width
190 rsdDlg.supportPointsSpinBox->setValue(s_supportPoints);
191 rsdDlg.maxNormDevAngleSpinBox->setValue(s_maxNormalDev_deg);
192 rsdDlg.probaDoubleSpinBox->setValue(s_proba);
193 rsdDlg.planeCheckBox->setChecked(s_primEnabled[0]);
194 rsdDlg.sphereCheckBox->setChecked(s_primEnabled[1]);
195 rsdDlg.cylinderCheckBox->setChecked(s_primEnabled[2]);
196 rsdDlg.coneCheckBox->setChecked(s_primEnabled[3]);
197 rsdDlg.torusCheckBox->setChecked(s_primEnabled[4]);
198 rsdDlg.allowFittingcheckBox->setChecked(s_allowFitting);
199 rsdDlg.simplifyShapescheckBox->setChecked(s_allowSimplification);
200 rsdDlg.saveLeftOverscheckBox->setChecked(s_createCloudFromLeftOverPoints);
201 rsdDlg.minSphereRadiuscheckBox->setChecked(s_minSphereRadiusEnabled);
202 rsdDlg.maxSphereRadiuscheckBox->setChecked(s_maxSphereRadiusEnabled);
203 rsdDlg.minCylinderRadiuscheckBox->setChecked(s_minCylinderRadiusEnabled);
204 rsdDlg.maxCylinderRadiuscheckBox->setChecked(s_maxCylinderRadiusEnabled);
205 rsdDlg.maxConeAnglecheckBox->setChecked(s_maxConeAngleEnabled);
206 rsdDlg.maxConeRadiuscheckBox->setChecked(s_maxConeRadiusEnabled);
207 rsdDlg.maxConeLengthcheckBox->setChecked(s_maxConeLengthEnabled);
208 rsdDlg.minSphereRadiusdoubleSpinBox->setValue(s_minSphereRadius);
209 rsdDlg.maxSphereRadiusdoubleSpinBox->setValue(s_maxSphereRadius);
210 rsdDlg.minCylinderRadiusdoubleSpinBox->setValue(s_minCylinderRadius);
211 rsdDlg.maxCylinderRadiusdoubleSpinBox->setValue(s_maxCylinderRadius);
212 rsdDlg.maxConeRadiusdoubleSpinBox->setValue(s_maxConeRadius);
213 rsdDlg.maxConeLengthdoubleSpinBox->setValue(s_maxConeLength);
214 rsdDlg.maxConeAngledoubleSpinBox->setValue(s_maxConeAngle_deg);
215 rsdDlg.minTorusMinorRadiuscheckBox->setChecked(s_minTorusMinorRadiusEnabled);
216 rsdDlg.minTorusMajorRadiuscheckBox->setChecked(s_minTorusMajorRadiusEnabled);
217 rsdDlg.maxTorusMinorRadiuscheckBox->setChecked(s_maxTorusMinorRadiusEnabled);
218 rsdDlg.maxTorusMajorRadiuscheckBox->setChecked(s_maxTorusMajorRadiusEnabled);
219 rsdDlg.maxTorusMinorRadiusdoubleSpinBox->setValue(s_maxTorusMinorRadius);
220 rsdDlg.maxTorusMajorRadiusdoubleSpinBox->setValue(s_maxTorusMajorRadius);
221 rsdDlg.randomColorcheckBox->setChecked(s_randomColor);
222 if (!rsdDlg.exec())
223 {
224 return;
225 }
226 s_minSphereRadiusEnabled = rsdDlg.minSphereRadiuscheckBox->isChecked();
227 s_maxSphereRadiusEnabled = rsdDlg.maxSphereRadiuscheckBox->isChecked();
228 s_minCylinderRadiusEnabled = rsdDlg.minCylinderRadiuscheckBox->isChecked();
229 s_maxCylinderRadiusEnabled = rsdDlg.maxCylinderRadiuscheckBox->isChecked();
230 s_maxConeRadiusEnabled = rsdDlg.maxConeRadiuscheckBox->isChecked();
231 s_maxConeLengthEnabled = rsdDlg.maxConeLengthcheckBox->isChecked();
232 s_maxConeAngleEnabled = rsdDlg.maxConeAnglecheckBox->isChecked();
233 s_minTorusMinorRadiusEnabled = rsdDlg.minTorusMinorRadiuscheckBox->isChecked();
234 s_minTorusMajorRadiusEnabled = rsdDlg.minTorusMajorRadiuscheckBox->isChecked();
235 s_maxTorusMinorRadiusEnabled = rsdDlg.maxTorusMinorRadiuscheckBox->isChecked();
236 s_maxTorusMajorRadiusEnabled = rsdDlg.maxTorusMajorRadiuscheckBox->isChecked();
237 s_randomColor = rsdDlg.randomColorcheckBox->isChecked();
238 RansacParams params;
239 {
240 params.epsilon = static_cast<float>(rsdDlg.epsilonDoubleSpinBox->value());
241 params.bitmapEpsilon = static_cast<float>(rsdDlg.bitmapEpsilonDoubleSpinBox->value());
242 params.maxNormalDev_deg = static_cast<float>(rsdDlg.maxNormDevAngleSpinBox->value());
243 params.probability = static_cast<float>(rsdDlg.probaDoubleSpinBox->value());
244 params.randomColor = s_randomColor;
245 params.supportPoints = static_cast<unsigned>(rsdDlg.supportPointsSpinBox->value());
246 params.primEnabled[RPT_PLANE] = rsdDlg.planeCheckBox->isChecked();
247 params.primEnabled[RPT_SPHERE] = rsdDlg.sphereCheckBox->isChecked();
248 params.primEnabled[RPT_CYLINDER] = rsdDlg.cylinderCheckBox->isChecked();
249 params.primEnabled[RPT_CONE] = rsdDlg.coneCheckBox->isChecked();
250 params.primEnabled[RPT_TORUS] = rsdDlg.torusCheckBox->isChecked();
251 params.createCloudFromLeftOverPoints = rsdDlg.saveLeftOverscheckBox->isChecked();
252 params.allowFitting = rsdDlg.allowFittingcheckBox->isChecked();
253 params.allowSimplification = rsdDlg.simplifyShapescheckBox->isChecked();
254 if (s_minSphereRadiusEnabled)
255 {
256 params.minSphereRadius = static_cast<float>(rsdDlg.minSphereRadiusdoubleSpinBox->value());
257 s_minSphereRadius = params.minSphereRadius;
258 }
259 if (s_maxSphereRadiusEnabled)
260 {
261 params.maxSphereRadius = static_cast<float>(rsdDlg.maxSphereRadiusdoubleSpinBox->value());
262 s_maxSphereRadius = params.maxSphereRadius;
263 }
264 if (s_minCylinderRadiusEnabled)
265 {
266 params.minCylinderRadius = static_cast<float>(rsdDlg.minCylinderRadiusdoubleSpinBox->value());
267 s_minCylinderRadius = params.minCylinderRadius;
268 }
269 if (s_maxCylinderRadiusEnabled)
270 {
271 params.maxCylinderRadius = static_cast<float>(rsdDlg.maxCylinderRadiusdoubleSpinBox->value());
272 s_maxCylinderRadius = params.maxCylinderRadius;
273 }
274 if (s_maxConeRadiusEnabled)
275 {
276 params.maxConeRadius = static_cast<float>(rsdDlg.maxConeRadiusdoubleSpinBox->value());
277 s_maxConeRadius = params.maxConeRadius;
278 }
279
280 if (s_maxConeLengthEnabled)
281 {
282 params.maxConeLength = static_cast<float>(rsdDlg.maxConeLengthdoubleSpinBox->value());
283 s_maxConeLength = params.maxConeLength;
284 }
285 if (s_maxConeAngleEnabled)
286 {
287 params.maxConeAngle_deg = static_cast<float>(rsdDlg.maxConeAngledoubleSpinBox->value());
288 s_maxConeAngle_deg = params.maxConeAngle_deg;
289 }
290 if (s_minTorusMinorRadiusEnabled)
291 {
292 params.minTorusMinorRadius = static_cast<float>(rsdDlg.minTorusMinorRadiusdoubleSpinBox->value());
293 s_minTorusMinorRadius = params.minTorusMinorRadius;
294 }
295 if (s_minTorusMajorRadiusEnabled)
296 {
297 params.minTorusMajorRadius = static_cast<float>(rsdDlg.minTorusMajorRadiusdoubleSpinBox->value());
298 s_minTorusMajorRadius = params.minTorusMajorRadius;
299 }
300 if (s_maxTorusMinorRadiusEnabled)
301 {
302 params.maxTorusMinorRadius = static_cast<float>(rsdDlg.maxTorusMinorRadiusdoubleSpinBox->value());
303 s_maxTorusMinorRadius = params.maxTorusMinorRadius;
304 }
305 if (s_maxTorusMajorRadiusEnabled)
306 {
307 params.maxTorusMajorRadius = static_cast<float>(rsdDlg.maxTorusMajorRadiusdoubleSpinBox->value());
308 s_maxTorusMajorRadius = params.maxTorusMajorRadius;
309 }
310 }
311
312 ccHObject* group = executeRANSAC(pc, params, false);
313
314
315 if (group)
316 {
317 m_app->addToDB(group);
318 m_app->refreshAll();
319 }
320 }
321
322
323 ccHObject* qRansacSD::executeRANSAC(ccPointCloud* ccPC, const RansacParams& params, bool silent)
324 {
325 //consistency check
326 {
327 unsigned char primCount = 0;
328 for (unsigned char k = 0; k < 5; ++k)
329 {
330 primCount += static_cast<unsigned>(params.primEnabled[k]);
331 }
332 if (primCount == 0)
333 {
334 ccLog::Error("[qRansacSD] No primitive type selected!");
335 return nullptr;
336 }
337 }
338 for (unsigned char k = 0; k < 5; ++k)
339 {
340 s_primEnabled[k] = params.primEnabled[k];
341 }
342 s_supportPoints = params.supportPoints;
343 s_maxNormalDev_deg = params.maxNormalDev_deg;
344 s_proba = params.probability;
345 s_createCloudFromLeftOverPoints = params.createCloudFromLeftOverPoints;
346 s_allowSimplification = params.allowSimplification;
347 unsigned count = ccPC->size();
348 bool hasNorms = ccPC->hasNormals();
349 CCVector3 bbMin, bbMax;
350 ccPC->getBoundingBox(bbMin, bbMax);
351 const CCVector3d& globalShift = ccPC->getGlobalShift();
352 double globalScale = ccPC->getGlobalScale();
353 PointCloud cloud;
354 {
355 try
356 {
357 cloud.reserve(count);
358 }
359 catch (...)
360 {
361 ccLog::Error("[qRansacSD] Could not create temporary cloud, Not enough memory!");
362 return nullptr;
363 }
364
365 //default point & normal
366 Point Pt;
367 Pt.normal[0] = 0.0;
368 Pt.normal[1] = 0.0;
369 Pt.normal[2] = 0.0;
370 for (unsigned i = 0; i < count; ++i)
371 {
372 const CCVector3* P = ccPC->getPoint(i);
373 Pt.pos[0] = static_cast<float>(P->x);
374 Pt.pos[1] = static_cast<float>(P->y);
375 Pt.pos[2] = static_cast<float>(P->z);
376 if (hasNorms)
377 {
378 const CCVector3& N = ccPC->getPointNormal(i);
379 Pt.normal[0] = static_cast<float>(N.x);
380 Pt.normal[1] = static_cast<float>(N.y);
381 Pt.normal[2] = static_cast<float>(N.z);
382 }
383 #ifdef POINTSWITHINDEX
384 Pt.index = i;
385 #endif
386 cloud.push_back(Pt);
387 }
388
389 //manually set bounding box!
390 Vec3f cbbMin, cbbMax;
391 cbbMin[0] = static_cast<float>(bbMin.x);
392 cbbMin[1] = static_cast<float>(bbMin.y);
393 cbbMin[2] = static_cast<float>(bbMin.z);
394 cbbMax[0] = static_cast<float>(bbMax.x);
395 cbbMax[1] = static_cast<float>(bbMax.y);
396 cbbMax[2] = static_cast<float>(bbMax.z);
397 cloud.setBBox(cbbMin, cbbMax);
398 }
399
400 RansacShapeDetector::Options ransacOptions;
401 {
402 ransacOptions.m_epsilon = params.epsilon;
403 ransacOptions.m_bitmapEpsilon = params.bitmapEpsilon;
404 ransacOptions.m_normalThresh = static_cast<float>(cos( CCCoreLib::DegreesToRadians( params.maxNormalDev_deg ) ));
405 assert(ransacOptions.m_normalThresh >= 0);
406 ransacOptions.m_probability = params.probability;
407 ransacOptions.m_minSupport = params.supportPoints;
408 ransacOptions.m_allowSimplification = params.allowSimplification;
409 ransacOptions.m_fitting = params.allowFitting ? RansacShapeDetector::Options::LS_FITTING : RansacShapeDetector::Options::NO_FITTING;
410 }
411 const float scale = cloud.getScale();
412
413 if (!hasNorms)
414 {
415 ccProgressDialog* pDlg = nullptr;
416 if (!silent)
417 {
418 pDlg = new ccProgressDialog(false, s_app ? s_app->getMainWindow() : nullptr);
419 pDlg->setWindowTitle("Ransac Shape Detection");
420 pDlg->setMethodTitle(tr("Computing normals (please wait)"));
421 pDlg->setRange(0, 0); // infinite loop
422 pDlg->show();
423 }
424 QApplication::processEvents();
425
426 cloud.calcNormals(.01f * scale);
427
428 if (ccPC->reserveTheNormsTable())
429 {
430 for (unsigned i = 0; i < count; ++i)
431 {
432 Vec3f& Nvi = cloud[i].normal;
433 CCVector3 Ni = CCVector3::fromArray(Nvi);
434 //normalize the vector in case of
435 Ni.normalize();
436 ccPC->addNorm(Ni);
437 }
438 ccPC->showNormals(true);
439
440 //currently selected entities appearance may have changed!
441 ccPC->prepareDisplayForRefresh_recursive();
442 }
443 else
444 {
445 ccLog::Error("[qRansacSD] Not enough memory to compute normals!");
446 if (pDlg)
447 {
448 pDlg->hide();
449 delete pDlg;
450 }
451 return nullptr;
452 }
453 if (pDlg)
454 {
455 pDlg->hide();
456 delete pDlg;
457 }
458 }
459
460 RansacShapeDetector detector(ransacOptions); // the detector object
461
462 if (params.primEnabled[RPT_PLANE])
463 detector.Add(new PlanePrimitiveShapeConstructor());
464 if (params.primEnabled[RPT_SPHERE])
465 detector.Add(new SpherePrimitiveShapeConstructor(params.minSphereRadius, params.maxSphereRadius));
466 if (params.primEnabled[RPT_CYLINDER])
467 detector.Add(new CylinderPrimitiveShapeConstructor(params.minCylinderRadius, params.maxCylinderRadius, params.maxCylinderLength));
468 if (params.primEnabled[RPT_CONE])
469 detector.Add(new ConePrimitiveShapeConstructor(params.maxConeRadius, CCCoreLib::DegreesToRadians(params.maxConeAngle_deg), params.maxConeLength));
470 if (params.primEnabled[RPT_TORUS])
471 detector.Add(new TorusPrimitiveShapeConstructor(false, params.minTorusMinorRadius, params.minTorusMajorRadius, params.maxTorusMinorRadius, params.maxTorusMajorRadius)); // Do not allow apple shaped torus
472
473
474 unsigned remaining = count;
475 typedef std::pair< MiscLib::RefCountPtr< PrimitiveShape >, size_t > DetectedShape;
476 MiscLib::Vector< DetectedShape > shapes; // stores the detected shapes
477
478 // run detection
479 // returns number of unassigned points
480 // the array shapes is filled with pointers to the detected shapes
481 // the second element per shapes gives the number of points assigned to that primitive (the support)
482 // the points belonging to the first shape (shapes[0]) have been sorted to the end of pc,
483 // i.e. into the range [ pc.size() - shapes[0].second, pc.size() )
484 // the points of shape i are found in the range
485 // [ pc.size() - \sum_{j=0..i} shapes[j].second, pc.size() - \sum_{j=0..i-1} shapes[j].second )
486
487 {
488 //progress dialog (Qtconcurrent::run can't be canceled!)
489 ccProgressDialog* pDlg = nullptr;
490 if (!silent)
491 {
492 pDlg = new ccProgressDialog(false, s_app ? s_app->getMainWindow() : nullptr);
493 pDlg->setWindowTitle("Ransac Shape Detection");
494 pDlg->setMethodTitle(tr("Operation in progress (please wait)"));
495 pDlg->setRange(0, 0); // infinite progress
496 pDlg->show();
497 }
498
499 //run in a separate thread
500 s_detector = &detector;
501 s_shapes = &shapes;
502 s_cloud = &cloud;
503 QElapsedTimer eTimer;
504 eTimer.start();
505 QFuture<void> future = QtConcurrent::run(doDetection);
506
507 while (!future.isFinished())
508 {
509 #if defined(CC_WINDOWS)
510 ::Sleep(500);
511 #else
512 usleep(500 * 1000);
513 #endif
514 if (!silent && pDlg)
515 {
516 pDlg->setValue(pDlg->value() + 1);
517 }
518 QApplication::processEvents();
519 }
520 remaining = static_cast<unsigned>(s_remainingPoints);
521
522 QApplication::processEvents();
523 if (pDlg)
524 {
525 pDlg->hide();
526 delete pDlg;
527 }
528 qint64 elapsedTime_ms = eTimer.elapsed();
529
530 ccLog::Print("[qRANSAC] Search Timing: %2.3f s", static_cast<double>(elapsedTime_ms) / 1.0e3);
531 }
532
533 #if 0 //def _DEBUG
534 FILE* fp = fopen("RANS_SD_trace.txt", "wt");
535
536 fprintf(fp, "[Options]\n");
537 fprintf(fp, "epsilon=%f\n", ransacOptions.m_epsilon);
538 fprintf(fp, "bitmap epsilon=%f\n", ransacOptions.m_bitmapEpsilon);
539 fprintf(fp, "normal thresh=%f\n", ransacOptions.m_normalThresh);
540 fprintf(fp, "min support=%i\n", ransacOptions.m_minSupport);
541 fprintf(fp, "probability=%f\n", ransacOptions.m_probability);
542
543 fprintf(fp, "\n[Statistics]\n");
544 fprintf(fp, "input points=%i\n", count);
545 fprintf(fp, "segmented=%i\n", count - remaining);
546 fprintf(fp, "remaining=%i\n", remaining);
547
548 if (shapes.size() > 0)
549 {
550 fprintf(fp, "\n[Shapes]\n");
551 for (unsigned i = 0; i < shapes.size(); ++i)
552 {
553 PrimitiveShape* shape = shapes[i].first;
554 size_t shapePointsCount = shapes[i].second;
555
556 std::string desc;
557 shape->Description(&desc);
558 fprintf(fp, "#%i - %s - %i points\n", i + 1, desc.c_str(), shapePointsCount);
559 }
560 }
561 fclose(fp);
562 #endif
563
564
565 if (remaining == count)
566 {
567 ccLog::Error("[qRansacSD] Segmentation failed...");
568 return nullptr;
569 }
570
571 if (shapes.size() > 0)
572 {
573 unsigned planeCount = 1;
574 unsigned sphereCount = 1;
575 unsigned cylinderCount = 1;
576 unsigned coneCount = 1;
577 unsigned torusCount = 1;
578 ccHObject* group = nullptr;
579 for (MiscLib::Vector<DetectedShape>::const_iterator it = shapes.begin(); it != shapes.end(); ++it)
580 {
581 const PrimitiveShape* shape = it->first;
582 unsigned shapePointsCount = static_cast<unsigned>(it->second);
583
584 //too many points?!
585 if (shapePointsCount > count)
586 {
587 ccLog::Error("[qRansacSD] Inconsistent result!");
588 break;
589 }
590
591 if (shapePointsCount < params.supportPoints)
592 {
593 ccLog::Warning("[qRansacSD] Skipping shape, did not meet minimum point requirement");
594 count -= shapePointsCount;
595 continue;
596 }
597
598 std::string desc;
599 shape->Description(&desc);
600
601 // points to current shapes last point in cloud
602 const auto shapeCloudIndex = count - 1;
603
604 //new cloud for sub-part
605 ccPointCloud* pcShape = nullptr;
606 bool saveNormals = true;
607 {
608 #ifdef POINTSWITHINDEX
609 CCCoreLib::ReferenceCloud refPcShape(ccPC);
610 //we fill cloud with sub-part points
611 if (!refPcShape.reserve(static_cast<unsigned>(shapePointsCount)))
612 {
613 ccLog::Error("[qRansacSD] Not enough memory!");
614 break;
615 }
616
617 for (unsigned j = 0; j < shapePointsCount; ++j)
618 {
619 refPcShape.addPointIndex(cloud[shapeCloudIndex - j].index);
620 }
621 int warnings = 0;
622 pcShape = ccPC->partialClone(&refPcShape, &warnings);
623 if (warnings != 0)
624 {
625 if ((warnings & ccPointCloud::WRN_OUT_OF_MEM_FOR_NORMALS) == ccPointCloud::WRN_OUT_OF_MEM_FOR_NORMALS)
626 {
627 saveNormals = false;
628 }
629 }
630
631 #else
632 pcShape = new ccPointCloud(desc.c_str());
633 if (!pcShape->reserve(static_cast<unsigned>(shapePointsCount)))
634 {
635 ccLog::Error("[qRansacSD] Not enough memory!");
636 delete pcShape;
637 break;
638 }
639 saveNormals = pcShape->reserveTheNormsTable();
640
641 for (unsigned j = 0; j < shapePointsCount; ++j)
642 {
643 pcShape->addPoint(CCVector3::fromArray(cloud[shapeCloudIndex - j].pos));
644 if (saveNormals)
645 {
646 pcShape->addNorm(CCVector3::fromArray(cloud[shapeCloudIndex - j].normal));
647 }
648
649 }
650 pcShape->setGlobalShift(globalShift);
651 pcShape->setGlobalScale(globalScale);
652 #endif
653 }
654 //random color
655 ccColor::Rgb col = ccColor::Generator::Random();
656 if (params.randomColor)
657 {
658 pcShape->setColor(col);
659 pcShape->showSF(false);
660 pcShape->showColors(true);
661 }
662 pcShape->showNormals(saveNormals);
663 pcShape->setVisible(true);
664
665
666 //convert detected primitive into a CC primitive type
667 ccGenericPrimitive* prim = nullptr;
668 switch (shape->Identifier())
669 {
670 case RPT_PLANE: //plane
671 {
672 const PlanePrimitiveShape* plane = static_cast<const PlanePrimitiveShape*>(shape);
673 Vec3f G = plane->Internal().getPosition();
674 Vec3f N = plane->Internal().getNormal();
675 Vec3f X = plane->getXDim();
676 Vec3f Y = plane->getYDim();
677
678 //we look for real plane extents
679 float minX, maxX, minY, maxY;
680 for (unsigned j = 0; j < shapePointsCount; ++j)
681 {
682 std::pair<float, float> param;
683 plane->Parameters(cloud[shapeCloudIndex - j].pos, &param);
684 if (j != 0)
685 {
686 if (minX < param.first)
687 minX = param.first;
688 else if (maxX > param.first)
689 maxX = param.first;
690 if (minY < param.second)
691 minY = param.second;
692 else if (maxY > param.second)
693 maxY = param.second;
694 }
695 else
696 {
697 minX = maxX = param.first;
698 minY = maxY = param.second;
699 }
700 }
701
702 //we recenter plane (as it is not always the case!)
703 float dX = maxX - minX;
704 float dY = maxY - minY;
705 G += X * (minX + dX / 2);
706 G += Y * (minY + dY / 2);
707
708 //we build matrix from these vectors
709 ccGLMatrix glMat(CCVector3::fromArray(X.getValue()),
710 CCVector3::fromArray(Y.getValue()),
711 CCVector3::fromArray(N.getValue()),
712 CCVector3::fromArray(G.getValue()));
713
714 //plane primitive
715 //ccLog::Print(QString("dX: %1, dY: %2").arg(dX).arg(dY));
716 prim = new ccPlane(std::abs(dX), std::abs(dY), &glMat);
717 prim->setSelectionBehavior(ccHObject::SELECTION_FIT_BBOX);
718 prim->enableStippling(true);
719 PointCoordinateType dip = 0.0f;
720 PointCoordinateType dipDir = 0.0f;
721 ccNormalVectors::ConvertNormalToDipAndDipDir(CCVector3::fromArray(N.getValue()), dip, dipDir);
722 QString dipAndDipDirStr = ccNormalVectors::ConvertDipAndDipDirToString(dip, dipDir);
723 prim->setName(dipAndDipDirStr);
724 pcShape->setName(QString("Plane_%1").arg(planeCount, 4, 10, QChar('0')));
725 planeCount++;
726 }
727 break;
728
729 case RPT_SPHERE: //sphere
730 {
731 const SpherePrimitiveShape* sphere = static_cast<const SpherePrimitiveShape*>(shape);
732 float radius = sphere->Internal().Radius();
733 Vec3f CC = sphere->Internal().Center();
734
735 //we build matrix from these vecctors
736 ccGLMatrix glMat;
737 glMat.setTranslation(CC.getValue());
738 //sphere primitive
739 prim = new ccSphere(radius, &glMat);
740 prim->setEnabled(false);
741 prim->setName(QString("Sphere (r=%1)").arg(radius, 0, 'f'));
742 pcShape->setName(QString("Sphere_%1").arg(sphereCount, 4, 10, QChar('0')));
743 sphereCount++;
744 }
745 break;
746
747 case RPT_CYLINDER: //cylinder
748 {
749 const CylinderPrimitiveShape* cyl = static_cast<const CylinderPrimitiveShape*>(shape);
750 Vec3f G = cyl->Internal().AxisPosition();
751 Vec3f N = cyl->Internal().AxisDirection();
752 Vec3f X = cyl->Internal().AngularDirection();
753 Vec3f Y = N.cross(X);
754 float r = cyl->Internal().Radius();
755 float hMin = cyl->MinHeight();
756 float hMax = cyl->MaxHeight();
757 float h = hMax - hMin;
758 G += N * (hMin + h / 2);
759
760 //we build matrix from these vecctors
761 ccGLMatrix glMat(CCVector3::fromArray(X.getValue()),
762 CCVector3::fromArray(Y.getValue()),
763 CCVector3::fromArray(N.getValue()),
764 CCVector3::fromArray(G.getValue()));
765
766 //cylinder primitive
767 prim = new ccCylinder(r, h, &glMat);
768 prim->setEnabled(false);
769 prim->setName(QString("Cylinder (r=%1/h=%2)").arg(r, 0, 'f').arg(h, 0, 'f'));
770 pcShape->setName(QString("Cylinder_%1").arg(cylinderCount, 4, 10, QChar('0')));
771 cylinderCount++;
772 }
773 break;
774
775 case RPT_CONE: //cone
776 {
777 const ConePrimitiveShape* cone = static_cast<const ConePrimitiveShape*>(shape);
778 Vec3f CC = cone->Internal().Center();
779 Vec3f CA = cone->Internal().AxisDirection();
780 float alpha_rad = cone->Internal().Angle();
781
782 //compute max height
783 Vec3f minP, maxP;
784 float minHeight, maxHeight;
785 minP = maxP = cloud[shapeCloudIndex].pos;
786 minHeight = maxHeight = cone->Internal().Height(cloud[shapeCloudIndex].pos);
787 for (size_t j = 1; j < shapePointsCount; ++j)
788 {
789 float h = cone->Internal().Height(cloud[shapeCloudIndex - j].pos);
790 if (h < minHeight)
791 {
792 minHeight = h;
793 minP = cloud[shapeCloudIndex - j].pos;
794 }
795 else if (h > maxHeight)
796 {
797 maxHeight = h;
798 maxP = cloud[shapeCloudIndex - j].pos;
799 }
800
801 }
802
803
804 float minRadius = tan(alpha_rad) * minHeight;
805 float maxRadius = tan(alpha_rad) * maxHeight;
806
807 //let's build the cone primitive
808 {
809 //the bottom should be the largest part so we inverse the axis direction
810 CCVector3 Z = -CCVector3::fromArray(CA.getValue());
811 Z.normalize();
812
813 //the center is halfway between the min and max height
814 float midHeight = (minHeight + maxHeight) / 2;
815 CCVector3 C = CCVector3::fromArray((CC + CA * midHeight).getValue());
816
817 //radial axis
818 CCVector3 X = CCVector3::fromArray((maxP - (CC + maxHeight * CA)).getValue());
819 X.normalize();
820
821 //orthogonal radial axis
822 CCVector3 Y = Z * X;
823
824 //we build the transformation matrix from these vecctors
825 ccGLMatrix glMat(X, Y, Z, C);
826
827 //eventually create the cone primitive
828 prim = new ccCone(maxRadius, minRadius, maxHeight - minHeight, 0, 0, &glMat);
829 prim->setEnabled(false);
830 prim->setName(QString("Cone (alpha=%1 deg / h=%2)").arg(CCCoreLib::RadiansToDegrees(alpha_rad), 0, 'f').arg(static_cast<double>(maxHeight) - minHeight, 0, 'f'));
831 pcShape->setName(QString("Cone_%1").arg(coneCount, 4, 10, QChar('0')));
832 coneCount++;
833 }
834
835 }
836 break;
837
838 case RPT_TORUS: //torus
839 {
840 const TorusPrimitiveShape* torus = static_cast<const TorusPrimitiveShape*>(shape);
841 if (torus->Internal().IsAppleShaped())
842 {
843 ccLog::Warning("[qRansacSD] Apple-shaped torus are not handled by CloudCompare!");
844 }
845 else
846 {
847 Vec3f CC = torus->Internal().Center();
848 Vec3f CA = torus->Internal().AxisDirection();
849 float minRadius = torus->Internal().MinorRadius();
850 float maxRadius = torus->Internal().MajorRadius();
851
852 CCVector3 Z = CCVector3::fromArray(CA.getValue());
853 CCVector3 C = CCVector3::fromArray(CC.getValue());
854 //construct remaining of base
855 CCVector3 X = Z.orthogonal();
856 CCVector3 Y = Z * X;
857
858 //we build matrix from these vecctors
859 ccGLMatrix glMat(X, Y, Z, C);
860
861 //torus primitive
862 prim = new ccTorus(maxRadius - minRadius, maxRadius + minRadius, M_PI * 2.0, false, 0, &glMat);
863 prim->setEnabled(false);
864 prim->setName(QString("Torus (r=%1/R=%2)").arg(minRadius, 0, 'f').arg(maxRadius, 0, 'f'));
865 pcShape->setName(QString("Torus_%1").arg(torusCount, 4, 10, QChar('0')));
866 torusCount++;
867 }
868
869 }
870 break;
871 }
872
873 //is there a primitive to add to part cloud?
874 if (prim)
875 {
876 prim->copyGlobalShiftAndScale(*ccPC);
877 prim->applyGLTransformation_recursive();
878 pcShape->addChild(prim);
879 prim->setDisplay(pcShape->getDisplay());
880 if (params.randomColor)
881 {
882 prim->setColor(col);
883 }
884 prim->showColors(true);
885 prim->setVisible(true);
886 if (!group)
887 {
888 group = new ccHObject(QString("Ransac Detected Shapes (%1)").arg(ccPC->getName()));
889 }
890 group->addChild(pcShape);
891 }
892
893
894 count -= shapePointsCount;
895
896 QApplication::processEvents();
897 }
898
899 if (group)
900 {
901 assert(group->getChildrenNumber() != 0);
902
903 //we hide input cloud
904 ccPC->setEnabled(false);
905 ccLog::Warning("[qRansacSD] Input cloud has been automtically hidden!");
906
907
908 group->setVisible(true);
909 group->setDisplay_recursive(ccPC->getDisplay());
910 #ifdef POINTSWITHINDEX
911 if (params.createCloudFromLeftOverPoints)
912 {
913 //new cloud for left overs
914 ccPointCloud* pcLeftOvers = nullptr;
915 CCCoreLib::ReferenceCloud refPcLO(ccPC);
916 //we fill cloud with left over points
917 if (!refPcLO.reserve(remaining))
918 {
919 ccLog::Error("[qRansacSD] Not enough memory!");
920 }
921
922 for (unsigned j = 0; j < remaining; ++j)
923 {
924 refPcLO.addPointIndex(cloud[j].index);
925 }
926 int warnings = 0;
927 pcLeftOvers = ccPC->partialClone(&refPcLO, &warnings);
928 bool saveNormals = true;
929 if (warnings != 0)
930 {
931 if ((warnings & ccPointCloud::WRN_OUT_OF_MEM_FOR_NORMALS) == ccPointCloud::WRN_OUT_OF_MEM_FOR_NORMALS)
932 {
933 saveNormals = false;
934 }
935 }
936 if (pcLeftOvers)
937 {
938 pcLeftOvers->setName("Leftovers");
939 group->addChild(pcLeftOvers);
940 }
941 }
942 #endif
943
944 return group;
945 }
946 }
947
948 return nullptr;
949 }