PDALWriter.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/PDALWriter.h>
29 #include <rtabmap/utilite/UFile.h>
30 #include <rtabmap/utilite/ULogger.h>
31 #include <rtabmap/utilite/UStl.h>
32 #include <rtabmap/utilite/UConversion.h>
33 #include <pdal/io/BufferReader.hpp>
34 #include <pdal/StageFactory.hpp>
35 #include <pdal/PluginManager.hpp>
36 
37 #ifdef RTABMAP_PDAL_16
38 #include <pdal/pdal_defines.h>
39 #else
40 #include <pdal/pdal_features.hpp>
41 #endif
42 
43 namespace rtabmap {
44 
45 std::string getPDALSupportedWriters()
46 {
47  std::string output;
48  // Force plugin loading.
49 #if PDAL_VERSION_MAJOR>1 || (PDAL_VERSION_MAJOR == 1 && PDAL_VERSION_MINOR > 7) || (PDAL_VERSION_MAJOR == 1 && PDAL_VERSION_MINOR == 7 && PDAL_VERSION_MINOR>=1)
50  pdal::StageFactory f;
51  pdal::PluginManager<pdal::Stage>::loadAll();
52  pdal::StringList stages = pdal::PluginManager<pdal::Stage>::names();
53 #else
54  pdal::StageFactory f(false);
55  pdal::StringList stages = pdal::PluginManager::names(PF_PluginType_Writer);
56 #endif
57  for(pdal::StringList::iterator iter=stages.begin(); iter!=stages.end(); ++iter)
58  {
59  if(!uStrContains(*iter, "writers"))
60  {
61  continue;
62  }
63  if(iter->compare("writers.null") == 0)
64  {
65  continue;
66  }
67  if(iter!=stages.begin())
68  {
69  output += " ";
70  }
71  output += UFile::getExtension(*iter);
72  }
73  return output;
74 }
75 
76 int savePDALFile(const std::string & filePath,
77  const pcl::PointCloud<pcl::PointXYZ> & cloud,
78  const std::vector<int> & cameraIds,
79  bool binary)
80 {
81  UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
82  uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
83 
84  pdal::PointTable table;
85 
86  if(!cameraIds.empty())
87  {
88  table.layout()->registerDims({
89  pdal::Dimension::Id::X,
90  pdal::Dimension::Id::Y,
91  pdal::Dimension::Id::Z,
92  pdal::Dimension::Id::PointSourceId});
93  }
94  else
95  {
96  table.layout()->registerDims({
97  pdal::Dimension::Id::X,
98  pdal::Dimension::Id::Y,
99  pdal::Dimension::Id::Z});
100  }
101  pdal::BufferReader bufferReader;
102 
103  pdal::PointViewPtr view(new pdal::PointView(table));
104  for(size_t i=0; i<cloud.size(); ++i)
105  {
106  view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
107  view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
108  view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
109  if(!cameraIds.empty())
110  {
111  view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
112  }
113  }
114  bufferReader.addView(view);
115 
116  pdal::StageFactory factory;
117  std::string ext = UFile::getExtension(filePath);
118  pdal::Stage *writer = factory.createStage("writers." + ext);
119  if(writer)
120  {
121  pdal::Options writerOps;
122  writerOps.add("filename", filePath);
123  if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
124  if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
125 
126  writer->setOptions(writerOps);
127  writer->setInput(bufferReader);
128 
129  writer->prepare(table);
130  writer->execute(table);
131  }
132  else
133  {
134  UERROR("PDAL: cannot find writer for extension \"%s\". Available extensions: \"%s\"",
135  UFile::getExtension(filePath).c_str(),
136  getPDALSupportedWriters().c_str());
137  return 1;
138  }
139 
140  return 0; //success
141 }
142 
143 int savePDALFile(const std::string & filePath,
144  const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
145  const std::vector<int> & cameraIds,
146  bool binary,
147  const std::vector<float> & intensities)
148 {
149  UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
150  uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
151  UASSERT_MSG(intensities.empty() || intensities.size() == cloud.size(),
152  uFormat("intensities=%d cloud=%d", (int)intensities.size(), (int)cloud.size()).c_str());
153 
154  pdal::PointTable table;
155 
156  if(!intensities.empty() && !cameraIds.empty())
157  {
158  table.layout()->registerDims({
159  pdal::Dimension::Id::X,
160  pdal::Dimension::Id::Y,
161  pdal::Dimension::Id::Z,
162  pdal::Dimension::Id::Red,
163  pdal::Dimension::Id::Green,
164  pdal::Dimension::Id::Blue,
165  pdal::Dimension::Id::PointSourceId,
166  pdal::Dimension::Id::Intensity});
167  }
168  else if(!intensities.empty())
169  {
170  table.layout()->registerDims({
171  pdal::Dimension::Id::X,
172  pdal::Dimension::Id::Y,
173  pdal::Dimension::Id::Z,
174  pdal::Dimension::Id::Red,
175  pdal::Dimension::Id::Green,
176  pdal::Dimension::Id::Blue,
177  pdal::Dimension::Id::Intensity});
178  }
179  else if(!cameraIds.empty())
180  {
181  table.layout()->registerDims({
182  pdal::Dimension::Id::X,
183  pdal::Dimension::Id::Y,
184  pdal::Dimension::Id::Z,
185  pdal::Dimension::Id::Red,
186  pdal::Dimension::Id::Green,
187  pdal::Dimension::Id::Blue,
188  pdal::Dimension::Id::PointSourceId});
189  }
190  else
191  {
192  table.layout()->registerDims({
193  pdal::Dimension::Id::X,
194  pdal::Dimension::Id::Y,
195  pdal::Dimension::Id::Z,
196  pdal::Dimension::Id::Red,
197  pdal::Dimension::Id::Green,
198  pdal::Dimension::Id::Blue});
199  }
200  pdal::BufferReader bufferReader;
201 
202  pdal::PointViewPtr view(new pdal::PointView(table));
203  for(size_t i=0; i<cloud.size(); ++i)
204  {
205  view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
206  view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
207  view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
208  view->setField(pdal::Dimension::Id::Red, i, cloud.at(i).r);
209  view->setField(pdal::Dimension::Id::Green, i, cloud.at(i).g);
210  view->setField(pdal::Dimension::Id::Blue, i, cloud.at(i).b);
211  if(!cameraIds.empty())
212  {
213  view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
214  }
215  if(!intensities.empty())
216  {
217  view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)intensities.at(i));
218  }
219  }
220  bufferReader.addView(view);
221 
222  pdal::StageFactory factory;
223  std::string ext = UFile::getExtension(filePath);
224  pdal::Stage *writer = factory.createStage("writers." + ext);
225  if(writer)
226  {
227  pdal::Options writerOps;
228  writerOps.add("filename", filePath);
229  if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
230  if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
231 
232  writer->setOptions(writerOps);
233  writer->setInput(bufferReader);
234 
235  writer->prepare(table);
236  writer->execute(table);
237  }
238  else
239  {
240  UERROR("PDAL: cannot find writer for extension \"%s\". Available extensions: \"%s\"",
241  UFile::getExtension(filePath).c_str(),
242  getPDALSupportedWriters().c_str());
243  return 1;
244  }
245 
246  return 0; //success
247 }
248 
249 int savePDALFile(const std::string & filePath,
250  const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
251  const std::vector<int> & cameraIds,
252  bool binary,
253  const std::vector<float> & intensities)
254 {
255  UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
256  uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
257  UASSERT_MSG(intensities.empty() || intensities.size() == cloud.size(),
258  uFormat("intensities=%d cloud=%d", (int)intensities.size(), (int)cloud.size()).c_str());
259 
260  pdal::PointTable table;
261 
262  if(!intensities.empty() && !cameraIds.empty())
263  {
264  table.layout()->registerDims({
265  pdal::Dimension::Id::X,
266  pdal::Dimension::Id::Y,
267  pdal::Dimension::Id::Z,
268  pdal::Dimension::Id::Red,
269  pdal::Dimension::Id::Green,
270  pdal::Dimension::Id::Blue,
271  pdal::Dimension::Id::NormalX,
272  pdal::Dimension::Id::NormalY,
273  pdal::Dimension::Id::NormalZ,
274  pdal::Dimension::Id::PointSourceId,
275  pdal::Dimension::Id::Intensity});
276  }
277  else if(!intensities.empty())
278  {
279  table.layout()->registerDims({
280  pdal::Dimension::Id::X,
281  pdal::Dimension::Id::Y,
282  pdal::Dimension::Id::Z,
283  pdal::Dimension::Id::Red,
284  pdal::Dimension::Id::Green,
285  pdal::Dimension::Id::Blue,
286  pdal::Dimension::Id::NormalX,
287  pdal::Dimension::Id::NormalY,
288  pdal::Dimension::Id::NormalZ,
289  pdal::Dimension::Id::Intensity});
290  }
291  else if(!cameraIds.empty())
292  {
293  table.layout()->registerDims({
294  pdal::Dimension::Id::X,
295  pdal::Dimension::Id::Y,
296  pdal::Dimension::Id::Z,
297  pdal::Dimension::Id::Red,
298  pdal::Dimension::Id::Green,
299  pdal::Dimension::Id::Blue,
300  pdal::Dimension::Id::NormalX,
301  pdal::Dimension::Id::NormalY,
302  pdal::Dimension::Id::NormalZ,
303  pdal::Dimension::Id::PointSourceId});
304  }
305  else
306  {
307  table.layout()->registerDims({
308  pdal::Dimension::Id::X,
309  pdal::Dimension::Id::Y,
310  pdal::Dimension::Id::Z,
311  pdal::Dimension::Id::Red,
312  pdal::Dimension::Id::Green,
313  pdal::Dimension::Id::Blue,
314  pdal::Dimension::Id::NormalX,
315  pdal::Dimension::Id::NormalY,
316  pdal::Dimension::Id::NormalZ});
317  }
318  pdal::BufferReader bufferReader;
319 
320  pdal::PointViewPtr view(new pdal::PointView(table));
321  for(size_t i=0; i<cloud.size(); ++i)
322  {
323  view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
324  view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
325  view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
326  view->setField(pdal::Dimension::Id::Red, i, cloud.at(i).r);
327  view->setField(pdal::Dimension::Id::Green, i, cloud.at(i).g);
328  view->setField(pdal::Dimension::Id::Blue, i, cloud.at(i).b);
329  view->setField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x);
330  view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y);
331  view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z);
332  if(!cameraIds.empty())
333  {
334  view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
335  }
336  if(!intensities.empty())
337  {
338  view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)intensities.at(i));
339  }
340  }
341  bufferReader.addView(view);
342 
343  pdal::StageFactory factory;
344  std::string ext = UFile::getExtension(filePath);
345  pdal::Stage *writer = factory.createStage("writers." + ext);
346  if(writer)
347  {
348  pdal::Options writerOps;
349  writerOps.add("filename", filePath);
350  if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
351  if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
352 
353  writer->setOptions(writerOps);
354  writer->setInput(bufferReader);
355 
356  writer->prepare(table);
357  writer->execute(table);
358  }
359  else
360  {
361  UERROR("PDAL: cannot find writer for extension \"%s\". Available extensions: \"%s\"",
362  UFile::getExtension(filePath).c_str(),
363  getPDALSupportedWriters().c_str());
364  return 1;
365  }
366 
367  return 0; //success
368 }
369 
370 int savePDALFile(const std::string & filePath,
371  const pcl::PointCloud<pcl::PointXYZI> & cloud,
372  const std::vector<int> & cameraIds,
373  bool binary)
374 {
375  UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
376  uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
377 
378  pdal::PointTable table;
379 
380  if(!cameraIds.empty())
381  {
382  table.layout()->registerDims({
383  pdal::Dimension::Id::X,
384  pdal::Dimension::Id::Y,
385  pdal::Dimension::Id::Z,
386  pdal::Dimension::Id::Intensity,
387  pdal::Dimension::Id::PointSourceId});
388  }
389  else
390  {
391  table.layout()->registerDims({
392  pdal::Dimension::Id::X,
393  pdal::Dimension::Id::Y,
394  pdal::Dimension::Id::Z,
395  pdal::Dimension::Id::Intensity});
396  }
397  pdal::BufferReader bufferReader;
398 
399  pdal::PointViewPtr view(new pdal::PointView(table));
400  for(size_t i=0; i<cloud.size(); ++i)
401  {
402  view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
403  view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
404  view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
405  view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)cloud.at(i).intensity);
406  if(!cameraIds.empty())
407  {
408  view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
409  }
410  }
411  bufferReader.addView(view);
412 
413  pdal::StageFactory factory;
414  std::string ext = UFile::getExtension(filePath);
415  pdal::Stage *writer = factory.createStage("writers." + ext);
416  if(writer)
417  {
418  pdal::Options writerOps;
419  writerOps.add("filename", filePath);
420  if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
421  if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
422 
423  writer->setOptions(writerOps);
424  writer->setInput(bufferReader);
425 
426  writer->prepare(table);
427  writer->execute(table);
428  }
429  else
430  {
431  UERROR("PDAL: cannot find writer for extension \"%s\". Available extensions: \"%s\"",
432  UFile::getExtension(filePath).c_str(),
433  getPDALSupportedWriters().c_str());
434  return 1;
435  }
436 
437  return 0; //success
438 }
439 
440 int savePDALFile(const std::string & filePath,
441  const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
442  const std::vector<int> & cameraIds,
443  bool binary)
444 {
445  UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
446  uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
447 
448  pdal::PointTable table;
449 
450  if(!cameraIds.empty())
451  {
452  table.layout()->registerDims({
453  pdal::Dimension::Id::X,
454  pdal::Dimension::Id::Y,
455  pdal::Dimension::Id::Z,
456  pdal::Dimension::Id::Intensity,
457  pdal::Dimension::Id::NormalX,
458  pdal::Dimension::Id::NormalY,
459  pdal::Dimension::Id::NormalZ,
460  pdal::Dimension::Id::PointSourceId});
461  }
462  else
463  {
464  table.layout()->registerDims({
465  pdal::Dimension::Id::X,
466  pdal::Dimension::Id::Y,
467  pdal::Dimension::Id::Z,
468  pdal::Dimension::Id::Intensity,
469  pdal::Dimension::Id::NormalX,
470  pdal::Dimension::Id::NormalY,
471  pdal::Dimension::Id::NormalZ});
472  }
473  pdal::BufferReader bufferReader;
474 
475  pdal::PointViewPtr view(new pdal::PointView(table));
476  for(size_t i=0; i<cloud.size(); ++i)
477  {
478  view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
479  view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
480  view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
481  view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)cloud.at(i).intensity);
482  view->setField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x);
483  view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y);
484  view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z);
485  if(!cameraIds.empty())
486  {
487  view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
488  }
489  }
490  bufferReader.addView(view);
491 
492  pdal::StageFactory factory;
493  std::string ext = UFile::getExtension(filePath);
494  pdal::Stage *writer = factory.createStage("writers." + ext);
495  if(writer)
496  {
497  pdal::Options writerOps;
498  writerOps.add("filename", filePath);
499  if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
500  if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
501 
502  writer->setOptions(writerOps);
503  writer->setInput(bufferReader);
504 
505  writer->prepare(table);
506  writer->execute(table);
507  }
508  else
509  {
510  UERROR("PDAL: cannot find writer for extension \"%s\". Available extensions: \"%s\"",
511  UFile::getExtension(filePath).c_str(),
512  getPDALSupportedWriters().c_str());
513  return 1;
514  }
515 
516  return 0; //success
517 }
518 
519 }
520 
rtabmap::savePDALFile
int savePDALFile(const std::string &filePath, const pcl::PointCloud< pcl::PointXYZ > &cloud, const std::vector< int > &cameraIds=std::vector< int >(), bool binary=false)
Definition: PDALWriter.cpp:76
f
f
UConversion.h
Some conversion functions.
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
uStrContains
bool uStrContains(const std::string &string, const std::string &substring)
Definition: UStl.h:787
UStl.h
Wrappers of STL for convenient functions.
rtabmap::getPDALSupportedWriters
std::string getPDALSupportedWriters()
Definition: PDALWriter.cpp:45
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29