FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2012 Werner Mayer <wmayer[at]users.sourceforge.net>     *
3
 *                                                                         *
4
 *   This file is part of the FreeCAD CAx development system.              *
5
 *                                                                         *
6
 *   This library is free software; you can redistribute it and/or         *
7
 *   modify it under the terms of the GNU Library General Public           *
8
 *   License as published by the Free Software Foundation; either          *
9
 *   version 2 of the License, or (at your option) any later version.      *
10
 *                                                                         *
11
 *   This library  is distributed in the hope that it will be useful,      *
12
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
13
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
14
 *   GNU Library General Public License for more details.                  *
15
 *                                                                         *
16
 *   You should have received a copy of the GNU Library General Public     *
17
 *   License along with this library; see the file COPYING.LIB. If not,    *
18
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
19
 *   Suite 330, Boston, MA  02111-1307, USA                                *
20
 *                                                                         *
21
 ***************************************************************************/
22

23
#include "PreCompiled.h"
24

25
#include <Base/Exception.h>
26
#include <Mod/Mesh/App/Core/Algorithm.h>
27
#include <Mod/Mesh/App/Core/Elements.h>
28
#include <Mod/Mesh/App/Core/MeshKernel.h>
29
#include <Mod/Mesh/App/Mesh.h>
30
#include <Mod/Points/App/Points.h>
31

32
#include "SurfaceTriangulation.h"
33

34

35
// http://svn.pointclouds.org/pcl/tags/pcl-1.5.1/test/
36
#if defined(HAVE_PCL_SURFACE)
37
#include <boost/math/special_functions/fpclassify.hpp>
38
#include <boost/random.hpp>
39
#include <pcl/common/common.h>
40
#include <pcl/common/io.h>
41
#include <pcl/features/normal_3d.h>
42
#include <pcl/pcl_config.h>
43
#include <pcl/point_traits.h>
44
#include <pcl/point_types.h>
45
#include <pcl/surface/ear_clipping.h>
46
#include <pcl/surface/gp3.h>
47
#include <pcl/surface/grid_projection.h>
48
#include <pcl/surface/marching_cubes_hoppe.h>
49
#include <pcl/surface/marching_cubes_rbf.h>
50
#include <pcl/surface/mls.h>
51
#include <pcl/surface/organized_fast_mesh.h>
52
#include <pcl/surface/poisson.h>
53

54
#ifndef PCL_REVISION_VERSION
55
#define PCL_REVISION_VERSION 0
56
#endif
57

58
using namespace pcl;
59
using namespace pcl::io;
60
using namespace std;
61
using namespace Reen;
62

63
// See
64
// http://www.ics.uci.edu/~gopi/PAPERS/Euro00.pdf
65
// http://www.ics.uci.edu/~gopi/PAPERS/CGMV.pdf
66
SurfaceTriangulation::SurfaceTriangulation(const Points::PointKernel& pts, Mesh::MeshObject& mesh)
67
    : myPoints(pts)
68
    , myMesh(mesh)
69
    , mu(0)
70
    , searchRadius(0)
71
{}
72

73
void SurfaceTriangulation::perform(int ksearch)
74
{
75
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
76
    PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
77
    search::KdTree<PointXYZ>::Ptr tree;
78
    search::KdTree<PointNormal>::Ptr tree2;
79

80
    cloud->reserve(myPoints.size());
81
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
82
        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y)
83
            && !boost::math::isnan(it->z)) {
84
            cloud->push_back(PointXYZ(it->x, it->y, it->z));
85
        }
86
    }
87

88
    // Create search tree
89
    tree.reset(new search::KdTree<PointXYZ>(false));
90
    tree->setInputCloud(cloud);
91

92
    // Normal estimation
93
    NormalEstimation<PointXYZ, Normal> n;
94
    PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
95
    n.setInputCloud(cloud);
96
    // n.setIndices (indices[B);
97
    n.setSearchMethod(tree);
98
    n.setKSearch(ksearch);
99
    n.compute(*normals);
100

101
    // Concatenate XYZ and normal information
102
    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
103

104
    // Create search tree
105
    tree2.reset(new search::KdTree<PointNormal>);
106
    tree2->setInputCloud(cloud_with_normals);
107

108
    // Init objects
