FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2011 Jürgen Riegel <juergen.riegel@web.de>              *
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
#ifndef _PreComp_
25
#ifdef FC_OS_LINUX
26
#include <unistd.h>
27
#endif
28
#include <memory>
29
#include <sstream>
30

31
#include <boost/algorithm/string.hpp>
32
#include <boost/lexical_cast.hpp>
33
#include <boost/math/special_functions/fpclassify.hpp>  // needed for compilation on some systems
34
#include <boost/regex.hpp>
35
#endif
36

37
#include <Base/Console.h>
38
#include <Base/Converter.h>
39
#include <Base/Exception.h>
40
#include <Base/FileInfo.h>
41
#include <Base/Sequencer.h>
42
#include <Base/Stream.h>
43

44
#include "PointsAlgos.h"
45
#include <E57Format.h>
46

47

48
using namespace Points;
49

50
void PointsAlgos::Load(PointKernel& points, const char* FileName)
51
{
52
    Base::FileInfo File(FileName);
53

54
    // checking on the file
55
    if (!File.isReadable()) {
56
        throw Base::FileException("File to load not existing or not readable", FileName);
57
    }
58

59
    if (File.hasExtension("asc")) {
60
        LoadAscii(points, FileName);
61
    }
62
    else {
63
        throw Base::RuntimeError("Unknown ending");
64
    }
65
}
66

67
void PointsAlgos::LoadAscii(PointKernel& points, const char* FileName)
68
{
69
    boost::regex rx("^\\s*([-+]?[0-9]*)\\.?([0-9]+([eE][-+]?[0-9]+)?)"
70
                    "\\s+([-+]?[0-9]*)\\.?([0-9]+([eE][-+]?[0-9]+)?)"
71
                    "\\s+([-+]?[0-9]*)\\.?([0-9]+([eE][-+]?[0-9]+)?)\\s*$");
72
    // boost::regex rx("(\\b[0-9]+\\.([0-9]+\\b)?|\\.[0-9]+\\b)");
73
    // boost::regex
74
    // rx("^\\s*(-?[0-9]*)\\.([0-9]+)\\s+(-?[0-9]*)\\.([0-9]+)\\s+(-?[0-9]*)\\.([0-9]+)\\s*$");
75
    boost::cmatch what;
76

77
    Base::Vector3d pt;
78
    int LineCnt = 0;
79
    std::string line;
80
    Base::FileInfo fi(FileName);
81

82
    Base::ifstream tmp_str(fi, std::ios::in);
83

84
    // estimating size
85
    while (std::getline(tmp_str, line)) {
86
        LineCnt++;
87
    }
88

89
    // resize the PointKernel
90
    points.resize(LineCnt);
91

92
    Base::SequencerLauncher seq("Loading points...", LineCnt);
93

94
    // again to the beginning
95
    Base::ifstream file(fi, std::ios::in);
96
    LineCnt = 0;
97

98
    try {
99
        // read file
100
        while (std::getline(file, line)) {
101
            if (boost::regex_match(line.c_str(), what, rx)) {
102
                pt.x = std::atof(what[1].first);
103
                pt.y = std::atof(what[4].first);
104
                pt.z = std::atof(what[7].first);
105

106
                points.setPoint(LineCnt, pt);
107
                seq.next();
108
                LineCnt++;
109
            }
110
        }
111
    }
112
    catch (...) {
113
        points.clear();
114
        throw Base::BadFormatError("Reading in points failed.");
115
    }
116

117
    // now remove the last points from the kernel
118
    // Note: first we allocate memory corresponding to the number of lines (points and comments)
119
    //       and read in the file twice. But then the size of the kernel is too high
120
    if (LineCnt < (int)points.size()) {
121
        points.erase(LineCnt, points.size());
122
    }
123
}
124

125
// ----------------------------------------------------------------------------
126

127
Reader::Reader() = default;
128

129
Reader::~Reader() = default;
130

131
void Reader::clear()
132
{
133
    intensity.clear();
134
    colors.clear();
135
    normals.clear();
136
}
137

138
const PointKernel& Reader::getPoints() const
139
{
140
    return points;
141
}
142

143
bool Reader::hasProperties() const
144
{
145
    return (hasIntensities() || hasColors() || hasNormals());
146
}
147

148
const std::vector<float>& Reader::getIntensities() const
149
{
150
    return intensity;
151
}
152

153
bool Reader::hasIntensities() const
154
{
155
    return (!intensity.empty());
156
}
157

158
const std::vector<App::Color>& Reader::getColors() const
159
{
160
    return colors;
161
}
162

163
bool Reader::hasColors() const
164
{
165
    return (!colors.empty());
166
}
167

168
const std::vector<Base::Vector3f>& Reader::getNormals() const
169
{
170
    return normals;
171
}
172

173
bool Reader::hasNormals() const
174
{
175
    return (!normals.empty());
176
}
177

178
bool Reader::isStructured() const
179
{
180
    return (width > 1 && height > 1);
181
}
182

183
int Reader::getWidth() const
184
{
185
    return width;
186
}
187

188
int Reader::getHeight() const
189
{
190
    return height;
191
}
192

193
// ----------------------------------------------------------------------------
194

195
AscReader::AscReader() = default;
196

197
void AscReader::read(const std::string& filename)
198
{
199
    points.load(filename.c_str());
200
    this->height = 1;
201
    this->width = points.size();
202
}
203

204
// ----------------------------------------------------------------------------
205

