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.compare("laz")==0?"las":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  if(ext.compare("laz")==0) writerOps.add("compression", "lazperf");
126 
127  writer->setOptions(writerOps);
128  writer->setInput(bufferReader);
129 
130  writer->prepare(table);
131  writer->execute(table);
132  }
133  else
134  {
135  UERROR("PDAL: cannot find writer for extension \"%s\". Available extensions: \"%s\"",
136  UFile::getExtension(filePath).c_str(),
137  getPDALSupportedWriters().c_str());
138  return 1;
139  }
140 
141  return 0; //success
142 }
143 
144 int savePDALFile(const std::string & filePath,
145  const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
146  const std::vector<int> & cameraIds,
147  bool binary,
148  const std::vector<float> & intensities)
149 {
150  UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
151  uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
152  UASSERT_MSG(intensities.empty() || intensities.size() == cloud.size(),
153  uFormat("intensities=%d cloud=%d", (int)intensities.size(), (int)cloud.size()).c_str());
154 
155  pdal::PointTable table;
156 
157  if(!intensities.empty() && !cameraIds.empty())
158  {
159  table.layout()->registerDims({
160  pdal::Dimension::Id::X,
161  pdal::Dimension::Id::Y,
162  pdal::Dimension::Id::Z,
163  pdal::Dimension::Id::Red,
164  pdal::Dimension::Id::Green,
165  pdal::Dimension::Id::Blue,
166  pdal::Dimension::Id::PointSourceId,
167  pdal::Dimension::Id::Intensity});
168  }
169  else if(!intensities.empty())
170  {
171  table.layout()->registerDims({
172  pdal::Dimension::Id::X,
173  pdal::Dimension::Id::Y,
174  pdal::Dimension::Id::Z,
175  pdal::Dimension::Id::Red,
176  pdal::Dimension::Id::Green,
177  pdal::Dimension::Id::Blue,
178  pdal::Dimension::Id::Intensity});
179  }
180  else if(!cameraIds.empty())
181  {
182  table.layout()->registerDims({
183  pdal::Dimension::Id::X,
184  pdal::Dimension::Id::Y,
185  pdal::Dimension::Id::Z,
186  pdal::Dimension::Id::Red,
187  pdal::Dimension::Id::Green,
188  pdal::Dimension::Id::Blue,
189  pdal::Dimension::Id::PointSourceId});
190  }
191  else
192  {
193  table.layout()->registerDims({
194  pdal::Dimension::Id::X,
195  pdal::Dimension::Id::Y,
196  pdal::Dimension::Id::Z,
197  pdal::Dimension::Id::Red,
198  pdal::Dimension::Id::Green,
199  pdal::Dimension::Id::Blue});
200  }
201  pdal::BufferReader bufferReader;
202 
203  pdal::PointViewPtr view(new pdal::PointView(table));
204  for(size_t i=0; i<cloud.size(); ++i)
205  {
206  view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
207  view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
208  view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
209  view->setField(pdal::Dimension::Id::Red, i, cloud.at(i).r);
210  view->setField(pdal::Dimension::Id::Green, i, cloud.at(i).g);
211  view->setField(pdal::Dimension::Id::Blue, i, cloud.at(i).b);
212  if(!cameraIds.empty())
213  {
214  view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
215  }
216  if(!intensities.empty())
217  {
218  view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)intensities.at(i));
219  }
220  }
221  bufferReader.addView(view);
222 
223  pdal::StageFactory factory;
224  std::string ext = UFile::getExtension(filePath);
225  pdal::Stage *writer = factory.createStage("writers." + (ext.compare("laz")==0?"las":ext));
226  if(writer)
227  {
228  pdal::Options writerOps;
229  writerOps.add("filename", filePath);
230  if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
231  if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
232  if(ext.compare("laz")==0) writerOps.add("compression", "lazperf");
233 
234  writer->setOptions(writerOps);
235  writer->setInput(bufferReader);
236 
237  writer->prepare(table);
238  writer->execute(table);
239  }
240  else
241  {
242  UERROR("PDAL: cannot find writer for extension \"%s\". Available extensions: \"%s\"",
243  UFile::getExtension(filePath).c_str(),
244  getPDALSupportedWriters().c_str());
245  return 1;
246  }
247 
248  return 0; //success
249 }
250 
251 int savePDALFile(const std::string & filePath,
252  const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
253  const std::vector<int> & cameraIds,
254  bool binary,
255  const std::vector<float> & intensities)
256 {
257  UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
258  uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
259  UASSERT_MSG(intensities.empty() || intensities.size() == cloud.size(),
260  uFormat("intensities=%d cloud=%d", (int)intensities.size(), (int)cloud.size()).c_str());
261 
262  pdal::PointTable table;
263 
264  if(!intensities.empty() && !cameraIds.empty())
265  {
266  table.layout()->registerDims({
267  pdal::Dimension::Id::X,
268  pdal::Dimension::Id::Y,
269  pdal::Dimension::Id::Z,
270  pdal::Dimension::Id::Red,
271  pdal::Dimension::Id::Green,
272  pdal::Dimension::Id::Blue,
273  pdal::Dimension::Id::NormalX,
274  pdal::Dimension::Id::NormalY,
275  pdal::Dimension::Id::NormalZ,
276  pdal::Dimension::Id::PointSourceId,
277  pdal::Dimension::Id::Intensity});
278  }
279  else if(!intensities.empty())
280  {
281  table.layout()->registerDims({
282  pdal::Dimension::Id::X,
283  pdal::Dimension::Id::Y,
284  pdal::Dimension::Id::Z,
285  pdal::Dimension::Id::Red,
286  pdal::Dimension::Id::Green,
287  pdal::Dimension::Id::Blue,
288  pdal::Dimension::Id::NormalX,
289  pdal::Dimension::Id::NormalY,
290  pdal::Dimension::Id::NormalZ,
291  pdal::Dimension::Id::Intensity});
292  }
293  else if(!cameraIds.empty())
294  {
295  table.layout()->registerDims({
296  pdal::Dimension::Id::X,
297  pdal::Dimension::Id::Y,
298  pdal::Dimension::Id::Z,
299  pdal::Dimension::Id::Red,
300  pdal::Dimension::Id::Green,
301  pdal::Dimension::Id::Blue,
302  pdal::Dimension::Id::NormalX,
303  pdal::Dimension::Id::NormalY,
304  pdal::Dimension::Id::NormalZ,
305  pdal::Dimension::Id::PointSourceId});
306  }
307  else
308  {
309  table.layout()->registerDims({
310  pdal::Dimension::Id::X,
311  pdal::Dimension::Id::Y,
312  pdal::Dimension::Id::Z,
313  pdal::Dimension::Id::Red,
314  pdal::Dimension::Id::Green,
315  pdal::Dimension::Id::Blue,
316  pdal::Dimension::Id::NormalX,
317  pdal::Dimension::Id::NormalY,
318  pdal::Dimension::Id::NormalZ});
319  }
320  pdal::BufferReader bufferReader;
321 
322  pdal::PointViewPtr view(new pdal::PointView(table));
323  for(size_t i=0; i<cloud.size(); ++i)
324  {
325  view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
326  view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
327  view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
328  view->setField(pdal::Dimension::Id::Red, i, cloud.at(i).r);
329  view->setField(pdal::Dimension::Id::Green, i, cloud.at(i).g);
330  view->setField(pdal::Dimension::Id::Blue, i, cloud.at(i).b);
331  view->setField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x);
332  view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y);
333  view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z);
334  if(!cameraIds.empty())
335  {
336  view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
337  }
338  if(!intensities.empty())
339  {
340  view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)intensities.at(i));
341  }
342  }
343  bufferReader.addView(view);
344 
345  pdal::StageFactory factory;
346  std::string ext = UFile::getExtension(filePath);
347  pdal::Stage *writer = factory.createStage("writers." + (ext.compare("laz")==0?"las":ext));
348  if(writer)
349  {
350  pdal::Options writerOps;
351  writerOps.add("filename", filePath);
352  if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
353  if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
354  if(ext.compare("laz")==0) writerOps.add("compression", "lazperf");
355 
356  writer->setOptions(writerOps);
357  writer->setInput(bufferReader);
358 
359  writer->prepare(table);
360  writer->execute(table);
361  }
362  else
363  {
364  UERROR("PDAL: cannot find writer for extension \"%s\". Available extensions: \"%s\"",
365  UFile::getExtension(filePath).c_str(),
366  getPDALSupportedWriters().c_str());
367  return 1;
368  }
369 
370  return 0; //success
371 }
372 
373 int savePDALFile(const std::string & filePath,
374  const pcl::PointCloud<pcl::PointXYZI> & cloud,
375  const std::vector<int> & cameraIds,
376  bool binary)
377 {
378  UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
379  uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
380 
381  pdal::PointTable table;
382 
383  if(!cameraIds.empty())
384  {
385  table.layout()->registerDims({
386  pdal::Dimension::Id::X,
387  pdal::Dimension::Id::Y,
388  pdal::Dimension::Id::Z,
389  pdal::Dimension::Id::Intensity,
390  pdal::Dimension::Id::PointSourceId});
391  }
392  else
393  {
394  table.layout()->registerDims({
395  pdal::Dimension::Id::X,
396  pdal::Dimension::Id::Y,
397  pdal::Dimension::Id::Z,
398  pdal::Dimension::Id::Intensity});
399  }
400  pdal::BufferReader bufferReader;
401 
402  pdal::PointViewPtr view(new pdal::PointView(table));
403  for(size_t i=0; i<cloud.size(); ++i)
404  {
405  view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
406  view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
407  view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
408  view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)cloud.at(i).intensity);
409  if(!cameraIds.empty())
410  {
411  view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
412  }
413  }
414  bufferReader.addView(view);
415 
416  pdal::StageFactory factory;
417  std::string ext = UFile::getExtension(filePath);
418  pdal::Stage *writer = factory.createStage("writers." + (ext.compare("laz")==0?"las":ext));
419  if(writer)
420  {
421  pdal::Options writerOps;
422  writerOps.add("filename", filePath);
423  if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
424  if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
425  if(ext.compare("laz")==0) writerOps.add("compression", "lazperf");
426 
427  writer->setOptions(writerOps);
428  writer->setInput(bufferReader);
429 
430  writer->prepare(table);
431  writer->execute(table);
432  }
433  else
434  {
435  UERROR("PDAL: cannot find writer for extension \"%s\". Available extensions: \"%s\"",
436  UFile::getExtension(filePath).c_str(),
437  getPDALSupportedWriters().c_str());
438  return 1;
439  }
440 
441  return 0; //success
442 }
443 
444 int savePDALFile(const std::string & filePath,
445  const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
446  const std::vector<int> & cameraIds,
447  bool binary)
448 {
449  UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
450  uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
451 
452  pdal::PointTable table;
453 
454  if(!cameraIds.empty())
455  {
456  table.layout()->registerDims({
457  pdal::Dimension::Id::X,
458  pdal::Dimension::Id::Y,
459  pdal::Dimension::Id::Z,
460  pdal::Dimension::Id::Intensity,
461  pdal::Dimension::Id::NormalX,
462  pdal::Dimension::Id::NormalY,
463  pdal::Dimension::Id::NormalZ,
464  pdal::Dimension::Id::PointSourceId});
465  }
466  else
467  {
468  table.layout()->registerDims({
469  pdal::Dimension::Id::X,
470  pdal::Dimension::Id::Y,
471  pdal::Dimension::Id::Z,
472  pdal::Dimension::Id::Intensity,
473  pdal::Dimension::Id::NormalX,
474  pdal::Dimension::Id::NormalY,
475  pdal::Dimension::Id::NormalZ});
476  }
477  pdal::BufferReader bufferReader;
478 
479  pdal::PointViewPtr view(new pdal::PointView(table));
480  for(size_t i=0; i<cloud.size(); ++i)
481  {
482  view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
483  view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
484  view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
485  view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)cloud.at(i).intensity);
486  view->setField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x);
487  view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y);
488  view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z);
489  if(!cameraIds.empty())
490  {
491  view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
492  }
493  }
494  bufferReader.addView(view);
495 
496  pdal::StageFactory factory;
497  std::string ext = UFile::getExtension(filePath);
498  pdal::Stage *writer = factory.createStage("writers." + (ext.compare("laz")==0?"las":ext));
499  if(writer)
500  {
501  pdal::Options writerOps;
502  writerOps.add("filename", filePath);
503  if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
504  if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
505  if(ext.compare("laz")==0) writerOps.add("compression", "lazperf");
506 
507  writer->setOptions(writerOps);
508  writer->setInput(bufferReader);
509 
510  writer->prepare(table);
511  writer->execute(table);
512  }
513  else
514  {
515  UERROR("PDAL: cannot find writer for extension \"%s\". Available extensions: \"%s\"",
516  UFile::getExtension(filePath).c_str(),
517  getPDALSupportedWriters().c_str());
518  return 1;
519  }
520 
521  return 0; //success
522 }
523 
524 }
525 
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
factory
rtabmap::getPDALSupportedWriters
std::string getPDALSupportedWriters()
Definition: PDALWriter.cpp:45
view
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set view
table
ArrayXXf table(10, 4)
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:785
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
PDALWriter.h
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
ULogger.h
ULogger class and convenient macros.
iter
iterator iter(handle obj)
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
i
int i
binary

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:58