46 #include <boost/type_traits/is_same.hpp> 74 HistogramMap::iterator it(
stats.find(name));
75 if (it ==
stats.end()) {
79 it->second.push_back(data);
86 for (BOOST_AUTO(it,
stats.begin()); it !=
stats.end(); ++it)
88 it->second.dumpStats(stream);
91 if (jt !=
stats.end())
99 for (BOOST_AUTO(it,
stats.begin()); it !=
stats.end(); ++it)
101 it->second.dumpStatsHeader(stream);
104 if (jt !=
stats.end())
151 if (boost::is_same<double, T>::value) {
164 stream <<
"# vtk DataFile Version 3.0\n";
165 stream <<
"File created by libpointmatcher\n";
167 stream <<
"DATASET POLYDATA\n";
169 stream <<
"POINTS " << features.cols() <<
" " << getTypeName<T>() <<
"\n";
171 if(features.rows() == 4)
180 stream <<
"VERTICES " << features.cols() <<
" "<< features.cols() * 2 <<
"\n";
181 for (
int i = 0; i < features.cols(); ++i){
183 stream.write(reinterpret_cast<const char*>(&
oneBigEndian),
sizeof(
int));
188 stream.write(converter.
bytes,
sizeof(
int));
190 stream <<
"1 " << i <<
"\n";
196 stream <<
"POINT_DATA " << features.cols() <<
"\n";
202 if(it->text ==
"normals")
206 else if(it->text ==
"eigVectors")
210 else if(it->text ==
"color")
215 else if(it->span == 1)
219 else if(it->span == 3 || it->span == 2)
225 LOG_WARNING_STREAM(
"Could not save label named " << it->text <<
" (dim=" << it->span <<
").");
243 assert(descriptors.cols() >= 15);
245 stream <<
"# vtk DataFile Version 3.0\n";
246 stream <<
"Triangle mesh\n";
248 stream <<
"DATASET POLYDATA\n";
250 stream <<
"POINTS " << descriptors.rows() * 3 <<
" float\n";
251 for (
int i = 0; i < descriptors.rows(); i++)
254 stream << descriptors.block(i, 3, 1, 3) <<
"\n";
255 stream << descriptors.block(i, 6, 1, 3) <<
"\n";
256 stream << descriptors.block(i, 9, 1, 3) <<
"\n";
264 stream <<
"POLYGONS " << descriptors.rows() <<
" " << descriptors.rows() * 4 <<
"\n";
265 for (
int i = 0; i < descriptors.rows(); i++)
267 stream <<
"3 " << (i*3) <<
" " << (i*3 + 1) <<
" " << (i*3 + 2) <<
"\n";
270 stream <<
"CELL_DATA " << descriptors.rows() <<
"\n";
272 stream <<
"NORMALS triangle_normals float\n";
273 stream << descriptors.block(0, 0, descriptors.rows(), 3) <<
"\n";
291 std::ostream& stream)
295 const int refPtCount(refFeatures.cols());
298 const int readingPtCount(readingFeatures.cols());
299 const int totalPtCount(refPtCount+readingPtCount);
301 stream <<
"# vtk DataFile Version 3.0\n";
302 stream <<
"comment\n";
304 stream <<
"DATASET POLYDATA\n";
306 stream <<
"POINTS " << totalPtCount <<
" float\n";
307 if(refFeatures.rows() == 4)
310 stream << refFeatures.topLeftCorner(3, refFeatures.cols()).transpose() <<
"\n";
312 stream << readingFeatures.topLeftCorner(3, readingFeatures.cols()).transpose() <<
"\n";
317 stream << refFeatures.transpose() <<
"\n";
319 stream << readingFeatures.transpose() <<
"\n";
321 const int knn = matches.
ids.rows();
323 size_t matchCount = readingPtCount*
knn;
324 for (
int k = 0; k <
knn; ++k)
326 for (
int i = 0; i < readingPtCount; ++i)
334 stream <<
"LINES " << matchCount <<
" " << matchCount * 3 <<
"\n";
336 for (
int k = 0; k <
knn; k++)
338 for (
int i = 0; i < readingPtCount; ++i)
340 const auto id = matches.
ids(k, i);
342 stream <<
"2 " << refPtCount + i <<
" " <<
id <<
"\n";
347 stream <<
"CELL_DATA " << matchCount <<
"\n";
348 stream <<
"SCALARS outlier float 1\n";
349 stream <<
"LOOKUP_TABLE default\n";
351 for (
int k = 0; k <
knn; k++)
353 for (
int i = 0; i < readingPtCount; ++i)
355 const auto id = matches.
ids(k, i);
357 stream << featureOutlierWeights(k, i) <<
"\n";
386 const size_t iterationNumber,
396 ostream* streamLinks(
openStream(
"link", iterationNumber));
397 dumpDataLinks(filteredReference, reading, matches, outlierWeights, *streamLinks);
402 ostream* streamRead(
openStream(
"reading", iterationNumber));
408 ostream* streamRef(
openStream(
"reference", iterationNumber));
418 if(iterationNumber == 0)
421 for(
unsigned int j = 0; j < transCheck.size(); j++)
423 for(
unsigned int i=0; i < transCheck[j]->getConditionVariableNames().size(); i++)
425 if (!(j == 0 && i == 0))
427 *
streamIter << transCheck[j]->getConditionVariableNames()[i] <<
", ";
428 *
streamIter << transCheck[j]->getLimitNames()[i];
436 for(
unsigned int j = 0; j < transCheck.size(); j++)
438 for(
unsigned int i=0; i < transCheck[j]->getConditionVariables().size(); i++)
441 if (!(j == 0 && i == 0))
444 *
streamIter << transCheck[j]->getConditionVariables()[i] <<
", ";
459 assert(desc.rows() <= forcedDim);
463 if(attribute.compare(
"COLOR_SCALARS") == 0)
465 stream << attribute <<
" " << nameTag <<
" " << forcedDim <<
"\n";
467 std::vector<unsigned char> buffer(forcedDim, 0);
468 for (
int i = 0; i < desc.cols(); ++i){
469 for(
int r=0; r < desc.rows(); ++r){
470 buffer[r] =
static_cast<unsigned int>(desc(r, i) *
static_cast<T>(255) + static_cast<T>(0.5));
472 stream.write(reinterpret_cast<char *>(&buffer.front()), forcedDim);
476 stream <<
padWithOnes(desc, forcedDim, desc.cols()).transpose();
481 stream << attribute <<
" " << nameTag <<
" " << getTypeName<T>() <<
"\n";
482 if(attribute.compare(
"SCALARS") == 0)
483 stream <<
"LOOKUP_TABLE default\n";
538 const Matrix descRef(ref.getDescriptorByName(name));
539 const Matrix descRead(reading.getDescriptorByName(name));
541 if(descRef.rows() != 0 && descRead.rows() != 0)
543 stream <<
"SCALARS " << name <<
" float\n";
544 stream <<
"LOOKUP_TABLE default\n";
550 descRead, 1, reading.
descriptors.cols()).transpose();
563 const Matrix descRef(ref.getDescriptorByName(name));
564 const Matrix descRead(reading.getDescriptorByName(name));
566 if(descRef.rows() != 0 && descRead.rows() != 0)
568 stream <<
"NORMALS " << name <<
" float\n";
574 descRead, 3, reading.
descriptors.cols()).transpose();
587 const Matrix descRef(ref.getDescriptorByName(name));
588 const Matrix descRead(reading.getDescriptorByName(name));
590 if(descRef.rows() != 0 && descRead.rows() != 0)
592 stream <<
"VECTORS " << name <<
" " << getTypeName<T>() <<
"\n";
598 descRead, 3, reading.
descriptors.cols()).transpose();
611 const Matrix descRef(ref.getDescriptorByName(name));
612 const Matrix descRead(reading.getDescriptorByName(name));
614 if(descRef.rows() != 0 && descRead.rows() != 0)
616 stream <<
"TENSORS " << name <<
" float\n";
622 descRead, 9, reading.
descriptors.cols()).transpose();
636 assert(time.rows() == 1);
641 Eigen::Matrix<uint32_t, 1, Eigen::Dynamic> high32(time.cols());
642 Eigen::Matrix<uint32_t, 1, Eigen::Dynamic> low32(time.cols());
644 for(
int i=0; i<time.cols(); i++)
646 high32(0, i) = (uint32_t)(time(0, i) >> 32);
647 low32(0, i) = (uint32_t)time(0, i);
650 stream <<
"SCALARS" <<
" " << name <<
"_splitTime_high32" <<
" " <<
"unsigned_int" <<
"\n";
651 stream <<
"LOOKUP_TABLE default\n";
657 stream <<
"SCALARS" <<
" " << name <<
"_splitTime_low32" <<
" " <<
"unsigned_int" <<
"\n";
658 stream <<
"LOOKUP_TABLE default\n";
668 const int expectedRow,
669 const int expectedCols)
671 assert(m.cols() <= expectedCols || m.rows() <= expectedRow);
672 if(m.cols() == expectedCols && m.rows() == expectedRow)
678 Matrix tmp = Matrix::Zero(expectedRow, expectedCols);
679 tmp.topLeftCorner(m.rows(), m.cols()) = m;
688 const int expectedRow,
689 const int expectedCols)
691 assert(m.cols() <= expectedCols || m.rows() <= expectedRow);
692 if(m.cols() == expectedCols && m.rows() == expectedRow)
698 Matrix tmp = Matrix::Ones(expectedRow, expectedCols);
699 tmp.topLeftCorner(m.rows(), m.cols()) = m;
736 this->
streamIter =
new ofstream(oss.str().c_str());
737 if (this->streamIter->fail())
738 throw std::runtime_error(
"Couldn't open the file \"" + oss.str() +
"\". Check if directory exist.");
752 string filteredStr = role;
753 if(role.substr(role.size()-4,4) ==
".vtk")
754 filteredStr = role.substr(0, role.size()-4);
760 oss << filteredStr <<
".vtk";
764 ofstream*
file =
new ofstream(oss.str().c_str(), std::ios::binary);
766 throw std::runtime_error(
"Couldn't open the file \"" + oss.str() +
"\". Check if directory exist.");
774 oss <<
baseFileName <<
"-" << role <<
"-" << iterationNumber <<
".vtk";
775 ofstream*
file =
new ofstream(oss.str().c_str());
777 throw std::runtime_error(
"Couldn't open the file \"" + oss.str() +
"\". Check if directory exist.");
void dumpMeshNodes(const DataPoints &data, std::ostream &stream)
void buildNormalStream(std::ostream &stream, const std::string &name, const DataPoints &ref, const DataPoints &reading)
virtual void dumpIteration(const size_t iterationNumber, const TransformationParameters ¶meters, const DataPoints &filteredReference, const DataPoints &reading, const Matches &matches, const OutlierWeights &outlierWeights, const TransformationCheckers &transformationCheckers)
Dump the state of a given iteration.
#define LOG_INFO_STREAM(args)
Matrix descriptors
descriptors of points in the cloud, might be empty
bool timeExists(const std::string &name) const
Look if a time with a given name exist.
const bool bDumpReference
const bool bDumpDataLinks
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
void buildTensorStream(std::ostream &stream, const std::string &name, const DataPoints &ref, const DataPoints &reading)
std::string getTypeName()
void buildTimeStream(std::ostream &stream, const std::string &name, const DataPoints &cloud)
virtual void closeStream(std::ostream *stream)
const bool isBigEndian
true if platform is big endian
void dumpDataLinks(const DataPoints &ref, const DataPoints &reading, const Matches &matches, const OutlierWeights &featureOutlierWeights, std::ostream &stream)
Labels timeLabels
labels of times.
VTKFileInspector(const Parameters ¶ms=Parameters())
void buildGenericAttributeStream(std::ostream &stream, const std::string &attribute, const std::string &nameTag, const DataPoints &cloud, const int forcedDim)
std::ostream & writeVtkData(bool writeBinary, const Matrix &data, std::ostream &out)
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
TimeConstView getTimeViewByName(const std::string &name) const
Get a const view on a time by name, throw an exception if it does not exist.
virtual void closeStream(std::ostream *stream)=0
virtual std::ostream * openStream(const std::string &role)=0
const std::string baseFileName
virtual void finish(const size_t iterationCount)
Tell the inspector the ICP operation is completed.
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Functions and classes that are not dependant on scalar type are defined in this namespace.
virtual void init()
Start a new ICP operation or sequence.
An inspector allows to log data at the different steps, for analysis.
PointMatcher< T >::Matrix Matrix
void buildVectorStream(std::ostream &stream, const std::string &name, const DataPoints &ref, const DataPoints &reading)
PointMatcher< T >::OutlierWeights OutlierWeights
Result of the data-association step (Matcher::findClosests), before outlier rejection.
bool descriptorExists(const std::string &name) const
Look if a descriptor with a given name exist.
Parameters parameters
parameters with their values encoded in string
void buildColorStream(std::ostream &stream, const std::string &name, const DataPoints &cloud)
const M::mapped_type & get(const M &m, const typename M::key_type &k)
AbstractVTKInspector(const std::string &className, const ParametersDoc paramsDoc, const Parameters ¶ms)
#define LOG_WARNING_STREAM(args)
The superclass of classes that are constructed using generic parameters. This class provides the para...
const int oneBigEndian
is always a big endian independent of the platforms endianness
std::vector< ParameterDoc > ParametersDoc
The documentation of all parameters.
S get(const std::string ¶mName)
Return the value of paramName, lexically-casted to S.
const std::string className
name of the class
Matrix padWithZeros(const Matrix m, const int expectedRow, const int expectedCols)
virtual void finish(const size_t iterationCount)
Tell the inspector the ICP operation is completed.
virtual std::ostream * openStream(const std::string &role)
Matrix padWithOnes(const Matrix m, const int expectedRow, const int expectedCols)
std::ostream * streamIter
void dumpDataPoints(const DataPoints &data, std::ostream &stream)
Ids ids
identifiers of closest points
Matrix features
features of points in the cloud
const bool bDumpIterationInfo
static constexpr int InvalidId
void buildScalarStream(std::ostream &stream, const std::string &name, const DataPoints &ref, const DataPoints &reading)
const bool bDumpIterationInfo
Labels descriptorLabels
labels of descriptors
PointMatcher< T >::TransformationParameters TransformationParameters