206
namespace Points
207
{
208
class Converter
209
{
210
public:
211
    Converter() = default;
212
    virtual ~Converter() = default;
213
    virtual std::string toString(double) const = 0;
214
    virtual double toDouble(Base::InputStream&) const = 0;
215
    virtual int getSizeOf() const = 0;
216

217
    Converter(const Converter&) = delete;
218
    Converter(Converter&&) = delete;
219
    Converter& operator=(const Converter&) = delete;
220
    Converter& operator=(Converter&&) = delete;
221
};
222
template<typename T>
223
class ConverterT: public Converter
224
{
225
public:
226
    std::string toString(double f) const override
227
    {
228
        T c = static_cast<T>(f);
229
        std::ostringstream oss;
230
        oss.precision(7);
231
        oss.setf(std::ostringstream::showpoint);
232
        oss << c;
233
        return oss.str();
234
    }
235
    double toDouble(Base::InputStream& str) const override
236
    {
237
        T c;
238
        str >> c;
239
        return static_cast<double>(c);
240
    }
241
    int getSizeOf() const override
242
    {
243
        return sizeof(T);
244
    }
245
};
246

247
using ConverterPtr = std::shared_ptr<Converter>;
248

249
class DataStreambuf: public std::streambuf
250
{
251
public:
252
    explicit DataStreambuf(const std::vector<char>& data)
253
        : _buffer(data)
254
        , _end(int(data.size()))
255
    {}
256
    ~DataStreambuf() override = default;
257

258
protected:
259
    int_type uflow() override
260
    {
261
        if (_cur == _end) {
262
            return traits_type::eof();
263
        }
264

265
        return static_cast<DataStreambuf::int_type>(_buffer[_cur++]) & 0x000000ff;
266
    }
267
    int_type underflow() override
268
    {
269
        if (_cur == _end) {
270
            return traits_type::eof();
271
        }
272

273
        return static_cast<DataStreambuf::int_type>(_buffer[_cur]) & 0x000000ff;
274
    }
275
    int_type pbackfail(int_type ch) override
276
    {
277
        if (_cur == _beg || (ch != traits_type::eof() && ch != _buffer[_cur - 1])) {
278
            return traits_type::eof();
279
        }
280

281
        return static_cast<DataStreambuf::int_type>(_buffer[--_cur]) & 0x000000ff;
282
    }
283
    std::streamsize showmanyc() override
284
    {
285
        return _end - _cur;
286
    }
287
    pos_type seekoff(std::streambuf::off_type off,
288
                     std::ios_base::seekdir way,
289
                     std::ios_base::openmode mode = std::ios::in | std::ios::out) override
290
    {
291
        (void)mode;
292
        int p_pos = -1;
293
        if (way == std::ios_base::beg) {
294
            p_pos = _beg;
295
        }
296
        else if (way == std::ios_base::end) {
297
            p_pos = _end;
298
        }
299
        else if (way == std::ios_base::cur) {
300
            p_pos = _cur;
301
        }
302

303
        if (p_pos > _end) {
304
            return traits_type::eof();
305
        }
306

307
        if (((p_pos + off) > _end) || ((p_pos + off) < _beg)) {
308
            return traits_type::eof();
309
        }
310

311
        _cur = p_pos + off;
312

313
        return ((p_pos + off) - _beg);
314
    }
315
    pos_type seekpos(std::streambuf::pos_type pos,
316
                     std::ios_base::openmode which = std::ios::in | std::ios::out) override
317
    {
318
        (void)which;
319
        return seekoff(pos, std::ios_base::beg);
320
    }
321

322
public:
323
    DataStreambuf(const DataStreambuf&) = delete;
324
    DataStreambuf(DataStreambuf&&) = delete;
325
    DataStreambuf& operator=(const DataStreambuf&) = delete;
326
    DataStreambuf& operator=(DataStreambuf&&) = delete;
327

328
private:
329
    const std::vector<char>& _buffer;
330
    int _beg {0}, _end {0}, _cur {0};
331
};
332

333
// NOLINTBEGIN
334
// Taken from https://github.com/PointCloudLibrary/pcl/blob/master/io/src/lzf.cpp
335
unsigned int
336
lzfDecompress(const void* const in_data, unsigned int in_len, void* out_data, unsigned int out_len)
337
{
338
    unsigned char const* ip = static_cast<const unsigned char*>(in_data);
339
    unsigned char* op = static_cast<unsigned char*>(out_data);
340
    unsigned char const* const in_end = ip + in_len;
341
    unsigned char* const out_end = op + out_len;
342

343
    do {
344
        unsigned int ctrl = *ip++;
345

346
        // Literal run
347
        if (ctrl < (1 << 5)) {
348
            ctrl++;
349

350
            if (op + ctrl > out_end) {
351
                errno = E2BIG;
352
                return (0);
353
            }
354

355
            // Check for overflow
356
            if (ip + ctrl > in_end) {
357
                errno = EINVAL;
358
                return (0);
359
            }
360
            switch (ctrl) {
361
                case 32:
362
                    *op++ = *ip++;
363
                    /* FALLTHRU */
364
                case 31:
365
                    *op++ = *ip++;
366
                    /* FALLTHRU */
367
                case 30:
368
                    *op++ = *ip++;
369
                    /* FALLTHRU */
370
                case 29:
371
                    *op++ = *ip++;
372
                    /* FALLTHRU */
373
                case 28:
374
                    *op++ = *ip++;
375
                    /* FALLTHRU */
376
                case 27:
377
                    *op++ = *ip++;
378
                    /* FALLTHRU */
379
                case 26:
380
                    *op++ = *ip++;
381
                    /* FALLTHRU */
382
                case 25:
383
                    *op++ = *ip++;
384
                    /* FALLTHRU */
385
                case 24:
386
                    *op++ = *ip++;
387
                    /* FALLTHRU */
388
                case 23:
389
                    *op++ = *ip++;
390
                    /* FALLTHRU */
391
                case 22:
392
                    *op++ = *ip++;
393
                    /* FALLTHRU */
394
                case 21:
395
                    *op++ = *ip++;
396
                    /* FALLTHRU */
397
                case 20:
398
                    *op++ = *ip++;
399
                    /* FALLTHRU */
400
                case 19:
401
                    *op++ = *ip++;
402
                    /* FALLTHRU */
403
                case 18:
404
                    *op++ = *ip++;
405
                    /* FALLTHRU */
406
                case 17:
407
                    *op++ = *ip++;
408
                    /* FALLTHRU */
409
                case 16:
410
                    *op++ = *ip++;
411
                    /* FALLTHRU */
412
                case 15:
413
                    *op++ = *ip++;
414
                    /* FALLTHRU */
415
                case 14:
416
                    *op++ = *ip++;
417
                    /* FALLTHRU */
418
                case 13:
419
                    *op++ = *ip++;
420
                    /* FALLTHRU */
421
                case 12:
422
                    *op++ = *ip++;
423
                    /* FALLTHRU */
424
                case 11:
425
                    *op++ = *ip++;
426
                    /* FALLTHRU */
427
                case 10:
428
                    *op++ = *ip++;
429
                    /* FALLTHRU */
430
                case 9:
431
                    *op++ = *ip++;
432
                    /* FALLTHRU */
433
                case 8:
434
                    *op++ = *ip++;
435
                    /* FALLTHRU */
436
                case 7:
437
                    *op++ = *ip++;
438
                    /* FALLTHRU */
439
                case 6:
440
                    *op++ = *ip++;
441
                    /* FALLTHRU */
442
                case 5:
443
                    *op++ = *ip++;
444
                    /* FALLTHRU */
445
                case 4:
446
                    *op++ = *ip++;
447
                    /* FALLTHRU */
448
                case 3:
449
                    *op++ = *ip++;
450
                    /* FALLTHRU */
451
                case 2:
452
                    *op++ = *ip++;
453
                    /* FALLTHRU */
454
                case 1:
455
                    *op++ = *ip++;
456
            }
457
        }
458
        // Back reference
459
        else {
460
            unsigned int len = ctrl >> 5;
461

462
            unsigned char* ref = op - ((ctrl & 0x1f) << 8) - 1;
463

464
            // Check for overflow
465
            if (ip >= in_end) {
466
                errno = EINVAL;
467
                return (0);
468
            }
469
            if (len == 7) {
470
                len += *ip++;
471
                // Check for overflow
472
                if (ip >= in_end) {
473
                    errno = EINVAL;
474
                    return (0);
475
                }
476
            }
477
            ref -= *ip++;
478

479
            if (op + len + 2 > out_end) {
480
                errno = E2BIG;
481
                return (0);
482
            }
483

484
            if (ref < static_cast<unsigned char*>(out_data)) {
485
                errno = EINVAL;
486
                return (0);
487
            }
488

489
            switch (len) {
490
                default: {
491
                    len += 2;
492

493
                    if (op >= ref + len) {
494
                        // Disjunct areas
495
                        memcpy(op, ref, len);
496
                        op += len;
497
                    }
498
                    else {
499
                        // Overlapping, use byte by byte copying
500
                        do {
501
                            *op++ = *ref++;
502
                        } while (--len);
503
                    }
504

505
                    break;
506
                }
507
                case 9:
508
                    *op++ = *ref++;
509
                    /* FALLTHRU */
510
                case 8:
511
                    *op++ = *ref++;
512
                    /* FALLTHRU */
513
                case 7:
514
                    *op++ = *ref++;
515
                    /* FALLTHRU */
516
                case 6:
517
                    *op++ = *ref++;
518
                    /* FALLTHRU */
519
                case 5:
520
                    *op++ = *ref++;
521
                    /* FALLTHRU */
522
                case 4:
523
                    *op++ = *ref++;
524
                    /* FALLTHRU */
525
                case 3:
526
                    *op++ = *ref++;
527
                    /* FALLTHRU */
528
                case 2:
529
                    *op++ = *ref++;
530
                    /* FALLTHRU */
531
                case 1:
532
                    *op++ = *ref++;
533
                    /* FALLTHRU */
534
                case 0:
535
                    *op++ = *ref++;  // two octets more
536
                    *op++ = *ref++;
537
            }
538
        }
539
    } while (ip < in_end);
540

541
    return (static_cast<unsigned int>(op - static_cast<unsigned char*>(out_data)));
542
}
543
}  // namespace Points
544
// NOLINTEND
545

546
PlyReader::PlyReader() = default;
547

