Debian Codegraph @

plugins/core/Standard/qMasonry/qAutoSeg/qAutoSeg.cpp

raw
1 //##########################################################################
2 //# #
3 //# CLOUDCOMPARE PLUGIN: qAutoSeg #
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 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 //# Cyberbuild,2020 #
15 //# v1.3 #
16 //# #
17 //##########################################################################
18
19 #include "qAutoSeg.h"
20
21 //Local
22 #include "profileImportDlg.h"
23
24 //PCL
25 #include <pcl/point_cloud.h>
26 #include <pcl/kdtree/kdtree_flann.h>
27 #include <pcl/common/common.h>
28 #include <pcl/common/transforms.h>
29
30 //Qt
31 #include <QtGui>
32 #include <QSettings>
33 #include <QFileInfo>
34 #include <QFile>
35 #include <QMessageBox>
36 #include <QTextStream>
37 #include <QMainWindow>
38
39 //qCC_db
40 #include <ccFileUtils.h>
41 #include <ccHObjectCaster.h>
42 #include <ccPointCloud.h>
43 #include <ccProgressDialog.h>
44 #include <ccMesh.h>
45 #include <ccPolyline.h>
46 #include <ccCone.h>
47 #include <ccScalarField.h>
48 #include <ccGLWindowInterface.h>
49
50 //qCCIO
51 #include <PlyFilter.h>
52
53 //OpenCV
54 #include <opencv2/opencv.hpp>
55 #include <opencv2/core/core.hpp>
56 #include <opencv2/highgui/highgui.hpp>
57 #include <opencv2/imgproc/imgproc.hpp>
58 #include <opencv2/imgcodecs.hpp>
59
60 //System
61 #define _USE_MATH_DEFINES
62 #include <cmath>
63 #include <iostream>
64 #include <fstream>
65 #include <unordered_set>
66 #include <random>
67 #include <algorithm>
68 #include <list>
69 #include <numeric>
70 #include <random>
71 #include <vector>
72 #include <ctime>
73
74 //CCCoreLib
75 #include <CloudSamplingTools.h>
76 #include <DistanceComputationTools.h>
77 #include <DgmOctreeReferenceCloud.h>
78 #include <GenericProgressCallback.h>
79 #include <Neighbourhood.h>
80 #include <PointCloud.h>
81 #include <ReferenceCloud.h>
82 #include <ScalarField.h>
83 #include <ScalarFieldTools.h>
84 #include <SimpleMesh.h>
85
86 ccAutoSeg::ccAutoSeg( QObject *parent )
87 : QObject( parent )
88 , ccStdPluginInterface( ":/CC/plugin/qAutoSeg/info.json" )
89 , m_action( nullptr )
90 {
91 }
92
93 //Calculate std
94 static double stddev(const std::vector<double>& func)
95 {
96 if (func.size() < 2)
97 {
98 assert(false);
99 return 0.0;
100 }
101
102 double mean = std::accumulate(func.begin(), func.end(), 0.0) / func.size();
103 double sq_sum = std::inner_product(func.begin(), func.end(), func.begin(), 0.0,
104 [](double const & x, double const & y) { return x + y; },
105 [mean](double const & x, double const & y) { return (x - mean)*(y - mean); });
106 return std::sqrt(sq_sum / (func.size() - 1));
107 }
108
109 static std::string dateStamp()
110 {
111 return QDateTime::currentDateTime().toString("hh:mm:ss.zzz").toStdString();
112 }
113
114 static double optRotY(const ccPointCloud& cloud)
115 {
116 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
117 pcl_cloud->width = cloud.size();
118 pcl_cloud->height = 1;
119 pcl_cloud->points.resize(pcl_cloud->width * pcl_cloud->height);
120
121 std::vector<int> v(cloud.size());
122 std::iota(v.begin(), v.end(), 0);
123 std::random_shuffle(v.begin(), v.end());
124 v.erase(v.begin(), v.begin() + cloud.size());
125
126 for (unsigned i = 0; i < cloud.size(); i++)
127 {
128 const CCVector3* P = cloud.getPoint(i);
129 pcl_cloud->points[i].x = P->x;
130 pcl_cloud->points[i].y = P->y;
131 pcl_cloud->points[i].z = P->z;
132 }
133
134 pcl::PointXYZRGB bbMin0, bbMax0;
135 pcl::getMinMax3D(*pcl_cloud, bbMin0, bbMax0);
136
137 pcl::PointCloud<pcl::PointXYZRGB> ::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
138 transformed_cloud->width = cloud.size();
139 transformed_cloud->height = 1;
140 transformed_cloud->points.resize(transformed_cloud->width * transformed_cloud->height);
141
142 double th0 = 0;
143 double area0 = (bbMax0.x - bbMin0.x)*(bbMax0.z - bbMin0.z);
144 for (int th = -45; th <= 45; th++)
145 {
146 //Apply this to a sparser cloud to make the process lighter
147 double thD = th * (M_PI / 180.0);
148
149 Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
150
151 // Define a translation
152 transform_2.translation() << 0, 0.0, 0.0;
153
154 // The same rotation matrix as before; theta radians around Y axis
155 transform_2.rotate(Eigen::AngleAxisf(thD, Eigen::Vector3f::UnitY()));
156
157 pcl::transformPointCloud(*pcl_cloud, *transformed_cloud, transform_2);
158
159 pcl::PointXYZRGB bbMin, bbMax;
160 pcl::getMinMax3D(*transformed_cloud, bbMin, bbMax);
161 double area = (bbMax.x - bbMin.x)*(bbMax.z - bbMin.z);
162 if (area < area0)
163 {
164 area0 = area;
165 th0 = thD;
166 }
167
168 Eigen::Affine3f transform_inv = transform_2.inverse();
169 pcl::transformPointCloud(*transformed_cloud, *pcl_cloud, transform_inv);
170 }
171
172 return th0;
173 }
174
175 #if 0 // not used
176 static ccPointCloud* rotY(const ccPointCloud& cloud, double th0)
177 {
178 bool hasColors = cloud.hasColors();
179
180 pcl::PointCloud<pcl::PointXYZRGB> ::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
181 pcl_cloud->width = cloud.size();
182 pcl_cloud->height = 1;
183 pcl_cloud->points.resize(pcl_cloud->width * pcl_cloud->height);
184
185 std::vector<int> v(cloud.size());
186 std::iota(v.begin(), v.end(), 0);
187 std::random_shuffle(v.begin(), v.end());
188 v.erase(v.begin(), v.begin() + cloud.size());
189
190 for (size_t i = 0; i < cloud.size(); i++)
191 {
192 pcl_cloud->points[i].x = cloud.getPoint(i)->x;
193 pcl_cloud->points[i].y = cloud.getPoint(i)->y;
194 pcl_cloud->points[i].z = cloud.getPoint(i)->z;
195
196 if (hasColors)
197 {
198 pcl_cloud->points[i].r = cloud.getPointColor(i).r;
199 pcl_cloud->points[i].g = cloud.getPointColor(i).g;
200 pcl_cloud->points[i].b = cloud.getPointColor(i).b;
201 }
202 }
203
204 //Bring cloud to (0,0,0)
205 Eigen::Vector4d centroid;
206 pcl::compute3DCentroid(*pcl_cloud, centroid);
207
208 Eigen::Affine3f t0 = Eigen::Affine3f::Identity();
209
210 t0.translation() << -centroid[0], -centroid[1], -centroid[2];
211
212 pcl::PointCloud<pcl::PointXYZRGB> ::Ptr transformed0(new pcl::PointCloud<pcl::PointXYZRGB>);
213 transformed0->width = pcl_cloud->size();
214 transformed0->height = 1;
215 transformed0->points.resize(transformed0->width * transformed0->height);
216
217 pcl::transformPointCloud(*pcl_cloud, *transformed0, t0);
218
219 //Rotate around Y axis
220 Eigen::Affine3f transform0 = Eigen::Affine3f::Identity();
221 transform0.translation() << 0.0, 0.0, 0.0;
222 transform0.rotate(Eigen::AngleAxisf(th0, Eigen::Vector3f::UnitY()));
223 pcl::transformPointCloud(*transformed0, *transformed0, transform0);
224
225 //To original UCS
226 Eigen::Affine3f t1 = Eigen::Affine3f::Identity();
227 t1.translation() << centroid[0], centroid[1], centroid[2];
228 pcl::transformPointCloud(*transformed0, *transformed0, t1);
229
230 //From PCL to CC object
231 ccPointCloud* cloudAligned = new ccPointCloud("Aligned");
232 if (!cloudAligned->reserve(static_cast<unsigned>(transformed0->size())))
233 {
234 //not enough memory
235 delete cloudAligned;
236 return nullptr;
237 }
238 if (hasColors && !cloudAligned->reserveTheRGBTable())
239 {
240 //not enough memory to preserve colors
241 hasColors = false;
242 }
243
244 for (size_t i = 0; i < transformed0->size(); i++)
245 {
246 CCVector3 p(transformed0->points[i].x, transformed0->points[i].y, transformed0->points[i].z);
247 cloudAligned->addPoint(p);
248
249 if (hasColors)
250 {
251 const auto& rgb = transformed0->points[i];
252 const ccColor::Rgb col{ rgb.r, rgb.g, rgb.b };
253 cloudAligned->addColor(col);
254 }
255 }
256
257 return cloudAligned;
258 }
259
260 #endif
261
262 static void cloud2binary( const ccPointCloud& cloud,
263 cv::Mat& corrMatS,
264 std::vector<cv::Point>& idxPxS,
265 std::vector<int>& idxPx1DS,
266 cv::Mat& imageBWS )
267 {
268 //Correspondence 2D->1D. Each px has an index instead of two coords
269 int cntC = 0;
270 for (int y = 0; y < corrMatS.rows; y++)
271 {
272 for (int x = 0; x < corrMatS.cols; x++)
273 {
274 corrMatS.at<int>(y, x) = cntC;
275 cntC++;
276 }
277 }
278
279 //Binary
280 unsigned s = cloud.size();
281 idxPxS.reserve(s);
282 idxPx1DS.reserve(s);
283 for (unsigned i = 0; i < s; i++)
284 {
285 //depth
286 const CCVector3* pointC0 = cloud.getPoint(i);
287 int z = std::floor(pointC0->z);
288 int x = std::floor(pointC0->x);
289 imageBWS.at<uchar>(z, x) = 255;
290
291 //idxPx per 3D point
292 idxPxS.push_back(cv::Point(z, x));
293 idxPx1DS.push_back(corrMatS.at<int>(z, x));
294 }
295 }
296
297 //Extract Skeleton from Binary (CV_8U)
298 static cv::Mat skeleton(const cv::Mat& I, bool flagInv)
299 {
300 cv::Mat toSkel;
301 if (flagInv)
302 {
303 //If black-white inversion is required
304 toSkel = 255 * cv::Mat::ones(I.rows, I.cols, CV_8U) - I;
305 }
306 else
307 {
308 toSkel = I;
309 }
310
311 cv::Mat skel(toSkel.size(), CV_8U, cv::Scalar(0));
312 cv::Mat element = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3, 3));
313
314 int iterations = 0;
315 bool done = false;
316 do
317 {
318 cv::Mat eroded;
319 cv::erode(toSkel, eroded, element);
320 cv::Mat temp;
321 cv::dilate(eroded, temp, element);
322 cv::subtract(toSkel, temp, temp);
323 cv::bitwise_or(skel, temp, skel);
324 eroded.copyTo(toSkel);
325
326 done = (cv::countNonZero(toSkel) == 0);
327 iterations++;
328 }
329 while (!done && (iterations < 100));
330
331 return skel;
332 }
333
334 #if 0 // not used
335 //Extract elements of a vector given the indices
336 static std::vector<cv::Point> extractVectorFromIdx(const std::vector<cv::Point>& v, const std::vector<int>& idx)
337 {
338 std::vector<cv::Point> subset;
339 subset.reserve(idx.size());
340 for (size_t i = 0; i < idx.size(); i++)
341 {
342 subset.push_back(v[idx[i]]);
343 }
344 return std::move(subset);
345 }
346
347 //Populate a matrix with a given value
348 static void populateMat(cv::Mat& populated, const std::vector<cv::Point>& pixList, int value)
349 {
350 for (size_t i = 0; i < pixList.size(); i++)
351 {
352 populated.at<uchar>(pixList[i].x, pixList[i].y) = value;
353 }
354 }
355
356 //Calculate difference set in two pixel lists (Matlab setdiff equivalent). Returns pixels in listOld that are not in listNew
357 static std::vector<cv::Point> setdiffPixs(const std::vector<cv::Point>& listNew, const std::vector<cv::Point>& listOld)
358 {
359 std::vector<cv::Point> setdiffList;
360
361 for (size_t i = 0; i < listOld.size(); i++)
362 {
363 bool same = false;
364 for (size_t j = 0; j < listNew.size(); j++)
365 {
366 if (listOld[i].x == listNew[j].x && listOld[i].y == listNew[j].y)
367 {
368 same = true;
369 break; // no need to process any further
370 }
371 }
372 if (!same)
373 {
374 setdiffList.push_back(listOld[i]);
375 }
376
377 }
378 return std::move(setdiffList);
379 }
380
381 //Calculate intersection set in two pixel lists. Returns idx pixels in listNew that are in listOld
382 static std::vector<int> setIntersectIdxPixs(const std::vector<cv::Point>& listNew, const std::vector<cv::Point>& listOld)
383 {
384 std::vector<int> commonList;
385
386 //Common
387 for (size_t i = 0; i < listOld.size(); i++)
388 {
389 for (size_t j = 0; j < listNew.size(); j++)
390 {
391 if (listOld[i].x == listNew[j].x && listOld[i].y == listNew[j].y)
392 {
393 commonList.push_back(i);
394 }
395 }
396 }
397
398 return std::move(commonList);
399 }
400
401 //Calculate exclusive OR in two pixel lists (Matlab setxor equivalent)
402 static std::vector<cv::Point> setxorPixs(std::vector<cv::Point> listNew, std::vector<cv::Point> listOld)
403 {
404 //Common
405 std::vector<int> commonList;
406 for (size_t i = 0; i < listNew.size(); i++)
407 {
408 for (size_t j = 0; j < listNew.size(); j++)
409 {
410 if (listOld[i].x == listNew[j].x && listOld[i].y == listNew[j].y)
411 {
412 commonList.push_back(i);
413 }
414 }
415 }
416
417 //Setxor
418 size_t nLabels = listNew.size();
419 std::vector<int> v(nLabels);
420 std::iota(std::begin(v), std::end(v), 0);
421 std::vector<int> setxorIdx;
422 std::set_difference(std::begin(v), std::end(v), std::begin(commonList), std::end(commonList), std::back_inserter(setxorIdx));
423
424 std::vector<cv::Point> setxorList;
425 setxorList.reserve(setxorIdx.size());
426 for (size_t j = 0; j < setxorIdx.size(); j++)
427 {
428 setxorList.push_back(listNew[setxorIdx[j]]);
429 }
430
431 return std::move(setxorList);
432 }
433
434 static std::vector<int> findIdx(const std::vector<int>& inputV, int thres)
435 {
436 std::vector<int> indices;
437
438 for (int i = 0; i < inputV.size(); i++)
439 {
440 if (inputV[i] == thres)
441 indices.push_back(i);
442 }
443
444 return indices;
445 }
446
447 //Calculates the conjugate of a complex matrix
448 static cv::Mat conjugate(cv::Mat_<std::complex<double>> M1, bool* flag)
449 {
450 using CP = std::complex<double>;
451 cv::Mat_<CP> M3(M1.rows, M1.cols);
452 //When there is no complex part, half of the values of the real part are considered as complex
453 try
454 {
455 double a = M1.at<CP>(M1.rows - 1, M1.cols - 1).real();
456 for (int i = 0; i < M1.rows; ++i)
457 {
458 for (int j = 0; j < M1.cols; ++j)
459 {
460
461 M3.at<CP>(i, j) = CP(M1.at<CP>(i, j).real(), -M1.at<CP>(i, j).imag());
462 }
463 }
464
465 *flag = true;
466 return M3;
467 }
468 catch (const std::exception)
469 {
470 //If there is no complex part
471 M3 = M1;
472 *flag = false;
473 return M3;
474 }
475 }
476 #endif
477
478 std::vector<int> setIntersectIdxPixs1D(const std::vector<int>& listNew, const std::vector<int>& listOld)
479 {
480 std::vector<int> commonList;
481
482 //Common
483 for (size_t i = 0; i < listNew.size(); ++i)
484 {
485 int e = listNew[i];
486
487 for (auto ee : listOld)
488 {
489 if (e == ee)
490 {
491 commonList.push_back(i);
492 break;
493 }
494 }
495 }
496
497 return std::move(commonList);
498 }
499
500 //Extract values of pixels per labelled segment in a binary image
501 static std::vector<int> pixelValues(const cv::Mat& inputM, const std::vector<cv::Point>& pixelList)
502 {
503 std::vector<int> pixelV;
504 pixelV.reserve(pixelList.size());
505 for (size_t i = 0; i < pixelList.size(); i++)
506 {
507 int val = inputM.at<uchar>(pixelList[i].x, pixelList[i].y);
508 pixelV.push_back(val);
509 }
510
511 return std::move(pixelV);
512 }
513
514 //Extract pixels per labelled segment in a binary image
515 std::vector<std::vector<cv::Point>> pixelListMat(const cv::Mat& inputM, int nLabels)
516 {
517 std::vector<std::vector<cv::Point>> pixelSegments(nLabels);
518 for (int i = 0; i < inputM.rows; i++)
519 {
520 for (int j = 0; j < inputM.cols; j++)
521 {
522 int pixelValue = inputM.at<int>(i, j);
523 if (pixelValue > 0)
524 {
525 cv::Point x(i, j);
526 pixelSegments[pixelValue - 1].push_back(x); //-1 to correct value 0 for background
527 }
528
529 }
530 }
531
532 return std::move(pixelSegments);
533 }
534
535 //Find values over/under a value in an array. op = 1: lt; op = 2: leq; op = 3: gt; op = 4: geq; op = 5: eq
536 std::vector<int> findMatlab(const std::vector<double>& inputV, int op, double thres)
537 {
538 std::vector<int> indices;
539
540 switch (op)
541 {
542 case 1:
543 for (int i = 0; i < inputV.size(); i++)
544 {
545 if (inputV[i] < thres)
546 indices.push_back(i);
547 }
548 break;
549
550 case 2:
551 for (int i = 0; i < inputV.size(); i++)
552 {
553 if (inputV[i] <= thres)
554 indices.push_back(i);
555 }
556 break;
557
558 case 3:
559 for (int i = 0; i < inputV.size(); i++)
560 {
561 if (inputV[i] > thres)
562 indices.push_back(i);
563 }
564 break;
565
566 case 4:
567 for (int i = 0; i < inputV.size(); i++)
568 {
569 if (inputV[i] >= thres)
570 indices.push_back(i);
571 }
572 break;
573
574 case 5:
575 for (int i = 0; i < inputV.size(); i++)
576 {
577 if (inputV[i] == thres)
578 indices.push_back(i);
579 }
580 break;
581
582 default:
583 assert(false);
584 break;
585 }
586
587 return std::move(indices);
588 }
589
590 //Dilation
591 static void dilationCC(int dilation_elem, int dilation_size, cv::Mat &src, cv::Mat &dilation_dst)
592 {
593 int dilation_type = cv::MORPH_RECT;
594 if (dilation_elem == 0)
595 {
596 //dilation_type = cv::MORPH_RECT;
597 }
598 else if (dilation_elem == 1)
599 {
600 dilation_type = cv::MORPH_CROSS;
601 }
602 else if (dilation_elem == 2)
603 {
604 dilation_type = cv::MORPH_ELLIPSE;
605 }
606 else
607 {
608 assert(false);
609 }
610
611 cv::Mat element = cv::getStructuringElement(dilation_type,
612 cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1),
613 cv::Point(dilation_size, dilation_size));
614
615 dilate(src, dilation_dst, element);
616 }
617
618 // Erosion
619 static void erosionCC(int erosion_elem, int erosion_size, cv::Mat& src, cv::Mat& erosion_dst)
620 {
621 int erosion_type = cv::MORPH_RECT;
622 if (erosion_elem == 0)
623 {
624 //erosion_type = cv::MORPH_RECT;
625 }
626 else if (erosion_elem == 1)
627 {
628 erosion_type = cv::MORPH_CROSS;
629 }
630 else if (erosion_elem == 2)
631 {
632 erosion_type = cv::MORPH_ELLIPSE;
633 }
634 else
635 {
636 assert(false);
637 }
638
639 cv::Mat element = cv::getStructuringElement(erosion_type,
640 cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1),
641 cv::Point(erosion_size, erosion_size));
642
643 erode(src, erosion_dst, element);
644 }
645
646
647 //Stones division in smaller segments (when joint by narrow areas)
648 static cv::Mat stonesDivision(const std::vector<std::vector<cv::Point>> pixsLabel, const cv::Mat& labels, const std::vector<int>& bigStones)
649 {
650 //smallIndices
651 size_t nLabels = pixsLabel.size();
652 std::vector<int> v(nLabels);
653 std::iota(std::begin(v), std::end(v), 0);
654
655 std::vector<int> noBigStones;
656 std::set_difference(std::begin(v), std::end(v),std::begin(bigStones), std::end(bigStones),std::back_inserter(noBigStones));
657
658 //smallMatrix
659 cv::Mat smallStones = cv::Mat::zeros(labels.size(), CV_8U);
660 for (size_t i = 0; i < noBigStones.size(); i++)
661 {
662 const auto& pixsLabelI = pixsLabel[noBigStones[i]];
663 for (size_t j = 0; j < pixsLabelI.size(); j++)
664 {
665 smallStones.at<uchar>(pixsLabelI[j].x, pixsLabelI[j].y) = 255;
666 }
667 }
668
669 //bigMatrix
670 cv::Mat input = cv::Mat::zeros(labels.size(), CV_8U);
671 for (size_t i = 0; i < bigStones.size(); i++)
672 {
673 const auto& pixsLabelI = pixsLabel[bigStones[i]];
674 for (size_t j = 0; j < pixsLabelI.size(); j++)
675 {
676 input.at<uchar>(pixsLabelI[j].x, pixsLabelI[j].y) = 255;
677 }
678 }
679
680 //Erosion of big stones (in matlab was open: erosion + dilation, but here we dilated before)
681 int erosion_elem = 2;
682 int erosion_size = 1;
683 cv::Mat output;
684 erosionCC(erosion_elem, erosion_size, input, output);
685
686 //Combine small + eroded big stones
687 cv::Mat combined = output+smallStones;
688
689 return combined; //new segments
690 }
691
692 // Gets only the biggest segments
693 static cv::Mat threshSegments(cv::Mat &src, double threshSize)
694 {
695 // FindContours:
696 std::vector<std::vector<cv::Point>> contours;
697 std::vector<cv::Vec4i> hierarchy;
698 cv::Mat srcBuffer, output;
699 src.copyTo(srcBuffer);
700 findContours(srcBuffer, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_TC89_KCOS);
701
702 std::vector<std::vector<cv::Point>> allSegments;
703
704 // For each segment:
705 for (size_t i = 0; i < contours.size(); ++i)
706 {
707 cv::drawContours(srcBuffer, contours, i, cv::Scalar(200, 0, 0), 1, 8, hierarchy, 0, cv::Point());
708 cv::Rect brect = cv::boundingRect(contours[i]);
709 cv::rectangle(srcBuffer, brect, cv::Scalar(255, 0, 0));
710
711 std::vector<cv::Point> segment;
712 for (int row = brect.y; row < brect.y + brect.height; ++row)
713 {
714 for (int col = brect.x; col < brect.x + brect.width; ++col)
715 {
716 int result = pointPolygonTest(contours[i], cv::Point(col, row), false);
717 if (result == 1 || result == 0)
718 {
719 segment.push_back(cv::Point(col, row));
720 }
721 }
722 }
723
724 allSegments.push_back(segment);
725 }
726
727 output = cv::Mat::zeros(src.size(), CV_8U);
728 int totalSize = output.rows*output.cols;
729 for (int segmentCount = 0; segmentCount < allSegments.size(); ++segmentCount)
730 {
731 const std::vector<cv::Point>& segment = allSegments[segmentCount];
732 if (segment.size() > threshSize)
733 {
734 for (size_t idx = 0; idx < segment.size(); ++idx)
735 {
736 output.at<uchar>(segment[idx].y, segment[idx].x) = 255;
737 }
738 }
739 }
740
741 return output;
742 }
743
744 // Returns the type of a matrix as a string
745 static std::string type2str(int type)
746 {
747 std::string r;
748
749 uchar depth = type & CV_MAT_DEPTH_MASK;
750 uchar chans = 1 + (type >> CV_CN_SHIFT);
751
752 switch (depth)
753 {
754 case CV_8U: r = "8U"; break;
755 case CV_8S: r = "8S"; break;
756 case CV_16U: r = "16U"; break;
757 case CV_16S: r = "16S"; break;
758 case CV_32S: r = "32S"; break;
759 case CV_32F: r = "32F"; break;
760 case CV_64F: r = "64F"; break;
761 default: r = "User"; break;
762 }
763
764 r += "C";
765 r += (chans + '0');
766
767 return r;
768 }
769
770 // Get stats and additional info from a binary matrix
771 static std::vector<std::vector<cv::Point>> extractInfo(cv::Mat &input, cv::Mat& labels, cv::Mat& stats, std::vector<std::vector<cv::Point>>* contours = nullptr)
772 {
773 //Get connected components and stats
774 const int connectivity_8 = 8;
775 cv::Mat centroids;
776
777 int nLabels = connectedComponentsWithStats(input, labels, stats, centroids, connectivity_8, CV_32S); //Note that component 0 is the background (i.e. nLabels = nsegments+1)
778
779 //Extract the contours
780 std::vector<cv::Vec4i> hierarchy;
781 std::vector<std::vector<cv::Point>> contours0;
782 cv::findContours(input, contours ? *contours : contours0, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
783
784 std::string ty = type2str(labels.type());
785
786 return std::move(pixelListMat(labels, nLabels - 1));
787 }
788
789 //Fill in the holes inside segments
790 static void fillEdgeImage(const cv::Mat& edgesIn, cv::Mat& filledEdgesOut, int z)
791 {
792 cv::Mat edgesNeg = edgesIn.clone();
793 std::string hola = type2str(edgesNeg.type());
794
795 ////////Check that seed is in mortar joints and not inside a stone
796 cv::Mat labels, stats;
797 const int connectivity_8 = 8;
798 cv::Mat centroids;
799 //invert 1s and 0s to label black areas from CWT instead of whites. In this way, we know that label 1 (the bigger segment) corresponds to mortar joints
800 cv::bitwise_not(edgesIn, edgesIn);
801
802 int nLabels = connectedComponentsWithStats(edgesIn, labels, stats, centroids, connectivity_8, CV_32S); //Note that component 0 is the background (i.e. nLabels = nsegments+1)
803
804 cv::bitwise_not(edgesIn, edgesIn);
805
806 int stSize = stats.at<int>(1, 4); //0 is stone area, largest (not considering that one) is continuous mortar
807 int cntBiggestSize = 1;
808 for (int i = 2; i < stats.rows; i++)
809 {
810 int loopSize = stats.at<int>(i, 4);
811
812 if (loopSize > stSize)
813 {
814 stSize = loopSize;
815 cntBiggestSize = i;
816 }
817 }
818
819 int f = 0, c = 0;
820 for (int i = 0; i < edgesNeg.rows; i++)
821 {
822 for (int j = 0; j < edgesNeg.cols; j++)
823 {
824 uchar h = edgesNeg.at<uchar>(i, j);
825 int l = labels.at<int>(i, j);
826 if (h == 0 && l == cntBiggestSize)
827 {
828 // seed is a 0 in CWT and belongs to mortar joints
829 f = i;
830 c = j;
831 goto stop;
832 }
833 }
834 }
835
836 stop:
837
838 cv::floodFill(edgesNeg, cv::Point(c, f), CV_RGB(255, 255, 255)); //The seed must be a black pixel
839
840 cv::bitwise_not(edgesNeg, edgesNeg);
841
842 filledEdgesOut = (edgesNeg | edgesIn);
843
844 return;
845 }
846
847 // Calculates the fft2
848 static void fft2(const cv::Mat& in, cv::Mat& complexI)
849 {
850 cv::Mat padded;
851 int m = cv::getOptimalDFTSize(in.rows);
852 int n = cv::getOptimalDFTSize(in.cols);
853 copyMakeBorder(in, padded, 0, m - in.rows, 0, n - in.cols, cv::BORDER_CONSTANT, cv::Scalar::all(0));
854
855 cv::Mat planes[] { cv::Mat_<float>(padded), cv::Mat::zeros(padded.size(), CV_32F) };
856 cv::merge(planes, 2, complexI);
857 cv::dft(complexI, complexI);
858 }
859
860 // Multiplies two complex matrices
861 static cv::Mat multiplicationCC(const cv::Mat_<std::complex<double>>& M1, const cv::Mat_<std::complex<double>>& M2)
862 {
863 using CP = std::complex<double>;
864 cv::Mat_<CP> M3(M1.rows, M1.cols);
865 for (int i = 0; i < M3.rows; ++i)
866 {
867 for (int j = 0; j < M3.cols; ++j)
868 {
869 M3.at<CP>(i, j) = CP(M1.at<CP>(i, j).real()*M2.at<CP>(i, j).real() - M1.at<CP>(i, j).imag()*M2.at<CP>(i, j).imag(), M1.at<CP>(i, j).real()*M2.at<CP>(i, j).imag() + M1.at<CP>(i, j).imag()*M2.at<CP>(i, j).real());
870 }
871 }
872 return M3;
873 }
874
875 //Calculates the Continuous Wavelet Transform
876 static void cwt2d(double sca, cv::Mat& image, cv::Mat& cwtcfs, cv::Mat& bincfs)
877 {
878 cv::Mat I = cv::imread("depthMap.bmp", cv::IMREAD_GRAYSCALE);
879 cv::Mat fImage = image; //(if there is 'image' matrix)
880
881 // FFT
882 //std::cout << "Direct transform...\n";
883 cv::Mat fourierTransform;
884 fft2(fImage, fourierTransform);
885
886 //Create frequency plane
887 cv::Size S = fourierTransform.size();
888 int H = S.height;
889 int W = S.width;
890
891 int W2 = (W - 1) / 2;
892 int H2 = (H - 1) / 2;
893
894 std::vector<int> w1(W2 + 1); // vector with W2 ints.
895 std::iota(std::begin(w1), std::end(w1), 0);
896 std::vector<int> w2(W2 + 1); // vector with W2 ints.
897 std::iota(std::begin(w2), std::end(w2), (W2 - W + 1));
898
899 std::vector<double> v;
900 v.reserve(w1.size() + w2.size()); // preallocate memory
901 v.insert(v.end(), w1.begin(), w1.end());
902 v.insert(v.end(), w2.begin(), w2.end());
903
904 std::vector<double> W_puls(v.size());
905 std::transform(v.begin(), v.end(), W_puls.begin(),
906 [W](int i) { return i * 2 * M_PI / W; });
907
908 std::vector<int> h1(H2 + 1); // vector with H2 ints.
909 std::iota(std::begin(h1), std::end(h1), 0);
910 std::vector<int> h2(H2 + 1); // vector with H2 ints.
911 std::iota(std::begin(h2), std::end(h2), (H2 - H + 1));
912
913 std::vector<double> v2;
914 v2.reserve(h1.size() + h2.size()); // preallocate memory
915 v2.insert(v2.end(), h1.begin(), h1.end());
916 v2.insert(v2.end(), h2.begin(), h2.end());
917
918 std::vector<double> H_puls(v2.size());
919 std::transform(v2.begin(), v2.end(), H_puls.begin(),
920 [H](int i) { return i * 2 * M_PI / H; });
921
922 cv::Mat matW(W_puls);
923 cv::Mat matH(H_puls);
924
925 cv::Mat1d xx, yy;
926 cv::repeat(matW.reshape(1, 1), matH.total(), 1, xx); //Matlab's Meshgrid equivalent
927 cv::repeat(matH.reshape(1, 1).t(), 1, matW.total(), yy);
928
929 double ang = 0;
930 cv::Mat nxx = sca * (cos(ang)*xx - sin(ang)*yy);
931 cv::Mat nyy = sca * (sin(ang)*xx + cos(ang)*yy);
932
933 int sigmax = 1;
934 int sigmay = 1;
935 int order = 2;
936
937 cv::Mat nxx2;
938 cv::pow(nxx, 2, nxx2);
939 cv::Mat nyy2;
940 cv::pow(nyy, 2, nyy2);
941
942 cv::Mat sum = nxx2 + nyy2;
943 cv::Mat sumPow;
944 cv::pow(sum, order / 2, sumPow);
945
946 cv::Mat prod1 = sigmax * nxx;
947 cv::Mat prod12;
948 cv::pow(prod1, 2, prod12);
949 cv::Mat prod2 = sigmay * nyy;
950 cv::Mat prod22;
951 cv::pow(prod2, 2, prod22);
952
953 cv::Mat ex = -(prod12 + prod22) / 2;
954 cv::Mat ex2;
955 cv::exp(ex, ex2);
956
957 cv::Mat waveft2 = -(2 * M_PI) * sumPow.mul(ex2);
958
959 //Complex mask
960 using CP = std::complex<double>;
961 cv::Mat_<CP> mask(cv::Size(waveft2.rows, waveft2.cols));
962 mask = sca * waveft2;
963
964 bool flag = false;
965 cv::Mat mask_conj = mask;
966 mask.convertTo(mask_conj, CV_32F);
967
968 cv::Mat g, mask2;
969 std::vector<cv::Mat> channels;
970 if (flag)
971 {
972 //Mask is complex. TBC if there is the case
973 }
974 else
975 {
976 g = cv::Mat::zeros(cv::Size(mask_conj.cols, mask_conj.rows), CV_32FC1);
977 mask2 = cv::Mat::zeros(cv::Size(mask_conj.cols, mask_conj.rows), CV_32FC2);
978
979 channels.push_back(mask_conj);
980 channels.push_back(g);
981
982 merge(channels, mask2);
983 }
984
985 //Matrices product
986 cv::Mat M3 = multiplicationCC(fourierTransform, mask2);
987
988 // IFFT
989 //std::cout << "Inverse transform...\n";
990 cv::dft(M3, cwtcfs, cv::DFT_INVERSE | cv::DFT_REAL_OUTPUT);
991
992 // Back to 8-bits. angle values in Matlab code
993 cwtcfs.convertTo(bincfs, CV_8U);
994 }
995
996 #if 0 // not used
997
998 // Returns the contour of a stone as a polyline
999 static ccPolyline* contourPoly(const ccPointCloud& stone)
1000 {
1001 std::vector<CCCoreLib::PointProjectionTools::IndexedCCVector2> points2D;
1002 points2D.reserve(stone.size());
1003
1004 for (unsigned i = 0; i < stone.size(); ++i)
1005 {
1006 const CCVector3* _P = stone.getPoint(i);
1007 CCCoreLib::PointProjectionTools::IndexedCCVector2 P(_P->x, _P->z, i);
1008 points2D.push_back(P);
1009 }
1010
1011 std::list<CCCoreLib::PointProjectionTools::IndexedCCVector2*> hullPoint2;
1012 CCCoreLib::PointProjectionTools::extractConcaveHull2D(points2D, hullPoint2, 0.0001);
1013
1014 std::vector<unsigned> Vec;
1015 Vec.reserve(hullPoint2.size());
1016 for (auto it = hullPoint2.begin(); it != hullPoint2.end(); ++it)
1017 {
1018 Vec.push_back((*it)->index);
1019 }
1020
1021 unsigned cnt = static_cast<unsigned>(Vec.size());
1022
1023 ccPointCloud* contour = new ccPointCloud(stone.getName() + " - Contour Cloud");
1024 if (!contour->reserve(cnt))
1025 {
1026 // not enough memory
1027 delete contour;
1028 return nullptr;
1029 }
1030 bool hasColors = (stone.hasColors() && contour->reserveTheRGBTable());
1031
1032 for (auto e : Vec)
1033 {
1034 contour->addPoint(*stone.getPoint(e));
1035 if (hasColors)
1036 {
1037 contour->addColor(stone.getPointColor(e));
1038 }
1039
1040 }
1041
1042 ccPolyline* pContour = new ccPolyline(contour);
1043 pContour->addChild(contour);
1044
1045 if (pContour->reserve(cnt))
1046 {
1047 pContour->addPointIndex(0, cnt);
1048 pContour->setClosed(true);
1049 pContour->setVisible(true);
1050 pContour->setName(stone.getName() +" - Contour");
1051 pContour->setColor(ccColor::cyan);
1052 pContour->showColors(true);
1053 pContour->showVertices(true);
1054 }
1055 else
1056 {
1057 // not enough memory
1058 delete pContour;
1059 pContour = nullptr;
1060 }
1061
1062 return pContour;
1063 }
1064
1065 #endif
1066
1067 static ccPolyline* contourPoly2(const ccPointCloud& cloud0, const std::vector<int>& V, const QString& name)
1068 {
1069 std::vector<CCCoreLib::PointProjectionTools::IndexedCCVector2> points2D;
1070 points2D.reserve(V.size());
1071
1072 for (size_t i = 0; i < V.size(); ++i)
1073 {
1074 const CCVector3* _P = cloud0.getPoint(V[i]);
1075 CCCoreLib::PointProjectionTools::IndexedCCVector2 P(_P->x, _P->z, i);
1076 points2D.push_back(P);
1077 }
1078
1079 std::list<CCCoreLib::PointProjectionTools::IndexedCCVector2*> hullPoint2;
1080 CCCoreLib::PointProjectionTools::extractConcaveHull2D(points2D, hullPoint2, 0.0001);
1081
1082 std::vector<unsigned> Vec;
1083 Vec.reserve(hullPoint2.size());
1084 for (auto it = hullPoint2.begin(); it != hullPoint2.end(); ++it)
1085 {
1086 Vec.push_back((*it)->index);
1087 }
1088
1089 unsigned cnt = static_cast<unsigned>(Vec.size());
1090
1091 ccPointCloud* contour = new ccPointCloud(name + " - Contour Cloud");
1092 if (!contour->reserve(cnt))
1093 {
1094 // not enough memory
1095 delete contour;
1096 return nullptr;
1097 }
1098 bool hasColors = (cloud0.hasColors() && contour->reserveTheRGBTable());
1099
1100 for (auto e : Vec)
1101 {
1102 contour->addPoint(*cloud0.getPoint(V[e]));
1103 if (hasColors)
1104 {
1105 contour->addColor(cloud0.getPointColor(V[e]));
1106 }
1107 }
1108
1109 ccPolyline* pContour = new ccPolyline(contour);
1110 pContour->addChild(contour);
1111
1112 if (pContour->reserve(cnt))
1113 {
1114 pContour->addPointIndex(0, cnt);
1115 pContour->setClosed(true);
1116 pContour->setVisible(true);
1117 pContour->setName(name + " - Contour");
1118 pContour->setColor(ccColor::cyan);
1119 pContour->showColors(true);
1120 pContour->showVertices(true);
1121 }
1122 else
1123 {
1124 // not enough memory
1125 delete pContour;
1126 pContour = nullptr;
1127 }
1128
1129 return pContour;
1130 }
1131
1132 // Returns mortar maps (Depth and width)
1133 static ccPointCloud* getMortarMaps(ccPointCloud& f_cloudStones, ccPointCloud& f_cloudMortar)
1134 {
1135 //Binary Mortar
1136 f_cloudMortar.scale(100, 1000, 100); //To cm mm cm
1137 CCVector3 minBox(0, 0, 0);
1138 CCVector3 maxBox(0, 0, 0);
1139 f_cloudMortar.getBoundingBox(minBox, maxBox);
1140 int rowsM = ceil(maxBox.z);
1141 int colsM = ceil(maxBox.x);
1142
1143 cv::Mat corrMatM = cv::Mat::zeros(rowsM, colsM, CV_32F);
1144 cv::Mat imageBWS = cv::Mat::zeros(rowsM, colsM, CV_8U);
1145
1146 std::vector<cv::Point> idxPxM;
1147 std::vector<int> idxPxM1D;
1148 cloud2binary(f_cloudMortar, corrMatM, idxPxM, idxPxM1D, imageBWS);
1149
1150 //Skeleton
1151 cv::Mat skel = skeleton(imageBWS, false);
1152
1153 f_cloudMortar.scale(0.01, 0.001, 0.01); //Scale back
1154
1155 //Skeleton 3D
1156 std::vector<int> pixelsSkel;
1157 for (int i = 0; i < skel.rows; i++)
1158 {
1159 for (int j = 0; j < skel.cols; j++)
1160 {
1161 unsigned char a = skel.at<char>(i, j);
1162 if (a == 255)
1163 {
1164 int idx = corrMatM.at<int>(i, j);
1165 pixelsSkel.push_back(idx);
1166 }
1167 }
1168 }
1169
1170 std::vector<int> idxCloudMortarSkel = setIntersectIdxPixs1D(idxPxM1D, pixelsSkel);
1171 ccPointCloud* skelM = new ccPointCloud("Skeleton Mortar");
1172 if (!skelM->reserve(static_cast<unsigned>(idxCloudMortarSkel.size())))
1173 {
1174 // not enough memory
1175 delete skelM;
1176 return nullptr;
1177 }
1178 bool hasColors = (f_cloudMortar.hasColors() && skelM->reserveTheRGBTable());
1179
1180 for (auto e : idxCloudMortarSkel)
1181 {
1182 skelM->addPoint(*f_cloudMortar.getPoint(e));
1183 if (hasColors)
1184 {
1185 skelM->addColor(f_cloudMortar.getPointColor(e));
1186 }
1187 }
1188
1189 CCCoreLib::CloudSamplingTools::SFModulationParams modParams(false); //Subsample result
1190 CCCoreLib::ReferenceCloud* refCloud = CCCoreLib::CloudSamplingTools::resampleCloudSpatially(skelM, 0.01, modParams, 0, 0); //1 point per cm^2
1191 if (!refCloud)
1192 {
1193 delete skelM;
1194 return nullptr;
1195 }
1196
1197 ccPointCloud* f_skelMortar = skelM->partialClone(refCloud); //save output
1198
1199 delete refCloud;
1200 refCloud = nullptr;
1201 delete skelM;
1202 skelM = nullptr;
1203
1204 if (!f_skelMortar)
1205 {
1206 return nullptr;
1207 }
1208
1209 // Maps using PCL
1210 //Mortar depth map
1211 pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloudStones(new pcl::PointCloud<pcl::PointXYZ>);
1212 pcl_cloudStones->width = f_cloudStones.size();
1213 pcl_cloudStones->height = 1;
1214 pcl_cloudStones->points.resize(pcl_cloudStones->width * pcl_cloudStones->height);
1215
1216 for (size_t i = 0; i < f_cloudStones.size(); ++i)
1217 {
1218 const CCVector3* P = f_cloudStones.getPoint(i);
1219 pcl_cloudStones->points[i].x = P->x;
1220 pcl_cloudStones->points[i].y = P->y;
1221 pcl_cloudStones->points[i].z = P->z;
1222 }
1223
1224 pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
1225 kdtree.setInputCloud(pcl_cloudStones);
1226
1227 std::vector<double> distStD, dist3St, distStD2;
1228 std::vector<double> distStW;
1229 for (unsigned i = 0; i < f_skelMortar->size(); i++)
1230 {
1231 pcl::PointXYZ searchPoint;
1232 searchPoint.x = f_skelMortar->getPoint(i)->x;
1233 searchPoint.y = f_skelMortar->getPoint(i)->y;
1234 searchPoint.z = f_skelMortar->getPoint(i)->z;
1235
1236 std::vector<int> pointIdxRadiusSearch;
1237 std::vector<float> pointRadiusSquaredDistance;
1238
1239 float radius = 0.1f; //10cm // TODO FIXME:the search radius is hardcoded?!
1240 pcl::PointXYZ nnPoint;
1241
1242 if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
1243 {
1244 nnPoint.x = pcl_cloudStones->points[pointIdxRadiusSearch[0]].x;
1245 nnPoint.y = pcl_cloudStones->points[pointIdxRadiusSearch[0]].y;
1246 nnPoint.z = pcl_cloudStones->points[pointIdxRadiusSearch[0]].z;
1247
1248 double disty = std::abs(searchPoint.y - nnPoint.y); //Depth
1249 double distx = std::abs(searchPoint.x - nnPoint.x);
1250 double distz = std::abs(searchPoint.z - nnPoint.z);
1251 double dist3 = std::sqrt(distx*distx + distz * distz);//2D distance (XZ) will be used as a reference for the creation of mortar width map
1252
1253 distStD.push_back(disty * 1000); //depth (y axis) in mm
1254 dist3St.push_back(dist3);
1255
1256 double dist1 = 1000.0; double dist2 = 1000.0; double dist1y = 1000.0; double dist2y = 1000.0;
1257 int idx1 = -1; int idx2 = -1;
1258 for (size_t j = 0; j < pointIdxRadiusSearch.size(); ++j)
1259 {
1260 double distx = std::abs(searchPoint.x - pcl_cloudStones->points[pointIdxRadiusSearch[j]].x); //2D distance
1261 double distz = std::abs(searchPoint.z - pcl_cloudStones->points[pointIdxRadiusSearch[j]].z);
1262 double disty = std::abs(searchPoint.y - pcl_cloudStones->points[pointIdxRadiusSearch[j]].y);
1263 double dist3 = std::sqrt(abs(distx)*abs(distx) + abs(distz)*abs(distz));
1264
1265 int idxSt = f_cloudStones.getPointScalarValue(pointIdxRadiusSearch[j]);
1266
1267 if (j == 0)
1268 {
1269 //Initialise NN stone 1
1270 dist1 = dist3;
1271 dist1y = disty;
1272 idx1 = idxSt;
1273 }
1274 else
1275 {
1276 if (idxSt == idx1)
1277 {
1278 //If point belongs to stone 1
1279 if (dist3 < dist1)
1280 dist1 = dist3;
1281 dist1y = disty;
1282 }
1283 else if (idx2 == -1)
1284 {
1285 //Initialise stone 2
1286 idx2 = idxSt;
1287 dist2 = dist3;
1288 dist2y = disty;
1289 }
1290 if (idxSt == idx2)
1291 {
1292 //If point belongs to stone 2
1293 if (dist3 < dist2)
1294 dist2 = dist3;
1295 dist2y = disty;
1296 }
1297 }
1298 } //Points are ordered by radius. Idx 0 is the nearest neighbour
1299
1300 //For each point, there is dist1 or dist1 and dist2
1301 if (idx1 > -1 && idx2 == -1 && dist1 < 1000)
1302 {
1303 //Only one stone (boundaries of wall)
1304 distStW.push_back(dist1 * 1000);
1305 distStD2.push_back(dist1y * 1000);
1306 }
1307 else if (idx1 > -1 && idx2 > -1)
1308 {
1309 distStW.push_back((dist1 + dist2) * 1000);
1310 distStD2.push_back((dist1y + dist2y) / 2 * 1000);
1311 }
1312 }
1313 else
1314 {
1315 distStD.push_back(100); //Add high values for non-calculated values (no neighbours available). These values can be modified according to the needs
1316 distStD2.push_back(100);
1317 dist3St.push_back(200);
1318 distStW.push_back(200);
1319 }
1320 }
1321
1322 //Add depth as a scalarField to f_skelMortar
1323 int sfIdxD = f_skelMortar->getScalarFieldIndexByName("Mortar relative depth (mm)");
1324 if (sfIdxD < 0)
1325 {
1326 sfIdxD = f_skelMortar->addScalarField("Mortar relative depth (mm)");
1327 }
1328 if (sfIdxD >= 0)
1329 {
1330 CCCoreLib::ScalarField* depthSF = f_skelMortar->getScalarField(sfIdxD);
1331
1332 for (unsigned i = 0; i < static_cast<unsigned>(distStD2.size()); i++)
1333 {
1334 ScalarType index = static_cast<ScalarType>(distStD2[i]);
1335 depthSF->setValue(i, index);
1336 }
1337 depthSF->computeMinAndMax();
1338
1339 f_skelMortar->setCurrentDisplayedScalarField(sfIdxD);
1340 f_skelMortar->showSF(true);
1341 }
1342 else
1343 {
1344 // not enough memory?
1345 }
1346
1347 //Add width as a scalarField to f_skelMortar
1348 int sfIdxW = f_skelMortar->getScalarFieldIndexByName("Mortar relative width (mm)");
1349 if (sfIdxW < 0)
1350 {
1351 sfIdxW = f_skelMortar->addScalarField("Mortar relative width (mm)");
1352 }
1353 if (sfIdxW >= 0)
1354 {
1355 CCCoreLib::ScalarField* widthSF = f_skelMortar->getScalarField(sfIdxW);
1356
1357 for (unsigned i = 0; i < static_cast<unsigned>(distStW.size()); i++)
1358 {
1359 ScalarType index = static_cast<ScalarType>(distStW[i]);
1360 widthSF->setValue(i, index);
1361 }
1362 widthSF->computeMinAndMax();
1363
1364 f_skelMortar->setCurrentDisplayedScalarField(sfIdxW);
1365 f_skelMortar->showSF(true);
1366 }
1367 else
1368 {
1369 // not enough memory?
1370 }
1371
1372 f_skelMortar->setName("Mortar Maps");
1373 f_skelMortar->setPointSize(7);
1374
1375 return f_skelMortar;
1376 }
1377
1378 #if 0 // not used
1379 static float getDensity(const ccPointCloud& cloud, const std::vector<int>& idx)
1380 {
1381 ccPointCloud* stone = new ccPointCloud("stone");
1382 if (!stone->reserve(static_cast<unsigned>(idx.size())))
1383 {
1384 // not enough memory
1385 return 0.0;
1386 }
1387
1388 for (auto e : idx)
1389 {
1390 stone->addPoint(*cloud.getPoint(e));
1391 }
1392
1393 CCVector3 minBox(0, 0, 0);
1394 CCVector3 maxBox(0, 0, 0);
1395 stone->getBoundingBox(minBox, maxBox);
1396
1397 float area = (maxBox.x - minBox.x)*(maxBox.z - minBox.z);
1398 float density = (area > 0 ? idx.size() / area : 0.0f);
1399 return density;
1400 }
1401 #endif
1402
1403 // This method should enable or disable your plugin actions
1404 // depending on the currently selected entities ('selectedEntities').
1405 void ccAutoSeg::onNewSelection(const ccHObject::Container& selectedEntities)
1406 {
1407 if (m_action)
1408 {
1409 m_action->setEnabled(selectedEntities.size() == 1 && selectedEntities.front()->isA(CC_TYPES::POINT_CLOUD));
1410 }
1411 }
1412
1413 // This method returns all the 'actions' your plugin can perform.
1414 // getActions() will be called only once, when plugin is loaded.
1415 QList<QAction*> ccAutoSeg::getActions()
1416 {
1417 if ( !m_action )
1418 {
1419 m_action = new QAction( getName(), this );
1420 m_action->setToolTip( getDescription() );
1421 m_action->setIcon(QIcon(QString::fromUtf8(":/CC/plugin/qAutoSeg/cyberbuildIcon.png")));
1422
1423 // Connect appropriate signal
1424 connect( m_action, &QAction::triggered, this, &ccAutoSeg::doAction );
1425 }
1426
1427 return { m_action };
1428 }
1429
1430 void ccAutoSeg::doAction()
1431 {
1432 if (m_app == nullptr)
1433 {
1434 // m_app should have already been initialized by CC when plugin is loaded
1435 Q_ASSERT(false);
1436
1437 return;
1438 }
1439
1440 //Select point cloud (manually, from CC UI)
1441 ccPointCloud* cloud0 = nullptr;
1442 {
1443 const ccHObject::Container& selectedEntities = m_app->getSelectedEntities();
1444
1445 if (!m_app->haveOneSelection() || !selectedEntities.front()->isA(CC_TYPES::POINT_CLOUD))
1446 {
1447 m_app->dispToConsole("Select one cloud!", ccMainAppInterface::ERR_CONSOLE_MESSAGE);
1448 return;
1449 }
1450
1451 cloud0 = static_cast<ccPointCloud*>(selectedEntities.front());
1452 }
1453
1454 //Create pop-up window
1455 ProfileImportDlg piDlg(m_app->getMainWindow());
1456 if (!piDlg.exec())
1457 {
1458 return;
1459 }
1460
1461 double joints = piDlg.jointsSpinBox->value(); //Mortar joint for CWT
1462 double segH = piDlg.segmentHSpinBox->value(); //Segmentation window
1463 double segV = piDlg.segmentVSpinBox->value();
1464
1465 bool checkAlignment = piDlg.alignmentCheckBox->isChecked();
1466 bool checkSegment = piDlg.segmentCheckBox->isChecked();
1467 bool checkMortar = piDlg.mortarCheckBox->isChecked();
1468
1469 //If mortar is selected but not segmentation, show an error message and ask for checking both
1470 if (checkMortar == true && checkSegment == false)
1471 {
1472 m_app->dispToConsole("Segmentation is required for the creation of mortar maps. Please, check both \"Automatic segmentation\" and \"Mortar maps\".", ccMainAppInterface::ERR_CONSOLE_MESSAGE);
1473 return;
1474 }
1475
1476 //Create log file
1477 std::ofstream auto_seg_log;
1478 {
1479 std::string filename("APS_log_");
1480 filename += QDateTime::currentDateTime().toString("yyyy_MM_dd_hh_mm_ss").toStdString();
1481 filename += ".txt";
1482 auto_seg_log.open(filename);
1483
1484 auto_seg_log << dateStamp() << " Importing point cloud" << endl;
1485
1486 std::string parentName = (cloud0->getParent() != nullptr ? cloud0->getParent()->getName().toStdString() : "");
1487
1488 std::string stamp = dateStamp();
1489 auto_seg_log << stamp << " File \"" << parentName << "\" loaded" << endl;
1490 auto_seg_log << stamp << " Cloud \"" << cloud0->getName().toStdString() << "\" imported" << endl;
1491
1492 //Add input parameters and tasks to be done to the log
1493 stamp = dateStamp();
1494 auto_seg_log << stamp << " Estimated mortar joints width: " << joints << "cm" << endl;
1495 auto_seg_log << stamp << " Segmentation window. X: " << segH << "m and Y: " << segV << "m" << endl;
1496 auto_seg_log << stamp << " Alignment: " << checkAlignment << endl;
1497 auto_seg_log << stamp << " Segmentation: " << checkSegment << endl;
1498 auto_seg_log << stamp << " Mortar maps: " << checkMortar << endl;
1499 auto_seg_log << " Starting process..." << endl;
1500 }
1501
1502 try
1503 {
1504 //Check plane orientation
1505 cv::Mat_<double> cldm(cloud0->size(), 3);
1506 for (unsigned i = 0; i < cloud0->size(); i++)
1507 {
1508 const CCVector3* P = cloud0->getPoint(i);
1509 cldm.row(i)(0) = P->x;
1510 cldm.row(i)(1) = P->y;
1511 cldm.row(i)(2) = P->z;
1512 }
1513
1514 cv::Mat_<double> mean;
1515 cv::PCA pca(cldm, mean, 150);
1516
1517 cv::Vec3d nrm = pca.eigenvectors.row(2); nrm = nrm / norm(nrm);
1518 cv::Vec3d x0 = pca.mean;
1519
1520 //Built-in function
1521 CCVector3 nrm2(nrm(0), nrm(1), nrm(2));
1522 CCVector3 a2(0, 1, 0);
1523 ccGLMatrix R2 = ccGLMatrix::FromToRotation(nrm2, a2);
1524
1525 cloud0->applyRigidTransformation(R2); //Another rotation to aligned boundaries of the wall to axes X and Z is needed (rotate in Y axis and take angle linked to smallest bounding box)
1526
1527 //Subsample cloud0 to obtain optimal rotY if size is over a threshold
1528 std::vector<int> v2(cloud0->size());
1529 std::iota(v2.begin(), v2.end(), 0);
1530 std::random_shuffle(v2.begin(), v2.end());
1531
1532 size_t nbSub = std::min(v2.size(), static_cast<size_t>(1000000));
1533
1534 ccPointCloud* cloud2Rot = new ccPointCloud("cloud2Rot"); //Kike
1535 if (!cloud2Rot->reserve(static_cast<unsigned>(nbSub)) || !cloud2Rot->reserveTheRGBTable())
1536 {
1537 // not enough memory
1538 ccLog::Error("Not enough memory");
1539 delete cloud2Rot;
1540 return;
1541 }
1542
1543 bool hasColors = cloud0->hasColors();
1544 if (hasColors && !cloud2Rot->reserveTheRGBTable())
1545 {
1546 // not enough memory to process colors
1547 hasColors = false;
1548 }
1549
1550 for (size_t i = 0; i < nbSub; i++)
1551 {
1552 cloud2Rot->addPoint(*cloud0->getPoint(v2[i]));
1553 if (hasColors)
1554 {
1555 cloud2Rot->addColor(cloud0->getPointColor(v2[i]));
1556 }
1557 }
1558
1559 double th0 = optRotY(*cloud2Rot);
1560
1561 CCVector3 c1, c2, c3, c4;
1562 c1[0] = cos(th0); c1[1] = 0; c1[2] = -sin(th0); c1[3] = 0;
1563 c2[0] = 0; c2[1] = 1; c2[2] = 0; c2[3] = 0;
1564 c3[0] = sin(th0); c3[1] = 0; c3[2] = cos(th0); c3[3] = 0;
1565 c4[0] = 0; c4[1] = 0; c4[2] = 0; c4[3] = 1;
1566 ccGLMatrix R3(c1, c2, c3, c4);
1567
1568 cloud0->applyRigidTransformation(R3);
1569
1570 //If only alignment is required
1571 if (checkAlignment == true && checkSegment == false && checkMortar == false)
1572 {
1573 ccGLWindowInterface* win = m_app->getActiveGLWindow();
1574 win->setView(CC_FRONT_VIEW);
1575 return;
1576 }
1577
1578 auto_seg_log << dateStamp() << "Cloud aligned to XZ." << endl;
1579
1580 //Bounding box
1581 CCVector3 minBox(0, 0, 0);
1582 CCVector3 maxBox(0, 0, 0);
1583 cloud0->getBoundingBox(minBox, maxBox);
1584
1585 //Move point cloud to origin
1586 cloud0->translate(-minBox);
1587
1588 double axX = maxBox.x - minBox.x;
1589 double axY = maxBox.z - minBox.z;
1590
1591 // targeted window dimensions
1592 float winX = segH;
1593 float winY = segV;
1594
1595 // Number of windows
1596 unsigned int nwinX = std::ceil(axX / winX);
1597 unsigned int nwinY = std::ceil(axY / winY);
1598
1599 //real window dimensions without overlaping
1600 double rwinX = axX / nwinX;
1601 double rwinY = axY / nwinY;
1602
1603 //boundaries of windows [(xmin1,xmax1,ymin1,ymax1),(xmin2,xmax2....)...]
1604 std::vector<std::vector<double>> boundaries;
1605
1606 double X = 0;
1607 std::vector<int> global_idx;
1608
1609 for (unsigned int i = 0; i < nwinX; i++)
1610 {
1611 double Y = 0;
1612 for (unsigned int j = 0; j < nwinY; j++)
1613 {
1614 boundaries.push_back({ X, X + rwinX + 0.3, Y, Y + rwinY + 0.3 });
1615 Y += rwinY;
1616 }
1617
1618 X += rwinX;
1619 }
1620
1621 //spliting points by index
1622 std::vector<std::vector<int>> idxClouds(boundaries.size());
1623
1624 auto_seg_log << dateStamp() << " Splitting cloud in patches (" << nwinX * nwinY << ")" << endl;
1625
1626 for (unsigned i = 0; i < cloud0->size(); i++)
1627 {
1628 for (size_t j = 0; j < boundaries.size(); j++)
1629 {
1630 const CCVector3* p = cloud0->getPoint(i);
1631 const auto& boundary = boundaries[j];
1632 if (p->x >= boundary[0] && p->x <= boundary[1] && p->z >= boundary[2] && p->z <= boundary[3])
1633 {
1634 idxClouds[j].push_back(i);
1635 }
1636
1637 }
1638 global_idx.push_back(i);
1639 }
1640
1641 // removing empty ones
1642 {
1643 std::vector<std::vector<int>> idxCloudsC;
1644 for (auto& e : idxClouds)
1645 if (e.size() != 0)
1646 idxCloudsC.push_back(std::move(e));
1647
1648 idxClouds.swap(idxCloudsC);
1649 }
1650
1651 auto_seg_log << dateStamp() << " Splitting done" << endl;
1652
1653 // adding points to the corresponding window
1654 auto_seg_log << dateStamp() << " Creating subclouds" << endl;
1655 std::vector<ccPointCloud*> sub_clouds;
1656 for (size_t i = 0; i < idxClouds.size(); i++)
1657 {
1658 QString str = "part_" + QString::number(i);
1659 if (idxClouds[i].size() != 0)
1660 {
1661 ccPointCloud* win_cloud = new ccPointCloud(str);
1662 if (!win_cloud->reserve(idxClouds[i].size()))
1663 {
1664 // not enough memory
1665 ccLog::Warning(QString("Failed to create sub cloud #%1 (not enough memory)").arg(i + 1));
1666 delete win_cloud;
1667 continue;
1668 }
1669 bool withColors = (cloud0->hasColors() && win_cloud->reserveTheRGBTable());
1670
1671 for (auto e : idxClouds[i])
1672 {
1673 win_cloud->addPoint(*cloud0->getPoint(e));
1674 if (withColors)
1675 {
1676 win_cloud->addColor(cloud0->getPointColor(e));
1677 }
1678 }
1679
1680 sub_clouds.push_back(win_cloud);
1681 }
1682 }
1683
1684 // Going through the sub_clouds vector
1685 std::vector<std::vector<std::pair<int, int>>> stones;
1686 int stoneIdx = 0;
1687
1688 for (size_t z = 0; z < sub_clouds.size(); ++z)
1689 {
1690 ccPointCloud* subCloud = sub_clouds[z];
1691
1692 auto_seg_log << dateStamp() << " Cloud #" << z+1 << ": " << subCloud->size() << " pts" << endl;
1693
1694 if (subCloud->size() == 0)
1695 {
1696 assert(false);
1697 continue;
1698 }
1699
1700 m_app->dispToConsole(QString("part_%1 has %2 points").arg(z).arg(subCloud->size()), ccMainAppInterface::STD_CONSOLE_MESSAGE);
1701
1702 //Bounding box
1703 CCVector3 minBox_0(0, 0, 0);
1704 CCVector3 maxBox_0(0, 0, 0);
1705 subCloud->getBoundingBox(minBox_0, maxBox_0);
1706
1707 //Move point cloud to origin
1708 subCloud->translate(-minBox_0);
1709
1710 //Create and populate maps
1711
1712 //Bounding box
1713 CCVector3 minBox2(0, 0, 0);
1714 CCVector3 maxBox2(0, 0, 0);
1715 subCloud->getBoundingBox(minBox2, maxBox2);
1716
1717 subCloud->scale(100, 1000, 100); //To cm mm cm
1718
1719 int rows = std::ceil(maxBox2.z);
1720 int cols = std::ceil(maxBox2.x);
1721
1722 //Correspondence 2D->1D. Each px has an index instead of two coords
1723 cv::Mat corrMat = cv::Mat::zeros(rows, cols, CV_32F);
1724 int cntC = 0;
1725 for (int y = 0; y < corrMat.rows; y++)
1726 {
1727 for (int x = 0; x < corrMat.cols; x++)
1728 {
1729 corrMat.at<int>(y, x) = cntC++;
1730 }
1731 }
1732
1733 //Depth
1734 cv::Mat image = cv::Mat::zeros(rows, cols, CV_32F);
1735
1736 std::vector<cv::Point> idxPx;
1737 std::vector<int> idxPx1D;
1738 for (unsigned i = 0; i < subCloud->size(); i++)
1739 {
1740 //depth
1741 const CCVector3* pointC = subCloud->getPoint(i);
1742 int z = std::floor(pointC->z);
1743 int x = std::floor(pointC->x);
1744 image.at<float>(z, x) = static_cast<float>(pointC->y);
1745
1746 //idxPx per 3D point
1747 idxPx.push_back(cv::Point(z, x));
1748 idxPx1D.push_back(corrMat.at<int>(z, x));
1749 }
1750
1751 //Median filter to fill holes
1752 cv::Mat imageFilt = cv::Mat::zeros(rows, cols, CV_32F);
1753 cv::medianBlur(image, imageFilt, 3);
1754
1755 //Check for black areas after filling holes. These are structural holes (windows, doors)
1756 std::vector<int> nullCols, nullRows;
1757
1758 for (int y = 0; y < image.rows; y++)
1759 {
1760 for (int x = 0; x < image.cols; x++)
1761 {
1762 if (image.at<float>(y, x) == 0)
1763 {
1764 image.at<float>(y, x) = imageFilt.at<float>(y, x);
1765
1766 if (image.at<float>(y, x) == 0)
1767 {
1768 nullCols.push_back(x);
1769 nullRows.push_back(y);
1770 }
1771 }
1772 }
1773 }
1774
1775 imageFilt.release();
1776
1777 //cv::Mat imagePlot;
1778 //image.convertTo(imagePlot, CV_8U);
1779 //imagePlot.release();
1780
1781 subCloud->scale(0.01, 0.001, 0.01); //Scale back
1782 subCloud->translate(minBox_0); //Translate back
1783
1784 auto_seg_log << dateStamp() << " Depth map created" << endl;
1785
1786 //CWT
1787 double scaleCWT = joints * 0.254;//1.27 for 5cm
1788 cv::Mat cwtcfs;
1789 cv::Mat bincfs = cv::Mat::zeros(cwtcfs.size(), CV_8UC1);
1790 cwt2d(scaleCWT, image, cwtcfs, bincfs);
1791 cwtcfs.release();
1792
1793 //Cropping borders added in cwt
1794
1795 cv::Rect myROI(0, 0, image.cols, image.rows);
1796 bincfs = bincfs(myROI);
1797 image.release();
1798
1799 auto_seg_log << dateStamp() << " CWT calculated" << endl;
1800
1801 //Clean areas from holes
1802 for (size_t i = 0; i < nullRows.size(); i++)
1803 {
1804 bincfs.at<uchar>(nullRows[i], nullCols[i]) = 0;
1805 }
1806
1807 //Fill in the holes
1808 cv::Mat hola;
1809 fillEdgeImage(bincfs, hola, z);
1810 bincfs.release();
1811
1812 //Remove small segments
1813 cv::Mat output = threshSegments(hola, 20);
1814 hola.release();
1815
1816 //Erosion + Dilation
1817 int erosion_elem = 2; //0 = morph_rect, 1 = morph_cross, 2 = morph_ellipse (I think Matlab uses 0)
1818 int erosion_size = 1;
1819 cv::Mat erodedM;
1820 erosionCC(erosion_elem, erosion_size, output, erodedM);
1821 output.release();
1822
1823
1824 ///This has been added here (not in matlab)
1825 int dilation_elem = 2; //0 = morph_rect, 1 = morph_cross, 2 = morph_ellipse
1826 int dilation_size = 1;
1827 cv::Mat dilatedM;
1828 dilationCC(dilation_elem, dilation_size, erodedM, dilatedM);
1829 erodedM.release();
1830
1831 auto_seg_log << dateStamp() << " CWT post-processing done" << endl;
1832
1833 //EXTRACT INFO FROM BINARY SEGMENTATION
1834 cv::Mat labels, stats;
1835 std::vector<std::vector<cv::Point>> pixsLabel = extractInfo(dilatedM, labels, stats);
1836 dilatedM.release();
1837
1838 if (stats.rows < 2)
1839 {
1840 assert(false);
1841 continue;
1842 }
1843
1844 //OPTIMISE CWT
1845 //areas
1846 std::vector<double> area(stats.rows - 1, 0); //nLabels-1 as i = 0 corresponds to background
1847 for (int i = 1; i < stats.rows; i++)
1848 {
1849 area.at(i - 1) = stats.at<int>(i, cv::CC_STAT_AREA);
1850 }
1851 //mean
1852 double meanArea = std::accumulate(area.begin(), area.end(), 0.0) / (stats.rows - 1);
1853 stats.release();
1854
1855 //std
1856 double stdevArea = 0.0;
1857 {
1858 std::vector<double> diff(area.size());
1859 std::transform(area.begin(), area.end(), diff.begin(), [meanArea](double x) { return x - meanArea; });
1860 double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
1861 stdevArea = std::sqrt(sq_sum / area.size());
1862 diff.clear();
1863 }
1864
1865 int op = 3; //op = 1: lt; op = 2: leq; op = 3: gt; op = 4: geq;
1866 double thres = meanArea + stdevArea; //meanArea + 5*stdevArea
1867 std::vector<int> bigStones = findMatlab(area, op, thres);
1868 area.clear();
1869
1870 //Divide big stones. Erode big > new segments. New matrix (from dilateM) without big ones but adding new segments
1871 cv::Mat firstSegmentation = stonesDivision(pixsLabel, labels, bigStones);
1872 bigStones.clear();
1873 pixsLabel.clear();
1874 labels.release();
1875
1876 std::vector<std::vector<cv::Point>> contours2;
1877 {
1878 cv::Mat labels2, stats2;
1879 std::vector<std::vector<cv::Point>> pixsLabel2 = extractInfo(firstSegmentation, labels2, stats2, &contours2); //Extract info after big stone division
1880 }
1881
1882 auto_seg_log << dateStamp() << " Big stones divided" << endl;
1883
1884 //cv::Mat skel = skeleton(firstSegmentation, 1);
1885 //skel.release();
1886
1887 // Find the convex hull object for each contour
1888 std::vector<std::vector<cv::Point>> hull(contours2.size());
1889 for (size_t i = 0; i < contours2.size(); i++)
1890 {
1891 convexHull(cv::Mat(contours2[i]), hull[i], false);
1892 }
1893 contours2.clear();
1894
1895 // Draw contours + hull results
1896 cv::RNG rng(12345);
1897 std::vector<std::vector<cv::Point>> pixsHullSt(hull.size());
1898 cv::Mat drawingAll = cv::Mat::zeros(firstSegmentation.size(), CV_8UC3);
1899
1900 // Segments growth
1901 std::vector<int> pixsLabels(firstSegmentation.cols*firstSegmentation.rows, -1); //Store idxs in a vector instead of vector of vectors //FK2310
1902 std::vector<int> pixsLabels2(firstSegmentation.cols*firstSegmentation.rows, -1); //Store idxs in a vector instead of vector of vectors. Consider overlapping //FK2310
1903 for (size_t i = 0; i < hull.size(); i++) //
1904 {
1905 cv::Mat drawing = cv::Mat::zeros(firstSegmentation.size(), CV_8UC1);
1906
1907 cv::Scalar color(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));
1908 cv::Scalar color2(255, 255, 255);
1909 drawContours(drawing, hull, i, color2, -1, 8, {}, 0, {}); //5th input param to -1 to colour segments
1910 drawContours(drawingAll, hull, i, color, -1, 8, {}, 0, {}); //To plot
1911
1912 cv::Mat labelsHull, statsHull;
1913 std::vector<std::vector<cv::Point>> pixsHull = extractInfo(drawing, labelsHull, statsHull); //Extract info from individual convex hulls
1914
1915 pixsHullSt[i] = pixsHull[0];
1916
1917 for (size_t j = 0; j < pixsHull[0].size(); j++)
1918 {
1919 int idx = corrMat.at<int>(pixsHull[0][j].x, pixsHull[0][j].y);
1920 if (pixsLabels.at(idx) == -1)
1921 {
1922 // Stones inside each other not considered as duplicated
1923 pixsLabels.at(idx) = i;
1924 }
1925 else
1926 {
1927 // Additional vector to consider pixels labelled as part of two stones (due to convex hull operation, for example)
1928 pixsLabels2.at(idx) = i;
1929 }
1930 }
1931 }
1932 firstSegmentation.release();
1933 hull.clear();
1934 drawingAll.release();
1935
1936 auto_seg_log << dateStamp() << " Convex hulls defined" << endl;
1937
1938 // EXTRACT 3D POINTS PER SEGMENT
1939 for (size_t i = 0; i < pixsHullSt.size(); i++)
1940 {
1941 std::vector<cv::Point>& pixels = pixsHullSt[i];
1942
1943 ////////// New 1D
1944 std::vector<int> pixels1d;
1945 for (const auto& e : pixels)
1946 {
1947 int idx = corrMat.at<int>(e.x, e.y);
1948 pixels1d.push_back(idx);
1949 }
1950
1951 std::vector<int> idxCloud;
1952 for (size_t j = 0; j < idxPx.size(); j++)
1953 {
1954 if (pixsLabels[idxPx1D[j]] == i)
1955 idxCloud.push_back(j);
1956 else if (pixsLabels2[idxPx1D[j]] == i) //Check potential second label (this will be cleaned afterwards)
1957 idxCloud.push_back(j);
1958 }
1959
1960 std::vector<std::pair<int, int>> global_SglStoneIdx;
1961 unsigned max_size = subCloud->size() / 1; // maximum size per single stone
1962 //float density = getDensity(*subCloud, idxCloud);
1963 //float max_density = 40000.0f;
1964 if (idxCloud.size() < max_size / 10)
1965 {
1966 for (auto e : idxCloud)
1967 {
1968 if (e < idxClouds[z].size())
1969 {
1970 global_SglStoneIdx.push_back(std::make_pair(idxClouds[z][e], stoneIdx));
1971 }
1972 else
1973 {
1974 auto_seg_log << "====> Error :" << z << " " << e << endl;
1975 }
1976 }
1977
1978 stones.push_back(global_SglStoneIdx);
1979 stoneIdx++;
1980 }
1981
1982 idxCloud.clear();
1983 global_SglStoneIdx.clear();
1984 pixels.clear();
1985 pixels1d.clear();
1986 }
1987
1988 auto_seg_log << dateStamp() << " 3D points per segment extracted" << endl;
1989
1990 /*** HERE ENDS THE ACTION ***/
1991
1992 //Deleting releasing memory ?
1993 auto_seg_log << dateStamp() << " Point cloud processed. Releasing memory ... " << endl;
1994
1995 corrMat.release();
1996 pixsHullSt.clear();
1997
1998 subCloud->removeMetaData("cloud");
1999 delete subCloud;
2000 sub_clouds[z] = subCloud = nullptr;
2001 }
2002
2003 auto_seg_log << dateStamp() << " Stitching stones" << endl;
2004
2005 //concatenating all stone's cloudidx and stoneidx pairs into one vector instead of a vector of vectors with separate stone in each vector
2006 std::vector<std::pair<int, int>> allStones;
2007 if (stones.size() != 0)
2008 {
2009 allStones = stones[0];
2010 }
2011
2012 for (size_t i = 1; i < stones.size(); i++)
2013 {
2014 allStones.insert(allStones.end(), stones[i].begin(), stones[i].end());
2015 }
2016 auto_seg_log << dateStamp() << " Total number of unstitched stones: " << stones.size() << endl;
2017
2018 //Start of stiching
2019
2020 //indentifying and merging indentical stones after the stitching
2021 //This can be defined as a function
2022
2023 // sorting a vector of pairs
2024 std::sort(allStones.begin(), allStones.end(), [](auto &left, auto &right) {return left.first < right.first; });
2025 auto_seg_log << dateStamp() << " Start looking for whole stones " << endl;
2026
2027 size_t i = 0;
2028 while (i < allStones.size())
2029 {
2030 size_t j = i + 1;
2031
2032 while (j < allStones.size() && allStones[i].first == allStones[j].first && allStones[i].second != allStones[j].second)
2033 {
2034 int oldIdx = allStones[j].second;
2035 int newIdx = allStones[i].second;
2036
2037 for (size_t k = 0; k < allStones.size(); k++)
2038 {
2039 if (allStones[k].second == oldIdx)
2040 {
2041 allStones[k].second = newIdx;
2042 }
2043 }
2044
2045 j++;
2046 }
2047 i = j;
2048 }
2049
2050 //End of stitching
2051 auto_seg_log << dateStamp() << " Indices corrected " << endl;
2052
2053 //eliminating repeated pairs ( originaly common points in stones)
2054 allStones.erase(unique(allStones.begin(), allStones.end()), allStones.end());
2055 auto_seg_log << dateStamp() << " Repeated pairs removed " << endl;
2056
2057 std::vector<int> allStones_Idx; // storing only cloud index of points that's within stones
2058
2059 auto_seg_log << dateStamp() << " Getting all stones idx " << endl;
2060 allStones_Idx.reserve(allStones.size());
2061 for (const auto& e : allStones)
2062 {
2063 allStones_Idx.push_back(e.first);
2064 }
2065 std::sort(allStones_Idx.begin(), allStones_Idx.end());
2066
2067 // Mortar idx
2068 std::vector<int> allMortar;
2069 std::set_difference(global_idx.begin(), global_idx.end(), allStones_Idx.begin(), allStones_Idx.end(), std::inserter(allMortar, allMortar.end()));
2070
2071 auto_seg_log << dateStamp() << " Mortar idx identified " << endl;
2072
2073 // correcting stone indices (make them 1,2,3 .... in order)
2074
2075 std::vector<int> stoneId; // saving all the existing and used indices for stones
2076 stoneId.reserve(allStones.size());
2077 for (const auto& e : allStones)
2078 {
2079 stoneId.push_back(e.second);
2080 }
2081
2082 std::sort(stoneId.begin(), stoneId.end());
2083
2084 stoneId.erase(unique(stoneId.begin(), stoneId.end()), stoneId.end());
2085
2086 std::vector<std::vector<int>> vecStones(stoneId.size());
2087 std::sort(allStones.begin(), allStones.end(), [](auto &left, auto &right) {return left.second < right.second; });
2088
2089 int old_idx = allStones[0].second;
2090 int new_idx = 0;
2091
2092 for (size_t i = 0; i < allStones.size(); i++)
2093 {
2094 if (allStones[i].second == old_idx)
2095 {
2096 allStones[i].second = new_idx;
2097 vecStones[new_idx].push_back(allStones[i].first);
2098 }
2099 else
2100 {
2101 ++new_idx;
2102 old_idx = allStones[i].second;
2103 allStones[i].second = new_idx;
2104 vecStones[new_idx].push_back(allStones[i].first);
2105 }
2106 }
2107
2108 stoneId.clear();
2109 allStones_Idx.clear(); // values were sorted
2110
2111 for (const auto& e : allStones)
2112 {
2113 allStones_Idx.push_back(e.first);
2114 stoneId.push_back(e.second);
2115 }
2116
2117 // suffling stone IDs for better visuals
2118 std::vector<int> stIdShuffle = stoneId;
2119 std::sort(stIdShuffle.begin(), stIdShuffle.end());
2120 stIdShuffle.erase(unique(stIdShuffle.begin(), stIdShuffle.end()), stIdShuffle.end());
2121
2122 auto rng = std::default_random_engine{};
2123 std::shuffle(stIdShuffle.begin(), stIdShuffle.end(), rng);
2124
2125 // generating final clouds
2126 auto_seg_log << dateStamp() << " Writing final clouds" << endl;
2127
2128 //stones
2129 ccPointCloud* f_cloudStones = nullptr;
2130 if (allStones_Idx.size() != 0)
2131 {
2132 f_cloudStones = new ccPointCloud("f_cloudStones");
2133 if (f_cloudStones->reserve(static_cast<unsigned>(allStones_Idx.size())) && f_cloudStones->reserveTheRGBTable())
2134 {
2135 for (auto e : allStones_Idx)
2136 {
2137 f_cloudStones->addPoint(*cloud0->getPoint(e));
2138 f_cloudStones->addColor(cloud0->getPointColor(e));
2139 }
2140
2141 auto_seg_log << dateStamp() << " Stone cloud OK. " << allStones_Idx.size() << " pts. " << vecStones.size() << " stones." << endl;
2142
2143 // adding scalar field of stoneId to stone point cloud
2144 int sfIdx = f_cloudStones->getScalarFieldIndexByName("Stone Index");
2145 if (sfIdx < 0)
2146 {
2147 sfIdx = f_cloudStones->addScalarField("Stone Index");
2148 }
2149 if (sfIdx >= 0)
2150 {
2151 CCCoreLib::ScalarField* stIdSF = f_cloudStones->getScalarField(sfIdx);
2152 for (unsigned int i = 0; i < stoneId.size(); i++)
2153 {
2154 int index = static_cast<int>(stIdShuffle[stoneId[i]]);
2155 stIdSF->setValue(i, index);
2156 }
2157 stIdSF->computeMinAndMax();
2158 f_cloudStones->setCurrentDisplayedScalarField(sfIdx);
2159 auto_seg_log << dateStamp() << " Stone cloud SF OK " << endl;
2160 }
2161 else
2162 {
2163 // not enough memory
2164 ccLog::Warning("Not enough memory");
2165 }
2166 }
2167 else
2168 {
2169 // not enough memory
2170 delete f_cloudStones;
2171 f_cloudStones = nullptr;
2172 ccLog::Warning("Not enough memory");
2173 }
2174 }
2175
2176 // Mortar
2177 ccPointCloud* f_cloudMortar = nullptr;
2178 if (allMortar.size() != 0)
2179 {
2180 f_cloudMortar = new ccPointCloud("f_cloudMortar");
2181 if (f_cloudMortar->reserve(static_cast<unsigned>(allMortar.size())) && f_cloudMortar->reserveTheRGBTable())
2182 {
2183 for (auto e : allMortar)
2184 {
2185 f_cloudMortar->addPoint(*cloud0->getPoint(e));
2186 f_cloudMortar->addColor(cloud0->getPointColor(e));
2187 }
2188 auto_seg_log << dateStamp() << " Mortar OK. " << allMortar.size() << " pts" << endl;
2189 }
2190 else
2191 {
2192 // not enough memory
2193 delete f_cloudMortar;
2194 f_cloudMortar = nullptr;
2195 ccLog::Warning("Not enough memory");
2196 }
2197 }
2198
2199 //ADDING CLOUDS TO DB
2200 ccPointCloud* mortarMaps = nullptr;
2201 if (checkMortar == true && f_cloudStones && f_cloudMortar)
2202 {
2203 if (allMortar.size() > 0)
2204 {
2205 auto_seg_log << dateStamp() << " Starting mortar maps generation... " << endl;
2206 mortarMaps = getMortarMaps(*f_cloudStones, *f_cloudMortar);
2207 if (mortarMaps)
2208 {
2209 auto_seg_log << dateStamp() << " Mortar maps OK " << endl;
2210 mortarMaps->showSF(true);
2211 }
2212 }
2213 }
2214
2215 // Move all the clouds back
2216 cloud0->translate(minBox);
2217 cloud0->applyRigidTransformation(R3.inverse());
2218 cloud0->applyRigidTransformation(R2.inverse());
2219
2220 if (f_cloudMortar)
2221 {
2222 f_cloudMortar->translate(minBox);
2223 f_cloudMortar->applyRigidTransformation(R3.inverse());
2224 f_cloudMortar->applyRigidTransformation(R2.inverse());
2225 f_cloudMortar->showColors(true);
2226 m_app->addToDB(f_cloudMortar, true, true, false, true);
2227 }
2228
2229 if (f_cloudStones)
2230 {
2231 f_cloudStones->translate(minBox);
2232 f_cloudStones->applyRigidTransformation(R3.inverse());
2233 f_cloudStones->applyRigidTransformation(R2.inverse());
2234 f_cloudStones->showColors(true);
2235 m_app->addToDB(f_cloudStones, true, true, false, true);
2236 }
2237
2238 if (mortarMaps)
2239 {
2240 mortarMaps->translate(minBox);
2241 mortarMaps->applyRigidTransformation(R3.inverse());
2242 mortarMaps->applyRigidTransformation(R2.inverse());
2243 m_app->addToDB(mortarMaps, true, true, false, true);
2244 }
2245
2246 // contours in an alternative way using stone indexes and original cloud
2247 if (vecStones.size() != 0)
2248 {
2249 //Generating contour polylines
2250 auto_seg_log << dateStamp() << " Start of contour generation" << endl;
2251
2252 std::vector<double> stonesSize;
2253 ccHObject* contoursContainer = nullptr;
2254 for (const auto& ve : vecStones)
2255 {
2256 QString qs_name = "stone " + QString::number(stonesSize.size());
2257
2258 if (ve.size() != 0)
2259 {
2260 ccPolyline* contour = contourPoly2(*cloud0, std::move(ve), qs_name);
2261 if (contour)
2262 {
2263 if (!contoursContainer)
2264 {
2265 contoursContainer = new ccHObject("Stone contours");
2266 }
2267 contoursContainer->addChild(contour);
2268 //Get BB per stone from contour.
2269 CCVector3 bbMinS(0, 0, 0);
2270 CCVector3 bbMaxS(0, 0, 0);
2271 contour->getBoundingBox(bbMinS, bbMaxS);
2272 double sizeS = (bbMaxS.x - bbMinS.x)*(bbMaxS.z - bbMinS.z);
2273 stonesSize.push_back(sizeS);
2274 }
2275 }
2276 }
2277
2278 if (contoursContainer)
2279 {
2280 m_app->addToDB(contoursContainer, true, true, false, true);
2281 }
2282
2283 //Mean and median size of stones
2284 if (stonesSize.size() != 0)
2285 {
2286 double meanSizeStones = accumulate(stonesSize.begin(), stonesSize.end(), 0.0) / stonesSize.size();
2287 double standDev = stddev(stonesSize);
2288
2289 auto_seg_log << dateStamp() << " Average size of stones (BB) " << meanSizeStones << " m2 +-" << standDev << " (" << meanSizeStones * 10000 << " cm2)" << endl;
2290
2291 std::sort(stonesSize.begin(), stonesSize.end());
2292 bool evenAmount = (stonesSize.size() % 2 == 0);
2293 int oddAmountIdx = ceil(stonesSize.size() / 2);
2294 double evenAmountMedian = (stonesSize[oddAmountIdx - 1] + stonesSize[oddAmountIdx]) / 2;
2295 double medianSizeStones = evenAmount ? evenAmountMedian : stonesSize[oddAmountIdx];
2296
2297 auto_seg_log << dateStamp() << " Median size of stones (BB) " << medianSizeStones << " m2 (" << medianSizeStones * 10000 << " cm2)" << endl;
2298 }
2299
2300 auto_seg_log << dateStamp() << " Contour generation done" << endl;
2301 }
2302
2303 ccGLWindowInterface* win = m_app->getActiveGLWindow();
2304 if (win)
2305 {
2306 win->setView(CC_FRONT_VIEW);
2307 }
2308 }
2309 catch (const std::bad_alloc)
2310 {
2311 ccLog::Error("Not enough memory");
2312 }
2313 }