109
    GreedyProjectionTriangulation<PointNormal> gp3;
110

111
    // Set parameters
112
    gp3.setInputCloud(cloud_with_normals);
113
    gp3.setSearchMethod(tree2);
114
    gp3.setSearchRadius(searchRadius);
115
    gp3.setMu(mu);
116
    gp3.setMaximumNearestNeighbors(100);
117
    gp3.setMaximumSurfaceAngle(M_PI / 4);  // 45 degrees
118
    gp3.setMinimumAngle(M_PI / 18);        // 10 degrees
119
    gp3.setMaximumAngle(2 * M_PI / 3);     // 120 degrees
120
    gp3.setNormalConsistency(false);
121
    gp3.setConsistentVertexOrdering(true);
122

123
    // Reconstruct
124
    PolygonMesh mesh;
125
    gp3.reconstruct(mesh);
126

127
    MeshConversion::convert(mesh, myMesh);
128

129
    // Additional vertex information
130
    // std::vector<int> parts = gp3.getPartIDs();
131
    // std::vector<int> states = gp3.getPointStates();
132
}
133

134
void SurfaceTriangulation::perform(const std::vector<Base::Vector3f>& normals)
135
{
136
    if (myPoints.size() != normals.size()) {
137
        throw Base::RuntimeError("Number of points doesn't match with number of normals");
138
    }
139

140
    PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
141
    search::KdTree<PointNormal>::Ptr tree;
142

143
    cloud_with_normals->reserve(myPoints.size());
144
    std::size_t num_points = myPoints.size();
145
    const std::vector<Base::Vector3f>& points = myPoints.getBasicPoints();
146
    for (std::size_t index = 0; index < num_points; index++) {
147
        const Base::Vector3f& p = points[index];
148
        const Base::Vector3f& n = normals[index];
149
        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
150
            PointNormal pn;
151
            pn.x = p.x;
152
            pn.y = p.y;
153
            pn.z = p.z;
154
            pn.normal_x = n.x;
155
            pn.normal_y = n.y;
156
            pn.normal_z = n.z;
157
            cloud_with_normals->push_back(pn);
158
        }
159
    }
160

161
    // Create search tree
162
    tree.reset(new search::KdTree<PointNormal>);
163
    tree->setInputCloud(cloud_with_normals);
164

165
    // Init objects
166
    GreedyProjectionTriangulation<PointNormal> gp3;
167

168
    // Set parameters
169
    gp3.setInputCloud(cloud_with_normals);
170
    gp3.setSearchMethod(tree);
171
    gp3.setSearchRadius(searchRadius);
172
    gp3.setMu(mu);
173
    gp3.setMaximumNearestNeighbors(100);
174
    gp3.setMaximumSurfaceAngle(M_PI / 4);  // 45 degrees
175
    gp3.setMinimumAngle(M_PI / 18);        // 10 degrees
176
    gp3.setMaximumAngle(2 * M_PI / 3);     // 120 degrees
177
    gp3.setNormalConsistency(true);
178
    gp3.setConsistentVertexOrdering(true);
179

180
    // Reconstruct
181
    PolygonMesh mesh;
182
    gp3.reconstruct(mesh);
183

184
    MeshConversion::convert(mesh, myMesh);
185

186
    // Additional vertex information
187
    // std::vector<int> parts = gp3.getPartIDs();
188
    // std::vector<int> states = gp3.getPointStates();
189
}
190

191
// ----------------------------------------------------------------------------
192

193
// See
194
// http://www.cs.jhu.edu/~misha/Code/PoissonRecon/Version8.0/
195
PoissonReconstruction::PoissonReconstruction(const Points::PointKernel& pts, Mesh::MeshObject& mesh)
196
    : myPoints(pts)
197
    , myMesh(mesh)
198
    , depth(-1)
199
    , solverDivide(-1)
200
    , samplesPerNode(-1.0f)
201
{}
202

203
void PoissonReconstruction::perform(int ksearch)
204
{
205
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
206
    PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
207
    search::KdTree<PointXYZ>::Ptr tree;
208
    search::KdTree<PointNormal>::Ptr tree2;
209

210
    cloud->reserve(myPoints.size());
211
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
212
        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y)
213
            && !boost::math::isnan(it->z)) {
214
            cloud->push_back(PointXYZ(it->x, it->y, it->z));
215
        }