548
void PlyReader::read(const std::string& filename)
549
{
550
    clear();
551

552
    Base::FileInfo fi(filename);
553
    Base::ifstream inp(fi, std::ios::in | std::ios::binary);
554

555
    std::string format;
556
    std::vector<std::string> fields;
557
    std::vector<std::string> types;
558
    std::vector<int> sizes;
559
    std::size_t offset = 0;
560
    Eigen::Index numPoints = Eigen::Index(readHeader(inp, format, offset, fields, types, sizes));
561

562
    this->width = numPoints;
563
    this->height = 1;
564

565
    Eigen::MatrixXd data(numPoints, fields.size());
566
    if (format == "ascii") {
567
        readAscii(inp, offset, data);
568
    }
569
    else if (format == "binary_little_endian") {
570
        readBinary(false, inp, offset, types, sizes, data);
571
    }
572
    else if (format == "binary_big_endian") {
573
        readBinary(true, inp, offset, types, sizes, data);
574
    }
575

576
    std::vector<std::string>::iterator it;
577
    Eigen::Index max_size = std::numeric_limits<Eigen::Index>::max();
578

579
    // x field
580
    Eigen::Index x = max_size;
581
    it = std::find(fields.begin(), fields.end(), "x");
582
    if (it != fields.end()) {
583
        x = std::distance(fields.begin(), it);
584
    }
585

586
    // y field
587
    Eigen::Index y = max_size;
588
    it = std::find(fields.begin(), fields.end(), "y");
589
    if (it != fields.end()) {
590
        y = std::distance(fields.begin(), it);
591
    }
592

593
    // z field
594
    Eigen::Index z = max_size;
595
    it = std::find(fields.begin(), fields.end(), "z");
596
    if (it != fields.end()) {
597
        z = std::distance(fields.begin(), it);
598
    }
599

600
    // normal x field
601
    Eigen::Index normal_x = max_size;
602
    it = std::find(fields.begin(), fields.end(), "normal_x");
603
    if (it == fields.end()) {
604
        it = std::find(fields.begin(), fields.end(), "nx");
605
    }
606
    if (it != fields.end()) {
607
        normal_x = std::distance(fields.begin(), it);
608
    }
609

610
    // normal y field
611
    Eigen::Index normal_y = max_size;
612
    it = std::find(fields.begin(), fields.end(), "normal_y");
613
    if (it == fields.end()) {
614
        it = std::find(fields.begin(), fields.end(), "ny");
615
    }
616
    if (it != fields.end()) {
617
        normal_y = std::distance(fields.begin(), it);
618
    }
619

620
    // normal z field
621
    Eigen::Index normal_z = max_size;
622
    it = std::find(fields.begin(), fields.end(), "normal_z");
623
    if (it == fields.end()) {
624
        it = std::find(fields.begin(), fields.end(), "nz");
625
    }
626
    if (it != fields.end()) {
627
        normal_z = std::distance(fields.begin(), it);
628
    }
629

630
    // intensity field
631
    Eigen::Index greyvalue = max_size;
632
    it = std::find(fields.begin(), fields.end(), "intensity");
633
    if (it != fields.end()) {
634
        greyvalue = std::distance(fields.begin(), it);
635
    }
636

637
    // rgb(a) field
638
    Eigen::Index red = max_size;
639
    Eigen::Index green = max_size;
640
    Eigen::Index blue = max_size;
641
    Eigen::Index alpha = max_size;
642
    it = std::find(fields.begin(), fields.end(), "red");
643
    if (it != fields.end()) {
644
        red = std::distance(fields.begin(), it);
645
    }
646

647
    it = std::find(fields.begin(), fields.end(), "green");
648
    if (it != fields.end()) {
649
        green = std::distance(fields.begin(), it);
650
    }
651

652
    it = std::find(fields.begin(), fields.end(), "blue");
653
    if (it != fields.end()) {
654
        blue = std::distance(fields.begin(), it);
655
    }
656

657
    it = std::find(fields.begin(), fields.end(), "alpha");
658
    if (it != fields.end()) {
659
        alpha = std::distance(fields.begin(), it);
660
    }
661

662
    // transfer the data
663
    bool hasData = (x != max_size && y != max_size && z != max_size);
664
    bool hasNormal = (normal_x != max_size && normal_y != max_size && normal_z != max_size);
665
    bool hasIntensity = (greyvalue != max_size);
666
    bool hasColor = (red != max_size && green != max_size && blue != max_size);
667

668
    if (hasData) {
669
        points.reserve(numPoints);
670
        for (Eigen::Index i = 0; i < numPoints; i++) {
671
            points.push_back(Base::Vector3d(data(i, x), data(i, y), data(i, z)));
672
        }
673
    }
674

675
    if (hasData && hasNormal) {
676
        normals.reserve(numPoints);
677
        for (Eigen::Index i = 0; i < numPoints; i++) {
678
            normals.emplace_back(data(i, normal_x), data(i, normal_y), data(i, normal_z));
679
        }
680
    }
681

682
    if (hasData && hasIntensity) {
683
        intensity.reserve(numPoints);
684
        for (Eigen::Index i = 0; i < numPoints; i++) {
685
            intensity.push_back(static_cast<float>(data(i, greyvalue)));
686
        }
687
    }
688

689
    if (hasData && hasColor) {
690
        colors.reserve(numPoints);
691
        float a = 1.0;
692
        if (types[red] == "uchar") {
693
            for (Eigen::Index i = 0; i < numPoints; i++) {
694
                float r = static_cast<float>(data(i, red));
695
                float g = static_cast<float>(data(i, green));
696
                float b = static_cast<float>(data(i, blue));
697
                if (alpha != max_size) {
698
                    a = static_cast<float>(data(i, alpha));
699
                }
700
                colors.emplace_back(static_cast<float>(r) / 255.0F,
701
                                    static_cast<float>(g) / 255.0F,
702
                                    static_cast<float>(b) / 255.0F,
703
                                    static_cast<float>(a) / 255.0F);
704
            }
705
        }
706
        else if (types[red] == "float") {
707
            for (Eigen::Index i = 0; i < numPoints; i++) {
708
                float r = static_cast<float>(data(i, red));
709
                float g = static_cast<float>(data(i, green));
710
                float b = static_cast<float>(data(i, blue));
711
                if (alpha != max_size) {
712
                    a = static_cast<float>(data(i, alpha));
713
                }
714
                colors.emplace_back(r, g, b, a);
715
            }
716
        }
717
    }
718
}
719

720
std::size_t PlyReader::readHeader(std::istream& in,
721
                                  std::string& format,
722
                                  std::size_t& offset,
723
                                  std::vector<std::string>& fields,
724
                                  std::vector<std::string>& types,
725
                                  std::vector<int>& sizes)
726
{
727
    std::string line;
728
    std::string element;
729
    std::vector<std::string> list;
730
    std::size_t numPoints = 0;
731
    // a pair of numbers of elements and the total size of the properties
732
    std::vector<std::pair<std::size_t, std::size_t>> count_props;
733

734
    // read in the first three characters
735
    char ply[3];
736
    in.read(ply, 3);
737
    in.ignore(1);
738
    if (!in || (ply[0] != 'p') || (ply[1] != 'l') || (ply[2] != 'y')) {
739
        throw Base::BadFormatError("Not a ply file");  // wrong header
740
    }
741

742
    while (std::getline(in, line)) {
743
        if (line.empty()) {
744
            continue;
745
        }
746

747
        // since the file is loaded in binary mode we may get the CR at the end
748
        boost::trim(line);
749
        boost::split(list, line, boost::is_any_of("\t\r "), boost::token_compress_on);
750

751
        std::istringstream str(line);
752
        str.imbue(std::locale::classic());
753

754
        std::string kw;
755
        str >> kw;
756
        if (kw == "format") {
757
            if (list.size() != 3) {
758
                throw Base::BadFormatError("Not a valid ply file");
759
            }
760

761
            std::string format_string = list[1];
762
            std::string version = list[2];
763

764
            if (format_string == "ascii") {
765
                format = format_string;
766
            }
767
            else if (format_string == "binary_big_endian") {
768
                format = format_string;
769
            }
770
            else if (format_string == "binary_little_endian") {
771
                format = format_string;
772
            }
773
            else {
774
                // wrong format version
775
                throw Base::BadFormatError("Wrong format version");
776
            }
777
            if (version != "1.0") {
778
                // wrong version
779
                throw Base::BadFormatError("Wrong version number");
780
            }
781
        }
782
        else if (kw == "element") {
783
            if (list.size() != 3) {
784
                throw Base::BadFormatError("Not a valid ply file");
785
            }
786

787
            std::string name = list[1];
788
            std::size_t count = boost::lexical_cast<std::size_t>(list[2]);
789
            if (name == "vertex") {
790
                element = name;
791
                numPoints = count;
792
            }
793
            else {
794
                // if another element than 'vertex' comes first then calculate the offset
795
                if (numPoints == 0) {
796
                    count_props.emplace_back(count, 0);
797
                }
798
                else {
799
                    // this happens for elements coming after 'vertex'
800
                    element.clear();
801
                }
802
            }
803
        }
804
        else if (kw == "property") {
805
            if (list.size() < 3) {
806
                throw Base::BadFormatError("Not a valid ply file");
807
            }
808

809
            std::string name = list.back();
810
            std::list<std::string> number;
811
            if (list[1] == "list") {
812
                number.insert(number.end(), list.begin(), list.end());
813
                number.pop_front();  // property
814
                number.pop_front();  // list
815
                number.pop_back();
816
            }
817
            else {
818
                number.push_back(list[1]);
819
            }
820

821
            for (const auto& it : number) {
822
                int size = 0;
823
                if (it == "char" || it == "int8") {
824
                    size = 1;
825
                }
826
                else if (it == "uchar" || it == "uint8") {
827
                    size = 1;
828
                }
829
                else if (it == "short" || it == "int16") {
830
                    size = 2;
831
                }
832
                else if (it == "ushort" || it == "uint16") {
833
                    size = 2;
834
                }
835
                else if (it == "int" || it == "int32") {
836
                    size = 4;
837
                }
838
                else if (it == "uint" || it == "uint32") {
839
                    size = 4;
840
                }
841
                else if (it == "float" || it == "float32") {
842
                    size = 4;
843
                }
844
                else if (it == "double" || it == "float64") {
845
                    size = 8;
846
                }
847
                else {
848
                    // no valid number type
849
                    throw Base::BadFormatError("Not a valid number type");
850
                }
851

852
                if (element == "vertex") {
853
                    // store the property name and type
854
                    fields.push_back(name);
855
                    types.push_back(it);
856
                    sizes.push_back(size);
857
                }
858
                else if (!count_props.empty()) {
859
                    count_props.back().second += size;
860
                }
861
            }
862
        }
863
        else if (kw == "end_header") {
864
            break;
865
        }
866
    }
867

868
    if (fields.size() != sizes.size() || fields.size() != types.size()) {
869
        throw Base::BadFormatError("");
870
    }
871

872
    offset = 0;
873
    if (format == "ascii") {
874
        // just sum up the number of lines to ignore
875
        std::vector<std::pair<std::size_t, std::size_t>>::iterator it;
876
        for (it = count_props.begin(); it != count_props.end(); ++it) {
877
            offset += it->first;
878
        }
879
    }
880
    else {
881
        std::vector<std::pair<std::size_t, std::size_t>>::iterator it;
882
        for (it = count_props.begin(); it != count_props.end(); ++it) {
883
            offset += it->first * it->second;
884
        }
885
    }
886

887
    return numPoints;
888
}
889

890
void PlyReader::readAscii(std::istream& inp, std::size_t offset, Eigen::MatrixXd& data)
891
{
892
    std::string line;
893
    Eigen::Index row = 0;
894
    Eigen::Index numPoints = Eigen::Index(data.rows());
895
    Eigen::Index numFields = Eigen::Index(data.cols());
896
    std::vector<std::string> list;
897
    while (std::getline(inp, line) && row < numPoints) {
898
        if (line.empty()) {
899
            continue;
900
        }
901

902
        if (offset > 0) {
903
            offset--;
904
            continue;
905
        }
906

907
        // since the file is loaded in binary mode we may get the CR at the end
908
        boost::trim(line);
909
        boost::split(list, line, boost::is_any_of("\t\r "), boost::token_compress_on);
910

911
        std::istringstream str(line);
912

913
        Eigen::Index size = Eigen::Index(list.size());
914
        for (Eigen::Index col = 0; col < size && col < numFields; col++) {
915
            double value = boost::lexical_cast<double>(list[col]);
916
            data(row, col) = value;
917
        }
918

919
        ++row;
920
    }
921
}
922

