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