InspectorsImpl.cpp
Go to the documentation of this file.
1 // kate: replace-tabs off; indent-width 4; indent-mode
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 "InspectorsImpl.h"
37 
38 #include "PointMatcherPrivate.h"
39 #include "IOFunctions.h"
40 
41 #include <cassert>
42 #include <iostream>
43 #include <sstream>
44 #include <fstream>
45 #include <algorithm>
46 #include <boost/type_traits/is_same.hpp>
47 
48 using namespace std;
49 using namespace PointMatcherSupport;
50 
51 template<typename T>
53  Inspector(className,paramsDoc,params),
54  baseFileName(Parametrizable::get<string>("baseFileName")),
55  bDumpPerfOnExit(Parametrizable::get<bool>("dumpPerfOnExit")),
56  bDumpStats(Parametrizable::get<bool>("dumpStats"))
57 
58 {//FIXME: do we need that constructor?
59 }
60 
61 template<typename T>
63  Inspector("PerformanceInspector", PerformanceInspector::availableParameters(), params),
64  baseFileName(Parametrizable::get<string>("baseFileName")),
65  bDumpPerfOnExit(Parametrizable::get<bool>("dumpPerfOnExit")),
66  bDumpStats(Parametrizable::get<bool>("dumpStats"))
67 {}
68 
69 template<typename T>
71 {
72  if (!bDumpStats) return;
73 
74  HistogramMap::iterator it(stats.find(name));
75  if (it == stats.end()) {
76  LOG_INFO_STREAM("Adding new stat: " << name);
77  it = stats.insert(HistogramMap::value_type(name, Histogram(16, name, baseFileName, bDumpPerfOnExit))).first;
78  }
79  it->second.push_back(data);
80 }
81 
82 template<typename T>
84 {
85  // Note: this dump format will most probably change in the future
86  for (BOOST_AUTO(it, stats.begin()); it != stats.end(); ++it)
87  {
88  it->second.dumpStats(stream);
89  BOOST_AUTO(jt, it);
90  ++jt;
91  if (jt != stats.end())
92  stream << ", ";
93  }
94 }
95 
96 template<typename T>
98 {
99  for (BOOST_AUTO(it, stats.begin()); it != stats.end(); ++it)
100  {
101  it->second.dumpStatsHeader(stream);
102  BOOST_AUTO(jt, it);
103  ++jt;
104  if (jt != stats.end())
105  stream << ", ";
106  }
107 }
108 
111 
112 /*
113  // This code creates a header according to information from the datapoints
114  std::ofstream ofs(fileName.c_str());
115  size_t labelIndex(0);
116  size_t labelComponent(0);
117  const typename DataPoints::Labels labels(data.featureLabels);
118  for (int i = 0; i < features.rows(); ++i)
119  {
120  // display label for row
121  if (labelIndex <= labels.size())
122  {
123  const typename DataPoints::Label label(labels[labelIndex]);
124  ofs << label.text;
125  if (label.span > 1)
126  ofs << "_" << labelComponent;
127  ++labelComponent;
128  if (labelComponent >= label.span)
129  ++labelIndex;
130  }
131  else
132  {
133  ofs << "?";
134  }
135  ofs << " ";
136  }*/
137 template<typename T>
139  PerformanceInspector(className,paramsDoc,params),
140  streamIter(0),
141  bDumpIterationInfo(Parametrizable::get<bool>("dumpIterationInfo")),
142  bDumpDataLinks(Parametrizable::get<bool>("dumpDataLinks")),
143  bDumpReading(Parametrizable::get<bool>("dumpReading")),
144  bDumpReference(Parametrizable::get<bool>("dumpReference")),
145  bWriteBinary(Parametrizable::get<bool>("writeBinary")),
146  precision(Parametrizable::get<unsigned>("precision"))
147 {
148 }
149 
150 template<typename T>
152  if (boost::is_same<double, T>::value) {
153  return "double";
154  } else {
155  return "float";
156  }
157 }
158 
159 template<typename T>
161 {
162  const Matrix& features(data.features);
163  //const Matrix& descriptors(data.descriptors);
164 
165  stream << "# vtk DataFile Version 3.0\n";
166  stream << "File created by libpointmatcher\n";
167  stream << (bWriteBinary ? "BINARY":"ASCII") << "\n";
168  stream << "DATASET POLYDATA\n";
169 
170  stream << "POINTS " << features.cols() << " " << getTypeName<T>() << "\n";
171 
172  if(features.rows() == 4)
173  {
174  writeVtkData(bWriteBinary, features.topLeftCorner(3, features.cols()).transpose(), stream) << "\n";
175  }
176  else
177  {
178  writeVtkData(bWriteBinary, features.transpose(), stream) << "\n";
179  }
180 
181  stream << "VERTICES " << features.cols() << " "<< features.cols() * 2 << "\n";
182  for (int i = 0; i < features.cols(); ++i){
183  if(bWriteBinary){
184  stream.write(reinterpret_cast<const char*>(&oneBigEndian), sizeof(int));
185  ConverterToAndFromBytes<int> converter(i);
186  if(!isBigEndian){
187  converter.swapBytes();
188  }
189  stream.write(converter.bytes, sizeof(int));
190  }else {
191  stream << "1 " << i << "\n";
192  }
193  }
194 
195 
196  // Save points
197  stream << "POINT_DATA " << features.cols() << "\n";
198 
199  // Loop through all descriptor and dispatch appropriate VTK tags
200  for(BOOST_AUTO(it, data.descriptorLabels.begin()); it != data.descriptorLabels.end(); it++)
201  {
202  // handle specific cases
203  if(it->text == "normals")
204  {
205  buildNormalStream(stream, "normals", data);
206  }
207  else if(it->text == "eigVectors")
208  {
209  buildTensorStream(stream, "eigVectors", data);
210  }
211  else if(it->text == "color")
212  {
213  buildColorStream(stream, "color", data);
214  }
215  // handle generic cases
216  else if(it->span == 1)
217  {
218  buildScalarStream(stream, it->text, data);
219  }
220  else if(it->span == 3 || it->span == 2)
221  {
222  buildVectorStream(stream, it->text, data);
223  }
224  else
225  {
226  LOG_WARNING_STREAM("Could not save label named " << it->text << " (dim=" << it->span << ").");
227  }
228  }
229 
230  // Loop through all time fields, split in high 32 bits and low 32 bits and export as two scalar
231  for(BOOST_AUTO(it, data.timeLabels.begin()); it != data.timeLabels.end(); it++)
232  {
233  buildTimeStream(stream, it->text, data);
234  }
235 
236 }
237 
238 template<typename T>
240 {
241  //const Matrix& features(data.features);
242  const Matrix& descriptors(data.descriptors.transpose());
243 
244  assert(descriptors.cols() >= 15);
245 
246  stream << "# vtk DataFile Version 3.0\n";
247  stream << "Triangle mesh\n";
248  stream << "ASCII\n";
249  stream << "DATASET POLYDATA\n";
250 
251  stream << "POINTS " << descriptors.rows() * 3 << " float\n"; // not optimal: points and edges are drawn several times!
252  for (int i = 0; i < descriptors.rows(); i++)
253  {
254  // TODO: use getDescriptorByName(nameTag) to access blocks
255  stream << descriptors.block(i, 3, 1, 3) << "\n";
256  stream << descriptors.block(i, 6, 1, 3) << "\n";
257  stream << descriptors.block(i, 9, 1, 3) << "\n";
258  }
259 
260  //TODO: add centroids...
261  /*
262  stream << features.transpose() << "\n";
263  */
264 
265  stream << "POLYGONS " << descriptors.rows() << " " << descriptors.rows() * 4 << "\n";
266  for (int i = 0; i < descriptors.rows(); i++)
267  {
268  stream << "3 " << (i*3) << " " << (i*3 + 1) << " " << (i*3 + 2) << "\n";
269  }
270 
271  stream << "CELL_DATA " << descriptors.rows() << "\n";
272 
273  stream << "NORMALS triangle_normals float\n";
274  stream << descriptors.block(0, 0, descriptors.rows(), 3) << "\n";
275 
276  // TODO: use descriptor labels, particularly to represent normals
277  //stream << "POINT_DATA " << descriptors.rows() << "\n";
278  //buildScalarStream(stream, "densities", data);
279  //buildNormalStream(stream, "normals", data);
280  //buildVectorStream(stream, "eigValues", data);
281  //buildTensorStream(stream, "eigVectors", data);
282  //buildVectorStream(stream, "observationDirections", data);
283 }
284 
285 // FIXME:rethink how we dump stuff (accumulate in a correctly-referenced table, and then dump?) and unify with previous
286 template<typename T>
288  const DataPoints& ref,
289  const DataPoints& reading,
290  const Matches& matches,
291  const OutlierWeights& featureOutlierWeights,
292  std::ostream& stream)
293 {
294 
295  const Matrix& refFeatures(ref.features);
296  const int refPtCount(refFeatures.cols());
297  //const int featDim(refFeatures.rows());
298  const Matrix& readingFeatures(reading.features);
299  const int readingPtCount(readingFeatures.cols());
300  const int totalPtCount(refPtCount+readingPtCount);
301 
302  stream << "# vtk DataFile Version 3.0\n";
303  stream << "comment\n";
304  stream << "ASCII\n";
305  stream << "DATASET POLYDATA\n";
306 
307  stream << "POINTS " << totalPtCount << " float\n";
308  if(refFeatures.rows() == 4)
309  {
310  // reference pt
311  stream << refFeatures.topLeftCorner(3, refFeatures.cols()).transpose() << "\n";
312  // reading pt
313  stream << readingFeatures.topLeftCorner(3, readingFeatures.cols()).transpose() << "\n";
314  }
315  else
316  {
317  // reference pt
318  stream << refFeatures.transpose() << "\n";
319  // reading pt
320  stream << readingFeatures.transpose() << "\n";
321  }
322  const int knn = matches.ids.rows();
323 
324  size_t matchCount = readingPtCount*knn;
325  for (int k = 0; k < knn; ++k)
326  {
327  for (int i = 0; i < readingPtCount; ++i)
328  {
329  if (matches.ids(k, i) == Matches::InvalidId){
330  matchCount --;
331  }
332  }
333  }
334 
335  stream << "LINES " << matchCount << " " << matchCount * 3 << "\n";
336  //int j = 0;
337  for (int k = 0; k < knn; k++) // knn
338  {
339  for (int i = 0; i < readingPtCount; ++i)
340  {
341  const auto id = matches.ids(k, i);
342  if (id != Matches::InvalidId){
343  stream << "2 " << refPtCount + i << " " << id << "\n";
344  }
345  }
346  }
347 
348  stream << "CELL_DATA " << matchCount << "\n";
349  stream << "SCALARS outlier float 1\n";
350  stream << "LOOKUP_TABLE default\n";
351  //stream << "LOOKUP_TABLE alphaOutlier\n";
352  for (int k = 0; k < knn; k++) // knn
353  {
354  for (int i = 0; i < readingPtCount; ++i) //nb pts
355  {
356  const auto id = matches.ids(k, i);
357  if (id != Matches::InvalidId){
358  stream << featureOutlierWeights(k, i) << "\n";
359  }
360  }
361  }
362 
363  //stream << "LOOKUP_TABLE alphaOutlier 2\n";
364  //stream << "1 0 0 0.5\n";
365  //stream << "0 1 0 1\n";
366 
367 }
368 
369 template<typename T>
371 {
372  ostream* stream(openStream(name));
373  stream->precision(precision);
374  dumpDataPoints(filteredReference, *stream);
375  closeStream(stream);
376 }
377 
378 template<typename T>
380 {
381  ostream* stream(openStream(name));
382  dumpMeshNodes(filteredReference, *stream);
383  closeStream(stream);
384 }
385 
386 template<typename T>
388  const size_t iterationNumber,
389  const TransformationParameters& parameters,
390  const DataPoints& filteredReference,
391  const DataPoints& reading,
392  const Matches& matches,
393  const OutlierWeights& outlierWeights,
394  const TransformationCheckers& transCheck)
395 {
396 
397  if (bDumpDataLinks){
398  ostream* streamLinks(openStream("link", iterationNumber));
399  dumpDataLinks(filteredReference, reading, matches, outlierWeights, *streamLinks);
400  closeStream(streamLinks);
401  }
402 
403  if (bDumpReading){
404  ostream* streamRead(openStream("reading", iterationNumber));
405  dumpDataPoints(reading, *streamRead);
406  closeStream(streamRead);
407  }
408 
409  if (bDumpReference){
410  ostream* streamRef(openStream("reference", iterationNumber));
411  dumpDataPoints(filteredReference, *streamRef);
412  closeStream(streamRef);
413  }
414 
415  if (!bDumpIterationInfo) return;
416 
417  // streamIter must be define by children
418  assert(streamIter);
419 
420  if(iterationNumber == 0)
421  {
422  //Build header
423  for(unsigned int j = 0; j < transCheck.size(); j++)
424  {
425  for(unsigned int i=0; i < transCheck[j]->getConditionVariableNames().size(); i++)
426  {
427  if (!(j == 0 && i == 0))
428  *streamIter << ", ";
429  *streamIter << transCheck[j]->getConditionVariableNames()[i] << ", ";
430  *streamIter << transCheck[j]->getLimitNames()[i];
431  }
432  }
433 
434  *streamIter << "\n";
435  }
436 
437 
438  for(unsigned int j = 0; j < transCheck.size(); j++)
439  {
440  for(unsigned int i=0; i < transCheck[j]->getConditionVariables().size(); i++)
441  {
442 
443  if (!(j == 0 && i == 0))
444  *streamIter << ", ";
445 
446  *streamIter << transCheck[j]->getConditionVariables()[i] << ", ";
447  *streamIter << transCheck[j]->getLimits()[i];
448  }
449  }
450 
451  *streamIter << "\n";
452 }
453 
454 template<typename T>
455 void InspectorsImpl<T>::AbstractVTKInspector::buildGenericAttributeStream(std::ostream& stream, const std::string& attribute, const std::string& nameTag, const DataPoints& cloud, const int forcedDim)
456 {
457  if (!cloud.descriptorExists(nameTag))
458  return;
459 
460  const BOOST_AUTO(desc, cloud.getDescriptorViewByName(nameTag));
461  assert(desc.rows() <= forcedDim);
462 
463  if(desc.rows() != 0)
464  {
465  if(attribute.compare("COLOR_SCALARS") == 0)
466  {
467  stream << attribute << " " << nameTag << " " << forcedDim << "\n";
468  if(bWriteBinary){
469  std::vector<unsigned char> buffer(forcedDim, 0);
470  for (int i = 0; i < desc.cols(); ++i){
471  for(int r=0; r < desc.rows(); ++r){
472  buffer[r] = static_cast<unsigned int>(desc(r, i) * static_cast<T>(255) + static_cast<T>(0.5)); // this is how libvtk implements it ( vtkScalarsToColors::ColorToUChar )
473  }
474  stream.write(reinterpret_cast<char *>(&buffer.front()), forcedDim);
475  }
476  }
477  else {
478  stream << padWithOnes(desc, forcedDim, desc.cols()).transpose();
479  }
480  }
481  else
482  {
483  stream << attribute << " " << nameTag << " " << getTypeName<T>() << "\n";
484  if(attribute.compare("SCALARS") == 0)
485  stream << "LOOKUP_TABLE default\n";
486 
487  writeVtkData(bWriteBinary, padWithZeros(desc, forcedDim, desc.cols()).transpose(), stream);
488  }
489  stream << "\n";
490  }
491 }
492 
493 template<typename T>
495  const std::string& name,
496  const DataPoints& cloud)
497 {
498  buildGenericAttributeStream(stream, "SCALARS", name, cloud, 1);
499 }
500 
501 template<typename T>
503  const std::string& name,
504  const DataPoints& cloud)
505 {
506  buildGenericAttributeStream(stream, "NORMALS", name, cloud, 3);
507 }
508 
509 template<typename T>
511  const std::string& name,
512  const DataPoints& cloud)
513 {
514  buildGenericAttributeStream(stream, "VECTORS", name, cloud, 3);
515 }
516 
517 template<typename T>
519  const std::string& name,
520  const DataPoints& cloud)
521 {
522  buildGenericAttributeStream(stream, "TENSORS", name, cloud, 9);
523 }
524 
525 template<typename T>
527  const std::string& name,
528  const DataPoints& cloud)
529 {
530  buildGenericAttributeStream(stream, "COLOR_SCALARS", name, cloud, 4);
531 }
532 
533 template<typename T>
535  const std::string& name,
536  const DataPoints& ref,
537  const DataPoints& reading)
538 {
539 
540  const Matrix descRef(ref.getDescriptorByName(name));
541  const Matrix descRead(reading.getDescriptorByName(name));
542 
543  if(descRef.rows() != 0 && descRead.rows() != 0)
544  {
545  stream << "SCALARS " << name << " float\n";
546  stream << "LOOKUP_TABLE default\n";
547 
548  stream << padWithZeros(
549  descRef, 1, ref.descriptors.cols()).transpose();
550  stream << "\n";
551  stream << padWithZeros(
552  descRead, 1, reading.descriptors.cols()).transpose();
553  stream << "\n";
554  }
555 }
556 
557 
558 template<typename T>
560  const std::string& name,
561  const DataPoints& ref,
562  const DataPoints& reading)
563 {
564 
565  const Matrix descRef(ref.getDescriptorByName(name));
566  const Matrix descRead(reading.getDescriptorByName(name));
567 
568  if(descRef.rows() != 0 && descRead.rows() != 0)
569  {
570  stream << "NORMALS " << name << " float\n";
571 
572  stream << padWithZeros(
573  descRef, 3, ref.descriptors.cols()).transpose();
574  stream << "\n";
575  stream << padWithZeros(
576  descRead, 3, reading.descriptors.cols()).transpose();
577  stream << "\n";
578  }
579 }
580 
581 
582 template<typename T>
584  const std::string& name,
585  const DataPoints& ref,
586  const DataPoints& reading)
587 {
588 
589  const Matrix descRef(ref.getDescriptorByName(name));
590  const Matrix descRead(reading.getDescriptorByName(name));
591 
592  if(descRef.rows() != 0 && descRead.rows() != 0)
593  {
594  stream << "VECTORS " << name << " " << getTypeName<T>() << "\n";
595 
596  stream << padWithZeros(
597  descRef, 3, ref.descriptors.cols()).transpose();
598  stream << "\n";
599  stream << padWithZeros(
600  descRead, 3, reading.descriptors.cols()).transpose();
601  stream << "\n";
602  }
603 }
604 
605 
606 template<typename T>
608  const std::string& name,
609  const DataPoints& ref,
610  const DataPoints& reading)
611 {
612 
613  const Matrix descRef(ref.getDescriptorByName(name));
614  const Matrix descRead(reading.getDescriptorByName(name));
615 
616  if(descRef.rows() != 0 && descRead.rows() != 0)
617  {
618  stream << "TENSORS " << name << " float\n";
619 
620  stream << padWithZeros(
621  descRef, 9, ref.descriptors.cols()).transpose();
622  stream << "\n";
623  stream << padWithZeros(
624  descRead, 9, reading.descriptors.cols()).transpose();
625  stream << "\n";
626  }
627 }
628 
629 template<typename T>
631 {
632  //TODO: this check is a reminder of the old implementation. Check
633  // if we still need that. FP
634  if (!cloud.timeExists(name))
635  return;
636 
637  const BOOST_AUTO(time, cloud.getTimeViewByName(name));
638  assert(time.rows() == 1);
639 
640  // Loop through the array to split the lower and higher part of int64_t
641  // TODO: if an Eigen matrix operator can do it without loop, change that
642 
643  Eigen::Matrix<uint32_t, 1, Eigen::Dynamic> high32(time.cols());
644  Eigen::Matrix<uint32_t, 1, Eigen::Dynamic> low32(time.cols());
645 
646  for(int i=0; i<time.cols(); i++)
647  {
648  high32(0, i) = (uint32_t)(time(0, i) >> 32);
649  low32(0, i) = (uint32_t)time(0, i);
650  }
651 
652  stream << "SCALARS" << " " << name << "_splitTime_high32" << " " << "unsigned_int" << "\n";
653  stream << "LOOKUP_TABLE default\n";
654 
655  writeVtkData(bWriteBinary, high32.transpose(), stream);
656 
657  stream << "\n";
658 
659  stream << "SCALARS" << " " << name << "_splitTime_low32" << " " << "unsigned_int" << "\n";
660  stream << "LOOKUP_TABLE default\n";
661 
662  writeVtkData(bWriteBinary, low32.transpose(), stream);
663 
664  stream << "\n";
665 }
666 
667 template<typename T>
669  const Matrix m,
670  const int expectedRow,
671  const int expectedCols)
672 {
673  assert(m.cols() <= expectedCols || m.rows() <= expectedRow);
674  if(m.cols() == expectedCols && m.rows() == expectedRow)
675  {
676  return m;
677  }
678  else
679  {
680  Matrix tmp = Matrix::Zero(expectedRow, expectedCols);
681  tmp.topLeftCorner(m.rows(), m.cols()) = m;
682  return tmp;
683  }
684 
685 }
686 
687 template<typename T>
689  const Matrix m,
690  const int expectedRow,
691  const int expectedCols)
692 {
693  assert(m.cols() <= expectedCols || m.rows() <= expectedRow);
694  if(m.cols() == expectedCols && m.rows() == expectedRow)
695  {
696  return m;
697  }
698  else
699  {
700  Matrix tmp = Matrix::Ones(expectedRow, expectedCols);
701  tmp.topLeftCorner(m.rows(), m.cols()) = m;
702  return tmp;
703  }
704 
705 }
706 
707 template<typename T>
708 void InspectorsImpl<T>::AbstractVTKInspector::finish(const size_t iterationCount)
709 {
710 }
711 
712 
713 //-----------------------------------
714 // VTK File inspector
715 
716 template<typename T>
718  AbstractVTKInspector("VTKFileInspector", VTKFileInspector::availableParameters(), params),
719  baseFileName(Parametrizable::get<string>("baseFileName")),
720  bDumpIterationInfo(Parametrizable::get<bool>("dumpIterationInfo")),
721  bDumpDataLinks(Parametrizable::get<bool>("dumpDataLinks")),
722  bDumpReading(Parametrizable::get<bool>("dumpReading")),
723  bDumpReference(Parametrizable::get<bool>("dumpReference")),
724  precision(Parametrizable::get<unsigned>("precision"))
725 {
726 }
727 
728 template<typename T>
730 {
731 
732  if (!bDumpIterationInfo) return;
733 
734  ostringstream oss;
735  oss << baseFileName << "-iterationInfo.csv";
736  //std::cerr << "writing to " << oss.str() << std::endl;
737  LOG_INFO_STREAM("writing to " << oss.str());
738 
739  this->streamIter = new ofstream(oss.str().c_str());
740  if (this->streamIter->fail())
741  throw std::runtime_error("Couldn't open the file \"" + oss.str() + "\". Check if directory exist.");
742 
743 }
744 
745 template<typename T>
746 void InspectorsImpl<T>::VTKFileInspector::finish(const size_t iterationCount)
747 {
748  if (!bDumpIterationInfo) return;
749  closeStream(this->streamIter);
750 }
751 
752 template<typename T>
754 {
755  string filteredStr = role;
756  if(role.substr(role.size()-4,4) == ".vtk")
757  filteredStr = role.substr(0, role.size()-4);
758 
759  ostringstream oss;
760  if(baseFileName != "")
761  oss << baseFileName << "-" << filteredStr << ".vtk";
762  else
763  oss << filteredStr << ".vtk";
764 
765  //std::cerr << "writing to " << oss.str() << std::endl;
766  LOG_INFO_STREAM("writing to " << oss.str());
767  ofstream* file = new ofstream(oss.str().c_str(), std::ios::binary);
768  if (file->fail())
769  throw std::runtime_error("Couldn't open the file \"" + oss.str() + "\". Check if directory exist.");
770  return file;
771 }
772 
773 template<typename T>
774 std::ostream* InspectorsImpl<T>::VTKFileInspector::openStream(const std::string& role, const size_t iterationNumber)
775 {
776  ostringstream oss;
777  oss << baseFileName << "-" << role << "-" << iterationNumber << ".vtk";
778  ofstream* file = new ofstream(oss.str().c_str());
779  if (file->fail())
780  throw std::runtime_error("Couldn't open the file \"" + oss.str() + "\". Check if directory exist.");
781  return file;
782 }
783 
784 template<typename T>
786 {
787  delete stream;
788 }
789 
790 
791 
InspectorsImpl::AbstractVTKInspector::dumpDataLinks
void dumpDataLinks(const DataPoints &ref, const DataPoints &reading, const Matches &matches, const OutlierWeights &featureOutlierWeights, std::ostream &stream)
Definition: InspectorsImpl.cpp:287
PointMatcher::DataPoints::timeExists
bool timeExists(const std::string &name) const
Look if a time with a given name exist.
Definition: pointmatcher/DataPoints.cpp:688
InspectorsImpl::AbstractVTKInspector::buildScalarStream
void buildScalarStream(std::ostream &stream, const std::string &name, const DataPoints &ref, const DataPoints &reading)
Definition: InspectorsImpl.cpp:534
InspectorsImpl::AbstractVTKInspector::dumpIteration
virtual void dumpIteration(const size_t iterationNumber, const TransformationParameters &parameters, const DataPoints &filteredReference, const DataPoints &reading, const Matches &matches, const OutlierWeights &outlierWeights, const TransformationCheckers &transformationCheckers)
Dump the state of a given iteration.
Definition: InspectorsImpl.cpp:387
InspectorsImpl::AbstractVTKInspector::padWithZeros
Matrix padWithZeros(const Matrix m, const int expectedRow, const int expectedCols)
Definition: InspectorsImpl.cpp:668
compute_overlap.knn
int knn
Definition: compute_overlap.py:128
InspectorsImpl::PerformanceInspector
Definition: InspectorsImpl.h:72
build_map.T
T
Definition: build_map.py:34
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
LOG_INFO_STREAM
#define LOG_INFO_STREAM(args)
Definition: PointMatcherPrivate.h:58
PointMatcher::DataPoints::descriptorExists
bool descriptorExists(const std::string &name) const
Look if a descriptor with a given name exist.
Definition: pointmatcher/DataPoints.cpp:583
InspectorsImpl::AbstractVTKInspector::buildGenericAttributeStream
void buildGenericAttributeStream(std::ostream &stream, const std::string &attribute, const std::string &nameTag, const DataPoints &cloud, const int forcedDim)
Definition: InspectorsImpl.cpp:455
PointMatcherPrivate.h
InspectorsImpl::AbstractVTKInspector
Definition: InspectorsImpl.h:107
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
testing::internal::string
::std::string string
Definition: gtest.h:1979
setup.file
file
Definition: setup.py:28
InspectorsImpl::AbstractVTKInspector::dumpDataPoints
void dumpDataPoints(const DataPoints &data, std::ostream &stream)
Definition: InspectorsImpl.cpp:160
IOFunctions.h
InspectorsImpl::VTKFileInspector::closeStream
virtual void closeStream(std::ostream *stream)
Definition: InspectorsImpl.cpp:785
PointMatcherSupport::writeVtkData
std::ostream & writeVtkData(bool writeBinary, const Matrix &data, std::ostream &out)
Definition: IOFunctions.h:81
InspectorsImpl::VTKFileInspector::VTKFileInspector
VTKFileInspector(const Parameters &params=Parameters())
Definition: InspectorsImpl.cpp:717
PointMatcherSupport::ConverterToAndFromBytes::swapBytes
void swapBytes()
Definition: IOFunctions.h:62
InspectorsImpl::VTKFileInspector::init
virtual void init()
Start a new ICP operation or sequence.
Definition: InspectorsImpl.cpp:729
PointMatcherSupport::ConverterToAndFromBytes
Definition: IOFunctions.h:52
getTypeName
std::string getTypeName()
Definition: InspectorsImpl.cpp:151
InspectorsImpl::Matrix
PointMatcher< T >::Matrix Matrix
Definition: InspectorsImpl.h:57
InspectorsImpl::OutlierWeights
PointMatcher< T >::OutlierWeights OutlierWeights
Definition: InspectorsImpl.h:54
PointMatcherSupport::Parametrizable::ParametersDoc
std::vector< ParameterDoc > ParametersDoc
The documentation of all parameters.
Definition: Parametrizable.h:187
align_sequence.params
params
Definition: align_sequence.py:13
PointMatcherSupport::oneBigEndian
const int oneBigEndian
is always a big endian independent of the platforms endianness
Definition: pointmatcher/IO.cpp:63
InspectorsImpl::PerformanceInspector::dumpStats
virtual void dumpStats(std::ostream &stream)
Dump all statistics in CSV format.
Definition: InspectorsImpl.cpp:83
InspectorsImpl::AbstractVTKInspector::finish
virtual void finish(const size_t iterationCount)
Tell the inspector the ICP operation is completed.
Definition: InspectorsImpl.cpp:708
icp_advance_api.matches
matches
Definition: icp_advance_api.py:114
icp.data
data
Definition: icp.py:50
PointMatcher::Matrix
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
Definition: PointMatcher.h:169
PointMatcherSupport::ConverterToAndFromBytes::bytes
char bytes[sizeof(T)]
Definition: IOFunctions.h:57
PointMatcher::DataPoints::getTimeViewByName
TimeConstView getTimeViewByName(const std::string &name) const
Get a const view on a time by name, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:659
LOG_WARNING_STREAM
#define LOG_WARNING_STREAM(args)
Definition: PointMatcherPrivate.h:68
icp.ref
ref
Definition: icp.py:49
InspectorsImpl::AbstractVTKInspector::buildColorStream
void buildColorStream(std::ostream &stream, const std::string &name, const DataPoints &cloud)
Definition: InspectorsImpl.cpp:526
InspectorsImpl::PerformanceInspector::PerformanceInspector
PerformanceInspector(const std::string &className, const ParametersDoc paramsDoc, const Parameters &params)
Definition: InspectorsImpl.cpp:52
InspectorsImpl::PerformanceInspector::dumpStatsHeader
virtual void dumpStatsHeader(std::ostream &stream)
Dump header for all statistics.
Definition: InspectorsImpl.cpp:97
PointMatcher::Matches
Result of the data-association step (Matcher::findClosests), before outlier rejection.
Definition: PointMatcher.h:371
PointMatcherSupport::Parametrizable
The superclass of classes that are constructed using generic parameters. This class provides the para...
Definition: Parametrizable.h:141
InspectorsImpl::PerformanceInspector::addStat
virtual void addStat(const std::string &name, double data)
Add a value for statistics name, create it if new.
Definition: InspectorsImpl.cpp:70
InspectorsImpl::AbstractVTKInspector::AbstractVTKInspector
AbstractVTKInspector(const std::string &className, const ParametersDoc paramsDoc, const Parameters &params)
Definition: InspectorsImpl.cpp:138
std
PointMatcherSupport::get
const M::mapped_type & get(const M &m, const typename M::key_type &k)
Definition: Bibliography.cpp:57
PointMatcher::Matches::InvalidId
static constexpr int InvalidId
Definition: PointMatcher.h:377
InspectorsImpl::AbstractVTKInspector::padWithOnes
Matrix padWithOnes(const Matrix m, const int expectedRow, const int expectedCols)
Definition: InspectorsImpl.cpp:688
InspectorsImpl::AbstractVTKInspector::buildTimeStream
void buildTimeStream(std::ostream &stream, const std::string &name, const DataPoints &cloud)
Definition: InspectorsImpl.cpp:630
InspectorsImpl.h
InspectorsImpl::AbstractVTKInspector::buildVectorStream
void buildVectorStream(std::ostream &stream, const std::string &name, const DataPoints &ref, const DataPoints &reading)
Definition: InspectorsImpl.cpp:583
PointMatcher::TransformationCheckers
A chain of TransformationChecker.
Definition: PointMatcher.h:612
InspectorsImpl::VTKFileInspector
Definition: InspectorsImpl.h:159
PointMatcher::Inspector
An inspector allows to log data at the different steps, for analysis.
Definition: PointMatcher.h:625
InspectorsImpl::TransformationParameters
PointMatcher< T >::TransformationParameters TransformationParameters
Definition: InspectorsImpl.h:55
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: Bibliography.cpp:45
InspectorsImpl::AbstractVTKInspector::buildTensorStream
void buildTensorStream(std::ostream &stream, const std::string &name, const DataPoints &ref, const DataPoints &reading)
Definition: InspectorsImpl.cpp:607
InspectorsImpl::VTKFileInspector::openStream
virtual std::ostream * openStream(const std::string &role)
Definition: InspectorsImpl.cpp:753
InspectorsImpl::AbstractVTKInspector::buildNormalStream
void buildNormalStream(std::ostream &stream, const std::string &name, const DataPoints &ref, const DataPoints &reading)
Definition: InspectorsImpl.cpp:559
PointMatcherSupport::Histogram
Definition: Histogram.h:46
PointMatcher::DataPoints::getDescriptorViewByName
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:555
InspectorsImpl::AbstractVTKInspector::dumpMeshNodes
void dumpMeshNodes(const DataPoints &data, std::ostream &stream)
Definition: InspectorsImpl.cpp:239
InspectorsImpl::VTKFileInspector::finish
virtual void finish(const size_t iterationCount)
Tell the inspector the ICP operation is completed.
Definition: InspectorsImpl.cpp:746
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Definition: Parametrizable.h:199
compute_overlap.reading
reading
Definition: compute_overlap.py:70


libpointmatcher
Author(s):
autogenerated on Mon Jul 1 2024 02:22:43