923
void PlyReader::readBinary(bool swapByteOrder,
924
                           std::istream& inp,
925
                           std::size_t offset,
926
                           const std::vector<std::string>& types,
927
                           const std::vector<int>& sizes,
928
                           Eigen::MatrixXd& data)
929
{
930
    Eigen::Index numPoints = data.rows();
931
    Eigen::Index numFields = data.cols();
932

933
    int neededSize = 0;
934
    ConverterPtr convert_float32(new ConverterT<float>);
935
    ConverterPtr convert_float64(new ConverterT<double>);
936
    ConverterPtr convert_int8(new ConverterT<int8_t>);
937
    ConverterPtr convert_uint8(new ConverterT<uint8_t>);
938
    ConverterPtr convert_int16(new ConverterT<int16_t>);
939
    ConverterPtr convert_uint16(new ConverterT<uint16_t>);
940
    ConverterPtr convert_int32(new ConverterT<int32_t>);
941
    ConverterPtr convert_uint32(new ConverterT<uint32_t>);
942

943
    std::vector<ConverterPtr> converters;
944
    for (Eigen::Index j = 0; j < numFields; j++) {
945
        const std::string& t = types[j];
946
        switch (sizes[j]) {
947
            case 1:
948
                if (t == "char" || t == "int8") {
949
                    converters.push_back(convert_int8);
950
                }
951
                else if (t == "uchar" || t == "uint8") {
952
                    converters.push_back(convert_uint8);
953
                }
954
                else {
955
                    throw Base::BadFormatError("Unexpected type");
956
                }
957
                break;
958
            case 2:
959
                if (t == "short" || t == "int16") {
960
                    converters.push_back(convert_int16);
961
                }
962
                else if (t == "ushort" || t == "uint16") {
963
                    converters.push_back(convert_uint16);
964
                }
965
                else {
966
                    throw Base::BadFormatError("Unexpected type");
967
                }
968
                break;
969
            case 4:
970
                if (t == "int" || t == "int32") {
971
                    converters.push_back(convert_int32);
972
                }
973
                else if (t == "uint" || t == "uint32") {
974
                    converters.push_back(convert_uint32);
975
                }
976
                else if (t == "float" || t == "float32") {
977
                    converters.push_back(convert_float32);
978
                }
979
                else {
980
                    throw Base::BadFormatError("Unexpected type");
981
                }
982
                break;
983
            case 8:
984
                if (t == "double" || t == "float64") {
985
                    converters.push_back(convert_float64);
986
                }
987
                else {
988
                    throw Base::BadFormatError("Unexpected type");
989
                }
990
                break;
991
            default:
992
                throw Base::BadFormatError("Unexpected type");
993
        }
994

995
        neededSize += converters.back()->getSizeOf();
996
    }
997

998
    std::streamoff ulSize = 0;
999
    std::streamoff ulCurr = 0;
1000
    std::streambuf* buf = inp.rdbuf();
1001
    if (buf) {
1002
        ulCurr = buf->pubseekoff(static_cast<std::streamoff>(offset), std::ios::cur, std::ios::in);
1003
        ulSize = buf->pubseekoff(0, std::ios::end, std::ios::in);
1004
        buf->pubseekoff(ulCurr, std::ios::beg, std::ios::in);
1005
        if (ulCurr + neededSize * static_cast<std::streamoff>(numPoints) > ulSize) {
1006
            throw Base::BadFormatError("File expects too many elements");
1007
        }
1008
    }
1009

1010
    Base::InputStream str(inp);
1011
    str.setByteOrder(swapByteOrder ? Base::Stream::BigEndian : Base::Stream::LittleEndian);
1012
    for (Eigen::Index i = 0; i < numPoints; i++) {
1013
        for (Eigen::Index j = 0; j < numFields; j++) {
1014
            double value = converters[j]->toDouble(str);
1015
            data(i, j) = value;
1016
        }
1017
    }
1018
}
1019

1020
// ----------------------------------------------------------------------------
1021

1022
PcdReader::PcdReader() = default;
1023

1024
void PcdReader::read(const std::string& filename)
1025
{
1026
    clear();
1027
    this->width = 0;
1028
    this->height = 1;
1029

1030
    Base::FileInfo fi(filename);
1031
    Base::ifstream inp(fi, std::ios::in | std::ios::binary);
1032

1033
    std::string format;
1034
    std::vector<std::string> fields;
1035
    std::vector<std::string> types;
1036
    std::vector<int> sizes;
1037
    Eigen::Index numPoints = Eigen::Index(readHeader(inp, format, fields, types, sizes));
1038

1039
    Eigen::MatrixXd data(numPoints, fields.size());
1040
    if (format == "ascii") {
1041
        readAscii(inp, data);
1042
    }
1043
    else if (format == "binary") {
1044
        readBinary(false, inp, types, sizes, data);
1045
    }
1046
    else if (format == "binary_compressed") {
1047
        unsigned int c {};
1048
        unsigned int u {};
1049
        Base::InputStream str(inp);
1050
        str >> c >> u;
1051

1052
        std::vector<char> compressed(c);
1053
        inp.read(compressed.data(), c);
1054
        std::vector<char> uncompressed(u);
1055
        if (lzfDecompress(compressed.data(), c, uncompressed.data(), u) == u) {
1056
            DataStreambuf ibuf(uncompressed);
1057
            std::istream istr(nullptr);
1058
            istr.rdbuf(&ibuf);
1059
            readBinary(true, istr, types, sizes, data);
1060
        }
1061
        else {
1062
            throw Base::BadFormatError("Failed to decompress binary data");
1063
        }
1064
    }
1065

1066
    std::vector<std::string>::iterator it;
1067
    Eigen::Index max_size = std::numeric_limits<Eigen::Index>::max();
1068

1069
    // x field
1070
    Eigen::Index x = max_size;
1071
    it = std::find(fields.begin(), fields.end(), "x");
1072
    if (it != fields.end()) {
1073
        x = std::distance(fields.begin(), it);
1074
    }
1075

1076
    // y field
1077
    Eigen::Index y = max_size;
1078
    it = std::find(fields.begin(), fields.end(), "y");
1079
    if (it != fields.end()) {
1080
        y = std::distance(fields.begin(), it);
1081
    }
1082

1083
    // z field
1084
    Eigen::Index z = max_size;
1085
    it = std::find(fields.begin(), fields.end(), "z");
1086
    if (it != fields.end()) {
1087
        z = std::distance(fields.begin(), it);
1088
    }
1089

1090
    // normal x field
1091
    Eigen::Index normal_x = max_size;
1092
    it = std::find(fields.begin(), fields.end(), "normal_x");
1093
    if (it == fields.end()) {
1094
        it = std::find(fields.begin(), fields.end(), "nx");
1095
    }
1096
    if (it != fields.end()) {
1097
        normal_x = std::distance(fields.begin(), it);
1098
    }
1099

1100
    // normal y field
1101
    Eigen::Index normal_y = max_size;
1102
    it = std::find(fields.begin(), fields.end(), "normal_y");
1103
    if (it == fields.end()) {
1104
        it = std::find(fields.begin(), fields.end(), "ny");
1105
    }
1106
    if (it != fields.end()) {
1107
        normal_y = std::distance(fields.begin(), it);
1108
    }
1109

1110
    // normal z field
1111
    Eigen::Index normal_z = max_size;
1112
    it = std::find(fields.begin(), fields.end(), "normal_z");
1113
    if (it == fields.end()) {
1114
        it = std::find(fields.begin(), fields.end(), "nz");
1115
    }
1116
    if (it != fields.end()) {
1117
        normal_z = std::distance(fields.begin(), it);
1118
    }
1119

1120
    // intensity field
1121
    Eigen::Index greyvalue = max_size;
1122
    it = std::find(fields.begin(), fields.end(), "intensity");
1123
    if (it != fields.end()) {
1124
        greyvalue = std::distance(fields.begin(), it);
1125
    }
1126

1127
    // rgb(a) field
1128
    Eigen::Index rgba = max_size;
1129
    it = std::find(fields.begin(), fields.end(), "rgb");
1130
    if (it == fields.end()) {
1131
        it = std::find(fields.begin(), fields.end(), "rgba");
1132
    }
1133
    if (it != fields.end()) {
1134
        rgba = std::distance(fields.begin(), it);
1135
    }
1136

1137
    // transfer the data
1138
    bool hasData = (x != max_size && y != max_size && z != max_size);
1139
    bool hasNormal = (normal_x != max_size && normal_y != max_size && normal_z != max_size);
1140
    bool hasIntensity = (greyvalue != max_size);
1141
    bool hasColor = (rgba != max_size);
1142

1143
    if (hasData) {
1144
        points.reserve(numPoints);
1145
        for (Eigen::Index i = 0; i < numPoints; i++) {
1146
            points.push_back(Base::Vector3d(data(i, x), data(i, y), data(i, z)));
1147
        }
1148
    }
1149

1150
    if (hasData && hasNormal) {
1151
        normals.reserve(numPoints);
1152
        for (Eigen::Index i = 0; i < numPoints; i++) {
1153
            normals.emplace_back(data(i, normal_x), data(i, normal_y), data(i, normal_z));
1154
        }
1155
    }
1156

1157
    if (hasData && hasIntensity) {
1158
        intensity.reserve(numPoints);
1159
        for (Eigen::Index i = 0; i < numPoints; i++) {
1160
            intensity.push_back(data(i, greyvalue));
1161
        }
1162
    }
1163

1164
    if (hasData && hasColor) {
1165
        colors.reserve(numPoints);
1166
        if (types[rgba] == "U") {
1167
            for (Eigen::Index i = 0; i < numPoints; i++) {
1168
                uint32_t packed = static_cast<uint32_t>(data(i, rgba));
1169
                App::Color col;
1170
                col.setPackedARGB(packed);
1171
                colors.emplace_back(col);
1172
            }
1173
        }
1174
        else if (types[rgba] == "F") {
1175
            static_assert(sizeof(float) == sizeof(uint32_t),
1176
                          "float and uint32_t have different sizes");
1177
            for (Eigen::Index i = 0; i < numPoints; i++) {
1178
                float f = static_cast<float>(data(i, rgba));
1179
                uint32_t packed {};
1180
                std::memcpy(&packed, &f, sizeof(packed));
1181
                App::Color col;
1182
                col.setPackedARGB(packed);
1183
                colors.emplace_back(col);
1184
            }
1185
        }
1186
    }
1187
}
1188