216
    }
217

218
    // Create search tree
219
    tree.reset(new search::KdTree<PointXYZ>(false));
220
    tree->setInputCloud(cloud);
221

222
    // Normal estimation
223
    NormalEstimation<PointXYZ, Normal> n;
224
    PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
225
    n.setInputCloud(cloud);
226
    // n.setIndices (indices[B);
227
    n.setSearchMethod(tree);
228
    n.setKSearch(ksearch);
229
    n.compute(*normals);
230

231
    // Concatenate XYZ and normal information
232
    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
233

234
    // Create search tree
235
    tree2.reset(new search::KdTree<PointNormal>);
236
    tree2->setInputCloud(cloud_with_normals);
237

238
    // Init objects
239
    Poisson<PointNormal> poisson;
240

241
    // Set parameters
242
    poisson.setInputCloud(cloud_with_normals);
243
    poisson.setSearchMethod(tree2);
244
    if (depth >= 1) {
245
        poisson.setDepth(depth);
246
    }
247
    if (solverDivide >= 1) {
248
        poisson.setSolverDivide(solverDivide);
249
    }
250
    if (samplesPerNode >= 1.0f) {
251
        poisson.setSamplesPerNode(samplesPerNode);
252
    }
253

254
    // Reconstruct
255
    PolygonMesh mesh;
256
    poisson.reconstruct(mesh);
257

258
    MeshConversion::convert(mesh, myMesh);
259
}
260

261
void PoissonReconstruction::perform(const std::vector<Base::Vector3f>& normals)
262
{
263
    if (myPoints.size() != normals.size()) {
264
        throw Base::RuntimeError("Number of points doesn't match with number of normals");
265
    }
266

267
    PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
268
    search::KdTree<PointNormal>::Ptr tree;
269

270
    cloud_with_normals->reserve(myPoints.size());
271
    std::size_t num_points = myPoints.size();
272
    const std::vector<Base::Vector3f>& points = myPoints.getBasicPoints();
273
    for (std::size_t index = 0; index < num_points; index++) {
274
        const Base::Vector3f& p = points[index];
275
        const Base::Vector3f& n = normals[index];
276
        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
277
            PointNormal pn;
278
            pn.x = p.x;
279
            pn.y = p.y;
280
            pn.z = p.z;
281
            pn.normal_x = n.x;
282
            pn.normal_y = n.y;
283
            pn.normal_z = n.z;
284
            cloud_with_normals->push_back(pn);
285
        }
286
    }
287

288
    // Create search tree
289
    tree.reset(new search::KdTree<PointNormal>);
290
    tree->setInputCloud(cloud_with_normals);
291

292
    // Init objects
293
    Poisson<PointNormal> poisson;
294

295
    // Set parameters
296
    poisson.setInputCloud(cloud_with_normals);
297
    poisson.setSearchMethod(tree);
298
    if (depth >= 1) {
299
        poisson.setDepth(depth);
300
    }
301
    if (solverDivide >= 1) {
302
        poisson.setSolverDivide(solverDivide);
303
    }
304
    if (samplesPerNode >= 1.0f) {
305
        poisson.setSamplesPerNode(samplesPerNode);
306
    }
307

308
    // Reconstruct
309
    PolygonMesh mesh;
310
    poisson.reconstruct(mesh);
311

312
    MeshConversion::convert(mesh, myMesh);
313
}
314

315
// ----------------------------------------------------------------------------
316

317
GridReconstruction::GridReconstruction(const Points::PointKernel& pts, Mesh::MeshObject& mesh)
318
    : myPoints(pts)
319
    , myMesh(mesh)
320
{}
321

322
void GridReconstruction::perform(int ksearch)
323
{
324
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
325
    PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
326
    search::KdTree<PointXYZ>::Ptr tree;
327
    search::KdTree<PointNormal>::Ptr tree2;
328

329
    cloud->reserve(myPoints.size());
330
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
331
        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y)
332
            && !boost::math::isnan(it->z)) {
333
            cloud->push_back(PointXYZ(it->x, it->y, it->z));
334
        }
335
    }
336

337
    // Create search tree
338
    tree.reset(new search::KdTree<PointXYZ>(false));
339
    tree->setInputCloud(cloud);
