IOUtils.cpp
Go to the documentation of this file.
1 
28 #include "lvr2/io/DataStruct.hpp"
29 #include "lvr2/io/IOUtils.hpp"
30 #include "lvr2/io/ModelFactory.hpp"
31 #include "lvr2/io/AsciiIO.hpp"
33 
34 #include <random>
35 #include <unordered_set>
36 
37 namespace lvr2
38 {
39 
41  boost::filesystem::path& transfromFile,
42  std::vector<float>& pts,
43  std::vector<float>& nrm)
44 {
45  std::cout << timestamp << "Transforming normals " << std::endl;
46 
47  char frames[2048];
48  char pose[2014];
49 
50  sprintf(frames, "%s/%s.frames", transfromFile.parent_path().c_str(),
51  transfromFile.stem().c_str());
52  sprintf(pose, "%s/%s.pose", transfromFile.parent_path().c_str(), transfromFile.stem().c_str());
53 
54  boost::filesystem::path framesPath(frames);
55  boost::filesystem::path posePath(pose);
56 
57 
58  Transformd transform = Transformd::Identity();
59 
60  if(boost::filesystem::exists(framesPath))
61  {
62  std::cout << timestamp << "Transforming according to " << framesPath.filename() << std::endl;
63  transform = getTransformationFromFrames<double>(framesPath);
64  }
65  else if(boost::filesystem::exists(posePath))
66  {
67  std::cout << timestamp << "Transforming according to " << posePath.filename() << std::endl;
68  transform = getTransformationFromFrames<double>(posePath);
69  }
70  else
71  {
72  std::cout << timestamp << "Warning: found no transformation for "
73  << transfromFile.filename() << std::endl;
74  }
75 
76  size_t n_normals;
77  size_t w_normals;
78  size_t n_points = buffer->numPoints();
79 
80  floatArr normals = buffer->getFloatArray("normals", n_normals, w_normals);
81  floatArr points = buffer->getPointArray();
82 
83  if (w_normals != 3)
84  {
85  std::cout << timestamp << "Warning: width of normals is not 3" << std::endl;
86  return;
87  }
88  if(n_normals != n_points)
89  {
90  std::cout << timestamp << "Warning: point and normal count mismatch" << std::endl;
91  return;
92  }
93 
94  for(size_t i = 0; i < n_points; i++)
95  {
96 
97  float x = points[3 * i];
98  float y = points[3 * i + 1];
99  float z = points[3 * i + 2];
100 
101  Vector4d v(x,y,z,1);
102  Vector4d tv = transform * v;
103 
104 // points[3 * i] = tv[0];
105 // points[3 * i + 1] = tv[1];
106 // points[3 * i + 2] = tv[2];
107 
108  pts.push_back(tv[0]);
109  pts.push_back(tv[1]);
110  pts.push_back(tv[2]);
111 
112  Rotationd rotation = transform.block(0, 0, 3, 3);
113 
114  float nx = normals[3 * i];
115  float ny = normals[3 * i + 1];
116  float nz = normals[3 * i + 2];
117 
118  Vector3d normal(nx, ny, nz);
119  Vector3d tn = rotation * normal;
120 
121 // normals[3 * i] = tn[0];
122 // normals[3 * i + 1] = tn[1];
123 // normals[3 * i + 2] = tn[2];
124 
125  nrm.push_back(tn[0]);
126  nrm.push_back(tn[1]);
127  nrm.push_back(tn[2]);
128  }
129 
130 }
131 
132 size_t countPointsInFile(const boost::filesystem::path& inFile)
133 {
134  std::ifstream in(inFile.c_str());
135  std::cout << timestamp << "Counting points in "
136  << inFile.filename().string() << "..." << std::endl;
137 
138  // Count lines in file
139  size_t n_points = 0;
140  char line[2048];
141  while(in.good())
142  {
143  in.getline(line, 1024);
144  n_points++;
145  }
146  in.close();
147 
148  std::cout << timestamp << "File " << inFile.filename().string()
149  << " contains " << n_points << " points." << std::endl;
150 
151  return n_points;
152 }
153 
154 void writePose(const BaseVector<float>& position, const BaseVector<float>& angles, const boost::filesystem::path& out)
155 {
156  std::ofstream o(out.c_str());
157  if(o.good())
158  {
159  o << position[0] << " " << position[1] << " " << position[2] << std::endl;
160  o << angles[0] << " " << angles[1] << " " << angles[2];
161  }
162 }
163 
164 size_t writeModel(ModelPtr model, const boost::filesystem::path& outfile)
165 {
166  size_t n_ip = model->m_pointCloud->numPoints();
167  floatArr arr = model->m_pointCloud->getPointArray();
168 
169  ModelFactory::saveModel(model, outfile.string());
170 
171  return n_ip;
172 }
173 
174 size_t writePointsToStream(ModelPtr model, std::ofstream& out, bool nocolor)
175 {
176  size_t n_ip, n_colors;
177  size_t w_colors;
178 
179  n_ip = model->m_pointCloud->numPoints();
180  floatArr arr = model->m_pointCloud->getPointArray();
181 
182  ucharArr colors = model->m_pointCloud->getUCharArray("colors", n_colors, w_colors);
183 
184  for(int a = 0; a < n_ip; a++)
185  {
186  out << arr[a * 3] << " " << arr[a * 3 + 1] << " " << arr[a * 3 + 2];
187 
188  if(n_colors && !(nocolor))
189  {
190  for (unsigned i = 0; i < w_colors; i++)
191  {
192  out << " " << (int)colors[a * w_colors + i];
193  }
194  }
195  out << std::endl;
196 
197  }
198 
199  return n_ip;
200 }
201 
202 size_t getReductionFactor(ModelPtr model, size_t reduction)
203 {
204  size_t n_points = model->m_pointCloud->numPoints();
205  floatArr arr = model->m_pointCloud->getPointArray();
206 
207 
208  std::cout << timestamp << "Point cloud contains " << n_points << " points." << std::endl;
209 
210 /*
211  * If reduction is less than the number of points it will segfault
212  * because the modulo operation is not defined for n mod 0
213  * and we have to keep all points anyways.
214  * Same if no targetSize was given.
215  */
216  if(reduction != 0)
217  {
218  if(reduction < n_points)
219  {
220  return (int)n_points / reduction;
221  }
222  }
223 
224  /* No reduction write all points */
225  return 1;
226 }
227 
228 size_t getReductionFactor(boost::filesystem::path& inFile, size_t targetSize)
229 {
230  /*
231  * If reduction is less than the number of points it will segfault
232  * because the modulo operation is not defined for n mod 0
233  * and we have to keep all points anyways.
234  * Same if no targetSize was given.
235  */
236  if(targetSize != 0)
237  {
238  // Count lines in file
239  size_t n_points = countPointsInFile(inFile);
240 
241  if(targetSize < n_points)
242  {
243  return (int)n_points / targetSize;
244  }
245  }
246 
247  /* No reduction write all points */
248  return 1;
249 
250 }
251 
252 void writePointsAndNormals(std::vector<float>& p, std::vector<float>& n, std::string outfile)
253 {
254 
255  ModelPtr model(new Model);
256  PointBufferPtr buffer(new PointBuffer);
257 
258  // Passing the raw data pointers from the vectors
259  // to a shared array is a bad idea. Due to the PointBuffer
260  // interface we have to copy the data :-(
261  // floatArr points(p.data());
262  // floatArr normals(n.data());
263 
264  floatArr points(new float[p.size()]);
265  floatArr normals(new float[n.size()]);
266 
267  std::cout << timestamp << "Copying buffers for output." << std::endl;
268  // Assuming p and n have the same size (which they should)
269  for(size_t i = 0; i < p.size(); i++)
270  {
271  points[i] = p[i];
272  normals[i] = n[i];
273  }
274 
275  buffer->setPointArray(points, p.size() / 3);
276  buffer->setNormalArray(normals, n.size() / 3);
277 
278  model->m_pointCloud = buffer;
279 
280  std::cout << timestamp << "Saving " << outfile << std::endl;
281  ModelFactory::saveModel(model, outfile);
282  std::cout << timestamp << "Done." << std::endl;
283 }
284 
285 void getPoseFromFile(BaseVector<float>& position, BaseVector<float>& angles, const boost::filesystem::path file)
286 {
287  ifstream in(file.c_str());
288  if(in.good())
289  {
290  in >> position.x >> position.y >> position.z;
291  in >> angles.y >> angles.y >> angles.z;
292  }
293 }
294 
295 size_t getNumberOfPointsInPLY(const std::string& filename)
296 {
297  size_t n_points = 0;
298  size_t n_vertices = 0;
299 
300  // Try to open file
301  std::ifstream in(filename.c_str());
302  if(in.good())
303  {
304  // Check for ply tag
305  std::string tag;
306  in >> tag;
307  if(tag == "PLY" || tag == "ply")
308  {
309  // Parse header
310  std::string token;
311  while (in.good() && token != "end_header" && token != "END_HEADER")
312  {
313  in >> token;
314 
315 
316  // Check for vertex field
317  if(token == "vertex" || token == "VERTEX")
318  {
319  in >> n_vertices;
320  }
321 
322  // Check for point field
323  if(token == "point" || token == "POINT")
324  {
325  in >> n_points;
326  }
327  }
328  if(n_points == 0 && n_vertices == 0)
329  {
330  std::cout << timestamp << "PLY contains neither vertices nor points." << std::endl;
331  return 0;
332  }
333 
334  // Prefer points over vertices
335  if(n_points)
336  {
337  return n_points;
338  }
339  else
340  {
341  return n_vertices;
342  }
343  }
344  else
345  {
346  std::cout << timestamp << filename << " is not a valid .ply file." << std::endl;
347  }
348 
349  }
350  return 0;
351 }
352 
353 template<typename T>
354 typename Channel<T>::Ptr subSampleChannel(Channel<T>& src, std::vector<size_t> ids)
355 {
356  // Create smaller channel of same type
357  size_t width = src.width();
358  typename Channel<T>::Ptr red(new Channel<T>(ids.size(), width));
359 
360  // Sample from original and insert into reduced
361  // channel
362  boost::shared_array<T> a(red->dataPtr());
363  boost::shared_array<T> b(src.dataPtr());
364  for(size_t i = 0; i < ids.size(); i++)
365  {
366  for(size_t j = 0; j < red->width(); j++)
367  {
368  a[i * width + j] = b[ids[i] * width + j];
369  }
370  }
371  return red;
372 }
373 
374 template<typename T>
375 void subsample(PointBufferPtr src, PointBufferPtr dst, const vector<size_t>& indices)
376 {
377  // Go over all supported channel types and sub-sample
378  std::map<std::string, Channel<T>> channels;
379  src->getAllChannelsOfType(channels);
380  for(auto i : channels)
381  {
382  std::cout << timestamp << "Subsampling channel " << i.first << std::endl;
383  typename Channel<T>::Ptr c = subSampleChannel(i.second, indices);
384  dst->addChannel<T>(c, i.first);
385  }
386 
387 }
388 
389 PointBufferPtr subSamplePointBuffer(PointBufferPtr src, const std::vector<size_t>& indices)
390 {
391  PointBufferPtr buffer(new PointBuffer);
392 
393  // Go over all supported channel types and sub-sample
394  subsample<char>(src, buffer, indices);
395  subsample<unsigned char>(src, buffer, indices);
396  subsample<short>(src, buffer, indices);
397  subsample<int>(src, buffer, indices);
398  subsample<unsigned int>(src, buffer, indices);
399  subsample<float>(src, buffer, indices);
400  subsample<double>(src, buffer, indices);
401 
402  return buffer;
403 }
404 
406 {
407  // Buffer for reduced points
408  PointBufferPtr buffer(new PointBuffer);
409  size_t numSrcPts = src->numPoints();
410 
411  // Setup random device and distribution
412  std::random_device dev;
413  std::mt19937 rng(dev());
414  std::uniform_int_distribution<std::mt19937::result_type> dist(0, numSrcPts);
415 
416  // Check buffer size
417  if(n <= numSrcPts)
418  {
419  // Create index buffer for sub-sampling, using set to avoid duplicates
420  std::unordered_set<size_t> index_set;
421  while(index_set.size() < n)
422  {
423  index_set.insert(dist(rng));
424  }
425 
426  // Copy indices into vector for faster access and []-operator support
427  //.In c++14 this is the fastest way. In C++17 a better alternative
428  // would be to use extract().
429  vector<size_t> indices;
430  indices.insert(indices.end(), index_set.begin(), index_set.end());
431  index_set.clear();
432 
433  // Go over all supported channel types and sub-sample
434  subsample<char>(src, buffer, indices);
435  subsample<unsigned char>(src, buffer, indices);
436  subsample<short>(src, buffer, indices);
437  subsample<int>(src, buffer, indices);
438  subsample<unsigned int>(src, buffer, indices);
439  subsample<float>(src, buffer, indices);
440  subsample<double>(src, buffer, indices);
441  }
442  else
443  {
444  std::cout << timestamp << "Sub-sampling not possible. Number of sampling points is " << std::endl;
445  std::cout << timestamp << "larger than number in src buffer. (" << n << " / " << numSrcPts << ")" << std::endl;
446  }
447 
448 
449  return buffer;
450 }
451 
453 {
454  // Get point channel
455  typename Channel<float>::Optional opt = src->getChannel<float>("points");
456  if(opt)
457  {
458  size_t n = opt->numElements();
459  floatArr points = opt->dataPtr();
460 
461  #pragma omp parallel for
462  for(size_t i = 0; i < n; i++)
463  {
464  float x = points[3 * i];
465  float y = points[3 * i + 1];
466  float z = points[3 * i + 2];
467 
468  points[3 * i] = z / 100.0f;
469  points[3 * i + 1] = -x / 100.0f;
470  points[3 * i + 2] = y / 100.0f;
471 
472  }
473  }
474 }
475 
476 void parseSLAMDirectory(std::string dir, vector<ScanPtr>& scans)
477 {
478  boost::filesystem::path directory(dir);
479  if(is_directory(directory))
480  {
481 
482  boost::filesystem::directory_iterator lastFile;
483  std::vector<boost::filesystem::path> scan_files;
484 
485  // First, look for .3d files
486  for(boost::filesystem::directory_iterator it(directory); it != lastFile; it++ )
487  {
488  boost::filesystem::path p = it->path();
489  if(p.extension().string() == ".3d")
490  {
491  // Check for naming convention "scanxxx.3d"
492  int num = 0;
493  if(sscanf(p.filename().string().c_str(), "scan%3d", &num))
494  {
495  scan_files.push_back(p);
496  }
497  }
498  }
499 
500  if(scan_files.size() > 0)
501  {
502  for(size_t i = 0; i < scan_files.size(); i++)
503  {
504  ScanPtr scan = ScanPtr(new Scan());
505 
506  std::string filename = (scan_files[i]).stem().string();
507  boost::filesystem::path frame_file(filename + ".frames");
508  boost::filesystem::path pose_file(filename + ".pose");
509 
510  boost::filesystem::path frame_path = directory/frame_file;
511  boost::filesystem::path pose_path = directory/pose_file;
512 
513  std::cout << "Loading '" << filename << "'" << std::endl;
514  AsciiIO io;
515  ModelPtr model = io.read(scan_files[i].string());
516  scan->points = model->m_pointCloud;
517 
518  size_t numPoints = scan->points->numPoints();
519  floatArr pts = scan->points->getPointArray();
520 
521  for (size_t i = 0; i < numPoints; i++)
522  {
523  BaseVector<float> pt(pts[i*3 + 0], pts[i*3 + 1], pts[i*3 + 2]);
524  scan->boundingBox.expand(pt);
525  }
526 
527  Transformd pose_estimate = Transformd::Identity();
528  Transformd registration = Transformd::Identity();
529 
530  if(boost::filesystem::exists(frame_path))
531  {
532  std::cout << timestamp << "Loading frame information from " << frame_path << std::endl;
533  registration = getTransformationFromFrames<double>(frame_path);
534  }
535  else
536  {
537  std::cout << timestamp << "Did not find a frame file for " << filename << std::endl;
538  }
539 
540  if(boost::filesystem::exists(pose_path))
541  {
542  std::cout << timestamp << "Loading pose estimation from " << pose_path << std::endl;
543  pose_estimate = getTransformationFromPose<double>(pose_path);
544  }
545  else
546  {
547  std::cout << timestamp << "Did not find a pose file for " << filename << std::endl;
548  }
549 
550  // transform points?
551  scan->registration = registration;
552  scan->poseEstimation = pose_estimate;
553 
554  scans.push_back(scan);
555  }
556  }
557  else
558  {
559  std::cout << timestamp << "Error in parseSLAMDirectory(): '"
560  << "Directory does not contain any .3d files." << std::endl;
561  }
562  }
563  else
564  {
565  std::cout << timestamp << "Error in parseSLAMDirectory(): '"
566  << dir << "' is nor a directory." << std::endl;
567  }
568 }
569 
570 } // namespace lvr2
lvr2::floatArr
boost::shared_array< float > floatArr
Definition: DataStruct.hpp:133
lvr2::ModelFactory::saveModel
static void saveModel(ModelPtr m, std::string file)
Definition: ModelFactory.cpp:225
lvr2::transformPointCloudAndAppend
void transformPointCloudAndAppend(PointBufferPtr &buffer, boost::filesystem::path &transfromFile, std::vector< float > &pts, std::vector< float > &nrm)
Transforms the given point buffer according to the transformation stored in transformFile and appends...
Definition: IOUtils.cpp:40
lvr2::BaseVector< float >
transform
Definition: src/tools/lvr2_transform/Options.cpp:44
lvr2::slamToLVRInPlace
void slamToLVRInPlace(PointBufferPtr src)
Transforms src, which is assumed to be in slam6Ds left-handed coordinate system into our right-handed...
Definition: IOUtils.cpp:452
lvr2::writeModel
size_t writeModel(ModelPtr model, const boost::filesystem::path &outfile)
Writes the given model to the given file.
Definition: IOUtils.cpp:164
lvr2::writePointsToStream
size_t writePointsToStream(ModelPtr model, std::ofstream &out, bool nocolor=false)
Writes the points stored in the given model to the fiven output stream. This function is used to apen...
Definition: IOUtils.cpp:174
AsciiIO.hpp
Read and write pointclouds from .pts and .3d files.
lvr2::Transformd
Transform< double > Transformd
4x4 double precision transformation matrix
Definition: MatrixTypes.hpp:71
lvr2::PointBufferPtr
std::shared_ptr< PointBuffer > PointBufferPtr
Definition: PointBuffer.hpp:130
lvr2::getPoseFromFile
void getPoseFromFile(BaseVector< float > &position, BaseVector< float > &angles, const boost::filesystem::path file)
Loads an Euler representation of from a pose file.
Definition: IOUtils.cpp:285
p
SharedPointer p
Definition: ConvertShared.hpp:42
lvr2::PointBuffer
A class to handle point information with an arbitrarily large number of attribute channels....
Definition: PointBuffer.hpp:51
IOUtils.hpp
lvr2::BaseVector::x
CoordT x
Definition: BaseVector.hpp:65
lvr2::Model
Definition: Model.hpp:51
lvr2::countPointsInFile
size_t countPointsInFile(const boost::filesystem::path &inFile)
Counts the number of points (i.e. number of lines) in the given file.
Definition: IOUtils.cpp:132
lvr2::Scan
Definition: ScanTypes.hpp:24
lvr2::BaseVector::y
CoordT y
Definition: BaseVector.hpp:66
lvr2::subsample
void subsample(PointBufferPtr src, PointBufferPtr dst, const vector< size_t > &indices)
Definition: IOUtils.cpp:375
lvr2::getNumberOfPointsInPLY
size_t getNumberOfPointsInPLY(const std::string &filename)
Get the Number Of Points (element points if present, vertex count otherwise) in a PLY file.
Definition: IOUtils.cpp:295
lvr2::getReductionFactor
size_t getReductionFactor(ModelPtr model, size_t targetSize)
Computes the reduction factor for a given target size (number of points) when reducing a point cloud ...
Definition: IOUtils.cpp:202
scripts.normalize_multiple.filename
filename
Definition: normalize_multiple.py:60
lvr2::Channel::Ptr
std::shared_ptr< Channel< T > > Ptr
Definition: Channel.hpp:46
lvr2::Channel::dataPtr
const DataPtr dataPtr() const
lvr2::timestamp
static Timestamp timestamp
A global time stamp object for program runtime measurement.
Definition: Timestamp.hpp:116
DataStruct.hpp
Datastructures for holding loaded data.
lvr2::Vector3d
Eigen::Vector3d Vector3d
Eigen 3D vector, double precision.
Definition: MatrixTypes.hpp:121
scripts.create_png.colors
colors
Definition: create_png.py:41
file
FILE * file
Definition: arithmeticencoder.cpp:77
lvr2::Channel
Definition: Channel.hpp:42
lvr2::AsciiIO::read
virtual ModelPtr read(string filename)
Reads the given file and stores point and color information in the given parameters.
Definition: AsciiIO.cpp:227
lvr2::parseSLAMDirectory
void parseSLAMDirectory(std::string dir, vector< ScanPtr > &scans)
Reads a directory containing data from slam6d. Represents.
Definition: IOUtils.cpp:476
lvr2::BaseVector::z
CoordT z
Definition: BaseVector.hpp:67
lvr2::ucharArr
boost::shared_array< unsigned char > ucharArr
Definition: DataStruct.hpp:137
lvr2::AsciiIO
A import / export class for point cloud data in plain text formats. Currently the file extensions ....
Definition: AsciiIO.hpp:55
lvr2::subSampleChannel
Channel< T >::Ptr subSampleChannel(Channel< T > &src, std::vector< size_t > ids)
Definition: IOUtils.cpp:354
lvr2
Definition: BaseBufferManipulators.hpp:39
lvr2::Vector4d
Eigen::Vector4d Vector4d
Eigen 4D vector, double precision.
Definition: MatrixTypes.hpp:134
lvr2::ModelPtr
std::shared_ptr< Model > ModelPtr
Definition: Model.hpp:80
lvr2::writePointsAndNormals
void writePointsAndNormals(std::vector< float > &p, std::vector< float > &n, std::string outfile)
Writes the points and normals (float triples) stored in p and n to the given output file....
Definition: IOUtils.cpp:252
ModelFactory.hpp
lvr2::Channel::width
size_t width() const
lvr2::writePose
void writePose(const BaseVector< float > &position, const BaseVector< float > &angles, const boost::filesystem::path &out)
Writes pose information in Euler representation to the given file.
Definition: IOUtils.cpp:154
lvr2::subSamplePointBuffer
PointBufferPtr subSamplePointBuffer(PointBufferPtr src, const size_t &n)
Computes a random sub-sampling of a point buffer by creating a uniform distribution over all point in...
Definition: IOUtils.cpp:405
lvr2::Rotationd
Rotation< double > Rotationd
Double precision 3x3 rotation matrix.
Definition: MatrixTypes.hpp:81
TransformUtils.hpp
lvr2::Channel::numElements
size_t numElements() const
lvr2::ScanPtr
std::shared_ptr< Scan > ScanPtr
Shared pointer to scans.
Definition: ScanTypes.hpp:98


lvr2
Author(s): Thomas Wiemann , Sebastian Pütz , Alexander Mock , Lars Kiesow , Lukas Kalbertodt , Tristan Igelbrink , Johan M. von Behren , Dominik Feldschnieders , Alexander Löhr
autogenerated on Wed Mar 2 2022 00:37:23