1189
std::size_t PcdReader::readHeader(std::istream& in,
1190
                                  std::string& format,
1191
                                  std::vector<std::string>& fields,
1192
                                  std::vector<std::string>& types,
1193
                                  std::vector<int>& sizes)
1194
{
1195
    std::string line;
1196
    std::vector<std::string> counts;
1197
    std::vector<std::string> list;
1198
    std::size_t points = 0;
1199

1200
    while (std::getline(in, line)) {
1201
        if (line.empty()) {
1202
            continue;
1203
        }
1204

1205
        // since the file is loaded in binary mode we may get the CR at the end
1206
        boost::trim(line);
1207
        boost::split(list, line, boost::is_any_of("\t\r "), boost::token_compress_on);
1208

1209
        std::istringstream str(line);
1210
        str.imbue(std::locale::classic());
1211

1212
        std::string kw;
1213
        str >> kw;
1214
        if (kw == "FIELDS") {
1215
            for (std::size_t i = 1; i < list.size(); i++) {
1216
                fields.push_back(list[i]);
1217
            }
1218
        }
1219
        else if (kw == "SIZE") {
1220
            for (std::size_t i = 1; i < list.size(); i++) {
1221
                sizes.push_back(boost::lexical_cast<int>(list[i]));
1222
            }
1223
        }
1224
        else if (kw == "TYPE") {
1225
            for (std::size_t i = 1; i < list.size(); i++) {
1226
                types.push_back(list[i]);
1227
            }
1228
        }
1229
        else if (kw == "COUNT") {
1230
            for (std::size_t i = 1; i < list.size(); i++) {
1231
                counts.push_back(list[i]);
1232
            }
1233
        }
1234
        else if (kw == "WIDTH") {
1235
            str >> std::ws >> this->width;
1236
        }
1237
        else if (kw == "HEIGHT") {
1238
            str >> std::ws >> this->height;
1239
        }
1240
        else if (kw == "POINTS") {
1241
            str >> std::ws >> points;
1242
        }
1243
        else if (kw == "DATA") {
1244
            str >> std::ws >> format;
1245
            break;
1246
        }
1247
    }
1248

1249
    std::size_t w = static_cast<std::size_t>(this->width);
1250
    std::size_t h = static_cast<std::size_t>(this->height);
1251
    std::size_t size = w * h;
1252
    if (fields.size() != sizes.size() || fields.size() != types.size()
1253
        || fields.size() != counts.size() || points != size) {
1254
        throw Base::BadFormatError("");
1255
    }
1256

1257
    return points;
1258
}
1259

1260
void PcdReader::readAscii(std::istream& inp, Eigen::MatrixXd& data)
1261
{
1262
    std::string line;
1263
    Eigen::Index row = 0;
1264
    Eigen::Index numPoints = data.rows();
1265
    Eigen::Index numFields = data.cols();
1266
    std::vector<std::string> list;
1267
    while (std::getline(inp, line) && row < numPoints) {
1268
        if (line.empty()) {
1269
            continue;
1270
        }
1271

1272
        // since the file is loaded in binary mode we may get the CR at the end
1273
        boost::trim(line);
1274
        boost::split(list, line, boost::is_any_of("\t\r "), boost::token_compress_on);
1275

1276
        std::istringstream str(line);
1277

1278
        Eigen::Index size = Eigen::Index(list.size());
1279
        for (Eigen::Index col = 0; col < size && col < numFields; col++) {
1280
            double value = boost::lexical_cast<double>(list[col]);
1281
            data(row, col) = value;
1282
        }
1283

1284
        ++row;
1285
    }
1286
}
1287

1288
void PcdReader::readBinary(bool transpose,
1289
                           std::istream& inp,
1290
                           const std::vector<std::string>& types,
1291
                           const std::vector<int>& sizes,
1292
                           Eigen::MatrixXd& data)
1293
{
1294
    Eigen::Index numPoints = data.rows();
1295
    Eigen::Index numFields = data.cols();
1296

1297
    int neededSize = 0;
1298
    ConverterPtr convert_float32(new ConverterT<float>);
1299
    ConverterPtr convert_float64(new ConverterT<double>);
1300
    ConverterPtr convert_int8(new ConverterT<int8_t>);
1301
    ConverterPtr convert_uint8(new ConverterT<uint8_t>);
1302
    ConverterPtr convert_int16(new ConverterT<int16_t>);
1303
    ConverterPtr convert_uint16(new ConverterT<uint16_t>);
1304
    ConverterPtr convert_int32(new ConverterT<int32_t>);
1305
    ConverterPtr convert_uint32(new ConverterT<uint32_t>);
1306

1307
    std::vector<ConverterPtr> converters;
1308
    for (Eigen::Index j = 0; j < numFields; j++) {
1309
        char t = types[j][0];
1310
        switch (sizes[j]) {
1311
            case 1:
1312
                if (t == 'I') {
1313
                    converters.push_back(convert_int8);
1314
                }
1315
                else if (t == 'U') {
1316
                    converters.push_back(convert_uint8);
1317
                }
1318
                else {
1319
                    throw Base::BadFormatError("Unexpected type");
1320
                }
1321
                break;
1322
            case 2:
1323
                if (t == 'I') {
1324
                    converters.push_back(convert_int16);
1325
                }
1326
                else if (t == 'U') {
1327
                    converters.push_back(convert_uint16);
1328
                }
1329
                else {
1330
                    throw Base::BadFormatError("Unexpected type");
1331
                }
1332
                break;
1333
            case 4:
1334
                if (t == 'I') {
1335
                    converters.push_back(convert_int32);
1336
                }
1337
                else if (t == 'U') {
1338
                    converters.push_back(convert_uint32);
1339
                }
1340
                else if (t == 'F') {
1341
                    converters.push_back(convert_float32);
1342
                }
1343
                else {
1344
                    throw Base::BadFormatError("Unexpected type");
1345
                }
1346
                break;
1347
            case 8:
1348
                if (t == 'F') {
1349
                    converters.push_back(convert_float64);
1350
                }
1351
                else {
1352
                    throw Base::BadFormatError("Unexpected type");
1353
                }
1354
                break;
1355
            default:
1356
                throw Base::BadFormatError("Unexpected type");
1357
        }
1358

1359
        neededSize += converters.back()->getSizeOf();
1360
    }
1361

1362
    std::streamoff ulSize = 0;
1363
    std::streamoff ulCurr = 0;
1364
    std::streambuf* buf = inp.rdbuf();
1365
    if (buf) {
1366
        ulCurr = buf->pubseekoff(0, std::ios::cur, std::ios::in);
1367
        ulSize = buf->pubseekoff(0, std::ios::end, std::ios::in);
1368
        buf->pubseekoff(ulCurr, std::ios::beg, std::ios::in);
1369
        if (ulCurr + neededSize * static_cast<std::streamoff>(numPoints) > ulSize) {
1370
            throw Base::BadFormatError("File expects too many elements");
1371
        }
1372
    }
1373

1374
    Base::InputStream str(inp);
1375
    if (transpose) {
1376
        for (Eigen::Index j = 0; j < numFields; j++) {
1377
            for (Eigen::Index i = 0; i < numPoints; i++) {
1378
                double value = converters[j]->toDouble(str);
1379
                data(i, j) = value;
1380
            }
1381
        }
1382
    }
1383
    else {
1384
        for (Eigen::Index i = 0; i < numPoints; i++) {
1385
            for (Eigen::Index j = 0; j < numFields; j++) {
1386
                double value = converters[j]->toDouble(str);
1387
                data(i, j) = value;
1388
            }
1389
        }
1390
    }
1391
}
1392

1393
// ----------------------------------------------------------------------------
1394