340

341
    // Normal estimation
342
    NormalEstimation<PointXYZ, Normal> n;
343
    PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
344
    n.setInputCloud(cloud);
345
    // n.setIndices (indices[B);
346
    n.setSearchMethod(tree);
347
    n.setKSearch(ksearch);
348
    n.compute(*normals);
349

350
    // Concatenate XYZ and normal information
351
    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
352

353
    // Create search tree
354
    tree2.reset(new search::KdTree<PointNormal>);
355
    tree2->setInputCloud(cloud_with_normals);
356

357
    // Init objects
358
    GridProjection<PointNormal> grid;
359

360
    // Set parameters
361
    grid.setResolution(0.005);
362
    grid.setPaddingSize(3);
363
    grid.setNearestNeighborNum(100);
364
    grid.setMaxBinarySearchLevel(10);
365
    grid.setInputCloud(cloud_with_normals);
366
    grid.setSearchMethod(tree2);
367

368
    // Reconstruct
369
    PolygonMesh mesh;
370
    grid.reconstruct(mesh);
371

372
    MeshConversion::convert(mesh, myMesh);
373
}
374

375
void GridReconstruction::perform(const std::vector<Base::Vector3f>& normals)
376
{
377
    if (myPoints.size() != normals.size()) {
378
        throw Base::RuntimeError("Number of points doesn't match with number of normals");
379
    }
380

381
    PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
382
    search::KdTree<PointNormal>::Ptr tree;
383

384
    cloud_with_normals->reserve(myPoints.size());
385
    std::size_t num_points = myPoints.size();
386
    const std::vector<Base::Vector3f>& points = myPoints.getBasicPoints();
387
    for (std::size_t index = 0; index < num_points; index++) {
388
        const Base::Vector3f& p = points[index];
389
        const Base::Vector3f& n = normals[index];
390
        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
391
            PointNormal pn;
392
            pn.x = p.x;
393
            pn.y = p.y;
394
            pn.z = p.z;
395
            pn.normal_x = n.x;
396
            pn.normal_y = n.y;
397
            pn.normal_z = n.z;
398
            cloud_with_normals->push_back(pn);
399
        }
400
    }
401

402
    // Create search tree
403
    tree.reset(new search::KdTree<PointNormal>);
404
    tree->setInputCloud(cloud_with_normals);
405

406
    // Init objects
407
    GridProjection<PointNormal> grid;
408

409
    // Set parameters
410
    grid.setResolution(0.005);
411
    grid.setPaddingSize(3);
412
    grid.setNearestNeighborNum(100);
413
    grid.setMaxBinarySearchLevel(10);
414
    grid.setInputCloud(cloud_with_normals);
415
    grid.setSearchMethod(tree);
416

417
    // Reconstruct
418
    PolygonMesh mesh;
419
    grid.reconstruct(mesh);
420

421
    MeshConversion::convert(mesh, myMesh);
422
}
423

424
// ----------------------------------------------------------------------------
425

426
ImageTriangulation::ImageTriangulation(int width,
427
                                       int height,
428
                                       const Points::PointKernel& pts,
429
                                       Mesh::MeshObject& mesh)
430
    : width(width)
431
    , height(height)
432
    , myPoints(pts)
433
    , myMesh(mesh)
434
{}
435

