1 // ----------------------------------------------------------------------------
2 // -                        Open3D: www.open3d.org                            -
3 // ----------------------------------------------------------------------------
4 // The MIT License (MIT)
5 //
6 // Copyright (c) 2018 www.open3d.org
7 //
8 // Permission is hereby granted, free of charge, to any person obtaining a copy
9 // of this software and associated documentation files (the "Software"), to deal
10 // in the Software without restriction, including without limitation the rights
11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 // copies of the Software, and to permit persons to whom the Software is
13 // furnished to do so, subject to the following conditions:
14 //
15 // The above copyright notice and this permission notice shall be included in
16 // all copies or substantial portions of the Software.
17 //
18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24 // IN THE SOFTWARE.
25 // ----------------------------------------------------------------------------
26 
27 #include <IO/ClassIO/PointCloudIO.h>
28 
29 #include <cstdio>
30 #include <sstream>
31 #include <cstdint>
32 #include <External/liblzf/lzf.h>
33 #include <Core/Utility/Console.h>
34 #include <Core/Utility/Helper.h>
35 
36 // References for PCD file IO
37 // http://pointclouds.org/documentation/tutorials/pcd_file_format.php
38 // https://github.com/PointCloudLibrary/pcl/blob/master/io/src/pcd_io.cpp
39 // https://www.mathworks.com/matlabcentral/fileexchange/40382-matlab-to-point-cloud-library
40 
41 namespace three{
42 
43 namespace {
44 
45 enum PCDDataType {
46     PCD_DATA_ASCII = 0,
47     PCD_DATA_BINARY = 1,
48     PCD_DATA_BINARY_COMPRESSED = 2
49 };
50 
51 struct PCLPointField {
52 public:
53     std::string name;
54     int size;
55     char type;
56     int count;
57     // helper variable
58     int count_offset;
59     int offset;
60 };
61 
62 struct PCDHeader {
63 public:
64     std::string version;
65     std::vector<PCLPointField> fields;
66     int width;
67     int height;
68     int points;
69     PCDDataType datatype;
70     std::string viewpoint;
71     // helper variables
72     int elementnum;
73     int pointsize;
74     bool has_points;
75     bool has_normals;
76     bool has_colors;
77 };
78 
CheckHeader(PCDHeader & header)79 bool CheckHeader(PCDHeader &header)
80 {
81     if (header.points <= 0 || header.pointsize <= 0) {
82         PrintDebug("[CheckHeader] PCD has no data.\n");
83         return false;
84     }
85     if (header.fields.size() == 0 || header.pointsize <= 0) {
86         PrintDebug("[CheckHeader] PCD has no fields.\n");
87         return false;
88     }
89     header.has_points = false;
90     header.has_normals = false;
91     header.has_colors = false;
92     bool has_x = false;
93     bool has_y = false;
94     bool has_z = false;
95     bool has_normal_x = false;
96     bool has_normal_y = false;
97     bool has_normal_z = false;
98     bool has_rgb = false;
99     bool has_rgba = false;
100     for (const auto &field : header.fields) {
101         if (field.name == "x") {
102             has_x = true;
103         } else if (field.name == "y") {
104             has_y = true;
105         } else if (field.name == "z") {
106             has_z = true;
107         } else if (field.name == "normal_x") {
108             has_normal_x = true;
109         } else if (field.name == "normal_y") {
110             has_normal_y = true;
111         } else if (field.name == "normal_z") {
112             has_normal_z = true;
113         } else if (field.name == "rgb") {
114             has_rgb = true;
115         } else if (field.name == "rgba") {
116             has_rgba = true;
117         }
118     }
119     header.has_points = (has_x && has_y && has_z);
120     header.has_normals = (has_normal_x && has_normal_y && has_normal_z);
121     header.has_colors = (has_rgb || has_rgba);
122     if (header.has_points == false) {
123         PrintDebug("[CheckHeader] Fields for point data are not complete.\n");
124         return false;
125     }
126     return true;
127 }
128 
ReadPCDHeader(FILE * file,PCDHeader & header)129 bool ReadPCDHeader(FILE *file, PCDHeader &header)
130 {
131     char line_buffer[DEFAULT_IO_BUFFER_SIZE];
132     size_t specified_channel_count = 0;
133 
134     while (fgets(line_buffer, DEFAULT_IO_BUFFER_SIZE, file)) {
135         std::string line(line_buffer);
136         if (line == "") {
137             continue;
138         }
139         std::vector<std::string> st;
140         SplitString(st, line, "\t\r\n ");
141         std::stringstream sstream(line);
142         sstream.imbue(std::locale::classic());
143         std::string line_type;
144         sstream >> line_type;
145         if (line_type.substr(0, 1) == "#") {
146         } else if (line_type.substr(0, 7) == "VERSION") {
147             if (st.size() >= 2) {
148                 header.version = st[1];
149             }
150         } else if (line_type.substr(0, 6) == "FIELDS" ||
151                 line_type.substr(0, 7) == "COLUMNS") {
152             specified_channel_count = st.size() - 1;
153             if (specified_channel_count == 0) {
154                 PrintDebug("[ReadPCDHeader] Bad PCD file format.\n");
155                 return false;
156             }
157             header.fields.resize(specified_channel_count);
158             int count_offset = 0, offset = 0;
159             for (size_t i = 0; i < specified_channel_count; i++,
160                     count_offset += 1, offset += 4) {
161                 header.fields[i].name = st[i + 1];
162                 header.fields[i].size = 4;
163                 header.fields[i].type = 'F';
164                 header.fields[i].count = 1;
165                 header.fields[i].count_offset = count_offset;
166                 header.fields[i].offset = offset;
167             }
168             header.elementnum = count_offset;
169             header.pointsize = offset;
170         } else if (line_type.substr(0, 4) == "SIZE") {
171             if (specified_channel_count != st.size() - 1) {
172                 PrintDebug("[ReadPCDHeader] Bad PCD file format.\n");
173                 return false;
174             }
175             int offset = 0, col_type = 0;
176             for (size_t i = 0; i < specified_channel_count; i++, offset +=
177                     col_type) {
178                 sstream >> col_type;
179                 header.fields[i].size = col_type;
180                 header.fields[i].offset = offset;
181             }
182             header.pointsize = offset;
183         } else if (line_type.substr(0, 4) == "TYPE") {
184             if (specified_channel_count != st.size() - 1) {
185                 PrintDebug("[ReadPCDHeader] Bad PCD file format.\n");
186                 return false;
187             }
188             for (size_t i = 0; i < specified_channel_count; i++) {
189                 header.fields[i].type = st[i + 1].c_str()[0];
190             }
191         } else if (line_type.substr(0, 5) == "COUNT") {
192             if (specified_channel_count != st.size() - 1) {
193                 PrintDebug("[ReadPCDHeader] Bad PCD file format.\n");
194                 return false;
195             }
196             int count_offset = 0, offset = 0, col_count = 0;
197             for (size_t i = 0; i < specified_channel_count; i++) {
198                 sstream >> col_count;
199                 header.fields[i].count = col_count;
200                 header.fields[i].count_offset = count_offset;
201                 header.fields[i].offset = offset;
202                 count_offset += col_count;
203                 offset += col_count * header.fields[i].size;
204             }
205             header.elementnum = count_offset;
206             header.pointsize = offset;
207         } else if (line_type.substr(0, 5) == "WIDTH") {
208             sstream >> header.width;
209         } else if (line_type.substr(0, 6) == "HEIGHT") {
210             sstream >> header.height;
211             header.points = header.width * header.height;
212         } else if (line_type.substr(0, 9) == "VIEWPOINT") {
213             if (st.size() >= 2) {
214                 header.viewpoint = st[1];
215             }
216         } else if (line_type.substr(0, 6) == "POINTS") {
217             sstream >> header.points;
218         } else if (line_type.substr(0, 4) == "DATA") {
219             header.datatype = PCD_DATA_ASCII;
220             if (st.size() >= 2) {
221                 if (st[1].substr(0, 17) == "binary_compressed") {
222                     header.datatype = PCD_DATA_BINARY_COMPRESSED;
223                 } else if (st[1].substr(0, 6) == "binary") {
224                     header.datatype = PCD_DATA_BINARY;
225                 }
226             }
227             break;
228         }
229     }
230     if (CheckHeader(header) == false) {
231         return false;
232     }
233     return true;
234 }
235 
UnpackBinaryPCDElement(const char * data_ptr,const char type,const int size)236 double UnpackBinaryPCDElement(const char *data_ptr, const char type,
237         const int size)
238 {
239     if (type == 'I') {
240         if (size == 1) {
241             std::int8_t data;
242             memcpy(&data, data_ptr, sizeof(data));
243             return (double)data;
244         } else if (size == 2) {
245             std::int16_t data;
246             memcpy(&data, data_ptr, sizeof(data));
247             return (double)data;
248         } else if (size == 4) {
249             std::int32_t data;
250             memcpy(&data, data_ptr, sizeof(data));
251             return (double)data;
252         } else {
253             return 0.0;
254         }
255     } else if (type == 'U') {
256         if (size == 1) {
257             std::uint8_t data;
258             memcpy(&data, data_ptr, sizeof(data));
259             return (double)data;
260         } else if (size == 2) {
261             std::uint16_t data;
262             memcpy(&data, data_ptr, sizeof(data));
263             return (double)data;
264         } else if (size == 4) {
265             std::uint32_t data;
266             memcpy(&data, data_ptr, sizeof(data));
267             return (double)data;
268         } else {
269             return 0.0;
270         }
271     } else if (type == 'F') {
272         if (size == 4) {
273             std::float_t data;
274             memcpy(&data, data_ptr, sizeof(data));
275             return (double)data;
276         } else {
277             return 0.0;
278         }
279     }
280     return 0.0;
281 }
282 
UnpackBinaryPCDColor(const char * data_ptr,const char type,const int size)283 Eigen::Vector3d UnpackBinaryPCDColor(const char *data_ptr, const char type,
284         const int size)
285 {
286     if (size == 4) {
287         std::uint8_t data[4];
288         memcpy(data, data_ptr, 4);
289         // color data is packed in BGR order.
290         return Eigen::Vector3d((double)data[2] / 255.0, (double)data[1] / 255.0,
291                 (double)data[0] / 255.0);
292     } else {
293         return Eigen::Vector3d::Zero();
294     }
295 }
296 
UnpackASCIIPCDElement(const char * data_ptr,const char type,const int size)297 double UnpackASCIIPCDElement(const char *data_ptr, const char type,
298         const int size)
299 {
300     char *end;
301     if (type == 'I') {
302         return (double)std::strtol(data_ptr, &end, 0);
303     } else if (type == 'U') {
304         return (double)std::strtoul(data_ptr, &end, 0);
305     } else if (type == 'F') {
306         return std::strtod(data_ptr, &end);
307     }
308     return 0.0;
309 }
310 
UnpackASCIIPCDColor(const char * data_ptr,const char type,const int size)311 Eigen::Vector3d UnpackASCIIPCDColor(const char *data_ptr, const char type,
312         const int size)
313 {
314     if (size == 4) {
315         std::uint8_t data[4];
316         char *end;
317         if (type == 'I') {
318             std::int32_t value = std::strtol(data_ptr, &end, 0);
319             memcpy(data, &value, 4);
320         } else if (type == 'U') {
321             std::uint32_t value = std::strtoul(data_ptr, &end, 0);
322             memcpy(data, &value, 4);
323         } else if (type == 'F') {
324             std::float_t value = std::strtof(data_ptr, &end);
325             memcpy(data, &value, 4);
326         }
327         return Eigen::Vector3d((double)data[2] / 255.0, (double)data[1] / 255.0,
328                 (double)data[0] / 255.0);
329     } else {
330         return Eigen::Vector3d::Zero();
331     }
332 }
333 
ReadPCDData(FILE * file,const PCDHeader & header,PointCloud & pointcloud)334 bool ReadPCDData(FILE *file, const PCDHeader &header, PointCloud &pointcloud)
335 {
336     // The header should have been checked
337     if (header.has_points) {
338         pointcloud.points_.resize(header.points);
339     } else {
340         PrintDebug("[ReadPCDData] Fields for point data are not complete.\n");
341         return false;
342     }
343     if (header.has_normals) {
344         pointcloud.normals_.resize(header.points);
345     }
346     if (header.has_colors) {
347         pointcloud.colors_.resize(header.points);
348     }
349     if (header.datatype == PCD_DATA_ASCII) {
350         char line_buffer[DEFAULT_IO_BUFFER_SIZE];
351         int idx = 0;
352         while (fgets(line_buffer, DEFAULT_IO_BUFFER_SIZE, file) &&
353                 idx < header.points) {
354             std::string line(line_buffer);
355             std::vector<std::string> strs;
356             SplitString(strs, line, "\t\r\n ");
357             if ((int)strs.size() < header.elementnum) {
358                 continue;
359             }
360             for (size_t i = 0; i < header.fields.size(); i++) {
361                 const auto &field = header.fields[i];
362                 if (field.name == "x") {
363                     pointcloud.points_[idx](0) = UnpackASCIIPCDElement(
364                             strs[field.count_offset].c_str(), field.type,
365                             field.size);
366                 } else if (field.name == "y") {
367                     pointcloud.points_[idx](1) = UnpackASCIIPCDElement(
368                             strs[field.count_offset].c_str(), field.type,
369                             field.size);
370                 } else if (field.name == "z") {
371                     pointcloud.points_[idx](2) = UnpackASCIIPCDElement(
372                             strs[field.count_offset].c_str(), field.type,
373                             field.size);
374                 } else if (field.name == "normal_x") {
375                     pointcloud.normals_[idx](0) = UnpackASCIIPCDElement(
376                             strs[field.count_offset].c_str(), field.type,
377                             field.size);
378                 } else if (field.name == "normal_y") {
379                     pointcloud.normals_[idx](1) = UnpackASCIIPCDElement(
380                             strs[field.count_offset].c_str(), field.type,
381                             field.size);
382                 } else if (field.name == "normal_z") {
383                     pointcloud.normals_[idx](2) = UnpackASCIIPCDElement(
384                             strs[field.count_offset].c_str(), field.type,
385                             field.size);
386                 } else if (field.name == "rgb" || field.name == "rgba") {
387                     pointcloud.colors_[idx] = UnpackASCIIPCDColor(
388                             strs[field.count_offset].c_str(), field.type,
389                             field.size);
390                 }
391             }
392             idx++;
393         }
394     } else if (header.datatype == PCD_DATA_BINARY) {
395         std::unique_ptr<char []> buffer(new char[header.pointsize]);
396         for (int i = 0; i < header.points; i++) {
397             if (fread(buffer.get(), header.pointsize, 1, file) != 1) {
398                 PrintDebug("[ReadPCDData] Failed to read data record.\n");
399                 pointcloud.Clear();
400                 return false;
401             }
402             for (const auto &field : header.fields) {
403                 if (field.name == "x") {
404                     pointcloud.points_[i](0) = UnpackBinaryPCDElement(
405                             buffer.get() + field.offset, field.type,
406                             field.size);
407                 } else if (field.name == "y") {
408                     pointcloud.points_[i](1) = UnpackBinaryPCDElement(
409                             buffer.get() + field.offset, field.type,
410                             field.size);
411                 } else if (field.name == "z") {
412                     pointcloud.points_[i](2) = UnpackBinaryPCDElement(
413                             buffer.get() + field.offset, field.type,
414                             field.size);
415                 } else if (field.name == "normal_x") {
416                     pointcloud.normals_[i](0) = UnpackBinaryPCDElement(
417                             buffer.get() + field.offset, field.type,
418                             field.size);
419                 } else if (field.name == "normal_y") {
420                     pointcloud.normals_[i](1) = UnpackBinaryPCDElement(
421                             buffer.get() + field.offset, field.type,
422                             field.size);
423                 } else if (field.name == "normal_z") {
424                     pointcloud.normals_[i](2) = UnpackBinaryPCDElement(
425                             buffer.get() + field.offset, field.type,
426                             field.size);
427                 } else if (field.name == "rgb" || field.name == "rgba") {
428                     pointcloud.colors_[i] = UnpackBinaryPCDColor(
429                             buffer.get() + field.offset, field.type,
430                             field.size);
431                 }
432             }
433         }
434     } else if (header.datatype == PCD_DATA_BINARY_COMPRESSED) {
435         std::uint32_t compressed_size;
436         std::uint32_t uncompressed_size;
437         if (fread(&compressed_size, sizeof(compressed_size), 1, file) != 1) {
438             PrintDebug("[ReadPCDData] Failed to read data record.\n");
439             pointcloud.Clear();
440             return false;
441         }
442         if (fread(&uncompressed_size, sizeof(uncompressed_size), 1, file) != 1) {
443             PrintDebug("[ReadPCDData] Failed to read data record.\n");
444             pointcloud.Clear();
445             return false;
446         }
447         PrintDebug("PCD data with %d compressed size, and %d uncompressed size.\n",
448                 compressed_size, uncompressed_size);
449         std::unique_ptr<char []> buffer_compressed(new char[compressed_size]);
450         if (fread(buffer_compressed.get(), 1, compressed_size, file) !=
451                 compressed_size) {
452             PrintDebug("[ReadPCDData] Failed to read data record.\n");
453             pointcloud.Clear();
454             return false;
455         }
456         std::unique_ptr<char []> buffer(new char[uncompressed_size]);
457         if (lzf_decompress(buffer_compressed.get(),
458                 (unsigned int)compressed_size, buffer.get(),
459                 (unsigned int)uncompressed_size) != uncompressed_size) {
460             PrintDebug("[ReadPCDData] Uncompression failed.\n");
461             pointcloud.Clear();
462             return false;
463         }
464         for (const auto &field : header.fields) {
465             const char *base_ptr = buffer.get() + field.offset * header.points;
466             if (field.name == "x") {
467                 for (int i = 0; i < header.points; i++) {
468                     pointcloud.points_[i](0) = UnpackBinaryPCDElement(
469                             base_ptr + i * field.size * field.count, field.type,
470                             field.size);
471                 }
472             } else if (field.name == "y") {
473                 for (int i = 0; i < header.points; i++) {
474                     pointcloud.points_[i](1) = UnpackBinaryPCDElement(
475                             base_ptr + i * field.size * field.count, field.type,
476                             field.size);
477                 }
478             } else if (field.name == "z") {
479                 for (int i = 0; i < header.points; i++) {
480                     pointcloud.points_[i](2) = UnpackBinaryPCDElement(
481                             base_ptr + i * field.size * field.count, field.type,
482                             field.size);
483                 }
484             } else if (field.name == "normal_x") {
485                 for (int i = 0; i < header.points; i++) {
486                     pointcloud.normals_[i](0) = UnpackBinaryPCDElement(
487                             base_ptr + i * field.size * field.count, field.type,
488                             field.size);
489                 }
490             } else if (field.name == "normal_y") {
491                 for (int i = 0; i < header.points; i++) {
492                     pointcloud.normals_[i](1) = UnpackBinaryPCDElement(
493                             base_ptr + i * field.size * field.count, field.type,
494                             field.size);
495                 }
496             } else if (field.name == "normal_z") {
497                 for (int i = 0; i < header.points; i++) {
498                     pointcloud.normals_[i](2) = UnpackBinaryPCDElement(
499                             base_ptr + i * field.size * field.count, field.type,
500                             field.size);
501                 }
502             } else if (field.name == "rgb" || field.name == "rgba") {
503                 for (int i = 0; i < header.points; i++) {
504                     pointcloud.colors_[i] = UnpackBinaryPCDColor(
505                             base_ptr + i * field.size * field.count, field.type,
506                             field.size);
507                 }
508             }
509         }
510     }
511     return true;
512 }
513 
RemoveNanData(PointCloud & pointcloud)514 void RemoveNanData(PointCloud &pointcloud)
515 {
516     bool has_normal = pointcloud.HasNormals();
517     bool has_color = pointcloud.HasColors();
518     size_t old_point_num = pointcloud.points_.size();
519     size_t k = 0;                                            // new index
520     for (size_t i = 0; i < old_point_num; i++) {            // old index
521         if (std::isnan(pointcloud.points_[i](0)) == false &&
522                 std::isnan(pointcloud.points_[i](1)) == false &&
523                 std::isnan(pointcloud.points_[i](0)) == false) {
524             pointcloud.points_[k] = pointcloud.points_[i];
525             if (has_normal) pointcloud.normals_[k] = pointcloud.normals_[i];
526             if (has_color) pointcloud.colors_[k] = pointcloud.colors_[i];
527             k++;
528         }
529     }
530     pointcloud.points_.resize(k);
531     if (has_normal) pointcloud.normals_.resize(k);
532     if (has_color) pointcloud.colors_.resize(k);
533     PrintDebug("[Purge] %d nan points have been removed.\n", (int)(old_point_num - k));
534 }
535 
GenerateHeader(const PointCloud & pointcloud,const bool write_ascii,const bool compressed,PCDHeader & header)536 bool GenerateHeader(const PointCloud &pointcloud, const bool write_ascii,
537         const bool compressed, PCDHeader &header)
538 {
539     if (pointcloud.HasPoints() == false) {
540         return false;
541     }
542     header.version = "0.7";
543     header.width = (int)pointcloud.points_.size();
544     header.height = 1;
545     header.points = header.width;
546     header.fields.clear();
547     PCLPointField field;
548     field.type = 'F';
549     field.size = 4;
550     field.count = 1;
551     field.name = "x";
552     header.fields.push_back(field);
553     field.name = "y";
554     header.fields.push_back(field);
555     field.name = "z";
556     header.fields.push_back(field);
557     header.elementnum = 3;
558     header.pointsize = 12;
559     if (pointcloud.HasNormals()) {
560         field.name = "normal_x";
561         header.fields.push_back(field);
562         field.name = "normal_y";
563         header.fields.push_back(field);
564         field.name = "normal_z";
565         header.fields.push_back(field);
566         header.elementnum += 3;
567         header.pointsize += 12;
568     }
569     if (pointcloud.HasColors()) {
570         field.name = "rgb";
571         header.fields.push_back(field);
572         header.elementnum ++;
573         header.pointsize += 4;
574     }
575     if (write_ascii) {
576         header.datatype = PCD_DATA_ASCII;
577     } else {
578         if (compressed) {
579             header.datatype = PCD_DATA_BINARY_COMPRESSED;
580         } else {
581             header.datatype = PCD_DATA_BINARY;
582         }
583     }
584     return true;
585 }
586 
WritePCDHeader(FILE * file,const PCDHeader & header)587 bool WritePCDHeader(FILE *file, const PCDHeader &header)
588 {
589     fprintf(file, "# .PCD v%s - Point Cloud Data file format\n",
590             header.version.c_str());
591     fprintf(file, "VERSION %s\n", header.version.c_str());
592     fprintf(file, "FIELDS");
593     for (const auto &field : header.fields) {
594         fprintf(file, " %s", field.name.c_str());
595     }
596     fprintf(file, "\n");
597     fprintf(file, "SIZE");
598     for (const auto &field : header.fields) {
599         fprintf(file, " %d", field.size);
600     }
601     fprintf(file, "\n");
602     fprintf(file, "TYPE");
603     for (const auto &field : header.fields) {
604         fprintf(file, " %c", field.type);
605     }
606     fprintf(file, "\n");
607     fprintf(file, "COUNT");
608     for (const auto &field : header.fields) {
609         fprintf(file, " %d", field.count);
610     }
611     fprintf(file, "\n");
612     fprintf(file, "WIDTH %d\n", header.width);
613     fprintf(file, "HEIGHT %d\n", header.height);
614     fprintf(file, "VIEWPOINT 0 0 0 1 0 0 0\n");
615     fprintf(file, "POINTS %d\n", header.points);
616 
617     switch (header.datatype) {
618     case PCD_DATA_BINARY:
619         fprintf(file, "DATA binary\n");
620         break;
621     case PCD_DATA_BINARY_COMPRESSED:
622         fprintf(file, "DATA binary_compressed\n");
623         break;
624     case PCD_DATA_ASCII:
625     default:
626         fprintf(file, "DATA ascii\n");
627         break;
628     }
629     return true;
630 }
631 
ConvertRGBToFloat(const Eigen::Vector3d & color)632 float ConvertRGBToFloat(const Eigen::Vector3d &color)
633 {
634     std::uint8_t rgba[4] = {0, 0, 0, 0};
635     rgba[2] = (std::uint8_t)std::max(std::min((int)(color(0) * 255.0), 255), 0);
636     rgba[1] = (std::uint8_t)std::max(std::min((int)(color(1) * 255.0), 255), 0);
637     rgba[0] = (std::uint8_t)std::max(std::min((int)(color(2) * 255.0), 255), 0);
638     float value;
639     memcpy(&value, rgba, 4);
640     return value;
641 }
642 
WritePCDData(FILE * file,const PCDHeader & header,const PointCloud & pointcloud)643 bool WritePCDData(FILE *file, const PCDHeader &header,
644         const PointCloud &pointcloud)
645 {
646     bool has_normal = pointcloud.HasNormals();
647     bool has_color = pointcloud.HasColors();
648     if (header.datatype == PCD_DATA_ASCII) {
649         for (size_t i = 0; i < pointcloud.points_.size(); i++) {
650             const auto &point = pointcloud.points_[i];
651             fprintf(file, "%.10g %.10g %.10g", point(0), point(1), point(2));
652             if (has_normal) {
653                 const auto &normal = pointcloud.normals_[i];
654                 fprintf(file, " %.10g %.10g %.10g",
655                         normal(0), normal(1), normal(2));
656             }
657             if (has_color) {
658                 const auto &color = pointcloud.colors_[i];
659                 fprintf(file, " %.10g", ConvertRGBToFloat(color));
660             }
661             fprintf(file, "\n");
662         }
663     } else if (header.datatype == PCD_DATA_BINARY) {
664         std::unique_ptr<float []> data(new float[header.elementnum]);
665         for (size_t i = 0; i < pointcloud.points_.size(); i++) {
666             const auto &point = pointcloud.points_[i];
667             data[0] = (float)point(0);
668             data[1] = (float)point(1);
669             data[2] = (float)point(2);
670             int idx = 3;
671             if (has_normal) {
672                 const auto &normal = pointcloud.normals_[i];
673                 data[idx + 0] = (float)normal(0);
674                 data[idx + 1] = (float)normal(1);
675                 data[idx + 2] = (float)normal(2);
676                 idx += 3;
677             }
678             if (has_color) {
679                 const auto &color = pointcloud.colors_[i];
680                 data[idx] = ConvertRGBToFloat(color);
681             }
682             fwrite(data.get(), sizeof(float), header.elementnum, file);
683         }
684     } else if (header.datatype == PCD_DATA_BINARY_COMPRESSED) {
685         int strip_size = header.points;
686         std::uint32_t buffer_size = (std::uint32_t)(header.elementnum *
687                 header.points);
688         std::unique_ptr<float []> buffer(new float[buffer_size]);
689         std::unique_ptr<float []> buffer_compressed(new float[buffer_size * 2]);
690         for (size_t i = 0; i < pointcloud.points_.size(); i++) {
691             const auto &point = pointcloud.points_[i];
692             buffer[0 * strip_size + i] = (float)point(0);
693             buffer[1 * strip_size + i] = (float)point(1);
694             buffer[2 * strip_size + i] = (float)point(2);
695             int idx = 3;
696             if (has_normal) {
697                 const auto &normal = pointcloud.normals_[i];
698                 buffer[(idx + 0) * strip_size + i] = (float)normal(0);
699                 buffer[(idx + 1) * strip_size + i] = (float)normal(1);
700                 buffer[(idx + 2) * strip_size + i] = (float)normal(2);
701                 idx += 3;
702             }
703             if (has_color) {
704                 const auto &color = pointcloud.colors_[i];
705                 buffer[idx * strip_size + i] = ConvertRGBToFloat(color);
706             }
707         }
708         std::uint32_t buffer_size_in_bytes = buffer_size * sizeof(float);
709         std::uint32_t size_compressed = lzf_compress(buffer.get(),
710                 buffer_size_in_bytes, buffer_compressed.get(),
711                 buffer_size_in_bytes * 2);
712         if (size_compressed == 0) {
713             PrintDebug("[WritePCDData] Failed to compress data.\n");
714             return false;
715         }
716         PrintDebug("[WritePCDData] %d bytes data compressed into %d bytes.\n",
717                 buffer_size_in_bytes, size_compressed);
718         fwrite(&size_compressed, sizeof(size_compressed), 1, file);
719         fwrite(&buffer_size_in_bytes, sizeof(buffer_size_in_bytes), 1, file);
720         fwrite(buffer_compressed.get(), 1, size_compressed, file);
721     }
722     return true;
723 }
724 
725 }    // unnamed namespace
726 
ReadPointCloudFromPCD(const std::string & filename,PointCloud & pointcloud)727 bool ReadPointCloudFromPCD(const std::string &filename, PointCloud &pointcloud)
728 {
729     PCDHeader header;
730     FILE *file = fopen(filename.c_str(), "rb");
731     if (file == NULL) {
732         PrintWarning("Read PCD failed: unable to open file: %s\n", filename.c_str());
733         return false;
734     }
735     if (ReadPCDHeader(file, header) == false) {
736         PrintWarning("Read PCD failed: unable to parse header.\n");
737         fclose(file);
738         return false;
739     }
740     PrintDebug("PCD header indicates %d fields, %d bytes per point, and %d points in total.\n",
741             (int)header.fields.size(), header.pointsize, header.points);
742     for (const auto &field : header.fields) {
743         PrintDebug("%s, %c, %d, %d, %d\n", field.name.c_str(),
744                 field.type, field.size, field.count, field.offset);
745     }
746     PrintDebug("Compression method is %d.\n", (int)header.datatype);
747     PrintDebug("Points: %s;  normals: %s;  colors: %s\n",
748             header.has_points ? "yes" : "no",
749             header.has_normals ? "yes" : "no",
750             header.has_colors ? "yes" : "no");
751     if (ReadPCDData(file, header, pointcloud) == false) {
752         PrintWarning("Read PCD failed: unable to read data.\n");
753         fclose(file);
754         return false;
755     }
756     fclose(file);
757     // Some PCD files include nan floating numbers. They should be removed.
758     RemoveNanData(pointcloud);
759     return true;
760 }
761 
WritePointCloudToPCD(const std::string & filename,const PointCloud & pointcloud,bool write_ascii,bool compressed)762 bool WritePointCloudToPCD(const std::string &filename,
763         const PointCloud &pointcloud, bool write_ascii/* = false*/,
764         bool compressed/* = false*/)
765 {
766     PCDHeader header;
767     if (GenerateHeader(pointcloud, write_ascii, compressed, header) == false){
768         PrintWarning("Write PCD failed: unable to generate header.\n");
769         return false;
770     }
771     FILE *file = fopen(filename.c_str(), "wb");
772     if (file == NULL) {
773         PrintWarning("Write PCD failed: unable to open file.\n");
774         return false;
775     }
776     if (WritePCDHeader(file, header) == false){
777         PrintWarning("Write PCD failed: unable to write header.\n");
778         fclose(file);
779         return false;
780     }
781     if (WritePCDData(file, header, pointcloud) == false) {
782         PrintWarning("Write PCD failed: unable to write data.\n");
783         fclose(file);
784         return false;
785     }
786     fclose(file);
787     return true;
788 }
789 
790 }    // namespace three
791