1395
namespace
1396
{
1397
class E57ReaderImp
1398
{
1399
public:
1400
    E57ReaderImp(const std::string& filename, bool color, bool state, double distance)
1401
        : imfi(filename, "r")
1402
        , useColor {color}
1403
        , checkState {state}
1404
        , minDistance {distance}
1405
    {}
1406

1407
    void read()
1408
    {
1409
        e57::StructureNode root = imfi.root();
1410
        if (root.isDefined("data3D")) {
1411
            e57::VectorNode data3D(root.get("data3D"));
1412
            readData3D(data3D);
1413
        }
1414
    }
1415

1416
    std::vector<App::Color> getColors() const
1417
    {
1418
        return colors;
1419
    }
1420

1421
    std::vector<float> getItensity() const
1422
    {
1423
        return intensity;
1424
    }
1425

1426
    const PointKernel& getPoints() const
1427
    {
1428
        return points;
1429
    }
1430

1431
    const std::vector<Base::Vector3f>& getNormals() const
1432
    {
1433
        return normals;
1434
    }
1435

1436
private:
1437
    void readData3D(const e57::VectorNode& data3D)
1438
    {
1439
        for (int child = 0; child < data3D.childCount(); ++child) {
1440
            e57::StructureNode scan_data(data3D.get(child));
1441
            Base::Placement plm;
1442
            bool hasPlacement = getPlacement(scan_data, plm);
1443

1444
            e57::CompressedVectorNode cvn(scan_data.get("points"));
1445
            e57::StructureNode prototype(cvn.prototype());
1446
            Proto proto = readProto(prototype);
1447
            processProto(cvn, proto, hasPlacement, plm);
1448
        }
1449
    }
1450

1451
    struct Proto
1452
    {
1453
        bool inty = false;
1454
        bool inv_state = false;
1455
        unsigned cnt_xyz = 0;
1456
        unsigned cnt_nor = 0;
1457
        unsigned cnt_rgb = 0;
1458

1459
        std::vector<double> xData;
1460
        std::vector<double> yData;
1461
        std::vector<double> zData;
1462

1463
        std::vector<double> xNormal;
1464
        std::vector<double> yNormal;
1465
        std::vector<double> zNormal;
1466

1467
        std::vector<unsigned> redData;
1468
        std::vector<unsigned> greenData;
1469
        std::vector<unsigned> blueData;
1470

1471
        std::vector<double> intensity;
1472
        std::vector<int64_t> state;
1473
        std::vector<int64_t> nil;
1474

1475
        std::vector<e57::SourceDestBuffer> sdb;
1476
    };
1477

1478
    Proto readProto(const e57::StructureNode& prototype)
1479
    {
1480
        Proto proto;
1481
        resizeArrays(proto);
1482

1483
        for (int i = 0; i < prototype.childCount(); ++i) {
1484
            e57::Node node(prototype.get(i));
1485
            if ((node.type() == e57::E57_FLOAT) || (node.type() == e57::E57_SCALED_INTEGER)) {
1486
                if (readCartesian(node, proto)) {}
1487
                else if (readNormal(node, proto)) {}
1488
                else if (readItensity(node, proto)) {}
1489
                else {
1490
                    readOther(node, proto);
1491
                }
1492
            }
1493
            else if (node.type() == e57::E57_INTEGER) {
1494
                if (readColor(node, proto)) {}
1495
                else if (readCartesianInvalidState(node, proto)) {}
1496
                else {
1497
                    readOther(node, proto);
1498
                }
1499
            }
1500
        }
1501

1502
        return proto;
1503
    }
1504

1505
    bool readCartesian(const e57::Node& node, Proto& proto)
1506
    {
1507
        if (node.elementName() == "cartesianX") {
1508
            proto.cnt_xyz++;
1509
            proto.sdb
1510
                .emplace_back(imfi, node.elementName(), proto.xData.data(), buf_size, true, true
1511

1512
                );
1513
            return true;
1514
        }
1515
        else if (node.elementName() == "cartesianY") {
1516
            proto.cnt_xyz++;
1517
            proto.sdb
1518
                .emplace_back(imfi, node.elementName(), proto.yData.data(), buf_size, true, true
1519

1520
                );
1521
            return true;
1522
        }
1523
        else if (node.elementName() == "cartesianZ") {
1524
            proto.cnt_xyz++;
1525
            proto.sdb
1526
                .emplace_back(imfi, node.elementName(), proto.zData.data(), buf_size, true, true
1527

1528
                );
1529
            return true;
1530
        }
1531

1532
        return false;
1533
    }
1534

1535
    bool readNormal(const e57::Node& node, Proto& proto)
1536
    {
1537
        if (node.elementName() == "nor:normalX") {
1538
            proto.cnt_nor++;
1539
            proto.sdb
1540
                .emplace_back(imfi, node.elementName(), proto.xNormal.data(), buf_size, true, true
1541

1542
                );
1543
            return true;
1544
        }
1545
        else if (node.elementName() == "nor:normalY") {
1546
            proto.cnt_nor++;
1547
            proto.sdb
1548
                .emplace_back(imfi, node.elementName(), proto.yNormal.data(), buf_size, true, true
1549

1550
                );
1551
            return true;
1552
        }
1553
        else if (node.elementName() == "nor:normalZ") {
1554
            proto.cnt_nor++;
1555
            proto.sdb
1556
                .emplace_back(imfi, node.elementName(), proto.zNormal.data(), buf_size, true, true
1557

1558
                );
1559
            return true;
1560
        }
1561

1562
        return false;
1563
    }
1564

1565
    bool readCartesianInvalidState(const e57::Node& node, Proto& proto)
1566
    {
1567
        if (node.elementName() == "cartesianInvalidState") {
1568
            proto.inv_state = true;
1569
            proto.sdb
1570
                .emplace_back(imfi, node.elementName(), proto.state.data(), buf_size, true, true
1571

1572
                );
1573
            return true;
1574
        }
1575

1576
        return false;
1577
    }
1578

1579
    bool readColor(const e57::Node& node, Proto& proto)
1580
    {
1581
        if (node.elementName() == "colorRed") {
1582
            proto.cnt_rgb++;
1583
            proto.sdb
1584
                .emplace_back(imfi, node.elementName(), proto.redData.data(), buf_size, true, true
1585

1586
                );
1587
            return true;
1588
        }
1589
        if (node.elementName() == "colorGreen") {
1590
            proto.cnt_rgb++;
1591
            proto.sdb
1592
                .emplace_back(imfi, node.elementName(), proto.greenData.data(), buf_size, true, true
1593

1594
                );
1595
            return true;
1596
        }
1597
        if (node.elementName() == "colorBlue") {
1598
            proto.cnt_rgb++;
1599
            proto.sdb
1600
                .emplace_back(imfi, node.elementName(), proto.blueData.data(), buf_size, true, true
1601

1602
                );
1603
            return true;
1604
        }
1605

1606
        return false;
1607
    }
1608

1609
    bool readItensity(const e57::Node& node, Proto& proto)
1610
    {
1611
        if (node.elementName() == "intensity") {
1612
            proto.inty = true;
1613
            proto.sdb
1614
                .emplace_back(imfi, node.elementName(), proto.intensity.data(), buf_size, true, true
1615

1616
                );
1617
            return true;
1618
        }
1619

1620
        return false;
1621
    }
1622

1623
    void readOther(const e57::Node& node, Proto& proto)
1624
    {
1625
        proto.sdb.emplace_back(imfi, node.elementName(), proto.nil.data(), buf_size, true, true
1626

1627
        );
1628
    }
1629

1630
    void processProto(e57::CompressedVectorNode& cvn,
1631
                      const Proto& proto,
1632
                      bool hasPlacement,
1633
                      const Base::Placement& plm)
1634
    {
1635
        if (proto.cnt_xyz != 3) {
1636
            throw Base::BadFormatError("Missing channels xyz");
1637
        }
1638
        unsigned count;
1639
        unsigned cnt_pts = 0;
1640
        Base::Vector3d pt, last;
1641
        e57::CompressedVectorReader cvr(cvn.reader(proto.sdb));
1642
        bool hasColor = (proto.cnt_rgb == 3) && useColor;
1643
        bool hasItensity = proto.inty;
1644
        bool hasNormal = (proto.cnt_nor == 3);
1645
        bool hasState = proto.inv_state && checkState;
1646
        bool filter = false;
1647

1648
        while ((count = cvr.read())) {
1649
            for (size_t i = 0; i < count; ++i) {
1650
                filter = false;
1651
                if (hasState) {
1652
                    if (proto.state[i] != 0) {
1653
                        filter = true;
1654
                    }
1655
                }
1656

1657
                pt = getCoord(proto, i, hasPlacement, plm);
1658

1659
                if ((!filter) && (cnt_pts > 0)) {
1660
                    if (Base::Distance(last, pt) < minDistance) {
1661
                        filter = true;
1662
                    }
1663
                }
1664
                if (!filter) {
1665
                    cnt_pts++;
1666
                    points.push_back(pt);
1667
                    last = pt;
1668
                    if (hasColor) {
1669
                        colors.push_back(getColor(proto, i));
1670
                    }
1671
                    if (hasItensity) {
1672
                        intensity.push_back(proto.intensity[i]);
1673
                    }
1674
                    if (hasNormal) {
1675
                        normals.push_back(getNormal(proto, i, hasPlacement, plm.getRotation()));
1676
                    }
1677
                }
1678
            }
1679
        }
1680
    }
1681

1682
    Base::Vector3d
1683
    getCoord(const Proto& proto, size_t index, bool hasPlacement, const Base::Placement& plm) const
1684
    {
1685
        Base::Vector3d pt;
1686
        pt.x = proto.xData[index];
1687
        pt.y = proto.yData[index];
1688
        pt.z = proto.zData[index];
1689
        if (hasPlacement) {
1690
            plm.multVec(pt, pt);
1691
        }
1692
        return pt;
1693
    }
1694

1695
    Base::Vector3f
1696
    getNormal(const Proto& proto, size_t index, bool hasPlacement, const Base::Rotation& rot) const
1697
    {
1698
        Base::Vector3f pt;
1699
        pt.x = proto.xNormal[index];
1700
        pt.y = proto.yNormal[index];
1701
        pt.z = proto.zNormal[index];
1702
        if (hasPlacement) {
1703
            rot.multVec(pt, pt);
1704
        }
1705
        return pt;
1706
    }
1707

1708
    App::Color getColor(const Proto& proto, size_t index) const
1709
    {
1710
        App::Color c;
1711
        c.r = static_cast<float>(proto.redData[index]) / 255.0F;
1712
        c.g = static_cast<float>(proto.greenData[index]) / 255.0F;
1713
        c.b = static_cast<float>(proto.blueData[index]) / 255.0F;
1714
        return c;
1715
    }
1716

1717
    void resizeArrays(Proto& proto)
1718
    {
1719
        proto.xData.resize(buf_size);
1720
        proto.yData.resize(buf_size);
1721
        proto.zData.resize(buf_size);
1722

1723
        proto.xNormal.resize(buf_size);
1724
        proto.yNormal.resize(buf_size);
1725
        proto.zNormal.resize(buf_size);
1726

1727
        proto.redData.resize(buf_size);
1728
        proto.greenData.resize(buf_size);
1729
        proto.blueData.resize(buf_size);
1730

1731
        proto.intensity.resize(buf_size);
1732
        proto.state.resize(buf_size);
1733
        proto.nil.resize(buf_size);
1734
    }
1735

1736
    bool getPlacement(const e57::StructureNode& scan_data, Base::Placement& plm) const
1737
    {
1738
        bool hasPlacement {false};
1739
        if (scan_data.isDefined("pose")) {
1740
            e57::StructureNode pose(scan_data.get("pose"));
1741
            if (pose.isDefined("rotation")) {
1742
                e57::StructureNode rotNode(pose.get("rotation"));
1743
                double quaternion[4];
1744
                quaternion[0] = e57::FloatNode(rotNode.get("x")).value();
1745
                quaternion[1] = e57::FloatNode(rotNode.get("y")).value();
1746
                quaternion[2] = e57::FloatNode(rotNode.get("z")).value();
1747
                quaternion[3] = e57::FloatNode(rotNode.get("w")).value();
1748
                Base::Rotation rotate(quaternion);
1749
                plm.setRotation(rotate);
1750
                hasPlacement = true;
1751
            }
1752
            if (pose.isDefined("translation")) {
1753
                Base::Vector3d move;
1754
                e57::StructureNode transNode(pose.get("translation"));
1755
                move.x = e57::FloatNode(transNode.get("x")).value();
1756
                move.y = e57::FloatNode(transNode.get("y")).value();
1757
                move.z = e57::FloatNode(transNode.get("z")).value();
1758
                plm.setPosition(move);
1759
                hasPlacement = true;
1760
            }
1761
        }
1762

1763
        return hasPlacement;
1764
    }
1765

1766
private:
1767
    e57::ImageFile imfi;
1768
    bool useColor;
1769
    bool checkState;
1770
    double minDistance;
1771
    const size_t buf_size = 1024;
1772
    std::vector<App::Color> colors;
1773
    std::vector<float> intensity;
1774
    PointKernel points;
1775
    std::vector<Base::Vector3f> normals;
1776
};
1777
}  // namespace
1778