436
void ImageTriangulation::perform()
437
{
438
    if (myPoints.size() != static_cast<std::size_t>(width * height)) {
439
        throw Base::RuntimeError("Number of points doesn't match with given width and height");
440
    }
441

442
    // construct dataset
443
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_organized(new pcl::PointCloud<pcl::PointXYZ>());
444
    cloud_organized->width = width;
445
    cloud_organized->height = height;
446
    cloud_organized->points.resize(cloud_organized->width * cloud_organized->height);
447

448
    const std::vector<Base::Vector3f>& points = myPoints.getBasicPoints();
449

450
    int npoints = 0;
451
    for (size_t i = 0; i < cloud_organized->height; i++) {
452
        for (size_t j = 0; j < cloud_organized->width; j++) {
453
            const Base::Vector3f& p = points[npoints];
454
            cloud_organized->points[npoints].x = p.x;
455
            cloud_organized->points[npoints].y = p.y;
456
            cloud_organized->points[npoints].z = p.z;
457
            npoints++;
458
        }
459
    }
460

461
    OrganizedFastMesh<PointXYZ> ofm;
462

463
    // Set parameters
464
    ofm.setInputCloud(cloud_organized);
465
    // This parameter is not yet implemented (pcl 1.7)
466
    ofm.setMaxEdgeLength(1.5);
467
    ofm.setTrianglePixelSize(1);
468
    ofm.setTriangulationType(OrganizedFastMesh<PointXYZ>::TRIANGLE_ADAPTIVE_CUT);
469
    ofm.storeShadowedFaces(true);
470

471
    // Reconstruct
472
    PolygonMesh mesh;
473
    ofm.reconstruct(mesh);
474

475
    MeshConversion::convert(mesh, myMesh);
476

477
    // remove invalid points
478
    //
479
    MeshCore::MeshKernel& kernel = myMesh.getKernel();
480
    const MeshCore::MeshFacetArray& face = kernel.GetFacets();
481
    MeshCore::MeshAlgorithm meshAlg(kernel);
482
    meshAlg.SetPointFlag(MeshCore::MeshPoint::INVALID);
483
    std::vector<MeshCore::PointIndex> validPoints;
484
    validPoints.reserve(face.size() * 3);
485
    for (MeshCore::MeshFacetArray::_TConstIterator it = face.begin(); it != face.end(); ++it) {
486
        validPoints.push_back(it->_aulPoints[0]);
487
        validPoints.push_back(it->_aulPoints[1]);
488
        validPoints.push_back(it->_aulPoints[2]);
489
    }
490

491
    // remove duplicates
492
    std::sort(validPoints.begin(), validPoints.end());
493
    validPoints.erase(std::unique(validPoints.begin(), validPoints.end()), validPoints.end());
494
    meshAlg.ResetPointsFlag(validPoints, MeshCore::MeshPoint::INVALID);
495

496
    unsigned long countInvalid = meshAlg.CountPointFlag(MeshCore::MeshPoint::INVALID);
497
    if (countInvalid > 0) {
498
        std::vector<MeshCore::PointIndex> invalidPoints;
499
        invalidPoints.reserve(countInvalid);
500
        meshAlg.GetPointsFlag(invalidPoints, MeshCore::MeshPoint::INVALID);
501

502
        kernel.DeletePoints(invalidPoints);
503
    }
504
}
505

506
// ----------------------------------------------------------------------------
507

508
Reen::MarchingCubesRBF::MarchingCubesRBF(const Points::PointKernel& pts, Mesh::MeshObject& mesh)
509
    : myPoints(pts)
510
    , myMesh(mesh)
511
{}
512

513
void Reen::MarchingCubesRBF::perform(int ksearch)
514
{
515
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
516
    PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
517
    search::KdTree<PointXYZ>::Ptr tree;
518
    search::KdTree<PointNormal>::Ptr tree2;
519

520
    cloud->reserve(myPoints.size());
521
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
522
        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y)
523
            && !boost::math::isnan(it->z)) {
524
            cloud->push_back(PointXYZ(it->x, it->y, it->z));
525
        }
526
    }
527

528
    // Create search tree
529
    tree.reset(new search::KdTree<PointXYZ>(false));
530
    tree->setInputCloud(cloud);
531

532
    // Normal estimation
533
    NormalEstimation<PointXYZ, Normal> n;
534
    PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
535
    n.setInputCloud(cloud);
536
    // n.setIndices (indices[B);
537
    n.setSearchMethod(tree);
538
    n.setKSearch(ksearch);
539
    n.compute(*normals);
540

541
    // Concatenate XYZ and normal information
542
    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
543

544
    // Create search tree
545
    tree2.reset(new search::KdTree<PointNormal>);
546
    tree2->setInputCloud(cloud_with_normals);
547

548
    // Init objects
549
    pcl::MarchingCubesRBF<PointNormal> rbf;
550

551
    // Set parameters
552
    rbf.setIsoLevel(0);
553
    rbf.setGridResolution(60, 60, 60);
554
    rbf.setPercentageExtendGrid(0.1f);
555
    rbf.setOffSurfaceDisplacement(0.02f);
556

557
    rbf.setInputCloud(cloud_with_normals);
558
    rbf.setSearchMethod(tree2);
559

