pointmatcher/IO.cpp
Go to the documentation of this file.
1 // kate: replace-tabs off; indent-width 4; indent-mode normal
2 // vim: ts=4:sw=4:noexpandtab
3 /*
4 
5 Copyright (c) 2010--2012,
6 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
7 You can contact the authors at <f dot pomerleau at gmail dot com> and
8 <stephane at magnenat dot net>
9 
10 All rights reserved.
11 
12 Redistribution and use in source and binary forms, with or without
13 modification, are permitted provided that the following conditions are met:
14  * Redistributions of source code must retain the above copyright
15  notice, this list of conditions and the following disclaimer.
16  * Redistributions in binary form must reproduce the above copyright
17  notice, this list of conditions and the following disclaimer in the
18  documentation and/or other materials provided with the distribution.
19  * Neither the name of the <organization> nor the
20  names of its contributors may be used to endorse or promote products
21  derived from this software without specific prior written permission.
22 
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
24 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
27 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 
34 */
35 
36 #include "IO.h"
37 #include "IOFunctions.h"
38 #include "InspectorsImpl.h"
39 
40 // For logging
41 #include "PointMatcherPrivate.h"
42 
43 #include <iostream>
44 #include <fstream>
45 #include <stdexcept>
46 #include <ctype.h>
47 #include "boost/algorithm/string.hpp"
48 #include "boost/filesystem.hpp"
49 #include "boost/filesystem/path.hpp"
50 #include "boost/filesystem/operations.hpp"
51 #include "boost/lexical_cast.hpp"
52 #include "boost/foreach.hpp"
53 
54 #ifdef WIN32
55 #define strtok_r strtok_s
56 #endif // WIN32
57 
58 namespace PointMatcherSupport {
59  namespace {
60  const int one = 1;
61  }
62  const bool isBigEndian = *reinterpret_cast<const unsigned char*>(&one) == static_cast<unsigned char>(0);
63  const int oneBigEndian = isBigEndian ? 1 : 1 << 8 * (sizeof(int) - 1);
64 }
65 
66 using namespace std;
67 using namespace PointMatcherSupport;
68 
69 
70 // Tokenize a string, excepted if it begins with a '%' (a comment in CSV)
71 static std::vector<string> csvLineToVector(const char* line)
72 {
73  std::vector<string> parsedLine;
74  char delimiters[] = " \t,;";
75  char *token;
76  char tmpLine[1024];
77  char *brkt = 0;
78  strcpy(tmpLine, line);
79  token = strtok_r(tmpLine, delimiters, &brkt);
80  if(line[0] != '%') // Jump line if it's commented
81  {
82  while (token)
83  {
84  parsedLine.push_back(string(token));
85  token = strtok_r(NULL, delimiters, &brkt);
86  }
87  }
88 
89  return parsedLine;
90 }
91 
92 // Open and parse a CSV file, return the data
93 CsvElements parseCsvWithHeader(const std::string& fileName)
94 {
95  validateFile(fileName);
96 
97  ifstream is(fileName.c_str());
98 
99  unsigned elementCount=0;
100  std::map<string, unsigned> keywordCols;
101  CsvElements data;
102 
103  bool firstLine(true);
104  unsigned lineCount=0;
105 
106  string line;
107  while (safeGetLine(is, line))
108  {
109 
110  if(firstLine)
111  {
112  std::vector<string> header = csvLineToVector(line.c_str());
113 
114  elementCount = header.size();
115  for(unsigned int i = 0; i < elementCount; i++)
116  {
117  keywordCols[header[i]] = i;
118  }
119 
120  firstLine = false;
121  }
122  else // load the rest of the file
123  {
124  std::vector<string> parsedLine = csvLineToVector(line.c_str());
125  if(parsedLine.size() != elementCount && parsedLine.size() !=0)
126  {
127  stringstream errorMsg;
128  errorMsg << "Error at line " << lineCount+1 << ": expecting " << elementCount << " columns but read " << parsedLine.size() << " elements.";
129  throw runtime_error(errorMsg.str());
130  }
131 
132  for(unsigned int i = 0; i < parsedLine.size(); i++)
133  {
134  for(BOOST_AUTO(it,keywordCols.begin()); it!=keywordCols.end(); it++)
135  {
136  if(i == (*it).second)
137  {
138  data[(*it).first].push_back(parsedLine[i]);
139  }
140  }
141  }
142  }
143 
144  lineCount++;
145  }
146 
147  // Use for debug
148 
149  //for(BOOST_AUTO(it,data.begin()); it!=data.end(); it++)
150  //{
151  // cout << "--------------------------" << endl;
152  // cout << "Header: |" << (*it).first << "|" << endl;
153  // //for(unsigned i=0; i<(*it).second.size(); i++)
154  // //{
155  // // cout << (*it).second[i] << endl;
156  // //}
157  //}
158 
159 
160  return data;
161 }
162 
163 
165 template<typename T>
166 PointMatcherIO<T>::FileInfo::FileInfo(const std::string& readingFileName, const std::string& referenceFileName, const std::string& configFileName, const TransformationParameters& initialTransformation, const TransformationParameters& groundTruthTransformation, const Vector& gravity):
167  readingFileName(readingFileName),
168  referenceFileName(referenceFileName),
169  configFileName(configFileName),
170  initialTransformation(initialTransformation),
171  groundTruthTransformation(groundTruthTransformation),
172  gravity(gravity)
173 {}
174 
175 template struct PointMatcherIO<float>::FileInfo;
176 template struct PointMatcherIO<double>::FileInfo;
177 
178 // Empty constructor
179 template<typename T>
180 PointMatcherIO<T>::FileInfoVector::FileInfoVector()
181 {
182 }
183 
185 
198 template<typename T>
199 PointMatcherIO<T>::FileInfoVector::FileInfoVector(const std::string& fileName, std::string dataPath, std::string configPath)
200 {
201  if (dataPath.empty())
202  {
203  #if BOOST_FILESYSTEM_VERSION >= 3
204  dataPath = boost::filesystem::path(fileName).parent_path().string();
205  #else
206  dataPath = boost::filesystem::path(fileName).parent_path().file_string();
207  #endif
208  }
209  if (configPath.empty())
210  {
211  #if BOOST_FILESYSTEM_VERSION >= 3
212  configPath = boost::filesystem::path(fileName).parent_path().string();
213  #else
214  configPath = boost::filesystem::path(fileName).parent_path().file_string();
215  #endif
216  }
217 
218  const CsvElements data = parseCsvWithHeader(fileName);
219 
220  // Look for transformations
221  const bool found3dInitialTrans(findTransform(data, "iT", 3));
222  bool found2dInitialTrans(findTransform(data, "iT", 2));
223  const bool found3dGroundTruthTrans(findTransform(data, "gT", 3));
224  bool found2dGroundTruthTrans(findTransform(data, "gT", 2));
225  if (found3dInitialTrans)
226  found2dInitialTrans = false;
227  if (found3dGroundTruthTrans)
228  found2dGroundTruthTrans = false;
229 
230  // Check for consistency
231  if (found3dInitialTrans && found2dGroundTruthTrans)
232  throw runtime_error("Initial transformation is in 3D but ground-truth is in 2D");
233  if (found2dInitialTrans && found3dGroundTruthTrans)
234  throw runtime_error("Initial transformation is in 2D but ground-truth is in 3D");
235  CsvElements::const_iterator readingIt(data.find("reading"));
236  if (readingIt == data.end())
237  throw runtime_error("Error transfering CSV to structure: The header should at least contain \"reading\".");
238  CsvElements::const_iterator referenceIt(data.find("reference"));
239  CsvElements::const_iterator configIt(data.find("config"));
240 
241  // Load reading
242  const std::vector<string>& readingFileNames = readingIt->second;
243  const unsigned lineCount = readingFileNames.size();
244  boost::optional<std::vector<string> > referenceFileNames;
245  boost::optional<std::vector<string> > configFileNames;
246  if (referenceIt != data.end())
247  {
248  referenceFileNames = referenceIt->second;
249  assert (referenceFileNames->size() == lineCount);
250  }
251  if (configIt != data.end())
252  {
253  configFileNames = configIt->second;
254  assert (configFileNames->size() == lineCount);
255  }
256 
257  // for every lines
258  for(unsigned line=0; line<lineCount; line++)
259  {
260  FileInfo info;
261 
262  // Files
263  info.readingFileName = localToGlobalFileName(dataPath, readingFileNames[line]);
264  if (referenceFileNames)
265  info.referenceFileName = localToGlobalFileName(dataPath, (*referenceFileNames)[line]);
266  if (configFileNames)
267  info.configFileName = localToGlobalFileName(configPath, (*configFileNames)[line]);
268 
269  // Load transformations
270  if(found3dInitialTrans)
271  info.initialTransformation = getTransform(data, "iT", 3, line);
272  if(found2dInitialTrans)
273  info.initialTransformation = getTransform(data, "iT", 2, line);
274  if(found3dGroundTruthTrans)
275  info.groundTruthTransformation = getTransform(data, "gT", 3, line);
276  if(found2dGroundTruthTrans)
277  info.groundTruthTransformation = getTransform(data, "gT", 2, line);
278 
279  // Build the list
280  this->push_back(info);
281  }
282 
283  // Debug: Print the list
284  /*for(unsigned i=0; i<list.size(); i++)
285  {
286  cout << "\n--------------------------" << endl;
287  cout << "Sequence " << i << ":" << endl;
288  cout << "Reading path: " << list[i].readingFileName << endl;
289  cout << "Reference path: " << list[i].referenceFileName << endl;
290  cout << "Extension: " << list[i].fileExtension << endl;
291  cout << "Tranformation:\n" << list[i].initialTransformation << endl;
292  cout << "Grativity:\n" << list[i].gravity << endl;
293  }
294  */
295 }
296 
298 template<typename T>
299 std::string PointMatcherIO<T>::FileInfoVector::localToGlobalFileName(const std::string& parentPath, const std::string& fileName)
300 {
301  std::string globalFileName(fileName);
302  if (!boost::filesystem::exists(globalFileName))
303  {
304  const boost::filesystem::path globalFilePath(boost::filesystem::path(parentPath) / boost::filesystem::path(fileName));
305  #if BOOST_FILESYSTEM_VERSION >= 3
306  globalFileName = globalFilePath.string();
307  #else
308  globalFileName = globalFilePath.file_string();
309  #endif
310  }
311  validateFile(globalFileName);
312  return globalFileName;
313 }
314 
316 template<typename T>
317 bool PointMatcherIO<T>::FileInfoVector::findTransform(const CsvElements& data, const std::string& prefix, unsigned dim)
318 {
319  bool found(true);
320  for(unsigned i=0; i<dim+1; i++)
321  {
322  for(unsigned j=0; j<dim+1; j++)
323  {
324  stringstream transName;
325  transName << prefix << i << j;
326  found = found && (data.find(transName.str()) != data.end());
327  }
328  }
329  return found;
330 }
331 
333 template<typename T>
334 typename PointMatcherIO<T>::TransformationParameters PointMatcherIO<T>::FileInfoVector::getTransform(const CsvElements& data, const std::string& prefix, unsigned dim, unsigned line)
335 {
336  TransformationParameters transformation(TransformationParameters::Identity(dim+1, dim+1));
337  for(unsigned i=0; i<dim+1; i++)
338  {
339  for(unsigned j=0; j<dim+1; j++)
340  {
341  stringstream transName;
342  transName << prefix << i << j;
343  CsvElements::const_iterator colIt(data.find(transName.str()));
344  const T value = boost::lexical_cast<T> (colIt->second[line]);
345  transformation(i,j) = value;
346  }
347  }
348  return transformation;
349 }
350 
351 template struct PointMatcherIO<float>::FileInfoVector;
352 template struct PointMatcherIO<double>::FileInfoVector;
353 
355 void PointMatcherSupport::validateFile(const std::string& fileName)
356 {
357  boost::filesystem::path fullPath(fileName);
358 
359  ifstream ifs(fileName.c_str());
360  if (!ifs.good() || !boost::filesystem::is_regular_file(fullPath))
361  #if BOOST_FILESYSTEM_VERSION >= 3
362  #if BOOST_VERSION >= 105000
363  throw runtime_error(string("Cannot open file ") + boost::filesystem::complete(fullPath).generic_string());
364  #else
365  throw runtime_error(string("Cannot open file ") + boost::filesystem3::complete(fullPath).generic_string());
366  #endif
367  #else
368  throw runtime_error(string("Cannot open file ") + boost::filesystem::complete(fullPath).native_file_string());
369  #endif
370 }
371 
372 
374 template<typename T>
375 typename PointMatcher<T>::DataPoints PointMatcher<T>::DataPoints::load(const std::string& fileName)
376 {
377  const boost::filesystem::path path(fileName);
378  const string& ext(boost::filesystem::extension(path));
379  if (boost::iequals(ext, ".vtk"))
380  return PointMatcherIO<T>::loadVTK(fileName);
381  else if (boost::iequals(ext, ".csv"))
382  return PointMatcherIO<T>::loadCSV(fileName);
383  else if (boost::iequals(ext, ".ply"))
384  return PointMatcherIO<T>::loadPLY(fileName);
385  else if (boost::iequals(ext, ".pcd"))
386  return PointMatcherIO<T>::loadPCD(fileName);
387  else
388  throw runtime_error("loadAnyFormat(): Unknown extension \"" + ext + "\" for file \"" + fileName + "\", extension must be either \".vtk\" or \".csv\"");
389 }
390 
391 template
392 PointMatcher<float>::DataPoints PointMatcher<float>::DataPoints::load(const std::string& fileName);
393 template
394 PointMatcher<double>::DataPoints PointMatcher<double>::DataPoints::load(const std::string& fileName);
395 
396 
407 template<typename T>
408 typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadCSV(const std::string& fileName)
409 {
410  ifstream ifs(fileName.c_str());
411 
412  validateFile(fileName);
413 
414  return loadCSV(ifs);
415 }
416 
417 template<typename T>
418 PointMatcherIO<T>::SupportedLabel::SupportedLabel(const std::string& internalName, const std::string& externalName, const PMPropTypes& type):
419  internalName(internalName),
420  externalName(externalName),
421  type(type)
422 {
423 }
424 
425 
426 // Class LabelGenerator
427 template<typename T>
428 void PointMatcherIO<T>::LabelGenerator::add(const std::string internalName)
429 {
430  bool findLabel = false;
431  for(size_t i=0; i<labels.size(); ++i)
432  {
433  if(internalName == labels[i].text)
434  {
435  labels[i].span++;
436  findLabel = true;
437  break;
438  }
439 
440  }
441 
442  if(!findLabel)
443  {
444  labels.push_back(Label(internalName,1));
445  }
446 }
447 
448 template<typename T>
449 void PointMatcherIO<T>::LabelGenerator::add(const std::string internalName, const unsigned int dim)
450 {
451  labels.push_back(Label(internalName, dim));
452 }
453 
454 
455 // Class LabelGenerator
456 template<typename T>
457 typename PointMatcher<T>::DataPoints::Labels PointMatcherIO<T>::LabelGenerator::getLabels() const
458 {
459  return labels;
460 }
461 
462 template
463 class PointMatcherIO<float>::LabelGenerator;
464 template
465 class PointMatcherIO<double>::LabelGenerator;
466 template <typename T>
467 
468 
469 std::string PointMatcherIO<T>::getColLabel(const Label& label, const int row)
470 {
471  std::string externalName;
472  if (label.text == "normals")
473  {
474  if (row == 0)
475  {
476  externalName = "nx";
477  }
478  if (row == 1)
479  {
480  externalName = "ny";
481  }
482  if (row == 2)
483  {
484  externalName = "nz";
485  }
486  }
487  else if (label.text == "color")
488  {
489  if (row == 0)
490  {
491  externalName = "red";
492  }
493  if (row == 1)
494  {
495  externalName = "green";
496  }
497  if (row == 2)
498  {
499  externalName = "blue";
500  }
501  if (row == 3)
502  externalName = "alpha";
503  }
504  else if (label.text == "eigValues")
505  {
506  externalName = "eigValues" + boost::lexical_cast<string>(row);
507  }
508  else if (label.text == "eigVectors")
509  {
510  // format: eigVectors<0-2><X-Z>
511  externalName = "eigVectors" + boost::lexical_cast<string>(row/3);
512 
513  int row_mod = row % 3;
514  if (row_mod == 0)
515  externalName += "X";
516  else if (row_mod == 1)
517  externalName += "Y";
518  else if (row_mod == 2)
519  externalName += "Z";
520  }
521  else if (label.span == 1)
522  {
523  externalName = label.text;
524  }
525  else
526  externalName = label.text + boost::lexical_cast<std::string>(row);
527 
528  return externalName;
529 }
530 
531 
534 template<typename T>
535 typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadCSV(std::istream& is)
536 {
537  vector<GenericInputHeader> csvHeader;
538  LabelGenerator featLabelGen, descLabelGen, timeLabelGen;
539  Matrix features;
540  Matrix descriptors;
541  Int64Matrix times;
542  unsigned int csvCol = 0;
543  unsigned int csvRow = 0;
544  bool hasHeader(false);
545  bool firstLine(true);
546 
547 
548  //count lines in the file
549  is.unsetf(std::ios_base::skipws);
550  unsigned int line_count = std::count(
551  std::istream_iterator<char>(is),
552  std::istream_iterator<char>(),
553  '\n');
554  //reset the stream
555  is.clear();
556  is.seekg(0, ios::beg);
557 
558  char delimiters[] = " \t,;";
559  char *token;
560  string line;
561  while (safeGetLine(is, line))
562  {
563 
564  // Skip empty lines
565  if(line.empty())
566  break;
567 
568  // Look for text header
569  unsigned int len = strspn(line.c_str(), " ,+-.1234567890Ee");
570  if(len != line.length())
571  {
572  //cout << "Header detected" << endl;
573  hasHeader = true;
574  }
575  else
576  {
577  hasHeader = false;
578  }
579 
580  // Count dimension using first line
581  if(firstLine)
582  {
583  unsigned int dim = 0;
584  char tmpLine[1024]; //FIXME: might be problematic for large file
585  strcpy(tmpLine, line.c_str());
586  char *brkt = 0;
587  token = strtok_r(tmpLine, delimiters, &brkt);
588 
589  //1- BUILD HEADER
590  while (token)
591  {
592  // Load text header
593  if(hasHeader)
594  {
595  csvHeader.push_back(GenericInputHeader(string(token)));
596  }
597  dim++;
598  token = strtok_r(NULL, delimiters, &brkt);
599  }
600 
601  if (!hasHeader)
602  {
603  // Check if it is a simple file with only coordinates
604  if (!(dim == 2 || dim == 3))
605  {
606  int idX=0, idY=0, idZ=0;
607 
608  cout << "WARNING: " << dim << " columns detected. Not obvious which columns to load for x, y or z." << endl;
609  cout << endl << "Enter column ID (starting from 0) for x: ";
610  cin >> idX;
611  cout << "Enter column ID (starting from 0) for y: ";
612  cin >> idY;
613  cout << "Enter column ID (starting from 0, -1 if 2D data) for z: ";
614  cin >> idZ;
615 
616  // Fill with unkown column names
617  for(unsigned int i=0; i<dim; i++)
618  {
619  std::ostringstream os;
620  os << "empty" << i;
621 
622  csvHeader.push_back(GenericInputHeader(os.str()));
623  }
624 
625  // Overwrite with user inputs
626  csvHeader[idX] = GenericInputHeader("x");
627  csvHeader[idY] = GenericInputHeader("y");
628  if(idZ != -1)
629  csvHeader[idZ] = GenericInputHeader("z");
630  }
631  else
632  {
633  // Assume logical order...
634  csvHeader.push_back(GenericInputHeader("x"));
635  csvHeader.push_back(GenericInputHeader("y"));
636  if(dim == 3)
637  csvHeader.push_back(GenericInputHeader("z"));
638  }
639  }
640 
641  //2- PROCESS HEADER
642  // Load known features, descriptors, and time
643  const SupportedLabels externalLabels = getSupportedExternalLabels();
644 
645  // Counters
646  int rowIdFeatures = 0;
647  int rowIdDescriptors = 0;
648  int rowIdTime = 0;
649 
650 
651  // Loop through all known external names (ordered list)
652  for(size_t i=0; i<externalLabels.size(); i++)
653  {
654  const SupportedLabel supLabel = externalLabels[i];
655 
656  for(size_t j=0; j < csvHeader.size(); j++)
657  {
658  if(supLabel.externalName == csvHeader[j].name)
659  {
660  csvHeader[j].matrixType = supLabel.type;
661 
662  switch (supLabel.type)
663  {
664  case FEATURE:
665  csvHeader[j].matrixRowId = rowIdFeatures;
666  featLabelGen.add(supLabel.internalName);
667  rowIdFeatures++;
668  break;
669  case DESCRIPTOR:
670  csvHeader[j].matrixRowId = rowIdDescriptors;
671  descLabelGen.add(supLabel.internalName);
672  rowIdDescriptors++;
673  break;
674  case TIME:
675  csvHeader[j].matrixRowId = rowIdTime;
676  timeLabelGen.add(supLabel.internalName);
677  rowIdTime++;
678  break;
679  default:
680  throw runtime_error(string("CSV parse error: encounter a type different from FEATURE, DESCRIPTOR and TIME. Implementation not supported. See the definition of 'enum PMPropTypes'"));
681  break;
682  }
683 
684  // we stop searching once we have a match
685  break;
686  }
687  }
688  }
689 
690  // loop through the remaining UNSUPPORTED labels and assigned them to a descriptor row
691  for(unsigned int i=0; i<csvHeader.size(); i++)
692  {
693  if(csvHeader[i].matrixType == UNSUPPORTED)
694  {
695  csvHeader[i].matrixType = DESCRIPTOR; // force descriptor
696  csvHeader[i].matrixRowId = rowIdDescriptors;
697  descLabelGen.add(csvHeader[i].name); // keep original name
698  rowIdDescriptors++;
699  }
700  }
701 
702 
703  //3- RESERVE MEMORY
704  if(hasHeader && line_count > 0)
705  line_count--;
706 
707  const unsigned int featDim = featLabelGen.getLabels().totalDim();
708  const unsigned int descDim = descLabelGen.getLabels().totalDim();
709  const unsigned int timeDim = timeLabelGen.getLabels().totalDim();
710  const unsigned int nbPoints = line_count;
711 
712  features = Matrix(featDim, nbPoints);
713  descriptors = Matrix(descDim, nbPoints);
714  times = Int64Matrix(timeDim, nbPoints);
715  }
716 
717 
718  //4- LOAD DATA (this start again from the first line)
719  char* brkt = 0;
720  char line_c[1024];//FIXME: this might be a problem for large files
721  strcpy(line_c,line.c_str());
722  token = strtok_r(line_c, delimiters, &brkt);
723 
724  if(!(hasHeader && firstLine))
725  {
726  // Parse a line
727  csvCol = 0;
728  while (token)
729  {
730  if(csvCol > (csvHeader.size() - 1))
731  {
732  // Error check (too much data)
733  throw runtime_error(
734  (boost::format("CSV parse error: at line %1%, too many elements to parse compare to the header number of columns (col=%2%).") % csvRow % csvHeader.size()).str());
735  }
736 
737  // Alias
738  const int matrixRow = csvHeader[csvCol].matrixRowId;
739  const int matrixCol = csvRow;
740 
741  switch (csvHeader[csvCol].matrixType)
742  {
743  case FEATURE:
744  features(matrixRow, matrixCol) = lexical_cast_scalar_to_string<T>(string(token));
745  break;
746  case DESCRIPTOR:
747  descriptors(matrixRow, matrixCol) = lexical_cast_scalar_to_string<T>(token);
748  break;
749  case TIME:
750  times(matrixRow, matrixCol) = lexical_cast_scalar_to_string<std::int64_t>(token);
751  break;
752  default:
753  throw runtime_error(string("CSV parse error: encounter a type different from FEATURE, DESCRIPTOR and TIME. Implementation not supported. See the definition of 'enum PMPropTypes'"));
754  break;
755 
756  }
757 
758  //fetch next element
759  token = strtok_r(NULL, delimiters, &brkt);
760  csvCol++;
761  }
762 
763 
764  // Error check (not enough data)
765  if(csvCol != (csvHeader.size()))
766  {
767  throw runtime_error(
768  (boost::format("CSV parse error: at line %1%, not enough elements to parse compare to the header number of columns (col=%2%).") % csvRow % csvHeader.size()).str());
769  }
770 
771  csvRow++;
772  }
773 
774  firstLine = false;
775 
776  }
777 
778  // 5- ASSEMBLE FINAL DATAPOINTS
779  DataPoints loadedPoints(features, featLabelGen.getLabels());
780 
781  if (descriptors.rows() > 0)
782  {
783  loadedPoints.descriptors = descriptors;
784  loadedPoints.descriptorLabels = descLabelGen.getLabels();
785  }
786 
787  if(times.rows() > 0)
788  {
789  loadedPoints.times = times;
790  loadedPoints.timeLabels = timeLabelGen.getLabels();
791  }
792 
793  // Ensure homogeous coordinates
794  if(!loadedPoints.featureExists("pad"))
795  {
796  loadedPoints.addFeature("pad", Matrix::Ones(1,features.cols()));
797  }
798 
799  return loadedPoints;
800 }
801 
802 template
803 PointMatcher<float>::DataPoints PointMatcherIO<float>::loadCSV(const std::string& fileName);
804 template
805 PointMatcher<double>::DataPoints PointMatcherIO<double>::loadCSV(const std::string& fileName);
806 
808 template<typename T>
809 void PointMatcher<T>::DataPoints::save(const std::string& fileName, bool binary, unsigned precision) const
810 {
811  const boost::filesystem::path path(fileName);
812  const string& ext(boost::filesystem::extension(path));
813  if (boost::iequals(ext, ".vtk"))
814  return PointMatcherIO<T>::saveVTK(*this, fileName, binary, precision);
815 
816  if (binary)
817  throw runtime_error("save(): Binary writing is not supported together with extension \"" + ext + "\". Currently binary writing is only supported with \".vtk\".");
818 
819  if (boost::iequals(ext, ".csv"))
820  return PointMatcherIO<T>::saveCSV(*this, fileName, precision);
821  else if (boost::iequals(ext, ".ply"))
822  return PointMatcherIO<T>::savePLY(*this, fileName, precision);
823  else if (boost::iequals(ext, ".pcd"))
824  return PointMatcherIO<T>::savePCD(*this, fileName, precision);
825  else
826  throw runtime_error("save(): Unknown extension \"" + ext + "\" for file \"" + fileName + "\", extension must be either \".vtk\", \".ply\", \".pcd\" or \".csv\"");
827 }
828 
829 template
830 void PointMatcher<float>::DataPoints::save(const std::string& fileName, bool binary, unsigned precision) const;
831 template
832 void PointMatcher<double>::DataPoints::save(const std::string& fileName, bool binary, unsigned precision) const;
833 
835 template<typename T>
836 void PointMatcherIO<T>::saveCSV(const DataPoints& data, const std::string& fileName, unsigned precision)
837 {
838  ofstream ofs(fileName.c_str());
839  if (!ofs.good())
840  throw runtime_error(string("Cannot open file ") + fileName);
841  ofs.precision(precision);
842  saveCSV(data, ofs);
843 }
844 
846 template<typename T>
847 void PointMatcherIO<T>::saveCSV(const DataPoints& data, std::ostream& os)
848 {
849  const int pointCount(data.features.cols());
850  const int dimCount(data.features.rows());
851  const int descDimCount(data.descriptors.rows());
852 
853  if (pointCount == 0)
854  {
855  LOG_WARNING_STREAM( "Warning, no points, doing nothing");
856  return;
857  }
858 
859  // write header
860  for (int i = 0; i < dimCount - 1; i++)
861  {
862  os << data.featureLabels[i].text;
863 
864  if (!((i == (dimCount - 2)) && descDimCount == 0))
865  os << ",";
866  }
867 
868  int n = 0;
869  for (size_t i = 0; i < data.descriptorLabels.size(); i++)
870  {
871  Label lab = data.descriptorLabels[i];
872  for (size_t s = 0; s < lab.span; s++)
873  {
874  os << getColLabel(lab,s);
875  if (n != (descDimCount - 1))
876  os << ",";
877  n++;
878  }
879  }
880 
881  os << "\n";
882 
883  // write points
884  for (int p = 0; p < pointCount; ++p)
885  {
886  for (int i = 0; i < dimCount-1; ++i)
887  {
888  os << data.features(i, p);
889  if(!((i == (dimCount - 2)) && descDimCount == 0))
890  os << " , ";
891  }
892 
893  for (int i = 0; i < descDimCount; i++)
894  {
895  os << data.descriptors(i,p);
896  if (i != (descDimCount - 1))
897  os << ",";
898  }
899  os << "\n";
900  }
901 
902 }
903 
904 template
905 void PointMatcherIO<float>::saveCSV(const DataPoints& data, const std::string& fileName, unsigned precision);
906 template
907 void PointMatcherIO<double>::saveCSV(const DataPoints& data, const std::string& fileName, unsigned precision);
908 
910 template<typename T>
911 typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadVTK(const std::string& fileName)
912 {
913  ifstream ifs(fileName.c_str(), std::ios::binary);
914  if (!ifs.good())
915  throw runtime_error(string("Cannot open file ") + fileName);
916  return loadVTK(ifs);
917 }
918 
919 void skipBlock(bool binary, int binarySize, std::istream & is, bool hasSeparateSizeParameter = true){
920  int n;
921  int size;
922  is >> n;
923  if(!is.good()){
924  throw std::runtime_error("File violates the VTK format : parameter 'n' is missing after a field name.");
925  }
926 
927  if(hasSeparateSizeParameter) {
928  is >> size;
929  if(!is.good()){
930  throw std::runtime_error("File violates the VTK format : parameter 'size' is missing after a field name.");
931  }
932  } else {
933  size = n;
934  }
935 
936  std::string line;
937  safeGetLine(is, line); // remove line end after parameters;
938  if(binary){
939  is.seekg(size * binarySize, std::ios_base::cur);
940  } else {
941  for (int p = 0; p < n; p++)
942  {
943  safeGetLine(is, line);
944  }
945  }
946 }
947 
949 template<typename T>
950 typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadVTK(std::istream& is)
951 {
952  std::map<std::string, SplitTime> labelledSplitTime;
953 
954  DataPoints loadedPoints;
955 
956  // parse header
957  string line;
958  safeGetLine(is, line);
959  if (line.find("# vtk DataFile Version") != 0)
960  throw runtime_error(string("Wrong magic header, found ") + line);
961  safeGetLine(is, line);
962  safeGetLine(is, line);
963 
964  const bool isBinary = (line == "BINARY");
965  if (line != "ASCII"){
966  if(!isBinary){
967  throw runtime_error(string("Wrong file type, expecting ASCII or BINARY, found ") + line);
968  }
969  }
970  safeGetLine(is, line);
971 
972  SupportedVTKDataTypes dataType;
973  if (line == "DATASET POLYDATA")
974  dataType = POLYDATA;
975  else if (line == "DATASET UNSTRUCTURED_GRID")
976  dataType = UNSTRUCTURED_GRID;
977  else
978  throw runtime_error(string("Wrong data type, expecting DATASET POLYDATA, found ") + line);
979 
980 
981  // parse points, descriptors and time
982  string fieldName;
983  string name;
984  int dim = 0;
985  int pointCount = 0;
986  string type;
987 
988  while (is >> fieldName)
989  {
990  // load features
991  if(fieldName == "POINTS")
992  {
993  is >> pointCount;
994  is >> type;
995  safeGetLine(is, line); // remove line end after parameters!
996 
997  if(!(type == "float" || type == "double"))
998  throw runtime_error(string("Field POINTS can only be of type double or float"));
999 
1000  Matrix features(4, pointCount);
1001  for (int p = 0; p < pointCount; ++p)
1002  {
1003  readVtkData(type, isBinary, features.template block<3, 1>(0, p), is);
1004  features(3, p) = 1.0;
1005  }
1006  loadedPoints.addFeature("x", features.row(0));
1007  loadedPoints.addFeature("y", features.row(1));
1008  loadedPoints.addFeature("z", features.row(2));
1009  loadedPoints.addFeature("pad", features.row(3));
1010  }
1011 
1013  // Dataset type
1014  // POLYDATA
1015  else if(dataType == POLYDATA && fieldName == "VERTICES")
1016  {
1017  skipBlock(isBinary, 4, is);
1018  }
1019 
1020  else if(dataType == POLYDATA && fieldName == "LINES")
1021  {
1022  skipBlock(isBinary, 4, is);
1023  }
1024 
1025  else if(dataType == POLYDATA && fieldName == "POLYGONS")
1026  {
1027  skipBlock(isBinary, 4, is);
1028  }
1029 
1030  else if(dataType == POLYDATA && fieldName == "TRIANGLE_STRIPS")
1031  {
1032  skipBlock(isBinary, 4, is);
1033  }
1034 
1035  // Unstructure Grid
1036  else if(dataType == UNSTRUCTURED_GRID && fieldName == "CELLS")
1037  {
1038  skipBlock(isBinary, 4, is);
1039  }
1040  else if(dataType == UNSTRUCTURED_GRID && fieldName == "CELL_TYPES")
1041  {
1042  skipBlock(isBinary, 4, is, false); // according to http://www.vtk.org/VTK/img/file-formats.pdf CELL_TYPES only has one parameter (n)
1043  }
1044 
1046  // Point data
1047  else if(fieldName == "POINT_DATA")
1048  {
1049  int descriptorCount;
1050  is >> descriptorCount;
1051  if(pointCount != descriptorCount)
1052  throw runtime_error(string("The size of POINTS is different than POINT_DATA"));
1053  }
1055  // Field data is ignored
1056  else if (fieldName == "FIELD")
1057  {
1058  string fieldDataName;
1059  int fieldDataCount;
1060  is >> fieldDataName >> fieldDataCount;
1061 
1062  for (int f = 0; f < fieldDataCount; f++)
1063  {
1064  int numTuples;
1065  is >> name >> dim >> numTuples >> type;
1066 
1067  if(type == "vtkIdType") // skip that type
1068  {
1069  if(isBinary){
1070  is.seekg(dim * numTuples * 4, std::ios_base::cur);
1071  } else {
1072  int t_val;
1073  for (int t = 0; t < dim * numTuples; t++ )
1074  {
1075  is >> t_val;
1076  }
1077  }
1078  }
1079  else if(!(type == "float" || type == "double"))
1080  throw runtime_error(string("Field " + fieldName + " is " + type + " but can only be of type double or float"));
1081 
1082 
1083  Matrix descriptor(dim, pointCount);
1084  readVtkData(type, isBinary, descriptor.transpose(), is);
1085  loadedPoints.addDescriptor(name, descriptor);
1086  }
1087  }
1088  else if(fieldName == "METADATA") // Skip METADATA block
1089  {
1090  safeGetLine(is, line);
1091  safeGetLine(is, line);
1092  while(!line.empty())
1093  {
1094  safeGetLine(is, line);
1095  }
1096  }
1097  else // Load descriptors or time
1098  {
1099 
1100  // label name
1101  is >> name;
1102 
1103  bool isTimeSec = false;
1104  bool isTimeNsec = false;
1105 
1106 
1107  if(boost::algorithm::ends_with(name, "_splitTime_high32"))
1108  {
1109  isTimeSec = true;
1110  boost::algorithm::erase_last(name, "_splitTime_high32");
1111  }
1112 
1113  if(boost::algorithm::ends_with(name, "_splitTime_low32"))
1114  {
1115  isTimeNsec = true;
1116  boost::algorithm::erase_last(name, "_splitTime_low32");
1117  }
1118 
1119 
1120  bool skipLookupTable = false;
1121  bool isColorScalars = false;
1122  if(fieldName == "SCALARS")
1123  {
1124  dim = 1;
1125  is >> type;
1126  skipLookupTable = true;
1127  }
1128  else if(fieldName == "VECTORS")
1129  {
1130  dim = 3;
1131  is >> type;
1132  }
1133  else if(fieldName == "TENSORS")
1134  {
1135  dim = 9;
1136  is >> type;
1137  }
1138  else if(fieldName == "NORMALS")
1139  {
1140  dim = 3;
1141  is >> type;
1142  }
1143  else if(fieldName == "COLOR_SCALARS")
1144  {
1145  is >> dim;
1146  type = "float";
1147  isColorScalars = true;
1148  }
1149  else
1150  throw runtime_error(string("Unknown field name " + fieldName + ", expecting SCALARS, VECTORS, TENSORS, NORMALS or COLOR_SCALARS."));
1151 
1152 
1153  safeGetLine(is, line); // remove rest of the parameter line including its line end;
1154 
1155 
1156  // Load time data
1157  if(isTimeSec || isTimeNsec)
1158  {
1159  // Skip LOOKUP_TABLE line
1160  if(skipLookupTable)
1161  {
1162  safeGetLine(is, line);
1163  }
1164 
1165  typename std::map<std::string, SplitTime>::iterator it;
1166 
1167  it = labelledSplitTime.find(name);
1168  // reserve memory
1169  if(it == labelledSplitTime.end())
1170  {
1171  SplitTime t;
1172  t.high32 = Eigen::Matrix<unsigned int, Eigen::Dynamic, Eigen::Dynamic> (dim, pointCount);
1173  t.low32 = t.high32;
1174  labelledSplitTime[name] = t;
1175  }
1176 
1177  // Load seconds
1178  if(isTimeSec)
1179  {
1180  assert(labelledSplitTime[name].isHigh32Found == false);
1181  readVtkData(type, isBinary, labelledSplitTime[name].high32.transpose(), is);
1182  labelledSplitTime[name].isHigh32Found = true;
1183  }
1184 
1185  // Load nano seconds
1186  if(isTimeNsec)
1187  {
1188  assert(labelledSplitTime[name].isLow32Found == false);
1189  readVtkData(type, isBinary, labelledSplitTime[name].low32.transpose(), is);
1190  labelledSplitTime[name].isLow32Found = true;
1191  }
1192  }
1193  else
1194  {
1195 
1196  Matrix descriptorData(dim, pointCount);
1197 
1198  if(isColorScalars && isBinary)
1199  {
1200  std::vector<unsigned char> buffer(dim);
1201  for (int i = 0; i < pointCount; ++i){
1202  is.read(reinterpret_cast<char *>(&buffer.front()), dim);
1203  for(int r=0; r < dim; ++r){
1204  descriptorData(r, i) = buffer[r] / static_cast<T>(255.0);
1205  }
1206  }
1207  }
1208  else
1209  {
1210  if(!(type == "float" || type == "double"))
1211  throw runtime_error(string("Field " + fieldName + " is " + type + " but can only be of type double or float."));
1212 
1213  // Skip LOOKUP_TABLE line
1214  if(skipLookupTable)
1215  {
1216  safeGetLine(is, line);
1217  }
1218  readVtkData(type, isBinary, descriptorData.transpose(), is);
1219  }
1220  loadedPoints.addDescriptor(name, descriptorData);
1221  }
1222  }
1223  }
1224 
1225  // Combine time and add to point cloud
1226  typename std::map<std::string, SplitTime>::iterator it;
1227  for(it=labelledSplitTime.begin(); it!=labelledSplitTime.end(); it++)
1228  {
1229  // Confirm that both parts were loaded
1230  if(it->second.isHigh32Found == false)
1231  {
1232  throw runtime_error(string("Missing time field representing the higher 32 bits. Expecting SCALARS with name " + it->first + "_splitTime_high32 in the VTK file."));
1233  }
1234 
1235  if(it->second.isLow32Found == false)
1236  {
1237  throw runtime_error(string("Missing time field representing the lower 32 bits. Expecting SCALARS with name " + it->first + "_splitTime_low32 in the VTK file."));
1238  }
1239 
1240  // Loop through points
1241  Int64Matrix timeData(1,pointCount);
1242  for(int i=0; i<it->second.high32.cols(); i++)
1243  {
1244 
1245  timeData(0,i) = (((std::int64_t) it->second.high32(0,i)) << 32) | ((std::int64_t) it->second.low32(0,i));
1246  }
1247 
1248  loadedPoints.addTime(it->first, timeData);
1249  }
1250 
1251  return loadedPoints;
1252 }
1253 
1254 template
1255 PointMatcherIO<float>::DataPoints PointMatcherIO<float>::loadVTK(const std::string& fileName);
1256 template
1257 PointMatcherIO<double>::DataPoints PointMatcherIO<double>::loadVTK(const std::string& fileName);
1258 
1259 
1261 template<typename T>
1262 void PointMatcherIO<T>::saveVTK(const DataPoints& data, const std::string& fileName, bool binary, unsigned precision)
1263 {
1264  typedef typename InspectorsImpl<T>::VTKFileInspector VTKInspector;
1265 
1266  Parametrizable::Parameters param;
1267  boost::assign::insert(param) ("baseFileName", "");
1268  boost::assign::insert(param) ("writeBinary", toParam(binary));
1269  boost::assign::insert(param) ("precision", toParam(precision));
1270  VTKInspector vtkInspector(param);
1271  vtkInspector.dumpDataPoints(data, fileName);
1272 }
1273 
1274 
1275 template
1276 void PointMatcherIO<float>::saveVTK(const PointMatcherIO<float>::DataPoints& data, const std::string& fileName, bool binary, unsigned precision);
1277 template
1278 void PointMatcherIO<double>::saveVTK(const PointMatcher<double>::DataPoints& data, const std::string& fileName, bool binary, unsigned precision);
1279 
1288 template<typename T>
1289 typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPLY(const std::string& fileName)
1290 {
1291  ifstream ifs(fileName.c_str());
1292  if (!ifs.good())
1293  throw runtime_error(string("Cannot open file ") + fileName);
1294  return loadPLY(ifs);
1295 }
1296 
1297 template
1298 PointMatcherIO<float>::DataPoints PointMatcherIO<float>::loadPLY(const string& fileName);
1299 template
1300 PointMatcherIO<double>::DataPoints PointMatcherIO<double>::loadPLY(const string& fileName);
1301 
1304 template <typename T>
1305 typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPLY(std::istream& is)
1306 {
1307  class Elements : public vector<PLYElement*>{
1308  public:
1309  ~Elements(){
1310  for (typename vector<PLYElement*>::const_iterator it = this->begin(); it != this->end(); it++ )
1311  {
1312  delete *it;
1313  }
1314  }
1315  };
1316 
1317  /*
1318  Steps:
1319  1- PARSE PLY HEADER
1320  2- ASSIGN PLY PROPERTIES TO DATAPOINTS ROWS
1321  3- Reserve memory for a DataPoints
1322  4- Parse PLY vertex to appropriate DataPoints cols and rows
1323  5- Assemble final DataPoints
1324 
1325  PLY organisation:
1326 
1327  element 1 [name, size]
1328  - property 1 [type, name]
1329  - property 2
1330  - ...
1331  element 2
1332  -...
1333 
1334  */
1335 
1336 
1338  // 1- PARSE PLY HEADER
1339  bool format_defined = false;
1340  bool header_processed = false;
1341 
1342  Elements elements;
1343  PLYElementF element_f; // factory
1344  PLYElement* current_element = NULL;
1345  bool skip_props = false; // flag to skip properties if element is not supported
1346  unsigned elem_offset = 0; // keep track of line position of elements that are supported
1347  string line;
1348  safeGetLine(is, line);
1349 
1350  if (line.find("ply") != 0) {
1351  throw runtime_error(string("PLY parse error: wrong magic header, found <") + line + string(">"));
1352  }
1353 
1354  while (!header_processed)
1355  {
1356  if(!safeGetLine(is, line))
1357  throw runtime_error("PLY parse error: reached end of file before end of header definition");
1358 
1359 
1360  if ( line.empty() )
1361  continue;
1362  istringstream stringstream (line);
1363 
1364  string keyword;
1365  stringstream >> keyword;
1366 
1367  // ignore comment
1368  if (keyword == "comment") {
1369  continue;
1370  }
1371 
1372  // We only support ascii and ply version 1.0
1373  if (keyword == "format")
1374  {
1375  if (format_defined)
1376  throw runtime_error("PLY parse error: format already defined");
1377 
1378  string format_str, version_str;
1379  stringstream >> format_str >> version_str;
1380 
1381  if (format_str != "ascii" && format_str != "binary_little_endian" && format_str != "binary_big_endian")
1382  throw runtime_error(string("PLY parse error: format <") + format_str + string("> is not supported"));
1383 
1384  if (format_str == "binary_little_endian" || format_str == "binary_big_endian")
1385  throw runtime_error(string("PLY parse error: binary PLY files are not supported"));
1386  if (version_str != "1.0")
1387  {
1388  throw runtime_error(string("PLY parse error: version <") + version_str + string("> of ply is not supported"));
1389  }
1390 
1391  format_defined = true;
1392 
1393  }
1394  else if (keyword == "element")
1395  {
1396 
1397 
1398  string elem_name, elem_num_s;
1399  stringstream >> elem_name >> elem_num_s;
1400 
1401  unsigned elem_num;
1402  try
1403  {
1404  elem_num = boost::lexical_cast<unsigned>(elem_num_s);
1405  }
1406  catch (boost::bad_lexical_cast&)
1407  {
1408  throw runtime_error(string("PLY parse error: bad number of elements ") + elem_num_s + string(" for element ") + elem_name);
1409  }
1410 
1411  if (element_f.elementSupported(elem_name))
1412  {
1413  // Initialize current element
1414  PLYElement* elem = element_f.createElement(elem_name, elem_num, elem_offset);
1415  current_element = elem;
1416 
1417  // check that element is not already defined
1418  for (typename Elements::const_iterator it = elements.begin(); it != elements.end(); it++ )
1419  {
1420  if (**it == *elem) {
1421  delete elem;
1422  throw runtime_error(string("PLY parse error: element: ") + elem_name + string( "is already defined"));
1423  }
1424  }
1425  elements.push_back(elem);
1426  skip_props = false;
1427  }
1428  else
1429  {
1430  LOG_WARNING_STREAM("PLY parse warning: element " << elem_name << " not supported. Skipping.");
1431  skip_props = true;
1432  }
1433 
1434  elem_offset += elem_num;
1435  }
1436  else if (keyword == "property")
1437  {
1438  if (current_element == NULL)
1439  {
1440  throw runtime_error("PLY parse error: property listed without defining an element");
1441  }
1442 
1443  if (skip_props)
1444  continue;
1445 
1446  string next, prop_type, prop_name;
1447  stringstream >> next;
1448 
1449  // PLY list property
1450  if (next == "list")
1451  {
1452  string prop_idx_type;
1453  stringstream >> prop_idx_type >> prop_type >> prop_name;
1454 
1455  PLYProperty list_prop(prop_idx_type, prop_type, prop_name, current_element->total_props);
1456 
1457  current_element->properties.push_back(list_prop);
1458  }
1459  // PLY regular property
1460  else
1461  {
1462  prop_type = next;
1463  stringstream >> prop_name;
1464  PLYProperty prop(prop_type, prop_name, current_element->total_props);
1465 
1466  current_element->properties.push_back(prop);
1467  }
1468 
1469  current_element->total_props++;
1470  }
1471  else if (keyword == "end_header")
1472  {
1473  if (!format_defined)
1474  {
1475  throw runtime_error(string("PLY parse error: format not defined in header"));
1476  }
1477 
1478  if (elements.size() == 0)
1479  {
1480  throw runtime_error(string("PLY parse error: no elements defined in header"));
1481  }
1482 
1483  header_processed = true;
1484  }
1485  }
1486 
1488  // 2- ASSIGN PLY PROPERTIES TO DATAPOINTS ROWS
1489 
1490  // Fetch vertex
1491  PLYElement* vertex = elements[0];
1492 
1493  if(vertex->name != "vertex")
1494  {
1495  throw runtime_error(string("PLY parse error: vertex should be the first element defined."));
1496  }
1497 
1498  // Fetch known features and descriptors
1499  const SupportedLabels & externalLabels = getSupportedExternalLabels();
1500 
1501  int rowIdFeatures = 0;
1502  int rowIdDescriptors = 0;
1503  int rowIdTime= 0;
1504 
1505  LabelGenerator featLabelGen, descLabelGen, timeLabelGen;
1506 
1507 
1508  // Loop through all known external names (ordered list)
1509  for(size_t i=0; i<externalLabels.size(); i++)
1510  {
1511  const SupportedLabel & supLabel = externalLabels[i];
1512 
1513  //Search if that feature exist
1514  for(it_PLYProp it=vertex->properties.begin(); it!=vertex->properties.end(); ++it)
1515  {
1516 
1517  if(supLabel.externalName == it->name)
1518  {
1519  it->pmType = supLabel.type;
1520 
1521  // Assign rowId in that order
1522  switch (supLabel.type)
1523  {
1524  case FEATURE:
1525  it->pmRowID = rowIdFeatures;
1526  featLabelGen.add(supLabel.internalName);
1527  rowIdFeatures++;
1528  break;
1529  case DESCRIPTOR:
1530  it->pmRowID = rowIdDescriptors;
1531  descLabelGen.add(supLabel.internalName);
1532  rowIdDescriptors++;
1533  break;
1534  case TIME:
1535  it->pmRowID = rowIdTime;
1536  timeLabelGen.add(supLabel.internalName);
1537  rowIdTime++;
1538  default:
1539  throw runtime_error(string("PLY Implementation Error: encounter a type different from FEATURE, DESCRIPTOR and TIME. Implementation not supported. See the definition of 'enum PMPropTypes'"));
1540  break;
1541  }
1542 
1543  // we stop searching once we have a match
1544  break;
1545  }
1546  }
1547  }
1548 
1549  // loop through the remaining UNSUPPORTED labels and assigned them to a single descriptor row
1550  for(it_PLYProp it=vertex->properties.begin(); it!=vertex->properties.end(); ++it)
1551  {
1552  if(it->pmType == UNSUPPORTED)
1553  {
1554  it->pmType = DESCRIPTOR; // force descriptor
1555  it->pmRowID = rowIdDescriptors;
1556  descLabelGen.add(it->name); // keep original name
1557  rowIdDescriptors++;
1558  }
1559  }
1560 
1562  // 3- RESERVE DATAPOINTS MEMORY
1563 
1564  const unsigned int featDim = featLabelGen.getLabels().totalDim();
1565  const unsigned int descDim = descLabelGen.getLabels().totalDim();
1566  const unsigned int timeDim = timeLabelGen.getLabels().totalDim();
1567  const unsigned int nbPoints = vertex->num;
1568 
1569  Matrix features = Matrix(featDim, nbPoints);
1570  Matrix descriptors = Matrix(descDim, nbPoints);
1571  Int64Matrix times = Int64Matrix(timeDim, nbPoints);
1572 
1573 
1575  // 4- PARSE PLY DATA (vertex)
1576  const int nbProp = vertex->total_props;
1577  const int nbValues = nbPoints*nbProp;
1578  int propID = 0;
1579  int col = 0;
1580  for(int i=0; i<nbValues; i++)
1581  {
1582  T value;
1583  if(!(is >> value))
1584  {
1585  throw runtime_error(
1586  (boost::format("PLY parse error: expected %1% values (%2% points with %3% properties) but only found %4% values.") % nbValues % nbPoints % nbProp % i).str());
1587  }
1588  else
1589  {
1590  const int row = vertex->properties[propID].pmRowID;
1591  const PMPropTypes type = vertex->properties[propID].pmType;
1592 
1593  // rescale color from [0,254] to [0, 1[
1594  // FIXME: do we need that?
1595  if (vertex->properties[propID].name == "red" || vertex->properties[propID].name == "green" || vertex->properties[propID].name == "blue" || vertex->properties[propID].name == "alpha") {
1596  value /= 255.0;
1597  }
1598 
1599  switch (type)
1600  {
1601  case FEATURE:
1602  features(row, col) = value;
1603  break;
1604  case DESCRIPTOR:
1605  descriptors(row, col) = value;
1606  break;
1607  case TIME:
1608  times(row, col) = value;
1609  break;
1610  case UNSUPPORTED:
1611  throw runtime_error("Implementation error in loadPLY(). This should not throw.");
1612  break;
1613  }
1614 
1615  ++propID;
1616 
1617  if(propID >= nbProp)
1618  {
1619  propID = 0;
1620  ++col;
1621  }
1622  }
1623  }
1624 
1625 
1626 
1628  // 5- ASSEMBLE FINAL DATAPOINTS
1629 
1630  DataPoints loadedPoints(features, featLabelGen.getLabels());
1631 
1632  if (descriptors.rows() > 0)
1633  {
1634  loadedPoints.descriptors = descriptors;
1635  loadedPoints.descriptorLabels = descLabelGen.getLabels();
1636  }
1637 
1638  if(times.rows() > 0)
1639  {
1640  loadedPoints.times = times;
1641  loadedPoints.timeLabels = timeLabelGen.getLabels();
1642  }
1643 
1644  // Ensure homogeous coordinates
1645  if(!loadedPoints.featureExists("pad"))
1646  {
1647  loadedPoints.addFeature("pad", Matrix::Ones(1,features.cols()));
1648  }
1649 
1650  return loadedPoints;
1651 
1652 }
1653 
1654 template<typename T>
1655 void PointMatcherIO<T>::savePLY(const DataPoints& data,
1656  const std::string& fileName, unsigned precision)
1657 {
1658  //typedef typename DataPoints::Labels Labels;
1659 
1660  ofstream ofs(fileName.c_str());
1661  if (!ofs.good())
1662  throw runtime_error(string("Cannot open file ") + fileName);
1663 
1664  ofs.precision(precision);
1665  const int pointCount(data.features.cols());
1666  const int featCount(data.features.rows());
1667  const int descRows(data.descriptors.rows());
1668 
1669 
1670  if (pointCount == 0)
1671  {
1672  LOG_WARNING_STREAM("Warning, no points, doing nothing");
1673  return;
1674  }
1675 
1676  ofs << "ply\n" <<"format ascii 1.0\n";
1677  ofs << "element vertex " << pointCount << "\n";
1678  for (int f=0; f <(featCount-1); f++)
1679  {
1680  ofs << "property float " << data.featureLabels[f].text << "\n";
1681  }
1682 
1683  for (size_t i = 0; i < data.descriptorLabels.size(); i++)
1684  {
1685  Label lab = data.descriptorLabels[i];
1686  for (size_t s = 0; s < lab.span; s++)
1687 
1688  {
1689  ofs << "property float " << getColLabel(lab,s) << "\n";
1690  }
1691  }
1692 
1693  ofs << "end_header\n";
1694 
1695  // write points
1696  for (int p = 0; p < pointCount; ++p)
1697  {
1698  for (int f = 0; f < featCount - 1; ++f)
1699  {
1700  ofs << data.features(f, p);
1701  if(!(f == featCount-2 && descRows == 0))
1702  ofs << " ";
1703  }
1704 
1705  bool datawithColor = data.descriptorExists("color");
1706  int colorStartingRow = data.getDescriptorStartingRow("color");
1707  int colorEndRow = colorStartingRow + data.getDescriptorDimension("color");
1708  for (int d = 0; d < descRows; ++d)
1709  {
1710  if (datawithColor && d >= colorStartingRow && d < colorEndRow) {
1711  ofs << static_cast<unsigned>(data.descriptors(d, p) * 255.0);
1712  } else {
1713  ofs << data.descriptors(d, p);
1714  }
1715  if(d != descRows-1)
1716  ofs << " ";
1717  }
1718  ofs << "\n";
1719  }
1720 
1721  ofs.close();
1722 }
1723 
1724 template
1725 void PointMatcherIO<float>::savePLY(const DataPoints& data, const std::string& fileName, unsigned precision);
1726 template
1727 void PointMatcherIO<double>::savePLY(const DataPoints& data, const std::string& fileName, unsigned precision);
1728 
1730 template<typename T>
1731 PointMatcherIO<T>::PLYProperty::PLYProperty(const std::string& type,
1732  const std::string& name, const unsigned pos) :
1733  name(name),
1734  type(type),
1735  pos(pos)
1736 {
1737  if (plyPropTypeValid(type))
1738  {
1739  is_list = false;
1740  }
1741  else
1742  {
1743  throw std::runtime_error(
1744  std::string("PLY parse error: property type ") + type
1745  + std::string(" for property ") + name
1746  + std::string(" is invalid"));
1747  }
1748 
1749  pmType = UNSUPPORTED;
1750  pmRowID = -1;
1751 
1752 }
1753 
1755 template<typename T>
1756 PointMatcherIO<T>::PLYProperty::PLYProperty(const std::string& idx_type,
1757  const std::string& type, const std::string& name, const unsigned pos) :
1758  name(name),
1759  type(type),
1760  idx_type(idx_type),
1761  pos(pos)
1762 {
1763  if (plyPropTypeValid(idx_type) && plyPropTypeValid(type))
1764  {
1765  is_list = true;
1766  }
1767  else
1768  {
1769  throw std::runtime_error(
1770  std::string("PLY parse error: property list type ") + idx_type
1771  + std::string(" ") + type
1772  + std::string(" for property ") + name
1773  + std::string(" is invalid"));
1774  }
1775 
1776  pmType = UNSUPPORTED;
1777  pmRowID = -1;
1778 }
1779 
1780 template
1781 class PointMatcherIO<float>::PLYElement;
1782 template
1783 class PointMatcherIO<double>::PLYElement;
1784 
1785 template
1786 struct PointMatcherIO<float>::PLYProperty;
1787 template
1788 struct PointMatcherIO<double>::PLYProperty;
1789 
1790 
1791 template <typename T>
1792 typename PointMatcherIO<T>::PLYElementF::ElementTypes PointMatcherIO<T>::PLYElementF::getElementType(const std::string& elem_name)
1793 {
1794  string lc = elem_name;
1795  boost::algorithm::to_lower(lc);
1796  if (lc == "vertex")
1797  {
1798  return VERTEX;
1799  }
1800  else
1801  {
1802  return UNSUPPORTED;
1803  }
1804 }
1805 
1806 
1807 template <typename T>
1808 bool PointMatcherIO<T>::PLYElementF::elementSupported(const std::string& elem_name)
1809 {
1810  return getElementType(elem_name) != UNSUPPORTED;
1811 }
1812 
1813 
1814 template<typename T>
1815 typename PointMatcherIO<T>::PLYElement* PointMatcherIO<T>::PLYElementF::createElement(
1816  const std::string& elem_name, const int elem_num, const unsigned offset) {
1817  ElementTypes type = getElementType(elem_name);
1818  if (type == VERTEX)
1819  return new PLYVertex(elem_num, offset);
1820  else
1821  return NULL;
1822 }
1823 
1824 
1825 template<typename T>
1826 bool PointMatcherIO<T>::plyPropTypeValid(const std::string& type) {
1827  return (type == "char" || type == "uchar" || type == "short"
1828  || type == "ushort" || type == "int" || type == "uint"
1829  || type == "float" || type == "double");
1830 }
1831 
1832 
1833 template <typename T>
1834 bool PointMatcherIO<T>::PLYElement::operator==(const PLYElement& rhs) const
1835 {
1836  return name == rhs.name;
1837 }
1838 
1839 
1840 template <typename T>
1841 bool PointMatcherIO<T>::PLYProperty::operator==(const PLYProperty& rhs) const
1842 {
1843  return name == rhs.name && type == rhs.type;
1844 }
1845 
1846 
1849 template<typename T>
1850 typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPCD(const string& fileName) {
1851  ifstream ifs(fileName.c_str());
1852  if (!ifs.good())
1853  throw runtime_error(string("Cannot open file ") + fileName);
1854  return loadPCD(ifs);
1855 }
1856 
1857 
1858 template
1859 PointMatcherIO<float>::DataPoints PointMatcherIO<float>::loadPCD(const string& fileName);
1860 template
1861 PointMatcherIO<double>::DataPoints PointMatcherIO<double>::loadPCD(const string& fileName);
1862 
1863 
1866 template<typename T>
1867 typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPCD(std::istream& is) {
1868 
1869 
1870  /*
1871  Steps:
1872  1- PARSE PCD HEADER
1873  2- ASSIGN PCD PROPERTIES TO DATAPOINTS ROWS
1874  3- Reserve memory for a DataPoints
1875  4- Parse PCD XXX to appropriate DataPoints cols and rows
1876  5- Assemble final DataPoints
1877 
1878  PCD organisation:
1879 
1880  # .PCD v.7 - Point Cloud Data file format
1881  VERSION number
1882  FIELDS prop1 prop2 prop3 ...
1883  SIZE nbBytes1 nbBytes2 nbBytes3
1884  TYPE type1 type2 type3
1885  COUNT nbDim1 nbDim2 nbDim3
1886  WIDTH w
1887  HEIGHT h
1888  VIEWPOINT 0 0 0 1 0 0 0
1889  POINTS size (should be w*h)
1890  DATA ascii or binary
1891  data1 data2 data3 ...
1892  data1 data2 data3 ...
1893  ...
1894 
1895  */
1896 
1897 
1899  // 1- PARSE PCD HEADER
1900 
1901  size_t lineNum = 0;
1902  PCDheader header;
1903 
1904  string line;
1905  while (safeGetLine(is, line))
1906  {
1907 
1908  // get rid of white spaces before/after
1909  boost::trim (line);
1910 
1911  // ignore comments or empty line
1912  if (line.substr(0,1) == "#" || line == "")
1913  {
1914  lineNum++;
1915  continue;
1916  }
1917 
1918  vector<string> tokens;
1919  boost::split(tokens, line, boost::is_any_of("\t\r "), boost::token_compress_on);
1920 
1921  string pcd_version_str;
1922  if (tokens[0] == "VERSION")
1923  {
1924  header.version = tokens[1];
1925 
1926  if (tokens[1] != "0.7" && tokens[1] != ".7")
1927  throw runtime_error("PCD Parse Error: Only PCD Version 0.7 is supported");
1928  }
1929 
1930  else if (tokens[0] == "FIELDS")
1931  {
1932  header.properties.resize(tokens.size() - 1);
1933 
1934  for (size_t i = 1; i < tokens.size(); i++)
1935  {
1936  header.properties[i-1].field = tokens[i];
1937  }
1938 
1939  }
1940 
1941  else if (tokens[0] == "SIZE")
1942  {
1943  if((tokens.size() - 1) != header.properties.size())
1944  throw runtime_error("PCD Parse Error: number of elements for SIZE must be the same as FIELDS");
1945 
1946  for (size_t i = 1; i < tokens.size(); i++)
1947  {
1948  const unsigned int size = boost::lexical_cast<unsigned int >(tokens[i]);
1949  header.properties[i-1].size = size;
1950  }
1951 
1952  }
1953 
1954  else if (tokens[0] == "TYPE")
1955  {
1956  if((tokens.size() - 1) != header.properties.size())
1957  throw runtime_error("PCD Parse Error: number of elements for TYPE must be the same as FIELDS");
1958 
1959  for (size_t i = 1; i < tokens.size(); i++)
1960  {
1961  const char type = boost::lexical_cast<char>(tokens[i]);
1962  header.properties[i-1].type = type;
1963  if (type != 'I' && type != 'U' && type != 'F')
1964  throw runtime_error("PCD Parse Error: invalid TYPE, it must be 'I', 'U', or 'F'");
1965  }
1966 
1967  }
1968 
1969  else if (tokens[0] == "COUNT")
1970  {
1971 
1972  if((tokens.size() - 1) != header.properties.size())
1973  throw runtime_error("PCD Parse Error: number of elements for COUNT must be the same as FIELDS");
1974 
1975  for (size_t i = 1; i < tokens.size(); i++)
1976  {
1977  const unsigned int count = boost::lexical_cast<unsigned int >(tokens[i]);
1978  header.properties[i-1].count = count;
1979  }
1980 
1981  }
1982 
1983  else if (tokens[0] == "WIDTH")
1984  {
1985  try
1986  {
1987  header.width = boost::lexical_cast<unsigned int >(tokens[1]);
1988  }
1989  catch (boost::bad_lexical_cast&)
1990  {
1991  throw runtime_error("PCD Parse Error: invalid WIDTH");
1992  }
1993 
1994  }
1995 
1996  else if (tokens[0] == "HEIGHT")
1997  {
1998  try
1999  {
2000  header.height= boost::lexical_cast<unsigned int >(tokens[1]);
2001  }
2002  catch (boost::bad_lexical_cast&)
2003  {
2004  throw runtime_error("PCD Parse Error: invalid HEIGHT");
2005  }
2006 
2007  }
2008 
2009  else if (tokens[0] == "VIEWPOINT")
2010  {
2011  if((tokens.size() - 1) != 7 )
2012  throw runtime_error("PCD Parse Error: number of elements for VIEWPOINT must be 7");
2013 
2014  for (size_t i = 1; i < tokens.size(); i++)
2015  {
2016  try
2017  {
2018  header.viewPoint(i-1, 0) = boost::lexical_cast<T>(tokens[i]);
2019  }
2020  catch (boost::bad_lexical_cast&)
2021  {
2022  stringstream ss;
2023  ss << "PCD Parse Error: invalid value(" << tokens[i] << ") of VIEWPOINT";
2024  throw runtime_error(ss.str());
2025  }
2026  }
2027 
2028  }
2029 
2030  else if (tokens[0] == "POINTS")
2031  {
2032  try
2033  {
2034  header.nbPoints = boost::lexical_cast<unsigned int>(tokens[1]);
2035  }
2036  catch (boost::bad_lexical_cast&)
2037  {
2038  stringstream ss;
2039  ss << "PCD Parse Error: the value in the element POINTS (" << tokens[1] << ") could not be cast as unsigned int";
2040  throw runtime_error(ss.str());
2041  }
2042  }
2043 
2044  else if (tokens[0] == "DATA")
2045  {
2046  header.dataType= tokens[1];
2047 
2048  if (header.dataType == "ascii")
2049  {
2050  // DATA is the last element of the header, we exit the loop
2051  break;
2052  }
2053  else if(header.dataType == "binary")
2054  {
2055  throw runtime_error("PCD Implementation Error: the option for DATA binary is not implemented yet");
2056  }
2057  else
2058  {
2059  stringstream ss;
2060  ss << "PCD Parse Error: the value in the element DATA (" << tokens[1] << ") must be ascii or binary";
2061  throw runtime_error(ss.str());
2062  }
2063 
2064  }
2065 
2066  lineNum++;
2067  }
2068  // Extra check for the number of points
2069  if (header.properties.size() == 0)
2070  throw runtime_error("PCD Parse Error: no FIELDS were find in the header");
2071 
2072 
2073  // Extra check for the number of points
2074  if (header.width * header.height != header.nbPoints)
2075  throw runtime_error("PCD Parse Error: POINTS field does not match WIDTH and HEIGHT fields");
2076 
2078  // 2- ASSIGN PCD PROPERTIES TO DATAPOINTS ROWS
2079 
2080  // Fetch known features and descriptors
2081  const SupportedLabels & externalLabels = getSupportedExternalLabels();
2082 
2083  int rowIdFeatures = 0;
2084  int rowIdDescriptors = 0;
2085  int rowIdTime= 0;
2086 
2087  LabelGenerator featLabelGen, descLabelGen, timeLabelGen;
2088 
2089  // Loop through all known external names (ordered list)
2090  for(size_t i=0; i<externalLabels.size(); i++)
2091  {
2092  const SupportedLabel & supLabel = externalLabels[i];
2093 
2094  //Search if that feature exist
2095  for(size_t i=0; i < header.properties.size(); i++)
2096  {
2097  const PCDproperty prop = header.properties[i];
2098 
2099  //TODO: prop.field == "rgb" might be tricky
2100 
2101  if(supLabel.externalName == prop.field)
2102  {
2103  header.properties[i].pmType = supLabel.type;
2104 
2105  // Assign rowId in that order
2106  switch (supLabel.type)
2107  {
2108  case FEATURE:
2109  header.properties[i].pmRowID = rowIdFeatures;
2110  featLabelGen.add(supLabel.internalName);
2111  rowIdFeatures++;
2112  if(prop.count != 1)
2113  {
2114  stringstream ss;
2115  ss << "PCD Parse Error: the field " << prop.field << " must have a count of 1";
2116  throw runtime_error(ss.str());
2117  }
2118  break;
2119  case DESCRIPTOR:
2120  header.properties[i].pmRowID = rowIdDescriptors;
2121  descLabelGen.add(supLabel.internalName, prop.count);
2122  rowIdDescriptors += prop.count;
2123  break;
2124  case TIME:
2125  header.properties[i].pmRowID = rowIdTime;
2126  timeLabelGen.add(supLabel.internalName, prop.count);
2127  rowIdTime += prop.count;
2128  default:
2129  throw runtime_error(string("PCD Implementation Error: encounter a type different from FEATURE, DESCRIPTOR and TIME. Implementation not supported. See the definition of 'enum PMPropTypes'"));
2130  break;
2131  }
2132 
2133  // we stop searching once we have a match
2134  break;
2135  }
2136  }
2137  }
2138 
2139  // loop through the remaining UNSUPPORTED labels and assigned them to a single descriptor row
2140  for(size_t i=0; i < header.properties.size(); i++)
2141  {
2142  const PCDproperty prop = header.properties[i];
2143  if(prop.pmType == UNSUPPORTED)
2144  {
2145  header.properties[i].pmType = DESCRIPTOR; // force descriptor
2146  header.properties[i].pmRowID = rowIdDescriptors;
2147  descLabelGen.add(prop.field, prop.count); // keep original name
2148  rowIdDescriptors += prop.count;
2149  }
2150  }
2151 
2152 
2154  // 3- RESERVE DATAPOINTS MEMORY
2155 
2156  const unsigned int featDim = featLabelGen.getLabels().totalDim();
2157  const unsigned int descDim = descLabelGen.getLabels().totalDim();
2158  const unsigned int timeDim = timeLabelGen.getLabels().totalDim();
2159  const unsigned int totalDim = featDim + descDim + timeDim;
2160  const unsigned int nbPoints = header.nbPoints;
2161 
2162  Matrix features = Matrix(featDim, nbPoints);
2163  Matrix descriptors = Matrix(descDim, nbPoints);
2164  Int64Matrix times = Int64Matrix(timeDim, nbPoints);
2165 
2166 
2168  // 4- PARSE PCD DATA
2169 
2170  size_t col = 0; // point count
2171  while (safeGetLine(is, line))
2172  {
2173 
2174  // get rid of white spaces before/after
2175  boost::trim (line);
2176 
2177  // ignore comments or empty line
2178  if (line.substr(0,1) == "#" || line == "")
2179  {
2180  lineNum++;
2181  continue;
2182  }
2183 
2184  vector<string> tokens;
2185  boost::split(tokens, line, boost::is_any_of("\t\r "), boost::token_compress_on);
2186 
2187 
2188  if (tokens.size() != totalDim)
2189  throw runtime_error(string("PCD Parse Error: number of data columns does not match number of fields at line: ") + boost::lexical_cast<string>(lineNum));
2190 
2191  unsigned int fileCol = 0;
2192  for(size_t i=0; i<header.properties.size(); i++)
2193  {
2194  const unsigned int count = header.properties[i].count;
2195  const unsigned int row = header.properties[i].pmRowID;
2196  const PMPropTypes type = header.properties[i].pmType;
2197 
2198 
2199  for(size_t j=0; j<count; j++)
2200  {
2201  switch (type)
2202  {
2203  case FEATURE:
2204  features(row+j, col) = boost::lexical_cast<T>(tokens[fileCol]);
2205  break;
2206  case DESCRIPTOR:
2207  descriptors(row+j, col) = boost::lexical_cast<T>(tokens[fileCol]);
2208  break;
2209  case TIME:
2210  times(row+j, col) = boost::lexical_cast<std::int64_t>(tokens[fileCol]);
2211  break;
2212  case UNSUPPORTED:
2213  throw runtime_error("Implementation error in loadPCD(). This should not throw.");
2214  break;
2215  }
2216 
2217  fileCol++;
2218  }
2219 
2220  }
2221 
2222  col++;
2223  lineNum++;
2224  }
2225 
2226  if (col != nbPoints)
2227  {
2228  stringstream ss;
2229  ss << "PCD Parse Error: the number of points in the file (" << col << ") is less than the specified number of points (" << nbPoints << ")";
2230  throw runtime_error(ss.str());
2231  }
2232 
2234  // 5- ASSEMBLE FINAL DATAPOINTS
2235 
2236  DataPoints loadedPoints(features, featLabelGen.getLabels());
2237 
2238  if (descriptors.rows() > 0)
2239  {
2240  loadedPoints.descriptors = descriptors;
2241  loadedPoints.descriptorLabels = descLabelGen.getLabels();
2242  }
2243 
2244  if(times.rows() > 0)
2245  {
2246  loadedPoints.times = times;
2247  loadedPoints.timeLabels = timeLabelGen.getLabels();
2248  }
2249 
2250  // Ensure homogeous coordinates
2251  if(!loadedPoints.featureExists("pad"))
2252  {
2253  loadedPoints.addFeature("pad", Matrix::Ones(1,features.cols()));
2254  }
2255 
2256  return loadedPoints;
2257 }
2258 
2259 template<typename T>
2260 void PointMatcherIO<T>::savePCD(const DataPoints& data,
2261  const std::string& fileName, unsigned precision) {
2262  ofstream ofs(fileName.c_str());
2263  if (!ofs.good())
2264  throw runtime_error(string("Cannot open file ") + fileName);
2265  ofs.precision(precision);
2266  const int pointCount(data.features.cols());
2267  const int featCount(data.features.rows());
2268  const int descRows(data.descriptors.rows());
2269  const int descCount(data.descriptorLabels.size());
2270 
2271  if (pointCount == 0)
2272  {
2273  LOG_WARNING_STREAM("Warning, no points, doing nothing");
2274  return;
2275  }
2276 
2277  ofs << "# .PCD v.7 - Point Cloud Data file format\n" <<"VERSION .7\n";
2278  ofs << "FIELDS";
2279 
2280  for (int f=0; f < (featCount - 1); f++)
2281  {
2282  ofs << " " << data.featureLabels[f].text;
2283  }
2284 
2285  if (descRows == 0)
2286  ofs << "\n";
2287  else
2288  {
2289  for (int i = 0; i < descCount; i++)
2290  {
2291  ofs << " " << data.descriptorLabels[i].text;
2292  }
2293  ofs << "\n";
2294  }
2295 
2296  ofs << "SIZE";
2297  for (int i =0; i < featCount - 1 + descCount; i++)
2298  {
2299  ofs << " 4"; // for float
2300  }
2301  ofs << "\n";
2302 
2303  ofs << "TYPE";
2304  for (int i =0; i < featCount - 1 + descCount; i++)
2305  {
2306  ofs << " F"; // for float
2307  }
2308  ofs << "\n";
2309 
2310  ofs << "COUNT";
2311  for (int f = 0; f < featCount - 1 ; f++ )
2312  ofs << " 1";
2313  if (descCount == 0)
2314  ofs << "\n";
2315  else
2316  {
2317  for (int i = 0; i < descCount; i++)
2318  {
2319  ofs << " " << data.descriptorLabels[i].span;
2320  }
2321  ofs << "\n";
2322  }
2323 
2324  ofs << "WIDTH " << pointCount << "\n";
2325  ofs << "HEIGHT 1\n";
2326  ofs << "POINTS " << pointCount << "\n";
2327  ofs << "DATA ascii\n";
2328 
2329  // write points
2330  for (int p = 0; p < pointCount; ++p)
2331  {
2332  for (int f = 0; f < featCount - 1; ++f)
2333  {
2334  ofs << data.features(f, p);
2335  if(!(f == featCount-2 && descRows == 0))
2336  ofs << " ";
2337  }
2338  for (int d = 0; d < descRows; ++d)
2339  {
2340  ofs << data.descriptors(d, p);
2341  if(d != descRows-1)
2342  ofs << " ";
2343  }
2344  ofs << "\n";
2345  }
2346 
2347  ofs.close();
2348 }
2349 
2350 template
2351 void PointMatcherIO<float>::savePCD(const DataPoints& data, const std::string& fileName, unsigned precision);
2352 template
2353 void PointMatcherIO<double>::savePCD(const DataPoints& data, const std::string& fileName, unsigned precision);
2354 
2355 
2356 
PointMatcherIO::PCDheader::nbPoints
unsigned int nbPoints
number of points, same as width*height
Definition: IO.h:395
PointMatcherIO::GenericInputHeader
Helper structure designed to parse file headers.
Definition: IO.h:87
PointMatcherIO::PLYProperty::idx_type
std::string idx_type
for list properties, type of number of elements
Definition: IO.h:272
PointMatcherSupport::toParam
std::string toParam(const S &value)
Return the string value using lexical_cast.
Definition: Parametrizable.h:123
PointMatcherIO::PLYVertex
Implementation of PLY vertex element.
Definition: IO.h:334
PointMatcherIO::UNSUPPORTED
@ UNSUPPORTED
Definition: IO.h:69
PointMatcherIO::SplitTime::high32
Eigen::Matrix< unsigned int, Eigen::Dynamic, Eigen::Dynamic > high32
Matrix containing file data representing the high 32 bits.
Definition: IO.h:203
PointMatcher::DataPoints::descriptorLabels
Labels descriptorLabels
labels of descriptors
Definition: PointMatcher.h:334
PointMatcherIO::PLYProperty::is_list
bool is_list
member is true of property is a list
Definition: IO.h:274
PointMatcherIO::loadVTK
static DataPoints loadVTK(const std::string &fileName)
Load point cloud from a file as VTK.
Definition: pointmatcher/IO.cpp:911
PointMatcher::DataPoints::Label::text
std::string text
name of the label
Definition: PointMatcher.h:223
PointMatcherSupport::validateFile
void validateFile(const std::string &fileName)
Throw a runtime_error exception if fileName cannot be opened.
Definition: pointmatcher/IO.cpp:355
PointMatcherIO::PLYElement::name
std::string name
name identifying the PLY element
Definition: IO.h:305
PointMatcherIO::PCDheader::height
unsigned int height
height of sensor matrix
Definition: IO.h:393
PointMatcherIO::LabelGenerator::getLabels
Labels getLabels() const
Return the vector of labels used to build a DataPoints.
Definition: pointmatcher/IO.cpp:457
PointMatcherIO::UNSTRUCTURED_GRID
@ UNSTRUCTURED_GRID
Definition: IO.h:194
PointMatcher::DataPoints::addTime
void addTime(const std::string &name, const Int64Matrix &newTime)
Add a time by name, remove first if already exists.
Definition: pointmatcher/DataPoints.cpp:637
build_map.T
T
Definition: build_map.py:34
PointMatcherIO::FEATURE
@ FEATURE
Definition: IO.h:66
PointMatcherIO::PCDheader::version
std::string version
Version of the PCD file used.
Definition: IO.h:390
PointMatcher::DataPoints::Labels
A vector of Label.
Definition: PointMatcher.h:229
icp_customized.name
string name
Definition: icp_customized.py:45
PointMatcherSupport::isBigEndian
const bool isBigEndian
true if platform is big endian
Definition: pointmatcher/IO.cpp:62
PointMatcherIO::PCDheader
All information contained in the header of a PCD file.
Definition: IO.h:388
PointMatcher::DataPoints::addFeature
void addFeature(const std::string &name, const Matrix &newFeature)
Add a feature by name, remove first if already exists. The 'pad' field will stay at the end for homog...
Definition: pointmatcher/DataPoints.cpp:436
PointMatcherIO::DESCRIPTOR
@ DESCRIPTOR
Definition: IO.h:67
PointMatcherIO::FileInfoVector::getTransform
TransformationParameters getTransform(const PointMatcherSupport::CsvElements &data, const std::string &prefix, unsigned dim, unsigned line)
Return the transformation named prefix from data.
Definition: pointmatcher/IO.cpp:334
PointMatcherIO::PCDproperty::pmType
PMPropTypes pmType
type of information in PointMatcher
Definition: IO.h:372
PointMatcherSupport::CsvElements
std::map< std::string, std::vector< std::string > > CsvElements
Data from a CSV file.
Definition: PointMatcher.h:125
PointMatcherIO::FileInfo::groundTruthTransformation
TransformationParameters groundTruthTransformation
matrix of the ground-truth transform
Definition: IO.h:238
PointMatcherIO::SupportedLabel::externalName
std::string externalName
name used in external format
Definition: IO.h:76
PointMatcherIO::FileInfo::readingFileName
std::string readingFileName
file name of the reading point cloud
Definition: IO.h:234
PointMatcherIO::PLYElement::num
unsigned num
number of occurences of the element
Definition: IO.h:306
PointMatcherIO::PCDheader::properties
std::vector< PCDproperty > properties
vector of properties
Definition: IO.h:391
PointMatcherSupport::readVtkData
std::istream & readVtkData(bool readBinary, MatrixRef into, std::istream &in)
Definition: IOFunctions.h:108
PointMatcher::DataPoints::addDescriptor
void addDescriptor(const std::string &name, const Matrix &newDescriptor)
Add a descriptor by name, remove first if already exists.
Definition: pointmatcher/DataPoints.cpp:533
PointMatcherIO::SplitTime
Storage for time loaded separatly.
Definition: IO.h:198
PointMatcherIO::savePLY
static void savePLY(const DataPoints &data, const std::string &fileName, unsigned precision)
save datapoints to PLY point cloud format
Definition: pointmatcher/IO.cpp:1655
PointMatcherIO::PLYProperty
Interface for PLY property.
Definition: IO.h:267
icp_customized.trim
trim
Definition: icp_customized.py:76
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:130
PointMatcherIO::PLYProperty::operator==
bool operator==(const PLYProperty &other) const
list prop ctor
Definition: pointmatcher/IO.cpp:1841
PointMatcherPrivate.h
PointMatcherIO::SupportedLabel::SupportedLabel
SupportedLabel(const std::string &internalName, const std::string &externalName, const PMPropTypes &type)
Constructor.
Definition: pointmatcher/IO.cpp:418
PointMatcherIO::FileInfo::referenceFileName
std::string referenceFileName
file name of the reference point cloud
Definition: IO.h:235
PointMatcherIO::PCDheader::width
unsigned int width
width of sensor matrix
Definition: IO.h:392
PointMatcherIO::PLYElement
Interface for all PLY elements.
Definition: IO.h:302
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
PointMatcherIO::FileInfoVector::FileInfoVector
FileInfoVector()
Definition: pointmatcher/IO.cpp:180
PointMatcherIO::PLYProperty::type
std::string type
type of PLY property
Definition: IO.h:271
PointMatcherIO::SupportedVTKDataTypes
SupportedVTKDataTypes
Enumeration of legacy VTK data types that can be parsed.
Definition: IO.h:191
PointMatcherIO::TIME
@ TIME
Definition: IO.h:68
testing::internal::string
::std::string string
Definition: gtest.h:1979
PointMatcher::DataPoints::timeLabels
Labels timeLabels
labels of times.
Definition: PointMatcher.h:336
PointMatcherIO::PLYProperty::pmRowID
int pmRowID
row id used in a DataPoints
Definition: IO.h:278
IOFunctions.h
PointMatcherIO::FileInfoVector::findTransform
bool findTransform(const PointMatcherSupport::CsvElements &data, const std::string &prefix, unsigned dim)
Return whether there is a valid transformation named prefix in data.
Definition: pointmatcher/IO.cpp:317
PointMatcherIO::SupportedLabel::type
PMPropTypes type
type of information in PointMatcher
Definition: IO.h:77
PointMatcherIO::PCDproperty::field
std::string field
Name of the property.
Definition: IO.h:366
PointMatcherIO::PLYProperty::PLYProperty
PLYProperty(const std::string &type, const std::string &name, const unsigned pos)
@(brief) Regular PLY property constructor
Definition: pointmatcher/IO.cpp:1731
PointMatcher::DataPoints::featureExists
bool featureExists(const std::string &name) const
Look if a feature with a given name exist.
Definition: pointmatcher/DataPoints.cpp:487
PointMatcherIO::loadCSV
static DataPoints loadCSV(const std::string &fileName)
Associate an external name to a DataPoints type of information.
Definition: pointmatcher/IO.cpp:408
PointMatcherIO::Vector
PointMatcher< T >::Vector Vector
alias
Definition: IO.h:45
PointMatcherIO::loadPLY
static DataPoints loadPLY(const std::string &fileName)
Load polygon file format (ply) file.
Definition: pointmatcher/IO.cpp:1289
PointMatcherIO::PLYProperty::name
std::string name
name of PLY property
Definition: IO.h:270
PointMatcherIO::SupportedLabels
std::vector< SupportedLabel > SupportedLabels
Vector of supported labels in PointMatcher and their external names.
Definition: IO.h:84
skipBlock
void skipBlock(bool binary, int binarySize, std::istream &is, bool hasSeparateSizeParameter=true)
Definition: pointmatcher/IO.cpp:919
PointMatcherIO::it_PLYProp
PLYProperties::iterator it_PLYProp
Iterator for a vector of PLY properties.
Definition: IO.h:299
PointMatcherSupport::safeGetLine
std::istream & safeGetLine(std::istream &is, std::string &t)
Replaces getline for handling windows style CR/LF line endings.
Definition: IOFunctions.cpp:7
PointMatcherIO::SupportedLabel::internalName
std::string internalName
name used in PointMatcher
Definition: IO.h:75
PointMatcherSupport::oneBigEndian
const int oneBigEndian
is always a big endian independent of the platforms endianness
Definition: pointmatcher/IO.cpp:63
PointMatcherIO::FileInfo::configFileName
std::string configFileName
file name of the yaml configuration
Definition: IO.h:236
PointMatcherIO::PMPropTypes
PMPropTypes
Type of information in a DataPoints. Each type is stored in its own dense matrix.
Definition: IO.h:64
PointMatcher::DataPoints::descriptors
Matrix descriptors
descriptors of points in the cloud, might be empty
Definition: PointMatcher.h:333
csvLineToVector
static std::vector< string > csvLineToVector(const char *line)
Definition: pointmatcher/IO.cpp:71
icp.data
data
Definition: icp.py:50
PointMatcherIO::PLYElement::operator==
bool operator==(const PLYElement &other) const
comparison operator for elements
Definition: pointmatcher/IO.cpp:1834
PointMatcherIO::FileInfoVector
A vector of file info, to be used in batch processing.
Definition: IO.h:245
PointMatcherIO::FileInfoVector::localToGlobalFileName
std::string localToGlobalFileName(const std::string &path, const std::string &fileName)
Join parentPath and fileName and return the result as a global path.
Definition: pointmatcher/IO.cpp:299
PointMatcherIO::PLYElement::properties
PLYProperties properties
all properties found in the header
Definition: IO.h:309
LOG_WARNING_STREAM
#define LOG_WARNING_STREAM(args)
Definition: PointMatcherPrivate.h:68
PointMatcherIO::Matrix
PointMatcher< T >::Matrix Matrix
alias
Definition: IO.h:46
PointMatcherIO::SupportedLabel
Structure containing all information required to map external information to PointMatcher internal re...
Definition: IO.h:73
PointMatcherIO::LabelGenerator::add
void add(const std::string internalName)
add a name to the vector of labels. If already there, will increament the dimension.
Definition: pointmatcher/IO.cpp:428
PointMatcherIO::saveCSV
static void saveCSV(const DataPoints &data, const std::string &fileName, unsigned precision)
Save point cloud to a file as CSV.
Definition: pointmatcher/IO.cpp:836
dataPath
std::string dataPath
Definition: utest.cpp:43
PointMatcherIO::getColLabel
static std::string getColLabel(const Label &label, const int row)
convert a descriptor label to an appropriate sub-label
Definition: pointmatcher/IO.cpp:469
PointMatcherIO::POLYDATA
@ POLYDATA
Definition: IO.h:193
PointMatcherIO::TransformationParameters
PointMatcher< T >::TransformationParameters TransformationParameters
alias
Definition: IO.h:49
PointMatcherIO::PCDheader::viewPoint
Eigen::Matrix< T, 7, 1 > viewPoint
not used
Definition: IO.h:394
PointMatcherIO::Int64Matrix
PointMatcher< T >::Int64Matrix Int64Matrix
alias
Definition: IO.h:47
PointMatcherIO::getSupportedExternalLabels
static const SupportedLabels & getSupportedExternalLabels()
Definition: IO.h:117
parseCsvWithHeader
CsvElements parseCsvWithHeader(const std::string &fileName)
Definition: pointmatcher/IO.cpp:93
PointMatcherIO::Label
PointMatcher< T >::DataPoints::Label Label
alias
Definition: IO.h:51
PointMatcherIO::PLYElement::total_props
unsigned total_props
total number of properties in PLY element
Definition: IO.h:307
PointMatcher::DataPoints::times
Int64Matrix times
time associated to each points, might be empty
Definition: PointMatcher.h:335
std
PointMatcherIO::FileInfo::initialTransformation
TransformationParameters initialTransformation
matrix of initial estimate transform
Definition: IO.h:237
PointMatcher::DataPoints::save
void save(const std::string &fileName, bool binary=false, unsigned precision=7) const
Save a point cloud to a file, determine format from extension.
Definition: pointmatcher/IO.cpp:809
PointMatcherIO::FileInfo
Information to exploit a reading from a file using this library. Fields might be left blank if unused...
Definition: IO.h:230
icp_advance_api.dim
dim
Definition: icp_advance_api.py:152
PointMatcher::DataPoints::Label::span
size_t span
number of data dimensions the label spans
Definition: PointMatcher.h:224
PointMatcherIO::savePCD
static void savePCD(const DataPoints &data, const std::string &fileName, unsigned precision)
save datapoints to PCD point cloud format
Definition: pointmatcher/IO.cpp:2260
PointMatcherIO::SplitTime::low32
Eigen::Matrix< unsigned int, Eigen::Dynamic, Eigen::Dynamic > low32
Matrix containing file data representing the low 32 bits.
Definition: IO.h:205
build_map.transformation
transformation
Definition: build_map.py:37
InspectorsImpl.h
InspectorsImpl::VTKFileInspector
Definition: InspectorsImpl.h:159
PointMatcherIO::PLYElementF::createElement
static PLYElement * createElement(const std::string &elem_name, const int elem_num, const unsigned offset)
factory function, build element defined by name with elem_num elements
Definition: pointmatcher/IO.cpp:1815
PointMatcherIO::saveVTK
static void saveVTK(const DataPoints &data, const std::string &fileName, bool binary=false, unsigned precision=7)
Save point cloud to a file as VTK.
Definition: pointmatcher/IO.cpp:1262
PointMatcher::DataPoints::Labels::totalDim
size_t totalDim() const
Return the sum of the spans of each label.
Definition: pointmatcher/DataPoints.cpp:87
PointMatcherIO::PLYElementF
Factory for PLY elements.
Definition: IO.h:349
PointMatcherIO::PLYElementF::getElementType
static ElementTypes getElementType(const std::string &elem_name)
Definition: pointmatcher/IO.cpp:1792
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: Bibliography.cpp:45
build_map.end
end
Definition: build_map.py:79
PointMatcherIO::PCDproperty
Information for a PCD property.
Definition: IO.h:364
PointMatcherIO::PLYElementF::ElementTypes
ElementTypes
Definition: IO.h:351
PointMatcher::DataPoints::load
static DataPoints load(const std::string &fileName)
Load a point cloud from a file, determine format from extension.
Definition: pointmatcher/IO.cpp:375
PointMatcherIO::PLYElementF::elementSupported
bool elementSupported(const std::string &elem_name)
returns true if element named elem_name is supported by this parser
Definition: pointmatcher/IO.cpp:1808
IO.h
PointMatcherIO::PCDproperty::count
unsigned int count
number of dimension
Definition: IO.h:369
PointMatcherIO::FileInfo::FileInfo
FileInfo(const std::string &readingPath="", const std::string &referencePath="", const std::string &configFileName="", const TransformationParameters &initialTransformation=TransformationParameters(), const TransformationParameters &groundTruthTransformation=TransformationParameters(), const Vector &gravity=Vector3::Zero())
Constructor, leave fields blank if unused.
Definition: pointmatcher/IO.cpp:166
PointMatcherIO::PCDheader::dataType
std::string dataType
ascii or binary
Definition: IO.h:396
PointMatcherIO::plyPropTypeValid
static bool plyPropTypeValid(const std::string &type)
Check that property defined by type is a valid PLY type note: type must be lowercase.
Definition: pointmatcher/IO.cpp:1826
PointMatcherIO::loadPCD
static DataPoints loadPCD(const std::string &fileName)
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Definition: Parametrizable.h:199
PointMatcherIO::PLYProperty::pmType
PMPropTypes pmType
type of information in PointMatcher
Definition: IO.h:277
PointMatcher::DataPoints::Label
The name for a certain number of dim.
Definition: PointMatcher.h:221
PointMatcherIO::LabelGenerator
Generate a vector of Labels by checking for collision is the same name is reused.
Definition: IO.h:162

libpointmatcher
Author(s):
autogenerated on Mon Sep 16 2024 02:24:09