1779
E57Reader::E57Reader(bool Color, bool State, double Distance)
1780
    : useColor {Color}
1781
    , checkState {State}
1782
    , minDistance {Distance}
1783
{}
1784

1785
void E57Reader::read(const std::string& filename)
1786
{
1787
    try {
1788
        E57ReaderImp reader(filename, useColor, checkState, minDistance);
1789
        reader.read();
1790
        points = reader.getPoints();
1791
        normals = reader.getNormals();
1792
        colors = reader.getColors();
1793
        intensity = reader.getItensity();
1794
        width = points.size();
1795
        height = 1;
1796
    }
1797
    catch (const Base::BadFormatError&) {
1798
        throw;
1799
    }
1800
    catch (...) {
1801
        throw Base::BadFormatError("Reading E57 file failed");
1802
    }
1803
}
1804

1805
// ----------------------------------------------------------------------------
1806

1807
Writer::Writer(const PointKernel& p)
1808
    : points(p)
1809
    , width(int(p.size()))
1810
    , height {1}
1811
{}
1812

1813
Writer::~Writer() = default;
1814

1815
void Writer::setIntensities(const std::vector<float>& i)
1816
{
1817
    intensity = i;
1818
}
1819

1820
void Writer::setColors(const std::vector<App::Color>& c)
1821
{
1822
    colors = c;
1823
}
1824

1825
void Writer::setNormals(const std::vector<Base::Vector3f>& n)
1826
{
1827
    normals = n;
1828
}
1829

1830
void Writer::setWidth(int w)
1831
{
1832
    width = w;
1833
}
1834

1835
void Writer::setHeight(int h)
1836
{
1837
    height = h;
1838
}
1839

1840
void Writer::setPlacement(const Base::Placement& p)
1841
{
1842
    placement = p;
1843
}
1844

1845
// ----------------------------------------------------------------------------
1846

1847
AscWriter::AscWriter(const PointKernel& p)
1848
    : Writer(p)
1849
{}
1850

1851
void AscWriter::write(const std::string& filename)
1852
{
1853
    if (placement.isIdentity()) {
1854
        points.save(filename.c_str());
1855
    }
1856
    else {
1857
        PointKernel copy = points;
1858
        copy.transformGeometry(placement.toMatrix());
1859
        copy.save(filename.c_str());
1860
    }
1861
}
1862

1863
// ----------------------------------------------------------------------------
1864

1865
PlyWriter::PlyWriter(const PointKernel& p)
1866
    : Writer(p)
1867
{}
1868

1869
void PlyWriter::write(const std::string& filename)
1870
{
1871
    std::list<std::string> properties;
1872
    properties.emplace_back("float x");
1873
    properties.emplace_back("float y");
1874
    properties.emplace_back("float z");
1875

1876
    ConverterPtr convert_float(new ConverterT<float>);
1877
    ConverterPtr convert_uint(new ConverterT<uint32_t>);
1878

1879
    std::vector<ConverterPtr> converters;
1880
    converters.push_back(convert_float);
1881
    converters.push_back(convert_float);
1882
    converters.push_back(convert_float);
1883

1884
    bool hasIntensity = (intensity.size() == points.size());
1885
    bool hasColors = (colors.size() == points.size());
1886
    bool hasNormals = (normals.size() == points.size());
1887

1888
    if (hasNormals) {
1889
        properties.emplace_back("float nx");
1890
        properties.emplace_back("float ny");
1891
        properties.emplace_back("float nz");
1892
        converters.push_back(convert_float);
1893
        converters.push_back(convert_float);
1894
        converters.push_back(convert_float);
1895
    }
1896

1897
    if (hasColors) {
1898
        properties.emplace_back("uchar red");
1899
        properties.emplace_back("uchar green");
1900
        properties.emplace_back("uchar blue");
1901
        properties.emplace_back("uchar alpha");
1902
        converters.push_back(convert_uint);
1903
        converters.push_back(convert_uint);
1904
        converters.push_back(convert_uint);
1905
        converters.push_back(convert_uint);
1906
    }
1907

1908
    if (hasIntensity) {
1909
        properties.emplace_back("float intensity");
1910
        converters.push_back(convert_float);
1911
    }
1912

1913
    Eigen::Index numPoints = Eigen::Index(points.size());
1914
    Eigen::Index numValid = 0;
1915
    const std::vector<Base::Vector3f>& pts = points.getBasicPoints();
1916
    for (Eigen::Index i = 0; i < numPoints; i++) {
1917
        const Base::Vector3f& p = pts[i];
1918
        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
1919
            numValid++;
1920
        }
1921
    }
1922

1923
    Eigen::MatrixXf data(numPoints, properties.size());
1924

1925
    if (placement.isIdentity()) {
1926
        for (Eigen::Index i = 0; i < numPoints; i++) {
1927
            data(i, 0) = pts[i].x;
1928
            data(i, 1) = pts[i].y;
1929
            data(i, 2) = pts[i].z;
1930
        }
1931
    }
1932
    else {
1933
        Base::Vector3d tmp;
1934
        for (Eigen::Index i = 0; i < numPoints; i++) {
1935
            tmp = Base::convertTo<Base::Vector3d>(pts[i]);
1936
            placement.multVec(tmp, tmp);
1937
            data(i, 0) = static_cast<float>(tmp.x);
1938
            data(i, 1) = static_cast<float>(tmp.y);
1939
            data(i, 2) = static_cast<float>(tmp.z);
1940
        }
1941
    }
1942

1943
    Eigen::Index col = 3;