560
    // Reconstruct
561
    PolygonMesh mesh;
562
    rbf.reconstruct(mesh);
563

564
    MeshConversion::convert(mesh, myMesh);
565
}
566

567
void Reen::MarchingCubesRBF::perform(const std::vector<Base::Vector3f>& normals)
568
{
569
    if (myPoints.size() != normals.size()) {
570
        throw Base::RuntimeError("Number of points doesn't match with number of normals");
571
    }
572

573
    PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
574
    search::KdTree<PointNormal>::Ptr tree;
575

576
    cloud_with_normals->reserve(myPoints.size());
577
    std::size_t num_points = myPoints.size();
578
    const std::vector<Base::Vector3f>& points = myPoints.getBasicPoints();
579
    for (std::size_t index = 0; index < num_points; index++) {
580
        const Base::Vector3f& p = points[index];
581
        const Base::Vector3f& n = normals[index];
582
        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
583
            PointNormal pn;
584
            pn.x = p.x;
585
            pn.y = p.y;
586
            pn.z = p.z;
587
            pn.normal_x = n.x;
588
            pn.normal_y = n.y;
589
            pn.normal_z = n.z;
590
            cloud_with_normals->push_back(pn);
591
        }
592
    }
593

594
    // Create search tree
595
    tree.reset(new search::KdTree<PointNormal>);
596
    tree->setInputCloud(cloud_with_normals);
597

598

599
    // Init objects
600
    pcl::MarchingCubesRBF<PointNormal> rbf;
601

602
    // Set parameters
603
    rbf.setIsoLevel(0);
604
    rbf.setGridResolution(60, 60, 60);
605
    rbf.setPercentageExtendGrid(0.1f);
606
    rbf.setOffSurfaceDisplacement(0.02f);
607

608
    rbf.setInputCloud(cloud_with_normals);
609
    rbf.setSearchMethod(tree);
610

611
    // Reconstruct
612
    PolygonMesh mesh;
613
    rbf.reconstruct(mesh);
614

615
    MeshConversion::convert(mesh, myMesh);
616
}
617

618
// ----------------------------------------------------------------------------
619

620
Reen::MarchingCubesHoppe::MarchingCubesHoppe(const Points::PointKernel& pts, Mesh::MeshObject& mesh)
621
    : myPoints(pts)
622
    , myMesh(mesh)
623
{}
624

625
void Reen::MarchingCubesHoppe::perform(int ksearch)
626
{
627
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
628
    PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
629
    search::KdTree<PointXYZ>::Ptr tree;
630
    search::KdTree<PointNormal>::Ptr tree2;
631

632
    cloud->reserve(myPoints.size());
633
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
634
        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y)
635
            && !boost::math::isnan(it->z)) {
636
            cloud->push_back(PointXYZ(it->x, it->y, it->z));
637
        }
638
    }
639

640
    // Create search tree
641
    tree.reset(new search::KdTree<PointXYZ>(false));
642
    tree->setInputCloud(cloud);
643

644
    // Normal estimation
645
    NormalEstimation<PointXYZ, Normal> n;
646
    PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
647
    n.setInputCloud(cloud);
648
    // n.setIndices (indices[B);
649
    n.setSearchMethod(tree);
650
    n.setKSearch(ksearch);
651
    n.compute(*normals);
652

653
    // Concatenate XYZ and normal information
654
    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
655

656
    // Create search tree
657
    tree2.reset(new search::KdTree<PointNormal>);
658
    tree2->setInputCloud(cloud_with_normals);
659

660
    // Init objects
661
    pcl::MarchingCubesHoppe<PointNormal> hoppe;
662

663
    // Set parameters
664
    hoppe.setIsoLevel(0);
665
    hoppe.setGridResolution(60, 60, 60);
666
    hoppe.setPercentageExtendGrid(0.1f);
667

668
    hoppe.setInputCloud(cloud_with_normals);
669
    hoppe.setSearchMethod(tree2);
670

671
    // Reconstruct
672
    PolygonMesh mesh;
673
    hoppe.reconstruct(mesh);
674

675
    MeshConversion::convert(mesh, myMesh);
676
}
677

