InspectorsImpl.cpp
Go to the documentation of this file.
00001 // kate: replace-tabs off; indent-width 4; indent-mode 
00002 // vim: ts=4:sw=4:noexpandtab
00003 /*
00004 
00005 Copyright (c) 2010--2012,
00006 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
00007 You can contact the authors at <f dot pomerleau at gmail dot com> and
00008 <stephane at magnenat dot net>
00009 
00010 All rights reserved.
00011 
00012 Redistribution and use in source and binary forms, with or without
00013 modification, are permitted provided that the following conditions are met:
00014     * Redistributions of source code must retain the above copyright
00015       notice, this list of conditions and the following disclaimer.
00016     * Redistributions in binary form must reproduce the above copyright
00017       notice, this list of conditions and the following disclaimer in the
00018       documentation and/or other materials provided with the distribution.
00019     * Neither the name of the <organization> nor the
00020       names of its contributors may be used to endorse or promote products
00021       derived from this software without specific prior written permission.
00022 
00023 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00024 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00025 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00026 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
00027 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00028 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00030 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00031 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033 
00034 */
00035 
00036 #include "InspectorsImpl.h"
00037 
00038 #include "PointMatcherPrivate.h"
00039 #include "IOFunctions.h"
00040 
00041 #include <cassert>
00042 #include <iostream>
00043 #include <sstream>
00044 #include <fstream>
00045 #include <algorithm>
00046 #include <boost/type_traits/is_same.hpp>
00047 
00048 using namespace std;
00049 using namespace PointMatcherSupport;
00050 
00051 template<typename T>
00052 InspectorsImpl<T>::PerformanceInspector::PerformanceInspector(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params):
00053         Inspector(className,paramsDoc,params),
00054         baseFileName(Parametrizable::get<string>("baseFileName")),
00055         bDumpPerfOnExit(Parametrizable::get<bool>("dumpPerfOnExit")),
00056         bDumpStats(Parametrizable::get<bool>("dumpStats"))
00057 
00058 {//FIXME: do we need that constructor?
00059 }
00060 
00061 template<typename T>
00062 InspectorsImpl<T>::PerformanceInspector::PerformanceInspector(const Parameters& params):
00063         Inspector("PerformanceInspector", PerformanceInspector::availableParameters(), params),
00064         baseFileName(Parametrizable::get<string>("baseFileName")),
00065         bDumpPerfOnExit(Parametrizable::get<bool>("dumpPerfOnExit")),
00066         bDumpStats(Parametrizable::get<bool>("dumpStats"))
00067 {}
00068 
00069 template<typename T>
00070 void InspectorsImpl<T>::PerformanceInspector::addStat(const std::string& name, double data)
00071 {
00072         if (!bDumpStats) return;
00073         
00074         HistogramMap::iterator it(stats.find(name));
00075         if (it == stats.end()) {
00076                 LOG_INFO_STREAM("Adding new stat: " << name);
00077                 it = stats.insert(HistogramMap::value_type(name, Histogram(16, name, baseFileName, bDumpPerfOnExit))).first;
00078         }
00079         it->second.push_back(data);
00080 }
00081 
00082 template<typename T>
00083 void InspectorsImpl<T>::PerformanceInspector::dumpStats(std::ostream& stream)
00084 {
00085         // Note: this dump format will most probably change in the future
00086         for (BOOST_AUTO(it, stats.begin()); it != stats.end(); ++it)
00087         {
00088                 it->second.dumpStats(stream);
00089                 BOOST_AUTO(jt, it);
00090                 ++jt;
00091                 if (jt != stats.end())
00092                         stream << ", ";
00093         }
00094 }
00095 
00096 template<typename T>
00097 void InspectorsImpl<T>::PerformanceInspector::dumpStatsHeader(std::ostream& stream)
00098 {
00099         for (BOOST_AUTO(it, stats.begin()); it != stats.end(); ++it)
00100         {
00101                 it->second.dumpStatsHeader(stream);
00102                 BOOST_AUTO(jt, it);
00103                 ++jt;
00104                 if (jt != stats.end())
00105                         stream << ", ";
00106         }
00107 }
00108 
00109 template struct InspectorsImpl<float>::PerformanceInspector;
00110 template struct InspectorsImpl<double>::PerformanceInspector;
00111 
00112 /*
00113         // This code creates a header according to information from the datapoints
00114         std::ofstream ofs(fileName.c_str());
00115         size_t labelIndex(0);
00116         size_t labelComponent(0);
00117         const typename DataPoints::Labels labels(data.featureLabels);
00118         for (int i = 0; i < features.rows(); ++i)
00119         {
00120                 // display label for row
00121                 if (labelIndex <= labels.size())
00122                 {
00123                         const typename DataPoints::Label label(labels[labelIndex]);
00124                         ofs << label.text;
00125                         if (label.span > 1)
00126                                 ofs << "_" << labelComponent;
00127                         ++labelComponent;
00128                         if (labelComponent >= label.span)
00129                                 ++labelIndex;
00130                 }
00131                 else
00132                 {
00133                         ofs << "?";
00134                 }
00135                 ofs << " ";
00136         }*/
00137 template<typename T>
00138 InspectorsImpl<T>::AbstractVTKInspector::AbstractVTKInspector(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params):
00139         PerformanceInspector(className,paramsDoc,params),
00140         streamIter(0),
00141         bDumpIterationInfo(Parametrizable::get<bool>("dumpIterationInfo")),
00142         bDumpDataLinks(Parametrizable::get<bool>("dumpDataLinks")),
00143         bDumpReading(Parametrizable::get<bool>("dumpReading")),
00144         bDumpReference(Parametrizable::get<bool>("dumpReference")),
00145         bWriteBinary(Parametrizable::get<bool>("writeBinary"))
00146 {
00147 }
00148 
00149 template<typename T>
00150 std::string getTypeName() {
00151         if (boost::is_same<double, T>::value) {
00152                 return "double";
00153         } else {
00154                 return "float";
00155         }
00156 }
00157 
00158 template<typename T>
00159 void InspectorsImpl<T>::AbstractVTKInspector::dumpDataPoints(const DataPoints& data, std::ostream& stream)
00160 {
00161         const Matrix& features(data.features);
00162         //const Matrix& descriptors(data.descriptors);
00163         
00164         stream << "# vtk DataFile Version 3.0\n";
00165         stream << "File created by libpointmatcher\n";
00166         stream << (bWriteBinary ? "BINARY":"ASCII") << "\n";
00167         stream << "DATASET POLYDATA\n";
00168 
00169         stream << "POINTS " << features.cols() << " " << getTypeName<T>() << "\n";
00170 
00171         if(features.rows() == 4)
00172         {
00173                 writeVtkData(bWriteBinary, features.topLeftCorner(3, features.cols()).transpose(), stream) << "\n";
00174         }
00175         else
00176         {
00177                 writeVtkData(bWriteBinary, features.transpose(), stream)  << "\n";
00178         }
00179         
00180         stream << "VERTICES "  << features.cols() << " "<< features.cols() * 2 << "\n";
00181         for (int i = 0; i < features.cols(); ++i){
00182                 if(bWriteBinary){
00183                         stream.write(reinterpret_cast<const char*>(&oneBigEndian), sizeof(int));
00184                         ConverterToAndFromBytes<int> converter(i);
00185                         if(!isBigEndian){
00186                                 converter.swapBytes();
00187                         }
00188                         stream.write(converter.bytes, sizeof(int));
00189                 }else {
00190                         stream << "1 " << i << "\n";
00191                 }
00192         }
00193         
00194 
00195         // Save points
00196         stream << "POINT_DATA " << features.cols() << "\n";
00197 
00198         // Loop through all descriptor and dispatch appropriate VTK tags
00199         for(BOOST_AUTO(it, data.descriptorLabels.begin()); it != data.descriptorLabels.end(); it++)
00200         {
00201                 // handle specific cases
00202                 if(it->text == "normals")
00203                 {
00204                         buildNormalStream(stream, "normals", data);
00205                 }
00206                 else if(it->text == "eigVectors")
00207                 {
00208                         buildTensorStream(stream, "eigVectors", data);
00209                 }
00210                 else if(it->text == "color")
00211                 {
00212                         buildColorStream(stream, "color", data);
00213                 }
00214                 // handle generic cases
00215                 else if(it->span == 1)
00216                 {
00217                         buildScalarStream(stream, it->text, data);
00218                 }
00219                 else if(it->span == 3 || it->span == 2)
00220                 {
00221                         buildVectorStream(stream, it->text, data);
00222                 }
00223                 else
00224                 {
00225                         LOG_WARNING_STREAM("Could not save label named " << it->text << " (dim=" << it->span << ").");
00226                 }
00227         }
00228         
00229         // Loop through all time fields, split in high 32 bits and low 32 bits and export as two scalar
00230         for(BOOST_AUTO(it, data.timeLabels.begin()); it != data.timeLabels.end(); it++)
00231         {
00232                 buildTimeStream(stream, it->text, data);
00233         }
00234 
00235 }
00236 
00237 template<typename T>
00238 void InspectorsImpl<T>::AbstractVTKInspector::dumpMeshNodes(const DataPoints& data, std::ostream& stream)
00239 {
00240         //const Matrix& features(data.features);
00241         const Matrix& descriptors(data.descriptors.transpose());
00242         
00243         assert(descriptors.cols() >= 15);
00244 
00245         stream << "# vtk DataFile Version 3.0\n";
00246         stream << "Triangle mesh\n";
00247         stream << "ASCII\n";
00248         stream << "DATASET POLYDATA\n";
00249 
00250         stream << "POINTS " << descriptors.rows() * 3 << " float\n"; // not optimal: points and edges are drawn several times!
00251         for (int i = 0; i < descriptors.rows(); i++)
00252         {
00253                 // TODO: use getDescriptorByName(nameTag) to access blocks 
00254                 stream << descriptors.block(i, 3, 1, 3) << "\n";
00255                 stream << descriptors.block(i, 6, 1, 3) << "\n";
00256                 stream << descriptors.block(i, 9, 1, 3) << "\n";
00257         }
00258 
00259         //TODO: add centroids...
00260         /*
00261         stream << features.transpose() << "\n";
00262         */
00263 
00264         stream << "POLYGONS " << descriptors.rows() << " " << descriptors.rows() * 4 << "\n";
00265         for (int i = 0; i < descriptors.rows(); i++)
00266         {
00267                 stream << "3 " << (i*3) << " " << (i*3 + 1) << " " << (i*3 + 2) << "\n";
00268         }
00269         
00270         stream << "CELL_DATA " << descriptors.rows() << "\n";
00271 
00272         stream << "NORMALS triangle_normals float\n";
00273         stream << descriptors.block(0, 0, descriptors.rows(), 3) << "\n";
00274 
00275         // TODO: use descriptor labels, particularly to represent normals
00276         //stream << "POINT_DATA " << descriptors.rows() << "\n";
00277         //buildScalarStream(stream, "densities", data);
00278         //buildNormalStream(stream, "normals", data);
00279         //buildVectorStream(stream, "eigValues", data);
00280         //buildTensorStream(stream, "eigVectors", data);
00281         //buildVectorStream(stream, "observationDirections", data);
00282 }
00283 
00284 // FIXME:rethink how we dump stuff (accumulate in a correctly-referenced table, and then dump?) and unify with previous
00285 template<typename T>
00286 void InspectorsImpl<T>::AbstractVTKInspector::dumpDataLinks(
00287         const DataPoints& ref, 
00288         const DataPoints& reading, 
00289         const Matches& matches, 
00290         const OutlierWeights& featureOutlierWeights, 
00291         std::ostream& stream)
00292 {
00293 
00294         const Matrix& refFeatures(ref.features);
00295         const int refPtCount(refFeatures.cols());
00296         //const int featDim(refFeatures.rows());
00297         const Matrix& readingFeatures(reading.features);
00298         const int readingPtCount(readingFeatures.cols());
00299         const int totalPtCount(refPtCount+readingPtCount);
00300         
00301         stream << "# vtk DataFile Version 3.0\n";
00302         stream << "comment\n";
00303         stream << "ASCII\n";
00304         stream << "DATASET POLYDATA\n";
00305         
00306         stream << "POINTS " << totalPtCount << " float\n";
00307         if(refFeatures.rows() == 4)
00308         {
00309                 // reference pt
00310                 stream << refFeatures.topLeftCorner(3, refFeatures.cols()).transpose() << "\n";
00311                 // reading pt
00312                 stream << readingFeatures.topLeftCorner(3, readingFeatures.cols()).transpose() << "\n";
00313         }
00314         else
00315         {
00316                 // reference pt
00317                 stream << refFeatures.transpose() << "\n";
00318                 // reading pt
00319                 stream << readingFeatures.transpose() << "\n";
00320         }
00321         const int knn = matches.ids.rows();
00322         
00323         size_t matchCount = readingPtCount*knn;
00324         for (int k = 0; k < knn; ++k)
00325         {
00326                 for (int i = 0; i < readingPtCount; ++i)
00327                 {
00328                         if (matches.ids(k, i) == Matches::InvalidId){
00329                                 matchCount --;
00330                         }
00331                 }
00332         }
00333 
00334         stream << "LINES " << matchCount << " "  << matchCount * 3 << "\n";
00335         //int j = 0;
00336         for (int k = 0; k < knn; k++) // knn
00337         {
00338                 for (int i = 0; i < readingPtCount; ++i)
00339                 {
00340                         const auto id = matches.ids(k, i);
00341                         if (id != Matches::InvalidId){
00342                                 stream << "2 " << refPtCount + i << " " << id << "\n";
00343                         }
00344                 }
00345         }
00346 
00347         stream << "CELL_DATA " << matchCount << "\n";
00348         stream << "SCALARS outlier float 1\n";
00349         stream << "LOOKUP_TABLE default\n";
00350         //stream << "LOOKUP_TABLE alphaOutlier\n";
00351         for (int k = 0; k < knn; k++) // knn
00352         {
00353                 for (int i = 0; i < readingPtCount; ++i) //nb pts
00354                 {
00355                         const auto id = matches.ids(k, i);
00356                         if (id != Matches::InvalidId){
00357                                 stream << featureOutlierWeights(k, i) << "\n";
00358                         }
00359                 }
00360         }
00361 
00362         //stream << "LOOKUP_TABLE alphaOutlier 2\n";
00363         //stream << "1 0 0 0.5\n";
00364         //stream << "0 1 0 1\n";
00365 
00366 }
00367 
00368 template<typename T>
00369 void InspectorsImpl<T>::AbstractVTKInspector::dumpDataPoints(const DataPoints& filteredReference, const std::string& name)
00370 {
00371         ostream* stream(openStream(name));
00372         dumpDataPoints(filteredReference, *stream);
00373         closeStream(stream);
00374 }
00375 
00376 template<typename T>
00377 void InspectorsImpl<T>::AbstractVTKInspector::dumpMeshNodes(const DataPoints& filteredReference, const std::string& name)
00378 {
00379         ostream* stream(openStream(name));
00380         dumpMeshNodes(filteredReference, *stream);
00381         closeStream(stream);
00382 }
00383 
00384 template<typename T>
00385 void InspectorsImpl<T>::AbstractVTKInspector::dumpIteration(
00386         const size_t iterationNumber,
00387         const TransformationParameters& parameters,
00388         const DataPoints& filteredReference,
00389         const DataPoints& reading,
00390         const Matches& matches,
00391         const OutlierWeights& outlierWeights, 
00392         const TransformationCheckers& transCheck)
00393 {
00394 
00395         if (bDumpDataLinks){
00396                 ostream* streamLinks(openStream("link", iterationNumber));
00397                 dumpDataLinks(filteredReference, reading, matches, outlierWeights, *streamLinks);
00398                 closeStream(streamLinks);
00399         }
00400         
00401         if (bDumpReading){
00402                 ostream* streamRead(openStream("reading", iterationNumber));
00403                 dumpDataPoints(reading, *streamRead);
00404                 closeStream(streamRead);
00405         }
00406         
00407         if (bDumpReference){
00408                 ostream* streamRef(openStream("reference", iterationNumber));
00409                 dumpDataPoints(filteredReference, *streamRef);
00410                 closeStream(streamRef);
00411         }
00412         
00413         if (!bDumpIterationInfo) return;
00414 
00415         // streamIter must be define by children
00416         assert(streamIter);
00417 
00418         if(iterationNumber == 0)
00419         {
00420                 //Build header
00421                 for(unsigned int j = 0; j < transCheck.size(); j++)
00422                 {
00423                         for(unsigned int i=0; i < transCheck[j]->getConditionVariableNames().size(); i++)
00424                         {
00425                                 if (!(j == 0 && i == 0))
00426                                         *streamIter << ", ";
00427                                 *streamIter << transCheck[j]->getConditionVariableNames()[i] << ", "; 
00428                                 *streamIter << transCheck[j]->getLimitNames()[i]; 
00429                         }
00430                 }
00431                 
00432                 *streamIter << "\n";
00433         }
00434 
00435         
00436         for(unsigned int j = 0; j < transCheck.size(); j++)
00437         {
00438                 for(unsigned int i=0; i < transCheck[j]->getConditionVariables().size(); i++)
00439                 {
00440                 
00441                         if (!(j == 0 && i == 0))
00442                                 *streamIter << ", ";
00443 
00444                         *streamIter << transCheck[j]->getConditionVariables()[i] << ", ";
00445                         *streamIter << transCheck[j]->getLimits()[i]; 
00446                 }
00447         }
00448 
00449         *streamIter << "\n";
00450 }
00451 
00452 template<typename T>
00453 void InspectorsImpl<T>::AbstractVTKInspector::buildGenericAttributeStream(std::ostream& stream, const std::string& attribute, const std::string& nameTag, const DataPoints& cloud, const int forcedDim)
00454 {
00455         if (!cloud.descriptorExists(nameTag))
00456                 return;
00457                 
00458         const BOOST_AUTO(desc, cloud.getDescriptorViewByName(nameTag));
00459         assert(desc.rows() <= forcedDim);
00460 
00461         if(desc.rows() != 0)
00462         {
00463                 if(attribute.compare("COLOR_SCALARS") == 0)
00464                 {
00465                         stream << attribute << " " << nameTag << " " << forcedDim << "\n";
00466                         if(bWriteBinary){
00467                                 std::vector<unsigned char> buffer(forcedDim, 0);
00468                                 for (int i = 0; i < desc.cols(); ++i){
00469                                         for(int r=0; r < desc.rows(); ++r){
00470                                                 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 )
00471                                         }
00472                                         stream.write(reinterpret_cast<char *>(&buffer.front()), forcedDim);
00473                                 }
00474                         }
00475                         else {
00476                                 stream << padWithOnes(desc, forcedDim, desc.cols()).transpose();
00477                         }
00478                 }
00479                 else
00480                 {
00481                         stream << attribute << " " << nameTag << " " << getTypeName<T>() << "\n";
00482                         if(attribute.compare("SCALARS") == 0)
00483                                 stream << "LOOKUP_TABLE default\n";
00484 
00485                         writeVtkData(bWriteBinary, padWithZeros(desc, forcedDim, desc.cols()).transpose(), stream);
00486                 }
00487                 stream << "\n";
00488         }
00489 }
00490 
00491 template<typename T>
00492 void InspectorsImpl<T>::AbstractVTKInspector::buildScalarStream(std::ostream& stream,
00493         const std::string& name,
00494         const DataPoints& cloud)
00495 {
00496         buildGenericAttributeStream(stream, "SCALARS", name, cloud, 1);
00497 }
00498 
00499 template<typename T>
00500 void InspectorsImpl<T>::AbstractVTKInspector::buildNormalStream(std::ostream& stream,
00501         const std::string& name,
00502         const DataPoints& cloud)
00503 {
00504         buildGenericAttributeStream(stream, "NORMALS", name, cloud, 3);
00505 }
00506 
00507 template<typename T>
00508 void InspectorsImpl<T>::AbstractVTKInspector::buildVectorStream(std::ostream& stream,
00509         const std::string& name,
00510         const DataPoints& cloud)
00511 {
00512         buildGenericAttributeStream(stream, "VECTORS", name, cloud, 3);
00513 }
00514 
00515 template<typename T>
00516 void InspectorsImpl<T>::AbstractVTKInspector::buildTensorStream(std::ostream& stream,
00517         const std::string& name,
00518         const DataPoints& cloud)
00519 {
00520         buildGenericAttributeStream(stream, "TENSORS", name, cloud, 9);
00521 }
00522 
00523 template<typename T>
00524 void InspectorsImpl<T>::AbstractVTKInspector::buildColorStream(std::ostream& stream,
00525         const std::string& name,
00526         const DataPoints& cloud)
00527 {
00528         buildGenericAttributeStream(stream, "COLOR_SCALARS", name, cloud, 4);
00529 }
00530 
00531 template<typename T>
00532 void InspectorsImpl<T>::AbstractVTKInspector::buildScalarStream(std::ostream& stream,
00533         const std::string& name,
00534         const DataPoints& ref, 
00535         const DataPoints& reading)
00536 {
00537                         
00538         const Matrix descRef(ref.getDescriptorByName(name));    
00539         const Matrix descRead(reading.getDescriptorByName(name));
00540 
00541         if(descRef.rows() != 0 && descRead.rows() != 0)
00542         {
00543                 stream << "SCALARS " << name << " float\n";
00544                 stream << "LOOKUP_TABLE default\n";
00545                 
00546                 stream << padWithZeros(
00547                                 descRef, 1, ref.descriptors.cols()).transpose();
00548                 stream << "\n";
00549                 stream << padWithZeros(
00550                                 descRead, 1, reading.descriptors.cols()).transpose();
00551                 stream << "\n";
00552         }
00553 }
00554 
00555 
00556 template<typename T>
00557 void InspectorsImpl<T>::AbstractVTKInspector::buildNormalStream(std::ostream& stream,
00558         const std::string& name,
00559         const DataPoints& ref, 
00560         const DataPoints& reading)
00561 {
00562                         
00563         const Matrix descRef(ref.getDescriptorByName(name));    
00564         const Matrix descRead(reading.getDescriptorByName(name));
00565 
00566         if(descRef.rows() != 0 && descRead.rows() != 0)
00567         {
00568                 stream << "NORMALS " << name << " float\n";
00569 
00570                 stream << padWithZeros(
00571                                 descRef, 3, ref.descriptors.cols()).transpose();
00572                 stream << "\n";
00573                 stream << padWithZeros(
00574                                 descRead, 3, reading.descriptors.cols()).transpose();
00575                 stream << "\n";
00576         }
00577 }
00578 
00579 
00580 template<typename T>
00581 void InspectorsImpl<T>::AbstractVTKInspector::buildVectorStream(std::ostream& stream,
00582         const std::string& name,
00583         const DataPoints& ref, 
00584         const DataPoints& reading)
00585 {
00586                         
00587         const Matrix descRef(ref.getDescriptorByName(name));    
00588         const Matrix descRead(reading.getDescriptorByName(name));
00589 
00590         if(descRef.rows() != 0 && descRead.rows() != 0)
00591         {
00592                 stream << "VECTORS " << name << " " << getTypeName<T>() << "\n";
00593 
00594                 stream << padWithZeros(
00595                                 descRef, 3, ref.descriptors.cols()).transpose();
00596                 stream << "\n";
00597                 stream << padWithZeros(
00598                                 descRead, 3, reading.descriptors.cols()).transpose();
00599                 stream << "\n";
00600         }
00601 }
00602 
00603 
00604 template<typename T>
00605 void InspectorsImpl<T>::AbstractVTKInspector::buildTensorStream(std::ostream& stream,
00606         const std::string& name,
00607         const DataPoints& ref, 
00608         const DataPoints& reading)
00609 {
00610                         
00611         const Matrix descRef(ref.getDescriptorByName(name));    
00612         const Matrix descRead(reading.getDescriptorByName(name));
00613 
00614         if(descRef.rows() != 0 && descRead.rows() != 0)
00615         {
00616                 stream << "TENSORS " << name << " float\n";
00617 
00618                 stream << padWithZeros(
00619                                 descRef, 9, ref.descriptors.cols()).transpose();
00620                 stream << "\n";
00621                 stream << padWithZeros(
00622                                 descRead, 9, reading.descriptors.cols()).transpose();
00623                 stream << "\n";
00624         }
00625 }
00626 
00627 template<typename T>
00628 void InspectorsImpl<T>::AbstractVTKInspector::buildTimeStream(std::ostream& stream, const std::string& name, const DataPoints& cloud)
00629 {
00630         //TODO: this check is a reminder of the old implementation. Check
00631         // if we still need that. FP
00632         if (!cloud.timeExists(name))
00633                 return;
00634                 
00635         const BOOST_AUTO(time, cloud.getTimeViewByName(name));
00636         assert(time.rows() == 1);
00637 
00638         // Loop through the array to split the lower and higher part of int64_t
00639         // TODO: if an Eigen matrix operator can do it without loop, change that
00640 
00641         Eigen::Matrix<uint32_t, 1, Eigen::Dynamic> high32(time.cols());
00642         Eigen::Matrix<uint32_t, 1, Eigen::Dynamic> low32(time.cols());
00643 
00644         for(int i=0; i<time.cols(); i++)
00645         {
00646                 high32(0, i) = (uint32_t)(time(0, i) >> 32);
00647                 low32(0, i) = (uint32_t)time(0, i);
00648         }
00649         
00650         stream << "SCALARS" << " " << name << "_splitTime_high32" << " " << "unsigned_int" << "\n";
00651         stream << "LOOKUP_TABLE default\n";
00652 
00653         writeVtkData(bWriteBinary, high32.transpose(), stream);
00654 
00655         stream << "\n";
00656 
00657         stream << "SCALARS" << " " << name << "_splitTime_low32" << " " << "unsigned_int" << "\n";
00658         stream << "LOOKUP_TABLE default\n";
00659 
00660         writeVtkData(bWriteBinary, low32.transpose(), stream);
00661 
00662         stream << "\n";
00663 }
00664 
00665 template<typename T>
00666 typename PointMatcher<T>::Matrix InspectorsImpl<T>::AbstractVTKInspector::padWithZeros(
00667         const Matrix m,
00668         const int expectedRow,
00669         const int expectedCols)
00670 {
00671         assert(m.cols() <= expectedCols || m.rows() <= expectedRow);
00672         if(m.cols() == expectedCols && m.rows() == expectedRow)
00673         {
00674                 return m;
00675         }
00676         else
00677         {
00678                 Matrix tmp = Matrix::Zero(expectedRow, expectedCols); 
00679                 tmp.topLeftCorner(m.rows(), m.cols()) = m;
00680                 return tmp;
00681         }
00682 
00683 }
00684 
00685 template<typename T>
00686 typename PointMatcher<T>::Matrix InspectorsImpl<T>::AbstractVTKInspector::padWithOnes(
00687         const Matrix m,
00688         const int expectedRow,
00689         const int expectedCols)
00690 {
00691         assert(m.cols() <= expectedCols || m.rows() <= expectedRow);
00692         if(m.cols() == expectedCols && m.rows() == expectedRow)
00693         {
00694                 return m;
00695         }
00696         else
00697         {
00698                 Matrix tmp = Matrix::Ones(expectedRow, expectedCols); 
00699                 tmp.topLeftCorner(m.rows(), m.cols()) = m;
00700                 return tmp;
00701         }
00702 
00703 }
00704 
00705 template<typename T>
00706 void InspectorsImpl<T>::AbstractVTKInspector::finish(const size_t iterationCount)
00707 {
00708 }
00709 
00710 
00711 //-----------------------------------
00712 // VTK File inspector
00713 
00714 template<typename T>
00715 InspectorsImpl<T>::VTKFileInspector::VTKFileInspector(const Parameters& params):
00716         AbstractVTKInspector("VTKFileInspector", VTKFileInspector::availableParameters(), params),
00717         baseFileName(Parametrizable::get<string>("baseFileName")),
00718         bDumpIterationInfo(Parametrizable::get<bool>("dumpIterationInfo")),
00719         bDumpDataLinks(Parametrizable::get<bool>("dumpDataLinks")),
00720         bDumpReading(Parametrizable::get<bool>("dumpReading")),
00721         bDumpReference(Parametrizable::get<bool>("dumpReference"))
00722 {
00723 }
00724 
00725 template<typename T>
00726 void InspectorsImpl<T>::VTKFileInspector::init()
00727 {
00728 
00729         if (!bDumpIterationInfo) return;
00730  
00731         ostringstream oss;
00732         oss << baseFileName << "-iterationInfo.csv";
00733         //std::cerr << "writing to " << oss.str() << std::endl;
00734         LOG_INFO_STREAM("writing to " << oss.str());
00735 
00736         this->streamIter = new ofstream(oss.str().c_str());
00737         if (this->streamIter->fail())
00738                 throw std::runtime_error("Couldn't open the file \"" + oss.str() + "\". Check if directory exist.");
00739         
00740 }
00741 
00742 template<typename T>
00743 void InspectorsImpl<T>::VTKFileInspector::finish(const size_t iterationCount)
00744 {
00745         if (!bDumpIterationInfo) return;
00746         closeStream(this->streamIter);
00747 }
00748 
00749 template<typename T>
00750 std::ostream* InspectorsImpl<T>::VTKFileInspector::openStream(const std::string& role)
00751 {
00752         string filteredStr = role;
00753         if(role.substr(role.size()-4,4) == ".vtk")
00754                 filteredStr = role.substr(0, role.size()-4);
00755 
00756         ostringstream oss;
00757         if(baseFileName != "")
00758                 oss << baseFileName << "-" << filteredStr << ".vtk";
00759         else
00760                 oss << filteredStr << ".vtk";
00761 
00762         //std::cerr << "writing to " << oss.str() << std::endl;
00763         LOG_INFO_STREAM("writing to " << oss.str());
00764         ofstream* file = new ofstream(oss.str().c_str(), std::ios::binary);
00765         if (file->fail())
00766                 throw std::runtime_error("Couldn't open the file \"" + oss.str() + "\". Check if directory exist.");
00767         return file;
00768 }
00769 
00770 template<typename T>
00771 std::ostream* InspectorsImpl<T>::VTKFileInspector::openStream(const std::string& role, const size_t iterationNumber)
00772 {
00773         ostringstream oss;
00774         oss << baseFileName << "-" << role << "-" << iterationNumber << ".vtk";
00775         ofstream* file = new ofstream(oss.str().c_str());
00776         if (file->fail())
00777                 throw std::runtime_error("Couldn't open the file \"" + oss.str() + "\". Check if directory exist.");
00778         return file;
00779 }
00780 
00781 template<typename T>
00782 void InspectorsImpl<T>::VTKFileInspector::closeStream(std::ostream* stream)
00783 {
00784         delete stream;
00785 }
00786 
00787 
00788 
00789 template struct InspectorsImpl<float>::VTKFileInspector;
00790 template struct InspectorsImpl<double>::VTKFileInspector;


libpointmatcher
Author(s):
autogenerated on Thu Jun 20 2019 19:51:31