1944
    if (hasNormals) {
1945
        Eigen::Index col0 = col;
1946
        Eigen::Index col1 = col + 1;
1947
        Eigen::Index col2 = col + 2;
1948
        Base::Rotation rot = placement.getRotation();
1949
        if (rot.isIdentity()) {
1950
            for (Eigen::Index i = 0; i < numPoints; i++) {
1951
                data(i, col0) = normals[i].x;
1952
                data(i, col1) = normals[i].y;
1953
                data(i, col2) = normals[i].z;
1954
            }
1955
        }
1956
        else {
1957
            Base::Vector3d tmp;
1958
            for (Eigen::Index i = 0; i < numPoints; i++) {
1959
                tmp = Base::convertTo<Base::Vector3d>(normals[i]);
1960
                rot.multVec(tmp, tmp);
1961
                data(i, col0) = static_cast<float>(tmp.x);
1962
                data(i, col1) = static_cast<float>(tmp.y);
1963
                data(i, col2) = static_cast<float>(tmp.z);
1964
            }
1965
        }
1966
        col += 3;
1967
    }
1968

1969
    if (hasColors) {
1970
        Eigen::Index col0 = col;
1971
        Eigen::Index col1 = col + 1;
1972
        Eigen::Index col2 = col + 2;
1973
        Eigen::Index col3 = col + 3;
1974
        for (Eigen::Index i = 0; i < numPoints; i++) {
1975
            App::Color c = colors[i];
1976
            data(i, col0) = (c.r * 255.0F + 0.5F);
1977
            data(i, col1) = (c.g * 255.0F + 0.5F);
1978
            data(i, col2) = (c.b * 255.0F + 0.5F);
1979
            data(i, col3) = (c.a * 255.0F + 0.5F);
1980
        }
1981
        col += 4;
1982
    }
1983

1984
    if (hasIntensity) {
1985
        for (Eigen::Index i = 0; i < numPoints; i++) {
1986
            data(i, col) = intensity[i];
1987
        }
1988
        col += 1;
1989
    }
1990

1991
    Base::ofstream out(Base::FileInfo(filename), std::ios::out);
1992
    out << "ply" << std::endl
1993
        << "format ascii 1.0" << std::endl
1994
        << "comment FreeCAD generated" << std::endl;
1995
    out << "element vertex " << numValid << std::endl;
1996

1997
    // the properties
1998
    for (const auto& prop : properties) {
1999
        out << "property " << prop << std::endl;
2000
    }
2001
    out << "end_header" << std::endl;
2002

2003
    for (Eigen::Index r = 0; r < numPoints; r++) {
2004
        if (boost::math::isnan(data(r, 0))) {
2005
            continue;
2006
        }
2007
        if (boost::math::isnan(data(r, 1))) {
2008
            continue;
2009
        }
2010
        if (boost::math::isnan(data(r, 2))) {
2011
            continue;
2012
        }
2013
        for (Eigen::Index c = 0; c < col; c++) {
2014
            float value = data(r, c);
2015
            out << converters[c]->toString(value) << " ";
2016
        }
2017
        out << std::endl;
2018
    }
2019
}
2020

2021
// ----------------------------------------------------------------------------
2022

2023
PcdWriter::PcdWriter(const PointKernel& p)
2024
    : Writer(p)
2025
{}
2026

2027
void PcdWriter::write(const std::string& filename)
2028
{
2029
    std::list<std::string> fields;
2030
    fields.emplace_back("x");
2031
    fields.emplace_back("y");
2032
    fields.emplace_back("z");
2033

2034
    std::list<std::string> types;
2035
    types.emplace_back("F");
2036
    types.emplace_back("F");
2037
    types.emplace_back("F");
2038

2039
    ConverterPtr convert_float(new ConverterT<float>);
2040
    ConverterPtr convert_uint(new ConverterT<uint32_t>);
2041

2042
    std::vector<ConverterPtr> converters;
2043
    converters.push_back(convert_float);
2044
    converters.push_back(convert_float);
2045
    converters.push_back(convert_float);
2046

2047
    bool hasIntensity = (intensity.size() == points.size());
2048
    bool hasColors = (colors.size() == points.size());
2049
    bool hasNormals = (normals.size() == points.size());
2050

2051
    if (hasNormals) {
2052
        fields.emplace_back("normal_x");
2053
        fields.emplace_back("normal_y");
2054
        fields.emplace_back("normal_z");
2055
        types.emplace_back("F");
2056
        types.emplace_back("F");
2057
        types.emplace_back("F");
2058
        converters.push_back(convert_float);
2059
        converters.push_back(convert_float);
2060
        converters.push_back(convert_float);
2061
    }
2062

2063
    if (hasColors) {
2064
        fields.emplace_back("rgba");
2065
        types.emplace_back("U");
2066
        converters.push_back(convert_uint);
2067
    }
2068

2069
    if (hasIntensity) {
2070
        fields.emplace_back("intensity");
2071
        types.emplace_back("F");
2072
        converters.push_back(convert_float);
2073
    }
2074

2075
    Eigen::Index numPoints = Eigen::Index(points.size());
2076
    const std::vector<Base::Vector3f>& pts = points.getBasicPoints();
2077

2078
    Eigen::MatrixXd data(numPoints, fields.size());
2079

2080
    if (placement.isIdentity()) {
2081
        for (Eigen::Index i = 0; i < numPoints; i++) {
2082
            data(i, 0) = pts[i].x;
2083
            data(i, 1) = pts[i].y;
2084
            data(i, 2) = pts[i].z;
2085
        }
2086
    }
2087
    else {
2088
        Base::Vector3d tmp;
2089
        for (Eigen::Index i = 0; i < numPoints; i++) {
2090
            tmp = Base::convertTo<Base::Vector3d>(pts[i]);
2091
            placement.multVec(tmp, tmp);
2092
            data(i, 0) = static_cast<float>(tmp.x);
2093
            data(i, 1) = static_cast<float>(tmp.y);
2094
            data(i, 2) = static_cast<float>(tmp.z);
2095
        }
2096
    }
2097

2098
    Eigen::Index col = 3;
2099
    if (hasNormals) {
2100
        Eigen::Index col0 = col;
2101
        Eigen::Index col1 = col + 1;
2102
        Eigen::Index col2 = col + 2;
2103
        Base::Rotation rot = placement.getRotation();
2104
        if (rot.isIdentity()) {
2105
            for (Eigen::Index i = 0; i < numPoints; i++) {
2106
                data(i, col0) = normals[i].x;
2107
                data(i, col1) = normals[i].y;
2108
                data(i, col2) = normals[i].z;
2109
            }
2110
        }
2111
        else {
2112
            Base::Vector3d tmp;
2113
            for (Eigen::Index i = 0; i < numPoints; i++) {
2114
                tmp = Base::convertTo<Base::Vector3d>(normals[i]);
2115
                rot.multVec(tmp, tmp);
2116
                data(i, col0) = static_cast<float>(tmp.x);
2117
                data(i, col1) = static_cast<float>(tmp.y);
2118
                data(i, col2) = static_cast<float>(tmp.z);
2119
            }
2120
        }
2121
        col += 3;
2122
    }
2123

2124
    if (hasColors) {
2125
        for (Eigen::Index i = 0; i < numPoints; i++) {
2126
            // http://docs.pointclouds.org/1.3.0/structpcl_1_1_r_g_b.html
2127
            data(i, col) = colors[i].getPackedARGB();
2128
        }
2129
        col += 1;
2130
    }
2131

2132
    if (hasIntensity) {
2133
        for (Eigen::Index i = 0; i < numPoints; i++) {
2134
            data(i, col) = intensity[i];
2135
        }
2136
        col += 1;
2137
    }
2138

2139
    std::size_t numFields = fields.size();
2140
    Base::ofstream out(Base::FileInfo(filename), std::ios::out);
2141
    out << "# .PCD v0.7 - Point Cloud Data file format" << std::endl << "VERSION 0.7" << std::endl;
2142

2143
    // the fields
2144
    out << "FIELDS";
2145
    for (const auto& field : fields) {
2146
        out << " " << field;
2147
    }
2148
    out << std::endl;
2149

2150
    // the sizes
2151
    out << "SIZE";
2152
    for (std::size_t i = 0; i < numFields; i++) {
2153
        out << " 4";
2154
    }
2155
    out << std::endl;
2156

2157
    // the types
2158
    out << "TYPE";
2159
    for (const auto& type : types) {
2160
        out << " " << type;
2161
    }
2162
    out << std::endl;
2163

2164
    // the count
2165
    out << "COUNT";
2166
    for (std::size_t i = 0; i < numFields; i++) {
2167
        out << " 1";
2168
    }
2169
    out << std::endl;
2170

2171
    out << "WIDTH " << width << std::endl;
2172
    out << "HEIGHT " << height << std::endl;
2173

2174
    Base::Placement plm;
2175
    Base::Vector3d p = plm.getPosition();
2176
    Base::Rotation o = plm.getRotation();
2177
    double x {};
2178
    double y {};
2179
    double z {};
2180
    double w {};
2181
    o.getValue(x, y, z, w);
2182
    out << "VIEWPOINT " << p.x << " " << p.y << " " << p.z << " " << w << " " << x << " " << y
2183
        << " " << z << std::endl;
2184

2185
    out << "POINTS " << numPoints << std::endl << "DATA ascii" << std::endl;
2186

2187
    for (Eigen::Index r = 0; r < numPoints; r++) {
2188
        for (Eigen::Index c = 0; c < col; c++) {
2189
            double value = data(r, c);
2190
            if (boost::math::isnan(value)) {
2191
                out << "nan ";
2192
            }
2193
            else {
2194
                out << converters[c]->toString(value) << " ";
2195
            }
2196
        }
2197
        out << std::endl;
2198
    }
2199
}
2200