678
void Reen::MarchingCubesHoppe::perform(const std::vector<Base::Vector3f>& normals)
679
{
680
    if (myPoints.size() != normals.size()) {
681
        throw Base::RuntimeError("Number of points doesn't match with number of normals");
682
    }
683

684
    PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
685
    search::KdTree<PointNormal>::Ptr tree;
686

687
    cloud_with_normals->reserve(myPoints.size());
688
    std::size_t num_points = myPoints.size();
689
    const std::vector<Base::Vector3f>& points = myPoints.getBasicPoints();
690
    for (std::size_t index = 0; index < num_points; index++) {
691
        const Base::Vector3f& p = points[index];
692
        const Base::Vector3f& n = normals[index];
693
        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
694
            PointNormal pn;
695
            pn.x = p.x;
696
            pn.y = p.y;
697
            pn.z = p.z;
698
            pn.normal_x = n.x;
699
            pn.normal_y = n.y;
700
            pn.normal_z = n.z;
701
            cloud_with_normals->push_back(pn);
702
        }
703
    }
704

705
    // Create search tree
706
    tree.reset(new search::KdTree<PointNormal>);
707
    tree->setInputCloud(cloud_with_normals);
708

709

710
    // Init objects
711
    pcl::MarchingCubesHoppe<PointNormal> hoppe;
712

713
    // Set parameters
714
    hoppe.setIsoLevel(0);
715
    hoppe.setGridResolution(60, 60, 60);
716
    hoppe.setPercentageExtendGrid(0.1f);
717

718
    hoppe.setInputCloud(cloud_with_normals);
719
    hoppe.setSearchMethod(tree);
720

721
    // Reconstruct
722
    PolygonMesh mesh;
723
    hoppe.reconstruct(mesh);
724

725
    MeshConversion::convert(mesh, myMesh);
726
}
727

728
// ----------------------------------------------------------------------------
729

730
void MeshConversion::convert(const pcl::PolygonMesh& pclMesh, Mesh::MeshObject& meshObject)
731
{
732
    // number of points
733
    size_t nr_points = pclMesh.cloud.width * pclMesh.cloud.height;
734
    size_t point_size = pclMesh.cloud.data.size() / nr_points;
735
    // number of faces for header
736
    size_t nr_faces = pclMesh.polygons.size();
737

738
    MeshCore::MeshPointArray points;
739
    points.reserve(nr_points);
740
    MeshCore::MeshFacetArray facets;
741
    facets.reserve(nr_faces);
742

743
    // get vertices
744
    MeshCore::MeshPoint vertex;
745
    for (size_t i = 0; i < nr_points; ++i) {
746
        int xyz = 0;
747
        for (size_t d = 0; d < pclMesh.cloud.fields.size(); ++d) {
748
            int c = 0;
749
            // adding vertex
750
            if ((pclMesh.cloud.fields[d].datatype ==
751
#if PCL_VERSION_COMPARE(>, 1, 6, 0)
752
                 pcl::PCLPointField::FLOAT32)
753
                &&
754
#else
755
                 sensor_msgs::PointField::FLOAT32)
756
                &&
757
#endif
758
                (pclMesh.cloud.fields[d].name == "x" || pclMesh.cloud.fields[d].name == "y"
759
                 || pclMesh.cloud.fields[d].name == "z")) {
760
                float value;
761
                memcpy(&value,
762
                       &pclMesh.cloud.data[i * point_size + pclMesh.cloud.fields[d].offset
763
                                           + c * sizeof(float)],
764
                       sizeof(float));
765
                vertex[xyz] = value;
766
                if (++xyz == 3) {
767
                    points.push_back(vertex);
768
                    break;
769
                }
770
            }
771
        }
772
    }
773
    // get faces
774
    MeshCore::MeshFacet face;
775
    for (size_t i = 0; i < nr_faces; i++) {
776
        face._aulPoints[0] = pclMesh.polygons[i].vertices[0];
777
        face._aulPoints[1] = pclMesh.polygons[i].vertices[1];
778
        face._aulPoints[2] = pclMesh.polygons[i].vertices[2];
779
        facets.push_back(face);
780
    }
781

782
    MeshCore::MeshKernel kernel;
783
    kernel.Adopt(points, facets, true);
784
    meshObject.swap(kernel);
785
    meshObject.harmonizeNormals();
786
}
787

788
#endif  // HAVE_PCL_SURFACE
789