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) 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);
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);
821  else if (boost::iequals(ext, ".ply"))
822  return PointMatcherIO<T>::savePLY(*this, fileName);
823  else if (boost::iequals(ext, ".pcd"))
824  return PointMatcherIO<T>::savePCD(*this, fileName);
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) const;
831 template
832 void PointMatcher<double>::DataPoints::save(const std::string& fileName, bool binary) const;
833 
835 template<typename T>
836 void PointMatcherIO<T>::saveCSV(const DataPoints& data, const std::string& fileName)
837 {
838  ofstream ofs(fileName.c_str());
839  if (!ofs.good())
840  throw runtime_error(string("Cannot open file ") + fileName);
841  saveCSV(data, ofs);
842 }
843 
845 template<typename T>
846 void PointMatcherIO<T>::saveCSV(const DataPoints& data, std::ostream& os)
847 {
848  const int pointCount(data.features.cols());
849  const int dimCount(data.features.rows());
850  const int descDimCount(data.descriptors.rows());
851 
852  if (pointCount == 0)
853  {
854  LOG_WARNING_STREAM( "Warning, no points, doing nothing");
855  return;
856  }
857 
858  // write header
859  for (int i = 0; i < dimCount - 1; i++)
860  {
861  os << data.featureLabels[i].text;
862 
863  if (!((i == (dimCount - 2)) && descDimCount == 0))
864  os << ",";
865  }
866 
867  int n = 0;
868  for (size_t i = 0; i < data.descriptorLabels.size(); i++)
869  {
870  Label lab = data.descriptorLabels[i];
871  for (size_t s = 0; s < lab.span; s++)
872  {
873  os << getColLabel(lab,s);
874  if (n != (descDimCount - 1))
875  os << ",";
876  n++;
877  }
878  }
879 
880  os << "\n";
881 
882  // write points
883  for (int p = 0; p < pointCount; ++p)
884  {
885  for (int i = 0; i < dimCount-1; ++i)
886  {
887  os << data.features(i, p);
888  if(!((i == (dimCount - 2)) && descDimCount == 0))
889  os << " , ";
890  }
891 
892  for (int i = 0; i < descDimCount; i++)
893  {
894  os << data.descriptors(i,p);
895  if (i != (descDimCount - 1))
896  os << ",";
897  }
898  os << "\n";
899  }
900 
901 }
902 
903 template
904 void PointMatcherIO<float>::saveCSV(const DataPoints& data, const std::string& fileName);
905 template
906 void PointMatcherIO<double>::saveCSV(const DataPoints& data, const std::string& fileName);
907 
909 template<typename T>
910 typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadVTK(const std::string& fileName)
911 {
912  ifstream ifs(fileName.c_str(), std::ios::binary);
913  if (!ifs.good())
914  throw runtime_error(string("Cannot open file ") + fileName);
915  return loadVTK(ifs);
916 }
917 
918 void skipBlock(bool binary, int binarySize, std::istream & is, bool hasSeparateSizeParameter = true){
919  int n;
920  int size;
921  is >> n;
922  if(!is.good()){
923  throw std::runtime_error("File violates the VTK format : parameter 'n' is missing after a field name.");
924  }
925 
926  if(hasSeparateSizeParameter) {
927  is >> size;
928  if(!is.good()){
929  throw std::runtime_error("File violates the VTK format : parameter 'size' is missing after a field name.");
930  }
931  } else {
932  size = n;
933  }
934 
935  std::string line;
936  safeGetLine(is, line); // remove line end after parameters;
937  if(binary){
938  is.seekg(size * binarySize, std::ios_base::cur);
939  } else {
940  for (int p = 0; p < n; p++)
941  {
942  safeGetLine(is, line);
943  }
944  }
945 }
946 
948 template<typename T>
949 typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadVTK(std::istream& is)
950 {
951  std::map<std::string, SplitTime> labelledSplitTime;
952 
953  DataPoints loadedPoints;
954 
955  // parse header
956  string line;
957  safeGetLine(is, line);
958  if (line.find("# vtk DataFile Version") != 0)
959  throw runtime_error(string("Wrong magic header, found ") + line);
960  safeGetLine(is, line);
961  safeGetLine(is, line);
962 
963  const bool isBinary = (line == "BINARY");
964  if (line != "ASCII"){
965  if(!isBinary){
966  throw runtime_error(string("Wrong file type, expecting ASCII or BINARY, found ") + line);
967  }
968  }
969  safeGetLine(is, line);
970 
971  SupportedVTKDataTypes dataType;
972  if (line == "DATASET POLYDATA")
973  dataType = POLYDATA;
974  else if (line == "DATASET UNSTRUCTURED_GRID")
975  dataType = UNSTRUCTURED_GRID;
976  else
977  throw runtime_error(string("Wrong data type, expecting DATASET POLYDATA, found ") + line);
978 
979 
980  // parse points, descriptors and time
981  string fieldName;
982  string name;
983  int dim = 0;
984  int pointCount = 0;
985  string type;
986 
987  while (is >> fieldName)
988  {
989  // load features
990  if(fieldName == "POINTS")
991  {
992  is >> pointCount;
993  is >> type;
994  safeGetLine(is, line); // remove line end after parameters!
995 
996  if(!(type == "float" || type == "double"))
997  throw runtime_error(string("Field POINTS can only be of type double or float"));
998 
999  Matrix features(4, pointCount);
1000  for (int p = 0; p < pointCount; ++p)
1001  {
1002  readVtkData(type, isBinary, features.template block<3, 1>(0, p), is);
1003  features(3, p) = 1.0;
1004  }
1005  loadedPoints.addFeature("x", features.row(0));
1006  loadedPoints.addFeature("y", features.row(1));
1007  loadedPoints.addFeature("z", features.row(2));
1008  loadedPoints.addFeature("pad", features.row(3));
1009  }
1010 
1012  // Dataset type
1013  // POLYDATA
1014  else if(dataType == POLYDATA && fieldName == "VERTICES")
1015  {
1016  skipBlock(isBinary, 4, is);
1017  }
1018 
1019  else if(dataType == POLYDATA && fieldName == "LINES")
1020  {
1021  skipBlock(isBinary, 4, is);
1022  }
1023 
1024  else if(dataType == POLYDATA && fieldName == "POLYGONS")
1025  {
1026  skipBlock(isBinary, 4, is);
1027  }
1028 
1029  else if(dataType == POLYDATA && fieldName == "TRIANGLE_STRIPS")
1030  {
1031  skipBlock(isBinary, 4, is);
1032  }
1033 
1034  // Unstructure Grid
1035  else if(dataType == UNSTRUCTURED_GRID && fieldName == "CELLS")
1036  {
1037  skipBlock(isBinary, 4, is);
1038  }
1039  else if(dataType == UNSTRUCTURED_GRID && fieldName == "CELL_TYPES")
1040  {
1041  skipBlock(isBinary, 4, is, false); // according to http://www.vtk.org/VTK/img/file-formats.pdf CELL_TYPES only has one parameter (n)
1042  }
1043 
1045  // Point data
1046  else if(fieldName == "POINT_DATA")
1047  {
1048  int descriptorCount;
1049  is >> descriptorCount;
1050  if(pointCount != descriptorCount)
1051  throw runtime_error(string("The size of POINTS is different than POINT_DATA"));
1052  }
1054  // Field data is ignored
1055  else if (fieldName == "FIELD")
1056  {
1057  string fieldDataName;
1058  int fieldDataCount;
1059  is >> fieldDataName >> fieldDataCount;
1060 
1061  for (int f = 0; f < fieldDataCount; f++)
1062  {
1063  int numTuples;
1064  is >> name >> dim >> numTuples >> type;
1065 
1066  if(type == "vtkIdType") // skip that type
1067  {
1068  if(isBinary){
1069  is.seekg(dim * numTuples * 4, std::ios_base::cur);
1070  } else {
1071  int t_val;
1072  for (int t = 0; t < dim * numTuples; t++ )
1073  {
1074  is >> t_val;
1075  }
1076  }
1077  }
1078  else if(!(type == "float" || type == "double"))
1079  throw runtime_error(string("Field " + fieldName + " is " + type + " but can only be of type double or float"));
1080 
1081 
1082  Matrix descriptor(dim, pointCount);
1083  readVtkData(type, isBinary, descriptor.transpose(), is);
1084  loadedPoints.addDescriptor(name, descriptor);
1085  }
1086  }
1087  else if(fieldName == "METADATA") // Skip METADATA block
1088  {
1089  safeGetLine(is, line);
1090  safeGetLine(is, line);
1091  while(!line.empty())
1092  {
1093  safeGetLine(is, line);
1094  }
1095  }
1096  else // Load descriptors or time
1097  {
1098 
1099  // label name
1100  is >> name;
1101 
1102  bool isTimeSec = false;
1103  bool isTimeNsec = false;
1104 
1105 
1106  if(boost::algorithm::ends_with(name, "_splitTime_high32"))
1107  {
1108  isTimeSec = true;
1109  boost::algorithm::erase_last(name, "_splitTime_high32");
1110  }
1111 
1112  if(boost::algorithm::ends_with(name, "_splitTime_low32"))
1113  {
1114  isTimeNsec = true;
1115  boost::algorithm::erase_last(name, "_splitTime_low32");
1116  }
1117 
1118 
1119  bool skipLookupTable = false;
1120  bool isColorScalars = false;
1121  if(fieldName == "SCALARS")
1122  {
1123  dim = 1;
1124  is >> type;
1125  skipLookupTable = true;
1126  }
1127  else if(fieldName == "VECTORS")
1128  {
1129  dim = 3;
1130  is >> type;
1131  }
1132  else if(fieldName == "TENSORS")
1133  {
1134  dim = 9;
1135  is >> type;
1136  }
1137  else if(fieldName == "NORMALS")
1138  {
1139  dim = 3;
1140  is >> type;
1141  }
1142  else if(fieldName == "COLOR_SCALARS")
1143  {
1144  is >> dim;
1145  type = "float";
1146  isColorScalars = true;
1147  }
1148  else
1149  throw runtime_error(string("Unknown field name " + fieldName + ", expecting SCALARS, VECTORS, TENSORS, NORMALS or COLOR_SCALARS."));
1150 
1151 
1152  safeGetLine(is, line); // remove rest of the parameter line including its line end;
1153 
1154 
1155  // Load time data
1156  if(isTimeSec || isTimeNsec)
1157  {
1158  // Skip LOOKUP_TABLE line
1159  if(skipLookupTable)
1160  {
1161  safeGetLine(is, line);
1162  }
1163 
1164  typename std::map<std::string, SplitTime>::iterator it;
1165 
1166  it = labelledSplitTime.find(name);
1167  // reserve memory
1168  if(it == labelledSplitTime.end())
1169  {
1170  SplitTime t;
1171  t.high32 = Eigen::Matrix<unsigned int, Eigen::Dynamic, Eigen::Dynamic> (dim, pointCount);
1172  t.low32 = t.high32;
1173  labelledSplitTime[name] = t;
1174  }
1175 
1176  // Load seconds
1177  if(isTimeSec)
1178  {
1179  assert(labelledSplitTime[name].isHigh32Found == false);
1180  readVtkData(type, isBinary, labelledSplitTime[name].high32.transpose(), is);
1181  labelledSplitTime[name].isHigh32Found = true;
1182  }
1183 
1184  // Load nano seconds
1185  if(isTimeNsec)
1186  {
1187  assert(labelledSplitTime[name].isLow32Found == false);
1188  readVtkData(type, isBinary, labelledSplitTime[name].low32.transpose(), is);
1189  labelledSplitTime[name].isLow32Found = true;
1190  }
1191  }
1192  else
1193  {
1194 
1195  Matrix descriptorData(dim, pointCount);
1196 
1197  if(isColorScalars && isBinary)
1198  {
1199  std::vector<unsigned char> buffer(dim);
1200  for (int i = 0; i < pointCount; ++i){
1201  is.read(reinterpret_cast<char *>(&buffer.front()), dim);
1202  for(int r=0; r < dim; ++r){
1203  descriptorData(r, i) = buffer[r] / static_cast<T>(255.0);
1204  }
1205  }
1206  }
1207  else
1208  {
1209  if(!(type == "float" || type == "double"))
1210  throw runtime_error(string("Field " + fieldName + " is " + type + " but can only be of type double or float."));
1211 
1212  // Skip LOOKUP_TABLE line
1213  if(skipLookupTable)
1214  {
1215  safeGetLine(is, line);
1216  }
1217  readVtkData(type, isBinary, descriptorData.transpose(), is);
1218  }
1219  loadedPoints.addDescriptor(name, descriptorData);
1220  }
1221  }
1222  }
1223 
1224  // Combine time and add to point cloud
1225  typename std::map<std::string, SplitTime>::iterator it;
1226  for(it=labelledSplitTime.begin(); it!=labelledSplitTime.end(); it++)
1227  {
1228  // Confirm that both parts were loaded
1229  if(it->second.isHigh32Found == false)
1230  {
1231  throw runtime_error(string("Missing time field representing the higher 32 bits. Expecting SCALARS with name " + it->first + "_splitTime_high32 in the VTK file."));
1232  }
1233 
1234  if(it->second.isLow32Found == false)
1235  {
1236  throw runtime_error(string("Missing time field representing the lower 32 bits. Expecting SCALARS with name " + it->first + "_splitTime_low32 in the VTK file."));
1237  }
1238 
1239  // Loop through points
1240  Int64Matrix timeData(1,pointCount);
1241  for(int i=0; i<it->second.high32.cols(); i++)
1242  {
1243 
1244  timeData(0,i) = (((std::int64_t) it->second.high32(0,i)) << 32) | ((std::int64_t) it->second.low32(0,i));
1245  }
1246 
1247  loadedPoints.addTime(it->first, timeData);
1248  }
1249 
1250  return loadedPoints;
1251 }
1252 
1253 template
1254 PointMatcherIO<float>::DataPoints PointMatcherIO<float>::loadVTK(const std::string& fileName);
1255 template
1256 PointMatcherIO<double>::DataPoints PointMatcherIO<double>::loadVTK(const std::string& fileName);
1257 
1258 
1260 template<typename T>
1261 void PointMatcherIO<T>::saveVTK(const DataPoints& data, const std::string& fileName, bool binary)
1262 {
1263  typedef typename InspectorsImpl<T>::VTKFileInspector VTKInspector;
1264 
1265  Parametrizable::Parameters param;
1266  boost::assign::insert(param) ("baseFileName", "");
1267  boost::assign::insert(param) ("writeBinary", toParam(binary));
1268  VTKInspector vtkInspector(param);
1269  vtkInspector.dumpDataPoints(data, fileName);
1270 }
1271 
1272 
1273 template
1274 void PointMatcherIO<float>::saveVTK(const PointMatcherIO<float>::DataPoints& data, const std::string& fileName, bool binary);
1275 template
1276 void PointMatcherIO<double>::saveVTK(const PointMatcher<double>::DataPoints& data, const std::string& fileName, bool binary);
1277 
1286 template<typename T>
1287 typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPLY(const std::string& fileName)
1288 {
1289  ifstream ifs(fileName.c_str());
1290  if (!ifs.good())
1291  throw runtime_error(string("Cannot open file ") + fileName);
1292  return loadPLY(ifs);
1293 }
1294 
1295 template
1296 PointMatcherIO<float>::DataPoints PointMatcherIO<float>::loadPLY(const string& fileName);
1297 template
1298 PointMatcherIO<double>::DataPoints PointMatcherIO<double>::loadPLY(const string& fileName);
1299 
1302 template <typename T>
1303 typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPLY(std::istream& is)
1304 {
1305  class Elements : public vector<PLYElement*>{
1306  public:
1307  ~Elements(){
1308  for (typename vector<PLYElement*>::const_iterator it = this->begin(); it != this->end(); it++ )
1309  {
1310  delete *it;
1311  }
1312  }
1313  };
1314 
1315  /*
1316  Steps:
1317  1- PARSE PLY HEADER
1318  2- ASSIGN PLY PROPERTIES TO DATAPOINTS ROWS
1319  3- Reserve memory for a DataPoints
1320  4- Parse PLY vertex to appropriate DataPoints cols and rows
1321  5- Assemble final DataPoints
1322 
1323  PLY organisation:
1324 
1325  element 1 [name, size]
1326  - property 1 [type, name]
1327  - property 2
1328  - ...
1329  element 2
1330  -...
1331 
1332  */
1333 
1334 
1336  // 1- PARSE PLY HEADER
1337  bool format_defined = false;
1338  bool header_processed = false;
1339 
1340  Elements elements;
1341  PLYElementF element_f; // factory
1342  PLYElement* current_element = NULL;
1343  bool skip_props = false; // flag to skip properties if element is not supported
1344  unsigned elem_offset = 0; // keep track of line position of elements that are supported
1345  string line;
1346  safeGetLine(is, line);
1347 
1348  if (line.find("ply") != 0) {
1349  throw runtime_error(string("PLY parse error: wrong magic header, found <") + line + string(">"));
1350  }
1351 
1352  while (!header_processed)
1353  {
1354  if(!safeGetLine(is, line))
1355  throw runtime_error("PLY parse error: reached end of file before end of header definition");
1356 
1357 
1358  if ( line.empty() )
1359  continue;
1360  istringstream stringstream (line);
1361 
1362  string keyword;
1363  stringstream >> keyword;
1364 
1365  // ignore comment
1366  if (keyword == "comment") {
1367  continue;
1368  }
1369 
1370  // We only support ascii and ply version 1.0
1371  if (keyword == "format")
1372  {
1373  if (format_defined)
1374  throw runtime_error("PLY parse error: format already defined");
1375 
1376  string format_str, version_str;
1377  stringstream >> format_str >> version_str;
1378 
1379  if (format_str != "ascii" && format_str != "binary_little_endian" && format_str != "binary_big_endian")
1380  throw runtime_error(string("PLY parse error: format <") + format_str + string("> is not supported"));
1381 
1382  if (format_str == "binary_little_endian" || format_str == "binary_big_endian")
1383  throw runtime_error(string("PLY parse error: binary PLY files are not supported"));
1384  if (version_str != "1.0")
1385  {
1386  throw runtime_error(string("PLY parse error: version <") + version_str + string("> of ply is not supported"));
1387  }
1388 
1389  format_defined = true;
1390 
1391  }
1392  else if (keyword == "element")
1393  {
1394 
1395 
1396  string elem_name, elem_num_s;
1397  stringstream >> elem_name >> elem_num_s;
1398 
1399  unsigned elem_num;
1400  try
1401  {
1402  elem_num = boost::lexical_cast<unsigned>(elem_num_s);
1403  }
1404  catch (boost::bad_lexical_cast& e)
1405  {
1406  throw runtime_error(string("PLY parse error: bad number of elements ") + elem_num_s + string(" for element ") + elem_name);
1407  }
1408 
1409  if (element_f.elementSupported(elem_name))
1410  {
1411  // Initialize current element
1412  PLYElement* elem = element_f.createElement(elem_name, elem_num, elem_offset);
1413  current_element = elem;
1414 
1415  // check that element is not already defined
1416  for (typename Elements::const_iterator it = elements.begin(); it != elements.end(); it++ )
1417  {
1418  if (**it == *elem) {
1419  delete elem;
1420  throw runtime_error(string("PLY parse error: element: ") + elem_name + string( "is already defined"));
1421  }
1422  }
1423  elements.push_back(elem);
1424  skip_props = false;
1425  }
1426  else
1427  {
1428  LOG_WARNING_STREAM("PLY parse warning: element " << elem_name << " not supported. Skipping.");
1429  skip_props = true;
1430  }
1431 
1432  elem_offset += elem_num;
1433  }
1434  else if (keyword == "property")
1435  {
1436  if (current_element == NULL)
1437  {
1438  throw runtime_error("PLY parse error: property listed without defining an element");
1439  }
1440 
1441  if (skip_props)
1442  continue;
1443 
1444  string next, prop_type, prop_name;
1445  stringstream >> next;
1446 
1447  // PLY list property
1448  if (next == "list")
1449  {
1450  string prop_idx_type;
1451  stringstream >> prop_idx_type >> prop_type >> prop_name;
1452 
1453  PLYProperty list_prop(prop_idx_type, prop_type, prop_name, current_element->total_props);
1454 
1455  current_element->properties.push_back(list_prop);
1456  }
1457  // PLY regular property
1458  else
1459  {
1460  prop_type = next;
1461  stringstream >> prop_name;
1462  PLYProperty prop(prop_type, prop_name, current_element->total_props);
1463 
1464  current_element->properties.push_back(prop);
1465  }
1466 
1467  current_element->total_props++;
1468  }
1469  else if (keyword == "end_header")
1470  {
1471  if (!format_defined)
1472  {
1473  throw runtime_error(string("PLY parse error: format not defined in header"));
1474  }
1475 
1476  if (elements.size() == 0)
1477  {
1478  throw runtime_error(string("PLY parse error: no elements defined in header"));
1479  }
1480 
1481  header_processed = true;
1482  }
1483  }
1484 
1486  // 2- ASSIGN PLY PROPERTIES TO DATAPOINTS ROWS
1487 
1488  // Fetch vertex
1489  PLYElement* vertex = elements[0];
1490 
1491  if(vertex->name != "vertex")
1492  {
1493  throw runtime_error(string("PLY parse error: vertex should be the first element defined."));
1494  }
1495 
1496  // Fetch known features and descriptors
1497  const SupportedLabels & externalLabels = getSupportedExternalLabels();
1498 
1499  int rowIdFeatures = 0;
1500  int rowIdDescriptors = 0;
1501  int rowIdTime= 0;
1502 
1503  LabelGenerator featLabelGen, descLabelGen, timeLabelGen;
1504 
1505 
1506  // Loop through all known external names (ordered list)
1507  for(size_t i=0; i<externalLabels.size(); i++)
1508  {
1509  const SupportedLabel & supLabel = externalLabels[i];
1510 
1511  //Search if that feature exist
1512  for(it_PLYProp it=vertex->properties.begin(); it!=vertex->properties.end(); ++it)
1513  {
1514 
1515  if(supLabel.externalName == it->name)
1516  {
1517  it->pmType = supLabel.type;
1518 
1519  // Assign rowId in that order
1520  switch (supLabel.type)
1521  {
1522  case FEATURE:
1523  it->pmRowID = rowIdFeatures;
1524  featLabelGen.add(supLabel.internalName);
1525  rowIdFeatures++;
1526  break;
1527  case DESCRIPTOR:
1528  it->pmRowID = rowIdDescriptors;
1529  descLabelGen.add(supLabel.internalName);
1530  rowIdDescriptors++;
1531  break;
1532  case TIME:
1533  it->pmRowID = rowIdTime;
1534  timeLabelGen.add(supLabel.internalName);
1535  rowIdTime++;
1536  default:
1537  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'"));
1538  break;
1539  }
1540 
1541  // we stop searching once we have a match
1542  break;
1543  }
1544  }
1545  }
1546 
1547  // loop through the remaining UNSUPPORTED labels and assigned them to a single descriptor row
1548  for(it_PLYProp it=vertex->properties.begin(); it!=vertex->properties.end(); ++it)
1549  {
1550  if(it->pmType == UNSUPPORTED)
1551  {
1552  it->pmType = DESCRIPTOR; // force descriptor
1553  it->pmRowID = rowIdDescriptors;
1554  descLabelGen.add(it->name); // keep original name
1555  rowIdDescriptors++;
1556  }
1557  }
1558 
1560  // 3- RESERVE DATAPOINTS MEMORY
1561 
1562  const unsigned int featDim = featLabelGen.getLabels().totalDim();
1563  const unsigned int descDim = descLabelGen.getLabels().totalDim();
1564  const unsigned int timeDim = timeLabelGen.getLabels().totalDim();
1565  const unsigned int nbPoints = vertex->num;
1566 
1567  Matrix features = Matrix(featDim, nbPoints);
1568  Matrix descriptors = Matrix(descDim, nbPoints);
1569  Int64Matrix times = Int64Matrix(timeDim, nbPoints);
1570 
1571 
1573  // 4- PARSE PLY DATA (vertex)
1574  const int nbProp = vertex->total_props;
1575  const int nbValues = nbPoints*nbProp;
1576  int propID = 0;
1577  int col = 0;
1578  for(int i=0; i<nbValues; i++)
1579  {
1580  T value;
1581  if(!(is >> value))
1582  {
1583  throw runtime_error(
1584  (boost::format("PLY parse error: expected %1% values (%2% points with %3% properties) but only found %4% values.") % nbValues % nbPoints % nbProp % i).str());
1585  }
1586  else
1587  {
1588  const int row = vertex->properties[propID].pmRowID;
1589  const PMPropTypes type = vertex->properties[propID].pmType;
1590 
1591  // rescale color from [0,254] to [0, 1[
1592  // FIXME: do we need that?
1593  if (vertex->properties[propID].name == "red" || vertex->properties[propID].name == "green" || vertex->properties[propID].name == "blue" || vertex->properties[propID].name == "alpha") {
1594  value /= 255.0;
1595  }
1596 
1597  switch (type)
1598  {
1599  case FEATURE:
1600  features(row, col) = value;
1601  break;
1602  case DESCRIPTOR:
1603  descriptors(row, col) = value;
1604  break;
1605  case TIME:
1606  times(row, col) = value;
1607  break;
1608  case UNSUPPORTED:
1609  throw runtime_error("Implementation error in loadPLY(). This should not throw.");
1610  break;
1611  }
1612 
1613  ++propID;
1614 
1615  if(propID >= nbProp)
1616  {
1617  propID = 0;
1618  ++col;
1619  }
1620  }
1621  }
1622 
1623 
1624 
1626  // 5- ASSEMBLE FINAL DATAPOINTS
1627 
1628  DataPoints loadedPoints(features, featLabelGen.getLabels());
1629 
1630  if (descriptors.rows() > 0)
1631  {
1632  loadedPoints.descriptors = descriptors;
1633  loadedPoints.descriptorLabels = descLabelGen.getLabels();
1634  }
1635 
1636  if(times.rows() > 0)
1637  {
1638  loadedPoints.times = times;
1639  loadedPoints.timeLabels = timeLabelGen.getLabels();
1640  }
1641 
1642  // Ensure homogeous coordinates
1643  if(!loadedPoints.featureExists("pad"))
1644  {
1645  loadedPoints.addFeature("pad", Matrix::Ones(1,features.cols()));
1646  }
1647 
1648  return loadedPoints;
1649 
1650 }
1651 
1652 template<typename T>
1653 void PointMatcherIO<T>::savePLY(const DataPoints& data,
1654  const std::string& fileName)
1655 {
1656  //typedef typename DataPoints::Labels Labels;
1657 
1658  ofstream ofs(fileName.c_str());
1659  if (!ofs.good())
1660  throw runtime_error(string("Cannot open file ") + fileName);
1661 
1662  const int pointCount(data.features.cols());
1663  const int featCount(data.features.rows());
1664  const int descRows(data.descriptors.rows());
1665 
1666 
1667  if (pointCount == 0)
1668  {
1669  LOG_WARNING_STREAM("Warning, no points, doing nothing");
1670  return;
1671  }
1672 
1673  ofs << "ply\n" <<"format ascii 1.0\n";
1674  ofs << "element vertex " << pointCount << "\n";
1675  for (int f=0; f <(featCount-1); f++)
1676  {
1677  ofs << "property float " << data.featureLabels[f].text << "\n";
1678  }
1679 
1680  for (size_t i = 0; i < data.descriptorLabels.size(); i++)
1681  {
1682  Label lab = data.descriptorLabels[i];
1683  for (size_t s = 0; s < lab.span; s++)
1684 
1685  {
1686  ofs << "property float " << getColLabel(lab,s) << "\n";
1687  }
1688  }
1689 
1690  ofs << "end_header\n";
1691 
1692  // write points
1693  for (int p = 0; p < pointCount; ++p)
1694  {
1695  for (int f = 0; f < featCount - 1; ++f)
1696  {
1697  ofs << data.features(f, p);
1698  if(!(f == featCount-2 && descRows == 0))
1699  ofs << " ";
1700  }
1701 
1702  bool datawithColor = data.descriptorExists("color");
1703  int colorStartingRow = data.getDescriptorStartingRow("color");
1704  int colorEndRow = colorStartingRow + data.getDescriptorDimension("color");
1705  for (int d = 0; d < descRows; ++d)
1706  {
1707  if (datawithColor && d >= colorStartingRow && d < colorEndRow) {
1708  ofs << static_cast<unsigned>(data.descriptors(d, p) * 255.0);
1709  } else {
1710  ofs << data.descriptors(d, p);
1711  }
1712  if(d != descRows-1)
1713  ofs << " ";
1714  }
1715  ofs << "\n";
1716  }
1717 
1718  ofs.close();
1719 }
1720 
1721 template
1722 void PointMatcherIO<float>::savePLY(const DataPoints& data, const std::string& fileName);
1723 template
1724 void PointMatcherIO<double>::savePLY(const DataPoints& data, const std::string& fileName);
1725 
1727 template<typename T>
1728 PointMatcherIO<T>::PLYProperty::PLYProperty(const std::string& type,
1729  const std::string& name, const unsigned pos) :
1730  name(name),
1731  type(type),
1732  pos(pos)
1733 {
1734  if (plyPropTypeValid(type))
1735  {
1736  is_list = false;
1737  }
1738  else
1739  {
1740  throw std::runtime_error(
1741  std::string("PLY parse error: property type ") + type
1742  + std::string(" for property ") + name
1743  + std::string(" is invalid"));
1744  }
1745 
1746  pmType = UNSUPPORTED;
1747  pmRowID = -1;
1748 
1749 }
1750 
1752 template<typename T>
1753 PointMatcherIO<T>::PLYProperty::PLYProperty(const std::string& idx_type,
1754  const std::string& type, const std::string& name, const unsigned pos) :
1755  name(name),
1756  type(type),
1757  idx_type(idx_type),
1758  pos(pos)
1759 {
1760  if (plyPropTypeValid(idx_type) && plyPropTypeValid(type))
1761  {
1762  is_list = true;
1763  }
1764  else
1765  {
1766  throw std::runtime_error(
1767  std::string("PLY parse error: property list type ") + idx_type
1768  + std::string(" ") + type
1769  + std::string(" for property ") + name
1770  + std::string(" is invalid"));
1771  }
1772 
1773  pmType = UNSUPPORTED;
1774  pmRowID = -1;
1775 }
1776 
1777 template
1778 class PointMatcherIO<float>::PLYElement;
1779 template
1780 class PointMatcherIO<double>::PLYElement;
1781 
1782 template
1783 class PointMatcherIO<float>::PLYProperty;
1784 template
1785 class PointMatcherIO<double>::PLYProperty;
1786 
1787 
1788 template <typename T>
1789 typename PointMatcherIO<T>::PLYElementF::ElementTypes PointMatcherIO<T>::PLYElementF::getElementType(const std::string& elem_name)
1790 {
1791  string lc = elem_name;
1792  boost::algorithm::to_lower(lc);
1793  if (lc == "vertex")
1794  {
1795  return VERTEX;
1796  }
1797  else
1798  {
1799  return UNSUPPORTED;
1800  }
1801 }
1802 
1803 
1804 template <typename T>
1805 bool PointMatcherIO<T>::PLYElementF::elementSupported(const std::string& elem_name)
1806 {
1807  return getElementType(elem_name) != UNSUPPORTED;
1808 }
1809 
1810 
1811 template<typename T>
1812 typename PointMatcherIO<T>::PLYElement* PointMatcherIO<T>::PLYElementF::createElement(
1813  const std::string& elem_name, const int elem_num, const unsigned offset) {
1814  ElementTypes type = getElementType(elem_name);
1815  if (type == VERTEX)
1816  return new PLYVertex(elem_num, offset);
1817  else
1818  return NULL;
1819 }
1820 
1821 
1822 template<typename T>
1823 bool PointMatcherIO<T>::plyPropTypeValid(const std::string& type) {
1824  return (type == "char" || type == "uchar" || type == "short"
1825  || type == "ushort" || type == "int" || type == "uint"
1826  || type == "float" || type == "double");
1827 }
1828 
1829 
1830 template <typename T>
1831 bool PointMatcherIO<T>::PLYElement::operator==(const PLYElement& rhs) const
1832 {
1833  return name == rhs.name;
1834 }
1835 
1836 
1837 template <typename T>
1838 bool PointMatcherIO<T>::PLYProperty::operator==(const PLYProperty& rhs) const
1839 {
1840  return name == rhs.name && type == rhs.type;
1841 }
1842 
1843 
1846 template<typename T>
1847 typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPCD(const string& fileName) {
1848  ifstream ifs(fileName.c_str());
1849  if (!ifs.good())
1850  throw runtime_error(string("Cannot open file ") + fileName);
1851  return loadPCD(ifs);
1852 }
1853 
1854 
1855 template
1856 PointMatcherIO<float>::DataPoints PointMatcherIO<float>::loadPCD(const string& fileName);
1857 template
1858 PointMatcherIO<double>::DataPoints PointMatcherIO<double>::loadPCD(const string& fileName);
1859 
1860 
1863 template<typename T>
1864 typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPCD(std::istream& is) {
1865 
1866 
1867  /*
1868  Steps:
1869  1- PARSE PCD HEADER
1870  2- ASSIGN PCD PROPERTIES TO DATAPOINTS ROWS
1871  3- Reserve memory for a DataPoints
1872  4- Parse PCD XXX to appropriate DataPoints cols and rows
1873  5- Assemble final DataPoints
1874 
1875  PCD organisation:
1876 
1877  # .PCD v.7 - Point Cloud Data file format
1878  VERSION number
1879  FIELDS prop1 prop2 prop3 ...
1880  SIZE nbBytes1 nbBytes2 nbBytes3
1881  TYPE type1 type2 type3
1882  COUNT nbDim1 nbDim2 nbDim3
1883  WIDTH w
1884  HEIGHT h
1885  VIEWPOINT 0 0 0 1 0 0 0
1886  POINTS size (should be w*h)
1887  DATA ascii or binary
1888  data1 data2 data3 ...
1889  data1 data2 data3 ...
1890  ...
1891 
1892  */
1893 
1894 
1896  // 1- PARSE PCD HEADER
1897 
1898  size_t lineNum = 0;
1899  PCDheader header;
1900 
1901  string line;
1902  while (safeGetLine(is, line))
1903  {
1904 
1905  // get rid of white spaces before/after
1906  boost::trim (line);
1907 
1908  // ignore comments or empty line
1909  if (line.substr(0,1) == "#" || line == "")
1910  {
1911  lineNum++;
1912  continue;
1913  }
1914 
1915  vector<string> tokens;
1916  boost::split(tokens, line, boost::is_any_of("\t\r "), boost::token_compress_on);
1917 
1918  string pcd_version_str;
1919  if (tokens[0] == "VERSION")
1920  {
1921  header.version = tokens[1];
1922 
1923  if (tokens[1] != "0.7" && tokens[1] != ".7")
1924  throw runtime_error("PCD Parse Error: Only PCD Version 0.7 is supported");
1925  }
1926 
1927  else if (tokens[0] == "FIELDS")
1928  {
1929  header.properties.resize(tokens.size() - 1);
1930 
1931  for (size_t i = 1; i < tokens.size(); i++)
1932  {
1933  header.properties[i-1].field = tokens[i];
1934  }
1935 
1936  }
1937 
1938  else if (tokens[0] == "SIZE")
1939  {
1940  if((tokens.size() - 1) != header.properties.size())
1941  throw runtime_error("PCD Parse Error: number of elements for SIZE must be the same as FIELDS");
1942 
1943  for (size_t i = 1; i < tokens.size(); i++)
1944  {
1945  const unsigned int size = boost::lexical_cast<unsigned int >(tokens[i]);
1946  header.properties[i-1].size = size;
1947  }
1948 
1949  }
1950 
1951  else if (tokens[0] == "TYPE")
1952  {
1953  if((tokens.size() - 1) != header.properties.size())
1954  throw runtime_error("PCD Parse Error: number of elements for TYPE must be the same as FIELDS");
1955 
1956  for (size_t i = 1; i < tokens.size(); i++)
1957  {
1958  const char type = boost::lexical_cast<char>(tokens[i]);
1959  header.properties[i-1].type = type;
1960  if (type != 'I' && type != 'U' && type != 'F')
1961  throw runtime_error("PCD Parse Error: invalid TYPE, it must be 'I', 'U', or 'F'");
1962  }
1963 
1964  }
1965 
1966  else if (tokens[0] == "COUNT")
1967  {
1968 
1969  if((tokens.size() - 1) != header.properties.size())
1970  throw runtime_error("PCD Parse Error: number of elements for COUNT must be the same as FIELDS");
1971 
1972  for (size_t i = 1; i < tokens.size(); i++)
1973  {
1974  const unsigned int count = boost::lexical_cast<unsigned int >(tokens[i]);
1975  header.properties[i-1].count = count;
1976  }
1977 
1978  }
1979 
1980  else if (tokens[0] == "WIDTH")
1981  {
1982  try
1983  {
1984  header.width = boost::lexical_cast<unsigned int >(tokens[1]);
1985  }
1986  catch (boost::bad_lexical_cast& e)
1987  {
1988  throw runtime_error("PCD Parse Error: invalid WIDTH");
1989  }
1990 
1991  }
1992 
1993  else if (tokens[0] == "HEIGHT")
1994  {
1995  try
1996  {
1997  header.height= boost::lexical_cast<unsigned int >(tokens[1]);
1998  }
1999  catch (boost::bad_lexical_cast& e)
2000  {
2001  throw runtime_error("PCD Parse Error: invalid HEIGHT");
2002  }
2003 
2004  }
2005 
2006  else if (tokens[0] == "VIEWPOINT")
2007  {
2008  if((tokens.size() - 1) != 7 )
2009  throw runtime_error("PCD Parse Error: number of elements for VIEWPOINT must be 7");
2010 
2011  for (size_t i = 1; i < tokens.size(); i++)
2012  {
2013  try
2014  {
2015  header.viewPoint(i-1, 0) = boost::lexical_cast<T>(tokens[i]);
2016  }
2017  catch (boost::bad_lexical_cast& e)
2018  {
2019  stringstream ss;
2020  ss << "PCD Parse Error: invalid value(" << tokens[i] << ") of VIEWPOINT";
2021  throw runtime_error(ss.str());
2022  }
2023  }
2024 
2025  }
2026 
2027  else if (tokens[0] == "POINTS")
2028  {
2029  try
2030  {
2031  header.nbPoints = boost::lexical_cast<unsigned int>(tokens[1]);
2032  }
2033  catch (boost::bad_lexical_cast& e)
2034  {
2035  stringstream ss;
2036  ss << "PCD Parse Error: the value in the element POINTS (" << tokens[1] << ") could not be cast as unsigned int";
2037  throw runtime_error(ss.str());
2038  }
2039  }
2040 
2041  else if (tokens[0] == "DATA")
2042  {
2043  header.dataType= tokens[1];
2044 
2045  if (header.dataType == "ascii")
2046  {
2047  // DATA is the last element of the header, we exit the loop
2048  break;
2049  }
2050  else if(header.dataType == "binary")
2051  {
2052  throw runtime_error("PCD Implementation Error: the option for DATA binary is not implemented yet");
2053  }
2054  else
2055  {
2056  stringstream ss;
2057  ss << "PCD Parse Error: the value in the element DATA (" << tokens[1] << ") must be ascii or binary";
2058  throw runtime_error(ss.str());
2059  }
2060 
2061  }
2062 
2063  lineNum++;
2064  }
2065  // Extra check for the number of points
2066  if (header.properties.size() == 0)
2067  throw runtime_error("PCD Parse Error: no FIELDS were find in the header");
2068 
2069 
2070  // Extra check for the number of points
2071  if (header.width * header.height != header.nbPoints)
2072  throw runtime_error("PCD Parse Error: POINTS field does not match WIDTH and HEIGHT fields");
2073 
2075  // 2- ASSIGN PCD PROPERTIES TO DATAPOINTS ROWS
2076 
2077  // Fetch known features and descriptors
2078  const SupportedLabels & externalLabels = getSupportedExternalLabels();
2079 
2080  int rowIdFeatures = 0;
2081  int rowIdDescriptors = 0;
2082  int rowIdTime= 0;
2083 
2084  LabelGenerator featLabelGen, descLabelGen, timeLabelGen;
2085 
2086  // Loop through all known external names (ordered list)
2087  for(size_t i=0; i<externalLabels.size(); i++)
2088  {
2089  const SupportedLabel & supLabel = externalLabels[i];
2090 
2091  //Search if that feature exist
2092  for(size_t i=0; i < header.properties.size(); i++)
2093  {
2094  const PCDproperty prop = header.properties[i];
2095 
2096  //TODO: prop.field == "rgb" might be tricky
2097 
2098  if(supLabel.externalName == prop.field)
2099  {
2100  header.properties[i].pmType = supLabel.type;
2101 
2102  // Assign rowId in that order
2103  switch (supLabel.type)
2104  {
2105  case FEATURE:
2106  header.properties[i].pmRowID = rowIdFeatures;
2107  featLabelGen.add(supLabel.internalName);
2108  rowIdFeatures++;
2109  if(prop.count != 1)
2110  {
2111  stringstream ss;
2112  ss << "PCD Parse Error: the field " << prop.field << " must have a count of 1";
2113  throw runtime_error(ss.str());
2114  }
2115  break;
2116  case DESCRIPTOR:
2117  header.properties[i].pmRowID = rowIdDescriptors;
2118  descLabelGen.add(supLabel.internalName, prop.count);
2119  rowIdDescriptors += prop.count;
2120  break;
2121  case TIME:
2122  header.properties[i].pmRowID = rowIdTime;
2123  timeLabelGen.add(supLabel.internalName, prop.count);
2124  rowIdTime += prop.count;
2125  default:
2126  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'"));
2127  break;
2128  }
2129 
2130  // we stop searching once we have a match
2131  break;
2132  }
2133  }
2134  }
2135 
2136  // loop through the remaining UNSUPPORTED labels and assigned them to a single descriptor row
2137  for(size_t i=0; i < header.properties.size(); i++)
2138  {
2139  const PCDproperty prop = header.properties[i];
2140  if(prop.pmType == UNSUPPORTED)
2141  {
2142  header.properties[i].pmType = DESCRIPTOR; // force descriptor
2143  header.properties[i].pmRowID = rowIdDescriptors;
2144  descLabelGen.add(prop.field, prop.count); // keep original name
2145  rowIdDescriptors += prop.count;
2146  }
2147  }
2148 
2149 
2151  // 3- RESERVE DATAPOINTS MEMORY
2152 
2153  const unsigned int featDim = featLabelGen.getLabels().totalDim();
2154  const unsigned int descDim = descLabelGen.getLabels().totalDim();
2155  const unsigned int timeDim = timeLabelGen.getLabels().totalDim();
2156  const unsigned int totalDim = featDim + descDim + timeDim;
2157  const unsigned int nbPoints = header.nbPoints;
2158 
2159  Matrix features = Matrix(featDim, nbPoints);
2160  Matrix descriptors = Matrix(descDim, nbPoints);
2161  Int64Matrix times = Int64Matrix(timeDim, nbPoints);
2162 
2163 
2165  // 4- PARSE PCD DATA
2166 
2167  size_t col = 0; // point count
2168  while (safeGetLine(is, line))
2169  {
2170 
2171  // get rid of white spaces before/after
2172  boost::trim (line);
2173 
2174  // ignore comments or empty line
2175  if (line.substr(0,1) == "#" || line == "")
2176  {
2177  lineNum++;
2178  continue;
2179  }
2180 
2181  vector<string> tokens;
2182  boost::split(tokens, line, boost::is_any_of("\t\r "), boost::token_compress_on);
2183 
2184 
2185  if (tokens.size() != totalDim)
2186  throw runtime_error(string("PCD Parse Error: number of data columns does not match number of fields at line: ") + boost::lexical_cast<string>(lineNum));
2187 
2188  unsigned int fileCol = 0;
2189  for(size_t i=0; i<header.properties.size(); i++)
2190  {
2191  const unsigned int count = header.properties[i].count;
2192  const unsigned int row = header.properties[i].pmRowID;
2193  const PMPropTypes type = header.properties[i].pmType;
2194 
2195 
2196  for(size_t j=0; j<count; j++)
2197  {
2198  switch (type)
2199  {
2200  case FEATURE:
2201  features(row+j, col) = boost::lexical_cast<T>(tokens[fileCol]);
2202  break;
2203  case DESCRIPTOR:
2204  descriptors(row+j, col) = boost::lexical_cast<T>(tokens[fileCol]);
2205  break;
2206  case TIME:
2207  times(row+j, col) = boost::lexical_cast<std::int64_t>(tokens[fileCol]);
2208  break;
2209  case UNSUPPORTED:
2210  throw runtime_error("Implementation error in loadPCD(). This should not throw.");
2211  break;
2212  }
2213 
2214  fileCol++;
2215  }
2216 
2217  }
2218 
2219  col++;
2220  lineNum++;
2221  }
2222 
2223  if (col != nbPoints)
2224  {
2225  stringstream ss;
2226  ss << "PCD Parse Error: the number of points in the file (" << col << ") is less than the specified number of points (" << nbPoints << ")";
2227  throw runtime_error(ss.str());
2228  }
2229 
2231  // 5- ASSEMBLE FINAL DATAPOINTS
2232 
2233  DataPoints loadedPoints(features, featLabelGen.getLabels());
2234 
2235  if (descriptors.rows() > 0)
2236  {
2237  loadedPoints.descriptors = descriptors;
2238  loadedPoints.descriptorLabels = descLabelGen.getLabels();
2239  }
2240 
2241  if(times.rows() > 0)
2242  {
2243  loadedPoints.times = times;
2244  loadedPoints.timeLabels = timeLabelGen.getLabels();
2245  }
2246 
2247  // Ensure homogeous coordinates
2248  if(!loadedPoints.featureExists("pad"))
2249  {
2250  loadedPoints.addFeature("pad", Matrix::Ones(1,features.cols()));
2251  }
2252 
2253  return loadedPoints;
2254 }
2255 
2256 template<typename T>
2257 void PointMatcherIO<T>::savePCD(const DataPoints& data,
2258  const std::string& fileName) {
2259  ofstream ofs(fileName.c_str());
2260  if (!ofs.good())
2261  throw runtime_error(string("Cannot open file ") + fileName);
2262 
2263  const int pointCount(data.features.cols());
2264  const int featCount(data.features.rows());
2265  const int descRows(data.descriptors.rows());
2266  const int descCount(data.descriptorLabels.size());
2267 
2268  if (pointCount == 0)
2269  {
2270  LOG_WARNING_STREAM("Warning, no points, doing nothing");
2271  return;
2272  }
2273 
2274  ofs << "# .PCD v.7 - Point Cloud Data file format\n" <<"VERSION .7\n";
2275  ofs << "FIELDS";
2276 
2277  for (int f=0; f < (featCount - 1); f++)
2278  {
2279  ofs << " " << data.featureLabels[f].text;
2280  }
2281 
2282  if (descRows == 0)
2283  ofs << "\n";
2284  else
2285  {
2286  for (int i = 0; i < descCount; i++)
2287  {
2288  ofs << " " << data.descriptorLabels[i].text;
2289  }
2290  ofs << "\n";
2291  }
2292 
2293  ofs << "SIZE";
2294  for (int i =0; i < featCount - 1 + descCount; i++)
2295  {
2296  ofs << " 4"; // for float
2297  }
2298  ofs << "\n";
2299 
2300  ofs << "TYPE";
2301  for (int i =0; i < featCount - 1 + descCount; i++)
2302  {
2303  ofs << " F"; // for float
2304  }
2305  ofs << "\n";
2306 
2307  ofs << "COUNT";
2308  for (int f = 0; f < featCount - 1 ; f++ )
2309  ofs << " 1";
2310  if (descCount == 0)
2311  ofs << "\n";
2312  else
2313  {
2314  for (int i = 0; i < descCount; i++)
2315  {
2316  ofs << " " << data.descriptorLabels[i].span;
2317  }
2318  ofs << "\n";
2319  }
2320 
2321  ofs << "WIDTH " << pointCount << "\n";
2322  ofs << "HEIGHT 1\n";
2323  ofs << "POINTS " << pointCount << "\n";
2324  ofs << "DATA ascii\n";
2325 
2326  // write points
2327  for (int p = 0; p < pointCount; ++p)
2328  {
2329  for (int f = 0; f < featCount - 1; ++f)
2330  {
2331  ofs << data.features(f, p);
2332  if(!(f == featCount-2 && descRows == 0))
2333  ofs << " ";
2334  }
2335  for (int d = 0; d < descRows; ++d)
2336  {
2337  ofs << data.descriptors(d, p);
2338  if(d != descRows-1)
2339  ofs << " ";
2340  }
2341  ofs << "\n";
2342  }
2343 
2344  ofs.close();
2345 }
2346 
2347 template
2348 void PointMatcherIO<float>::savePCD(const DataPoints& data, const std::string& fileName);
2349 template
2350 void PointMatcherIO<double>::savePCD(const DataPoints& data, const std::string& fileName);
2351 
2352 
2353 
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 a string value using lexical_cast.
Definition: Parametrizable.h:92
PointMatcherIO::PLYVertex
Implementation of PLY vertex element.
Definition: IO.h:334
PointMatcherIO::UNSUPPORTED
@ UNSUPPORTED
Definition: IO.h:69
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:910
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::LabelGenerator::getLabels
Labels getLabels() const
Return the vector of labels used to build a DataPoints.
Definition: pointmatcher/IO.cpp:457
s
XmlRpcServer s
kitti-run-seq.f
string f
Definition: kitti-run-seq.py:12
PointMatcherIO::saveVTK
static void saveVTK(const DataPoints &data, const std::string &fileName, bool binary=false)
Save point cloud to a file as VTK.
Definition: pointmatcher/IO.cpp:1261
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:636
PointMatcherIO::FEATURE
@ FEATURE
Definition: IO.h:66
PointMatcher::DataPoints::Labels
A vector of Label.
Definition: PointMatcher.h:229
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:435
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
PointMatcher::DataPoints::descriptorExists
bool descriptorExists(const std::string &name) const
Look if a descriptor with a given name exist.
Definition: pointmatcher/DataPoints.cpp:582
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
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:532
PointMatcherIO::SplitTime
Storage for time loaded separatly.
Definition: IO.h:198
PointMatcherIO::PLYProperty
Interface for PLY property.
Definition: IO.h:267
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:1838
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::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
PointMatcher::DataPoints::getDescriptorStartingRow
unsigned getDescriptorStartingRow(const std::string &name) const
Return the starting row of a descriptor with a given name. Return 0 if the name is not found.
Definition: pointmatcher/DataPoints.cpp:603
PointMatcherIO::saveCSV
static void saveCSV(const DataPoints &data, const std::string &fileName)
Save point cloud to a file as CSV.
Definition: pointmatcher/IO.cpp:836
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:1728
PointMatcher::DataPoints::featureExists
bool featureExists(const std::string &name) const
Look if a feature with a given name exist.
Definition: pointmatcher/DataPoints.cpp:486
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:1287
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:918
PointMatcher::DataPoints::featureLabels
Labels featureLabels
labels of features
Definition: PointMatcher.h:332
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
d
d
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
PointMatcherIO::PLYElement::operator==
bool operator==(const PLYElement &other) const
comparison operator for elements
Definition: pointmatcher/IO.cpp:1831
PointMatcherIO::FileInfoVector
A vector of file info, to be used in batch processing.
Definition: IO.h:245
PointMatcherIO::savePLY
static void savePLY(const DataPoints &data, const std::string &fileName)
save datapoints to PLY point cloud format
Definition: pointmatcher/IO.cpp:1653
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:66
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
dataPath
std::string dataPath
Definition: utest.cpp:43
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
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::savePCD
static void savePCD(const DataPoints &data, const std::string &fileName)
save datapoints to PCD point cloud format
Definition: pointmatcher/IO.cpp:2257
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
PointMatcherIO::FileInfo
Information to exploit a reading from a file using this library. Fields might be left blank if unused...
Definition: IO.h:230
PointMatcher::DataPoints::Label::span
size_t span
number of data dimensions the label spans
Definition: PointMatcher.h:224
InspectorsImpl.h
PointMatcher::DataPoints::getDescriptorDimension
unsigned getDescriptorDimension(const std::string &name) const
Return the dimension of a descriptor with a given name. Return 0 if the name is not found.
Definition: pointmatcher/DataPoints.cpp:596
InspectorsImpl::VTKFileInspector
Definition: InspectorsImpl.h:158
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:1812
param
T param(const std::string &param_name, const T &default_val)
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:1789
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: Bibliography.cpp:45
header
const std::string header
hasHeader
bool hasHeader()
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:1805
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
PointMatcher::DataPoints::save
void save(const std::string &fileName, bool binary=false) const
Save a point cloud to a file, determine format from extension.
Definition: pointmatcher/IO.cpp:809
t
geometry_msgs::TransformStamped t
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:1823
PointMatcherIO::DataPoints
PointMatcher< T >::DataPoints DataPoints
alias
Definition: IO.h:48
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:156
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

mp2p_icp
Author(s):
autogenerated on Mon May 26 2025 02:45:49