IO.cpp

Go to the documentation of this file.

// kate: replace-tabs off; indent-width 4; indent-mode normal
// vim: ts=4:sw=4:noexpandtab
/*

Copyright (c) 2010--2012,
François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
You can contact the authors at <f dot pomerleau at gmail dot com> and
<stephane at magnenat dot net>

All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "IO.h"
#include "IOFunctions.h"
#include "InspectorsImpl.h"

// For logging
#include "PointMatcherPrivate.h"

#include <iostream>
#include <fstream>
#include <stdexcept>
#include <ctype.h>
#include "boost/algorithm/string.hpp"
#include "boost/filesystem.hpp"
#include "boost/filesystem/path.hpp"
#include "boost/filesystem/operations.hpp"
#include "boost/lexical_cast.hpp"
#include "boost/foreach.hpp"

#ifdef WIN32
#define strtok_r strtok_s
#endif // WIN32

namespace PointMatcherSupport {
        namespace {
                const int one = 1;
        }
        const bool isBigEndian = *reinterpret_cast<const unsigned char*>(&one) == static_cast<unsigned char>(0);
        const int oneBigEndian = isBigEndian ? 1 : 1 << 8 * (sizeof(int) - 1);
}

using namespace std;
using namespace PointMatcherSupport;


// Tokenize a string, excepted if it begins with a '%' (a comment in CSV)
static std::vector<string> csvLineToVector(const char* line)
{
        std::vector<string> parsedLine;
        char delimiters[] = " \t,;";
        char *token;
        char tmpLine[1024];
        char *brkt = 0;
        strcpy(tmpLine, line);
        token = strtok_r(tmpLine, delimiters, &brkt);
        if(line[0] != '%') // Jump line if it's commented
        {
                while (token)
                {
                        parsedLine.push_back(string(token));
                        token = strtok_r(NULL, delimiters, &brkt);
                }
        }

        return parsedLine;
}

// Open and parse a CSV file, return the data
CsvElements parseCsvWithHeader(const std::string& fileName)
{
        validateFile(fileName);
        
        ifstream is(fileName.c_str());

        unsigned elementCount=0;
        std::map<string, unsigned> keywordCols;
        CsvElements data;

        bool firstLine(true);
        unsigned lineCount=0;
        
        string line; 
        while (safeGetLine(is, line))
        {

                if(firstLine)
                {
                        std::vector<string> header = csvLineToVector(line.c_str());
                                
                        elementCount = header.size();
                        for(unsigned int i = 0; i < elementCount; i++)
                        {
                                keywordCols[header[i]] = i;
                        }

                        firstLine = false;
                }
                else // load the rest of the file
                {
                        std::vector<string> parsedLine = csvLineToVector(line.c_str());
                        if(parsedLine.size() != elementCount && parsedLine.size() !=0)
                        {
                                stringstream errorMsg;
                                errorMsg << "Error at line " << lineCount+1 << ": expecting " << elementCount << " columns but read " << parsedLine.size() << " elements.";
                                throw runtime_error(errorMsg.str());    
                        }

                        for(unsigned int i = 0; i < parsedLine.size(); i++)
                        {
                                for(BOOST_AUTO(it,keywordCols.begin()); it!=keywordCols.end(); it++)
                                {
                                        if(i == (*it).second)
                                        {
                                                data[(*it).first].push_back(parsedLine[i]);     
                                        }
                                }
                        }
                }

                lineCount++;
        }
        
        // Use for debug
        
        //for(BOOST_AUTO(it,data.begin()); it!=data.end(); it++)
        //{
        //      cout << "--------------------------" << endl;
        //      cout << "Header: |" << (*it).first << "|" << endl;
        //      //for(unsigned i=0; i<(*it).second.size(); i++)
        //      //{
        //      //      cout << (*it).second[i] << endl;
        //      //}
        //}
        

        return data;
}


template<typename T>
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):
        readingFileName(readingFileName),
        referenceFileName(referenceFileName),
        configFileName(configFileName),
        initialTransformation(initialTransformation),
        groundTruthTransformation(groundTruthTransformation),
        gravity(gravity)
{}

template struct PointMatcherIO<float>::FileInfo;
template struct PointMatcherIO<double>::FileInfo;

// Empty constructor
template<typename T>
PointMatcherIO<T>::FileInfoVector::FileInfoVector()
{
}


template<typename T>
PointMatcherIO<T>::FileInfoVector::FileInfoVector(const std::string& fileName, std::string dataPath, std::string configPath)
{
        if (dataPath.empty())
        {
                #if BOOST_FILESYSTEM_VERSION >= 3
                dataPath = boost::filesystem::path(fileName).parent_path().string();
                #else
                dataPath = boost::filesystem::path(fileName).parent_path().file_string();
                #endif
        }
        if (configPath.empty())
        {
                #if BOOST_FILESYSTEM_VERSION >= 3
                configPath = boost::filesystem::path(fileName).parent_path().string();
                #else
                configPath = boost::filesystem::path(fileName).parent_path().file_string();
                #endif
        }
        
        const CsvElements data = parseCsvWithHeader(fileName);
        
        // Look for transformations
        const bool found3dInitialTrans(findTransform(data, "iT", 3));
        bool found2dInitialTrans(findTransform(data, "iT", 2));
        const bool found3dGroundTruthTrans(findTransform(data, "gT", 3));
        bool found2dGroundTruthTrans(findTransform(data, "gT", 2));
        if (found3dInitialTrans)
                found2dInitialTrans = false;
        if (found3dGroundTruthTrans)
                found2dGroundTruthTrans = false;
        
        // Check for consistency
        if (found3dInitialTrans && found2dGroundTruthTrans)
                throw runtime_error("Initial transformation is in 3D but ground-truth is in 2D");
        if (found2dInitialTrans && found3dGroundTruthTrans)
                throw runtime_error("Initial transformation is in 2D but ground-truth is in 3D");
        CsvElements::const_iterator readingIt(data.find("reading"));
        if (readingIt == data.end())
                throw runtime_error("Error transfering CSV to structure: The header should at least contain \"reading\".");
        CsvElements::const_iterator referenceIt(data.find("reference"));
        CsvElements::const_iterator configIt(data.find("config"));
        
        // Load reading
        const std::vector<string>& readingFileNames = readingIt->second;
        const unsigned lineCount = readingFileNames.size();
        boost::optional<std::vector<string> > referenceFileNames;
        boost::optional<std::vector<string> > configFileNames;
        if (referenceIt != data.end())
        {
                referenceFileNames = referenceIt->second;
                assert (referenceFileNames->size() == lineCount);
        }
        if (configIt != data.end())
        {
                configFileNames = configIt->second;
                assert (configFileNames->size() == lineCount);
        }

        // for every lines
        for(unsigned line=0; line<lineCount; line++)
        {
                FileInfo info;
                
                // Files
                info.readingFileName = localToGlobalFileName(dataPath, readingFileNames[line]);
                if (referenceFileNames)
                        info.referenceFileName = localToGlobalFileName(dataPath, (*referenceFileNames)[line]);
                if (configFileNames)
                        info.configFileName = localToGlobalFileName(configPath, (*configFileNames)[line]);
                
                // Load transformations
                if(found3dInitialTrans)
                        info.initialTransformation = getTransform(data, "iT", 3, line);
                if(found2dInitialTrans)
                        info.initialTransformation = getTransform(data, "iT", 2, line);
                if(found3dGroundTruthTrans)
                        info.groundTruthTransformation = getTransform(data, "gT", 3, line);
                if(found2dGroundTruthTrans)
                        info.groundTruthTransformation = getTransform(data, "gT", 2, line);
                
                // Build the list
                this->push_back(info);
        }
        
        // Debug: Print the list
        /*for(unsigned i=0; i<list.size(); i++)
        {
                cout << "\n--------------------------" << endl;
                cout << "Sequence " << i << ":" << endl;
                cout << "Reading path: " << list[i].readingFileName << endl;
                cout << "Reference path: " << list[i].referenceFileName << endl;
                cout << "Extension: " << list[i].fileExtension << endl;
                cout << "Tranformation:\n" << list[i].initialTransformation << endl;
                cout << "Grativity:\n" << list[i].gravity << endl;
        }
        */
}

template<typename T>
std::string PointMatcherIO<T>::FileInfoVector::localToGlobalFileName(const std::string& parentPath, const std::string& fileName)
{
        std::string globalFileName(fileName);
        if (!boost::filesystem::exists(globalFileName))
        {
                const boost::filesystem::path globalFilePath(boost::filesystem::path(parentPath) /  boost::filesystem::path(fileName));
                #if BOOST_FILESYSTEM_VERSION >= 3
                globalFileName = globalFilePath.string();
                #else
                globalFileName = globalFilePath.file_string();
                #endif
        }
        validateFile(globalFileName);
        return globalFileName;
}

template<typename T>
bool PointMatcherIO<T>::FileInfoVector::findTransform(const CsvElements& data, const std::string& prefix, unsigned dim)
{
        bool found(true);
        for(unsigned i=0; i<dim+1; i++)
        {
                for(unsigned j=0; j<dim+1; j++)
                {
                        stringstream transName;
                        transName << prefix << i << j;
                        found = found && (data.find(transName.str()) != data.end());
                }
        }
        return found;
}

template<typename T>
typename PointMatcherIO<T>::TransformationParameters PointMatcherIO<T>::FileInfoVector::getTransform(const CsvElements& data, const std::string& prefix, unsigned dim, unsigned line)
{
        TransformationParameters transformation(TransformationParameters::Identity(dim+1, dim+1));
        for(unsigned i=0; i<dim+1; i++)
        {
                for(unsigned j=0; j<dim+1; j++)
                {
                        stringstream transName;
                        transName << prefix << i << j;
                        CsvElements::const_iterator colIt(data.find(transName.str()));
                        const T value = boost::lexical_cast<T> (colIt->second[line]);
                        transformation(i,j) = value;
                }
        }
        return transformation;
}

template struct PointMatcherIO<float>::FileInfoVector;
template struct PointMatcherIO<double>::FileInfoVector;

void PointMatcherSupport::validateFile(const std::string& fileName)
{
        boost::filesystem::path fullPath(fileName);

        ifstream ifs(fileName.c_str());
        if (!ifs.good() || !boost::filesystem::is_regular_file(fullPath))
        #if BOOST_FILESYSTEM_VERSION >= 3
                #if BOOST_VERSION >= 105000
                                throw runtime_error(string("Cannot open file ") + boost::filesystem::complete(fullPath).generic_string());
                #else
                                throw runtime_error(string("Cannot open file ") + boost::filesystem3::complete(fullPath).generic_string());
                #endif
        #else
                throw runtime_error(string("Cannot open file ") + boost::filesystem::complete(fullPath).native_file_string());
    #endif
}


template<typename T>
typename PointMatcher<T>::DataPoints PointMatcher<T>::DataPoints::load(const std::string& fileName)
{
        const boost::filesystem::path path(fileName);
        const string& ext(boost::filesystem::extension(path));
        if (boost::iequals(ext, ".vtk"))
                return PointMatcherIO<T>::loadVTK(fileName);
        else if (boost::iequals(ext, ".csv"))
                return PointMatcherIO<T>::loadCSV(fileName);
        else if (boost::iequals(ext, ".ply"))
                return PointMatcherIO<T>::loadPLY(fileName);
        else if (boost::iequals(ext, ".pcd"))
                return PointMatcherIO<T>::loadPCD(fileName);
        else
                throw runtime_error("loadAnyFormat(): Unknown extension \"" + ext + "\" for file \"" + fileName + "\", extension must be either \".vtk\" or \".csv\"");
}

template
PointMatcher<float>::DataPoints PointMatcher<float>::DataPoints::load(const std::string& fileName);
template
PointMatcher<double>::DataPoints PointMatcher<double>::DataPoints::load(const std::string& fileName);


template<typename T>
typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadCSV(const std::string& fileName)
{
        ifstream ifs(fileName.c_str());
        
        validateFile(fileName);

        return loadCSV(ifs);
}

template<typename T>
PointMatcherIO<T>::SupportedLabel::SupportedLabel(const std::string& internalName, const std::string& externalName, const PMPropTypes& type):
        internalName(internalName),
        externalName(externalName),
        type(type)
{
}


// Class LabelGenerator
template<typename T>
void PointMatcherIO<T>::LabelGenerator::add(const std::string internalName)
{
        bool findLabel = false;
        for(size_t i=0; i<labels.size(); ++i)
        {
                if(internalName == labels[i].text)
                {
                        labels[i].span++;
                        findLabel = true;
                        break;
                }
                
        }

        if(!findLabel)
        {
                labels.push_back(Label(internalName,1));
        }
}

template<typename T>
void PointMatcherIO<T>::LabelGenerator::add(const std::string internalName, const unsigned int dim)
{
                labels.push_back(Label(internalName, dim));
}


// Class LabelGenerator
template<typename T>
typename PointMatcher<T>::DataPoints::Labels PointMatcherIO<T>::LabelGenerator::getLabels() const
{
        return labels;
}

template
class PointMatcherIO<float>::LabelGenerator;
template
class PointMatcherIO<double>::LabelGenerator;
template <typename T>


std::string PointMatcherIO<T>::getColLabel(const Label& label, const int row)
{
        std::string externalName;
        if (label.text == "normals")
        {
                if (row == 0)
                {
                        externalName = "nx";
                }
                if (row == 1)
                {
                        externalName = "ny";
                }
                if (row == 2)
                {
                        externalName = "nz";
                }
        }
        else if (label.text == "color")
        {
                if (row == 0)
                {
                        externalName = "red";
                }
                if (row == 1)
                {
                        externalName = "green";
                }
                if (row == 2)
                {
                        externalName = "blue";
                }
                if (row == 3)
                        externalName = "alpha";
        }
        else if (label.text == "eigValues")
        {
                externalName = "eigValues" + boost::lexical_cast<string>(row);
        }
        else if (label.text == "eigVectors")
        {
                // format: eigVectors<0-2><X-Z>
                externalName = "eigVectors" + boost::lexical_cast<string>(row/3);

                int row_mod = row % 3;
                if (row_mod == 0)
                        externalName += "X";
                else if (row_mod == 1)
                        externalName += "Y";
                else if (row_mod == 2)
                        externalName += "Z";
        }
        else if (label.span  == 1)
        {
                externalName = label.text;
        }
        else
                externalName = label.text + boost::lexical_cast<std::string>(row);

        return externalName;
}


template<typename T>
typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadCSV(std::istream& is)
{
        vector<GenericInputHeader> csvHeader;
        LabelGenerator featLabelGen, descLabelGen, timeLabelGen;
        Matrix features;
        Matrix descriptors;
        Int64Matrix times;
        unsigned int csvCol = 0;
        unsigned int csvRow = 0;
        bool hasHeader(false);
        bool firstLine(true);
        

        //count lines in the file
        is.unsetf(std::ios_base::skipws);
        unsigned int line_count = std::count(
                std::istream_iterator<char>(is),
                        std::istream_iterator<char>(), 
                        '\n');
        //reset the stream
        is.clear();
        is.seekg(0, ios::beg);

        char delimiters[] = " \t,;";
        char *token;
        string line;
        while (safeGetLine(is, line))
        {
                
                // Skip empty lines
                if(line.empty())
                        break;
        
                // Look for text header
                unsigned int len = strspn(line.c_str(), " ,+-.1234567890Ee");
                if(len != line.length())
                {
                        //cout << "Header detected" << endl;
                        hasHeader = true;
                }
                else
                {
                        hasHeader = false;
                }

                // Count dimension using first line
                if(firstLine)
                {
                        unsigned int dim = 0;
                        char tmpLine[1024]; //FIXME: might be problematic for large file
                        strcpy(tmpLine, line.c_str());
                        char *brkt = 0;
                        token = strtok_r(tmpLine, delimiters, &brkt);

                        //1- BUILD HEADER
                        while (token)
                        {
                                // Load text header
                                if(hasHeader)
                                {
                                        csvHeader.push_back(GenericInputHeader(string(token)));
                                }
                                dim++;
                                token = strtok_r(NULL, delimiters, &brkt);
                        }
                        
                        if (!hasHeader)
                        {
                                // Check if it is a simple file with only coordinates
                                if (!(dim == 2 || dim == 3))
                                {
                                        int idX=0, idY=0, idZ=0;

                                        cout << "WARNING: " << dim << " columns detected. Not obvious which columns to load for x, y or z." << endl;
                                        cout << endl << "Enter column ID (starting from 0) for x: ";
                                        cin >> idX;
                                        cout << "Enter column ID (starting from 0) for y: ";
                                        cin >> idY;
                                        cout << "Enter column ID (starting from 0, -1 if 2D data) for z: ";
                                        cin >> idZ;

                                        // Fill with unkown column names
                                        for(unsigned int i=0; i<dim; i++)
                                        {
                                                std::ostringstream os;
                                                os << "empty" << i;

                                                csvHeader.push_back(GenericInputHeader(os.str()));
                                        }
                                        
                                        // Overwrite with user inputs
                                        csvHeader[idX] = GenericInputHeader("x");
                                        csvHeader[idY] = GenericInputHeader("y");
                                        if(idZ != -1)
                                                csvHeader[idZ] = GenericInputHeader("z");
                                }
                                else
                                {
                                        // Assume logical order...
                                        csvHeader.push_back(GenericInputHeader("x"));
                                        csvHeader.push_back(GenericInputHeader("y"));
                                        if(dim == 3)
                                                csvHeader.push_back(GenericInputHeader("z"));
                                }
                        }

                        //2- PROCESS HEADER
                        // Load known features, descriptors, and time
                        const SupportedLabels externalLabels = getSupportedExternalLabels();

                        // Counters
                        int rowIdFeatures = 0;
                        int rowIdDescriptors = 0;
                        int rowIdTime = 0;

                        
                        // Loop through all known external names (ordered list)
                        for(size_t i=0; i<externalLabels.size(); i++)
                        {
                                const SupportedLabel supLabel = externalLabels[i];

                                for(size_t j=0; j < csvHeader.size(); j++)
                                {
                                        if(supLabel.externalName == csvHeader[j].name)
                                        {
                                                csvHeader[j].matrixType = supLabel.type;

                                                switch (supLabel.type)
                                                {
                                                        case FEATURE:
                                                                csvHeader[j].matrixRowId = rowIdFeatures;
                                                                featLabelGen.add(supLabel.internalName);
                                                                rowIdFeatures++;
                                                                break;
                                                        case DESCRIPTOR:
                                                                csvHeader[j].matrixRowId = rowIdDescriptors;
                                                                descLabelGen.add(supLabel.internalName);
                                                                rowIdDescriptors++;
                                                                break;
                                                        case TIME:
                                                                csvHeader[j].matrixRowId = rowIdTime;
                                                                timeLabelGen.add(supLabel.internalName);
                                                                rowIdTime++;
                                                                break;
                                                        default:
                                                                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'"));
                                                                break;
                                                }
                                                
                                                // we stop searching once we have a match
                                                break;
                                        }
                                }
                        }

                        // loop through the remaining UNSUPPORTED labels and assigned them to a descriptor row
                        for(unsigned int i=0; i<csvHeader.size(); i++)
                        {
                                if(csvHeader[i].matrixType == UNSUPPORTED)
                                {
                                        csvHeader[i].matrixType = DESCRIPTOR; // force descriptor
                                        csvHeader[i].matrixRowId = rowIdDescriptors;
                                        descLabelGen.add(csvHeader[i].name); // keep original name
                                        rowIdDescriptors++;
                                }
                        }
                

                        //3- RESERVE MEMORY
                        if(hasHeader && line_count > 0)
                                line_count--;

                        const unsigned int featDim = featLabelGen.getLabels().totalDim();
                        const unsigned int descDim = descLabelGen.getLabels().totalDim();
                        const unsigned int timeDim = timeLabelGen.getLabels().totalDim();
                        const unsigned int nbPoints = line_count;

                        features = Matrix(featDim, nbPoints);
                        descriptors = Matrix(descDim, nbPoints);
                        times = Int64Matrix(timeDim, nbPoints);
                }


                //4- LOAD DATA (this start again from the first line)
                char* brkt = 0;
                char line_c[1024];//FIXME: this might be a problem for large files
                strcpy(line_c,line.c_str());
                token = strtok_r(line_c, delimiters, &brkt);
                
                if(!(hasHeader && firstLine))
                {
                        // Parse a line
                        csvCol = 0;
                        while (token)
                        {
                                if(csvCol > (csvHeader.size() - 1))
                                {
                                        // Error check (too much data)
                                        throw runtime_error(
                                        (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());
                                }
                                
                                // Alias
                                const int matrixRow = csvHeader[csvCol].matrixRowId;
                                const int matrixCol = csvRow;
                                
                                switch (csvHeader[csvCol].matrixType)
                                {
                                        case FEATURE:
                                                features(matrixRow, matrixCol) = lexical_cast_scalar_to_string<T>(string(token));
                                                break;
                                        case DESCRIPTOR:
                                                descriptors(matrixRow, matrixCol) = lexical_cast_scalar_to_string<T>(token);
                                                break;
                                        case TIME:
                                                times(matrixRow, matrixCol) = lexical_cast_scalar_to_string<std::int64_t>(token);
                                                break;
                                        default:
                                                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'"));
                                                break;

                                }

                                //fetch next element
                                token = strtok_r(NULL, delimiters, &brkt);
                                csvCol++;
                        }
                
                
                        // Error check (not enough data)
                        if(csvCol != (csvHeader.size()))
                        {
                                throw runtime_error(
                                (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());
                        }

                        csvRow++;
                }

                firstLine = false;
                
        }

        // 5- ASSEMBLE FINAL DATAPOINTS
        DataPoints loadedPoints(features, featLabelGen.getLabels());

        if (descriptors.rows() > 0)
        {
                loadedPoints.descriptors = descriptors;
                loadedPoints.descriptorLabels = descLabelGen.getLabels();
        }

        if(times.rows() > 0)
        {
                loadedPoints.times = times;
                loadedPoints.timeLabels = timeLabelGen.getLabels();     
        }

        // Ensure homogeous coordinates
        if(!loadedPoints.featureExists("pad"))
        {
                loadedPoints.addFeature("pad", Matrix::Ones(1,features.cols()));
        }

        return loadedPoints;
}

template
PointMatcher<float>::DataPoints PointMatcherIO<float>::loadCSV(const std::string& fileName);
template
PointMatcher<double>::DataPoints PointMatcherIO<double>::loadCSV(const std::string& fileName);

template<typename T>
void PointMatcher<T>::DataPoints::save(const std::string& fileName, bool binary) const
{
        const boost::filesystem::path path(fileName);
        const string& ext(boost::filesystem::extension(path));
        if (boost::iequals(ext, ".vtk"))
                return PointMatcherIO<T>::saveVTK(*this, fileName, binary);

        if (binary)
                throw runtime_error("save(): Binary writing is not supported together with extension \"" + ext + "\". Currently binary writing is only supported with \".vtk\".");

        if (boost::iequals(ext, ".csv"))
                return PointMatcherIO<T>::saveCSV(*this, fileName);
        else if (boost::iequals(ext, ".ply"))
                return PointMatcherIO<T>::savePLY(*this, fileName);
        else if (boost::iequals(ext, ".pcd"))
                return PointMatcherIO<T>::savePCD(*this, fileName);
        else
                throw runtime_error("save(): Unknown extension \"" + ext + "\" for file \"" + fileName + "\", extension must be either \".vtk\", \".ply\", \".pcd\" or \".csv\"");
}

template
void PointMatcher<float>::DataPoints::save(const std::string& fileName, bool binary) const;
template
void PointMatcher<double>::DataPoints::save(const std::string& fileName, bool binary) const;

template<typename T>
void PointMatcherIO<T>::saveCSV(const DataPoints& data, const std::string& fileName)
{
        ofstream ofs(fileName.c_str());
        if (!ofs.good())
                throw runtime_error(string("Cannot open file ") + fileName);
        saveCSV(data, ofs);
}

template<typename T>
void PointMatcherIO<T>::saveCSV(const DataPoints& data, std::ostream& os)
{
        const int pointCount(data.features.cols());
        const int dimCount(data.features.rows());
        const int descDimCount(data.descriptors.rows());
        
        if (pointCount == 0)
        {
                LOG_WARNING_STREAM( "Warning, no points, doing nothing");
                return;
        }
        
        // write header
        for (int i = 0; i < dimCount - 1; i++)
        {
                os << data.featureLabels[i].text;

                if (!((i == (dimCount - 2)) && descDimCount == 0))
                        os << ",";
        }

        int n = 0;
        for (size_t i = 0; i < data.descriptorLabels.size(); i++)
        {
                Label lab = data.descriptorLabels[i];
                for (size_t s = 0; s < lab.span; s++)
                {
                        os << getColLabel(lab,s);
                        if (n != (descDimCount - 1))
                                os << ",";
                        n++;
                }
        }

        os << "\n";

        // write points
        for (int p = 0; p < pointCount; ++p)
        {
                for (int i = 0; i < dimCount-1; ++i)
                {
                        os << data.features(i, p);
                        if(!((i == (dimCount - 2)) && descDimCount == 0))
                                os << " , ";
                }

                for (int i = 0; i < descDimCount; i++)
                {
                        os << data.descriptors(i,p);
                        if (i != (descDimCount - 1))
                                os << ",";
                }
                os << "\n";
        }
        
}

template
void PointMatcherIO<float>::saveCSV(const DataPoints& data, const std::string& fileName);
template
void PointMatcherIO<double>::saveCSV(const DataPoints& data, const std::string& fileName);

template<typename T>
typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadVTK(const std::string& fileName)
{
        ifstream ifs(fileName.c_str(), std::ios::binary);
        if (!ifs.good())
                throw runtime_error(string("Cannot open file ") + fileName);
        return loadVTK(ifs);
}

void skipBlock(bool binary, int binarySize, std::istream & is, bool hasSeparateSizeParameter = true){
        int n;
        int size;
        is >> n;
        if(!is.good()){
                throw std::runtime_error("File violates the VTK format : parameter 'n' is missing after a field name.");
        }

        if(hasSeparateSizeParameter) {
                is >> size;
                if(!is.good()){
                        throw std::runtime_error("File violates the VTK format : parameter 'size' is missing after a field name.");
                }
        } else {
                size = n;
        }

        std::string line;
        safeGetLine(is, line); // remove line end after parameters;
        if(binary){
                is.seekg(size * binarySize, std::ios_base::cur);
        } else {
                for (int p = 0; p < n; p++)
                {
                        safeGetLine(is, line);
                }
        }
}

template<typename T>
typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadVTK(std::istream& is)
{
        std::map<std::string, SplitTime> labelledSplitTime;
        
        DataPoints loadedPoints;

        // parse header
        string line;
        safeGetLine(is, line);
        if (line.find("# vtk DataFile Version") != 0)
                throw runtime_error(string("Wrong magic header, found ") + line);
        safeGetLine(is, line);
        safeGetLine(is, line);

        const bool isBinary = (line == "BINARY");
        if (line != "ASCII"){
                if(!isBinary){
                        throw runtime_error(string("Wrong file type, expecting ASCII or BINARY, found ") + line);
                }
        }
        safeGetLine(is, line);

        SupportedVTKDataTypes dataType;
        if (line == "DATASET POLYDATA")
                dataType = POLYDATA;
        else if (line == "DATASET UNSTRUCTURED_GRID")
                dataType = UNSTRUCTURED_GRID;
        else
                throw runtime_error(string("Wrong data type, expecting DATASET POLYDATA, found ") + line);


        // parse points, descriptors and time
        string fieldName;
        string name;
        int dim = 0;
        int pointCount = 0;
        string type;
        
        while (is >> fieldName)
        {
                // load features
                if(fieldName == "POINTS")
                {
                        is >> pointCount;
                        is >> type;
                        safeGetLine(is, line); // remove line end after parameters!

                        if(!(type == "float" || type == "double"))
                                        throw runtime_error(string("Field POINTS can only be of type double or float"));

                        Matrix features(4, pointCount);
                        for (int p = 0; p < pointCount; ++p)
                        {
                                readVtkData(type, isBinary, features.template block<3, 1>(0, p), is);
                                features(3, p) = 1.0;
                        }
                        loadedPoints.addFeature("x", features.row(0));
                        loadedPoints.addFeature("y", features.row(1));
                        loadedPoints.addFeature("z", features.row(2));
                        loadedPoints.addFeature("pad", features.row(3));
                }

                // Dataset type
                // POLYDATA
                else if(dataType == POLYDATA && fieldName == "VERTICES")
                {
                        skipBlock(isBinary, 4, is);
                }

                else if(dataType == POLYDATA && fieldName == "LINES")
                {
                        skipBlock(isBinary, 4, is);
                }

                else if(dataType == POLYDATA && fieldName == "POLYGONS")
                {
                        skipBlock(isBinary, 4, is);
                }

                else if(dataType == POLYDATA && fieldName == "TRIANGLE_STRIPS")
                {
                        skipBlock(isBinary, 4, is);
                }

                // Unstructure Grid
                else if(dataType == UNSTRUCTURED_GRID && fieldName == "CELLS")
                {
                        skipBlock(isBinary, 4, is);
                }
                else if(dataType == UNSTRUCTURED_GRID && fieldName == "CELL_TYPES")
                {
                        skipBlock(isBinary, 4, is, false); // according to http://www.vtk.org/VTK/img/file-formats.pdf CELL_TYPES only has one parameter (n)
                }

                // Point data
                else if(fieldName == "POINT_DATA")
                {
                        int descriptorCount;
                        is >> descriptorCount;
                        if(pointCount != descriptorCount)
                                throw runtime_error(string("The size of POINTS is different than POINT_DATA"));
                }
                // Field data is ignored
                else if (fieldName == "FIELD")
                {
                        string fieldDataName;
                        int fieldDataCount;
                        is >> fieldDataName >> fieldDataCount;

                        for (int f = 0; f < fieldDataCount; f++)
                        {
                                int numTuples;
                                is >> name >> dim >> numTuples >> type;

                                if(type == "vtkIdType") // skip that type
                                {
                                        if(isBinary){
                                                is.seekg(dim * numTuples * 4, std::ios_base::cur);
                                        } else {
                                                int t_val;
                                                for (int t = 0; t < dim * numTuples; t++ )
                                                {
                                                        is >> t_val;
                                                }
                                        }
                                }
                                else if(!(type == "float" || type == "double"))
                                                throw runtime_error(string("Field " + fieldName + " is " + type + " but can only be of type double or float"));
                                                 

                                Matrix descriptor(dim, pointCount);
                                readVtkData(type, isBinary, descriptor.transpose(), is);
                                loadedPoints.addDescriptor(name, descriptor);
                        }
                }
                else if(fieldName == "METADATA") // Skip METADATA block
                {
                        safeGetLine(is, line);
                        safeGetLine(is, line);
                        while(!line.empty())
                        {
                                safeGetLine(is, line);
                        }
                }
                else // Load descriptors or time
                {

                        // label name
                        is >> name;
                        
                        bool isTimeSec = false;
                        bool isTimeNsec = false;


                        if(boost::algorithm::ends_with(name, "_splitTime_high32"))
                        {
                                isTimeSec = true;
                                boost::algorithm::erase_last(name, "_splitTime_high32");
                        }
                        
                        if(boost::algorithm::ends_with(name, "_splitTime_low32"))
                        {
                                isTimeNsec = true;
                                boost::algorithm::erase_last(name, "_splitTime_low32");
                        }

                        
                        bool skipLookupTable = false;
                        bool isColorScalars = false;
                        if(fieldName == "SCALARS")
                        {
                                dim = 1;
                                is >> type;
                                skipLookupTable = true;
                        }
                        else if(fieldName == "VECTORS")
                        {
                                dim = 3;
                                is >> type;
                        }
                        else if(fieldName == "TENSORS")
                        {
                                dim = 9;
                                is >> type;
                        }
                        else if(fieldName == "NORMALS")
                        {
                                dim = 3;
                                is >> type;
                        }
                        else if(fieldName == "COLOR_SCALARS")
                        {
                                is >> dim;
                                type = "float";
                                isColorScalars = true;
                        }
                        else
                                throw runtime_error(string("Unknown field name " + fieldName + ", expecting SCALARS, VECTORS, TENSORS, NORMALS or COLOR_SCALARS."));

                        
                        safeGetLine(is, line); // remove rest of the parameter line including its line end;

                        
                        // Load time data
                        if(isTimeSec || isTimeNsec)
                        {
                                // Skip LOOKUP_TABLE line
                                if(skipLookupTable)
                                {
                                        safeGetLine(is, line);
                                }
                                
                                typename std::map<std::string, SplitTime>::iterator it;

                                it = labelledSplitTime.find(name);
                                // reserve memory
                                if(it == labelledSplitTime.end())
                                {
                                        SplitTime t;
                                        t.high32 = Eigen::Matrix<unsigned int, Eigen::Dynamic, Eigen::Dynamic> (dim, pointCount);
                                        t.low32 = t.high32;
                                        labelledSplitTime[name] = t;
                                }

                                // Load seconds
                                if(isTimeSec)
                                {
                                        assert(labelledSplitTime[name].isHigh32Found == false);
                                        readVtkData(type, isBinary, labelledSplitTime[name].high32.transpose(), is);
                                        labelledSplitTime[name].isHigh32Found = true;
                                }
                                
                                // Load nano seconds
                                if(isTimeNsec)
                                {
                                        assert(labelledSplitTime[name].isLow32Found == false);
                                        readVtkData(type, isBinary, labelledSplitTime[name].low32.transpose(), is);
                                        labelledSplitTime[name].isLow32Found = true;
                                }
                        }
                        else
                        {
                                
                                Matrix descriptorData(dim, pointCount);
                                
                                if(isColorScalars && isBinary) 
                                {
                                        std::vector<unsigned char> buffer(dim);
                                        for (int i = 0; i < pointCount; ++i){
                                                is.read(reinterpret_cast<char *>(&buffer.front()), dim);
                                                for(int r=0; r < dim; ++r){
                                                        descriptorData(r, i) = buffer[r] / static_cast<T>(255.0);
                                                }
                                        }
                                } 
                                else 
                                {
                                        if(!(type == "float" || type == "double"))
                                                throw runtime_error(string("Field " + fieldName + " is " + type + " but can only be of type double or float."));

                                        // Skip LOOKUP_TABLE line
                                        if(skipLookupTable)
                                        {
                                                safeGetLine(is, line);
                                        }
                                        readVtkData(type, isBinary, descriptorData.transpose(), is);
                                }
                                loadedPoints.addDescriptor(name, descriptorData);
                        }
                }
        }

        // Combine time and add to point cloud
        typename std::map<std::string, SplitTime>::iterator it;
        for(it=labelledSplitTime.begin(); it!=labelledSplitTime.end(); it++)
        {
                // Confirm that both parts were loaded
                if(it->second.isHigh32Found == false)
                {
                        throw runtime_error(string("Missing time field representing the higher 32 bits. Expecting SCALARS with name " + it->first + "_splitTime_high32 in the VTK file."));
                }
                
                if(it->second.isLow32Found == false)
                {
                        throw runtime_error(string("Missing time field representing the lower 32 bits. Expecting SCALARS with name " + it->first + "_splitTime_low32 in the VTK file."));
                }

                // Loop through points
                Int64Matrix timeData(1,pointCount);
                for(int i=0; i<it->second.high32.cols(); i++)
                {
                
                        timeData(0,i) = (((std::int64_t) it->second.high32(0,i)) << 32) | ((std::int64_t) it->second.low32(0,i));
                }

                loadedPoints.addTime(it->first, timeData);
        }
        
        return loadedPoints;
}

template
PointMatcherIO<float>::DataPoints PointMatcherIO<float>::loadVTK(const std::string& fileName);
template
PointMatcherIO<double>::DataPoints PointMatcherIO<double>::loadVTK(const std::string& fileName);


template<typename T>
void PointMatcherIO<T>::saveVTK(const DataPoints& data, const std::string& fileName, bool binary)
{
        typedef typename InspectorsImpl<T>::VTKFileInspector VTKInspector;
        
        Parametrizable::Parameters param;
        boost::assign::insert(param) ("baseFileName", "");
        boost::assign::insert(param) ("writeBinary", toParam(binary));
        VTKInspector vtkInspector(param);
        vtkInspector.dumpDataPoints(data, fileName);
}


template
void PointMatcherIO<float>::saveVTK(const PointMatcherIO<float>::DataPoints& data, const std::string& fileName, bool binary);
template
void PointMatcherIO<double>::saveVTK(const PointMatcher<double>::DataPoints& data, const std::string& fileName, bool binary);

template<typename T>
typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPLY(const std::string& fileName)
{
        ifstream ifs(fileName.c_str());
        if (!ifs.good())
                throw runtime_error(string("Cannot open file ") + fileName);
        return loadPLY(ifs);
}

template
PointMatcherIO<float>::DataPoints PointMatcherIO<float>::loadPLY(const string& fileName);
template
PointMatcherIO<double>::DataPoints PointMatcherIO<double>::loadPLY(const string& fileName);

template <typename T>
typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPLY(std::istream& is)
{
        class Elements : public vector<PLYElement*>{
         public:
                ~Elements(){
                        for (typename vector<PLYElement*>::const_iterator it = this->begin(); it != this->end(); it++ )
                        {
                                delete *it;
                        }
                }
        };

        /*
        Steps:
        1- PARSE PLY HEADER
        2- ASSIGN PLY PROPERTIES TO DATAPOINTS ROWS
        3- Reserve memory for a DataPoints
        4- Parse PLY vertex to appropriate DataPoints cols and rows 
        5- Assemble final DataPoints

        PLY organisation:

        element 1 [name, size]
                - property 1 [type, name]
                - property 2
                - ...
        element 2
                -...

        */


        // 1- PARSE PLY HEADER
        bool format_defined = false;
        bool header_processed = false;

        Elements elements;
        PLYElementF element_f; // factory
        PLYElement* current_element = NULL;
        bool skip_props = false; // flag to skip properties if element is not supported
        unsigned elem_offset = 0; // keep track of line position of elements that are supported
        string line;
        safeGetLine(is, line);

        if (line.find("ply") != 0) {
                throw runtime_error(string("PLY parse error: wrong magic header, found <") + line + string(">"));
        }

        while (!header_processed)
        {
                if(!safeGetLine(is, line))
                        throw runtime_error("PLY parse error: reached end of file before end of header definition");


                if ( line.empty() )
                        continue;
                istringstream stringstream (line);

                string keyword;
                stringstream >> keyword;

                // ignore comment
                if (keyword == "comment") {
                        continue;
                }

                // We only support ascii and ply version 1.0
                if (keyword == "format")
                {
                        if (format_defined)
                                throw runtime_error("PLY parse error: format already defined");

                        string format_str, version_str;
                        stringstream >> format_str >> version_str;

                        if (format_str != "ascii" && format_str != "binary_little_endian" && format_str != "binary_big_endian")
                                throw runtime_error(string("PLY parse error: format <") + format_str + string("> is not supported"));

                        if (format_str == "binary_little_endian" || format_str == "binary_big_endian")
                                throw runtime_error(string("PLY parse error: binary PLY files are not supported"));
                        if (version_str != "1.0")
                        {
                                throw runtime_error(string("PLY parse error: version <") + version_str + string("> of ply is not supported"));
                        }

                        format_defined = true;

                }
                else if (keyword == "element")
                {
                        

                        string elem_name, elem_num_s;
                        stringstream >> elem_name >> elem_num_s;

                        unsigned elem_num;
                        try
                        {
                                elem_num = boost::lexical_cast<unsigned>(elem_num_s);
                        }
                        catch (boost::bad_lexical_cast& e)
                        {
                                throw runtime_error(string("PLY parse error: bad number of elements ") + elem_num_s + string(" for element ") + elem_name);
                        }

                        if (element_f.elementSupported(elem_name))
                        {
                                // Initialize current element
                                PLYElement* elem = element_f.createElement(elem_name, elem_num, elem_offset);
                                current_element = elem;

                                // check that element is not already defined
                                for (typename Elements::const_iterator it = elements.begin(); it != elements.end(); it++ )
                                {
                                        if (**it == *elem) {
                                                delete elem;
                                                throw runtime_error(string("PLY parse error: element: ") + elem_name + string( "is already defined"));
                                        }
                                }
                                elements.push_back(elem);
                                skip_props = false;
                        }
                        else
                        {
                                LOG_WARNING_STREAM("PLY parse warning: element " << elem_name << " not supported. Skipping.");
                                skip_props = true;
                        }

                        elem_offset += elem_num;
                }
                else if (keyword == "property")
                {
                        if (current_element == NULL)
                        {
                                throw runtime_error("PLY parse error: property listed without defining an element");
                        }

                        if (skip_props)
                                continue;

                        string next, prop_type, prop_name;
                        stringstream >> next;

                        // PLY list property
                        if (next == "list")
                        {
                                string prop_idx_type;
                                stringstream >> prop_idx_type >> prop_type >> prop_name;

                                PLYProperty list_prop(prop_idx_type, prop_type, prop_name, current_element->total_props);

                                current_element->properties.push_back(list_prop);
                        }
                        // PLY regular property
                        else
                        {
                                prop_type = next;
                                stringstream >> prop_name;
                                PLYProperty prop(prop_type, prop_name, current_element->total_props);

                                current_element->properties.push_back(prop);
                        }

                        current_element->total_props++;
                }
                else if (keyword == "end_header")
                {
                        if (!format_defined)
                        {
                                throw runtime_error(string("PLY parse error: format not defined in header"));
                        }

                        if (elements.size() == 0)
                        {
                                throw runtime_error(string("PLY parse error: no elements defined in header"));
                        }

                        header_processed = true;
                }
        }

        // 2- ASSIGN PLY PROPERTIES TO DATAPOINTS ROWS
        
        // Fetch vertex
        PLYElement* vertex = elements[0];
        
        if(vertex->name != "vertex")
        {
                throw runtime_error(string("PLY parse error: vertex should be the first element defined."));
        }
                
        // Fetch known features and descriptors
        const SupportedLabels & externalLabels = getSupportedExternalLabels();
        
        int rowIdFeatures = 0;
        int rowIdDescriptors = 0;
        int rowIdTime= 0;
        
        LabelGenerator featLabelGen, descLabelGen, timeLabelGen;

        
        // Loop through all known external names (ordered list)
        for(size_t i=0; i<externalLabels.size(); i++)
        {
                const SupportedLabel & supLabel = externalLabels[i];

                //Search if that feature exist
                for(it_PLYProp it=vertex->properties.begin(); it!=vertex->properties.end(); ++it)
                {

                        if(supLabel.externalName == it->name)
                        {
                                it->pmType = supLabel.type;

                                // Assign rowId in that order
                                switch (supLabel.type)
                                {
                                        case FEATURE:
                                                it->pmRowID = rowIdFeatures;
                                                featLabelGen.add(supLabel.internalName);
                                                rowIdFeatures++;
                                                break;
                                        case DESCRIPTOR:
                                                it->pmRowID = rowIdDescriptors;
                                                descLabelGen.add(supLabel.internalName);
                                                rowIdDescriptors++;
                                                break;
                                        case TIME:
                                                it->pmRowID = rowIdTime;
                                                timeLabelGen.add(supLabel.internalName);
                                                rowIdTime++;
                                        default:
                                                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'"));
                                                break;
                                }

                                // we stop searching once we have a match
                                break;
                        }
                }
        }
        
        // loop through the remaining UNSUPPORTED labels and assigned them to a single descriptor row
        for(it_PLYProp it=vertex->properties.begin(); it!=vertex->properties.end(); ++it)
        {
                if(it->pmType == UNSUPPORTED)
                {
                        it->pmType = DESCRIPTOR; // force descriptor
                        it->pmRowID = rowIdDescriptors;
                        descLabelGen.add(it->name); // keep original name
                        rowIdDescriptors++;
                }
        }

        // 3- RESERVE DATAPOINTS MEMORY

        const unsigned int featDim = featLabelGen.getLabels().totalDim();
        const unsigned int descDim = descLabelGen.getLabels().totalDim();
        const unsigned int timeDim = timeLabelGen.getLabels().totalDim();
        const unsigned int nbPoints = vertex->num;

        Matrix features = Matrix(featDim, nbPoints);
        Matrix descriptors = Matrix(descDim, nbPoints);
        Int64Matrix times = Int64Matrix(timeDim, nbPoints);


        // 4- PARSE PLY DATA (vertex)
        const int nbProp = vertex->total_props;
        const int nbValues = nbPoints*nbProp;
        int propID = 0;
        int col = 0;
        for(int i=0; i<nbValues; i++)
        {
                T value;
                if(!(is >> value))
                {
                        throw runtime_error(
                        (boost::format("PLY parse error: expected %1% values (%2% points with %3% properties) but only found %4% values.") % nbValues % nbPoints % nbProp % i).str());
                }
                else
                {
                        const int row = vertex->properties[propID].pmRowID;
                        const PMPropTypes type = vertex->properties[propID].pmType;
                        
                        // rescale color from [0,254] to [0, 1[
                        // FIXME: do we need that?
                        if (vertex->properties[propID].name == "red" || vertex->properties[propID].name == "green" || vertex->properties[propID].name == "blue" || vertex->properties[propID].name == "alpha") {
                                value /= 255.0;
                        }

                        switch (type)
                        {
                                case FEATURE:
                                        features(row, col) = value;
                                        break;
                                case DESCRIPTOR:
                                        descriptors(row, col) = value;
                                        break;
                                case TIME:
                                        times(row, col) = value;
                                        break;
                                case UNSUPPORTED:
                                        throw runtime_error("Implementation error in loadPLY(). This should not throw.");
                                        break;
                        }

                        ++propID;

                        if(propID >= nbProp)
                        {
                                propID = 0;
                                ++col;
                        }
                }
        }



        // 5- ASSEMBLE FINAL DATAPOINTS
        
        DataPoints loadedPoints(features, featLabelGen.getLabels());

        if (descriptors.rows() > 0)
        {
                loadedPoints.descriptors = descriptors;
                loadedPoints.descriptorLabels = descLabelGen.getLabels();
        }

        if(times.rows() > 0)
        {
                loadedPoints.times = times;
                loadedPoints.timeLabels = timeLabelGen.getLabels();     
        }

        // Ensure homogeous coordinates
        if(!loadedPoints.featureExists("pad"))
        {
                loadedPoints.addFeature("pad", Matrix::Ones(1,features.cols()));
        }

        return loadedPoints;

}

template<typename T>
void PointMatcherIO<T>::savePLY(const DataPoints& data,
                const std::string& fileName)
{
        //typedef typename DataPoints::Labels Labels;

        ofstream ofs(fileName.c_str());
        if (!ofs.good())
                throw runtime_error(string("Cannot open file ") + fileName);

        const int pointCount(data.features.cols());
        const int featCount(data.features.rows());
        const int descRows(data.descriptors.rows());


        if (pointCount == 0)
        {
                LOG_WARNING_STREAM("Warning, no points, doing nothing");
                return;
        }

        ofs << "ply\n" <<"format ascii 1.0\n";
        ofs << "element vertex " << pointCount << "\n";
        for (int f=0; f <(featCount-1); f++)
        {
                ofs << "property float " << data.featureLabels[f].text << "\n";
        }

        for (size_t i = 0; i < data.descriptorLabels.size(); i++)
        {
                Label lab = data.descriptorLabels[i];
                for (size_t s = 0; s < lab.span; s++)

                {
                        ofs << "property float " << getColLabel(lab,s) << "\n";
                }
        }

        ofs << "end_header\n";

        // write points
        for (int p = 0; p < pointCount; ++p)
        {
                for (int f = 0; f < featCount - 1; ++f)
                {
                        ofs << data.features(f, p);
                        if(!(f == featCount-2 && descRows == 0))
                                ofs << " ";
                }

                bool datawithColor = data.descriptorExists("color");
                int colorStartingRow = data.getDescriptorStartingRow("color");
                int colorEndRow = colorStartingRow + data.getDescriptorDimension("color");
                for (int d = 0; d < descRows; ++d)
                {
                        if (datawithColor && d >= colorStartingRow && d < colorEndRow) {
                                ofs << static_cast<unsigned>(data.descriptors(d, p) * 255.0);
                        } else {
                                ofs << data.descriptors(d, p);
                        }
                        if(d != descRows-1)
                                ofs << " ";
                }
                ofs << "\n";
        }

        ofs.close();
}

template
void PointMatcherIO<float>::savePLY(const DataPoints& data, const std::string& fileName);
template
void PointMatcherIO<double>::savePLY(const DataPoints& data, const std::string& fileName);

template<typename T>
PointMatcherIO<T>::PLYProperty::PLYProperty(const std::string& type,
                const std::string& name, const unsigned pos) :
                name(name), 
                type(type), 
                pos(pos) 
{
        if (plyPropTypeValid(type))
        {
                is_list = false;
        }
        else
        {
                throw std::runtime_error(
                                std::string("PLY parse error: property type ") + type
                                                + std::string(" for property ") + name
                                                + std::string(" is invalid"));
        }

        pmType = UNSUPPORTED;
        pmRowID = -1;

}

template<typename T>
PointMatcherIO<T>::PLYProperty::PLYProperty(const std::string& idx_type,
                const std::string& type, const std::string& name, const unsigned pos) :
                name(name), 
                type(type), 
                idx_type(idx_type), 
                pos(pos)
{
        if (plyPropTypeValid(idx_type) && plyPropTypeValid(type)) 
        {
                is_list = true;
        } 
        else
        {
                throw std::runtime_error(
                                std::string("PLY parse error: property list type ") + idx_type
                                                + std::string(" ") + type
                                                + std::string(" for property ") + name
                                                + std::string(" is invalid"));
        }

        pmType = UNSUPPORTED;
        pmRowID = -1;
}

template
class PointMatcherIO<float>::PLYElement;
template
class PointMatcherIO<double>::PLYElement;

template
class PointMatcherIO<float>::PLYProperty;
template
class PointMatcherIO<double>::PLYProperty;


template <typename T>
typename PointMatcherIO<T>::PLYElementF::ElementTypes PointMatcherIO<T>::PLYElementF::getElementType(const std::string& elem_name)
{
        string lc = elem_name;
        boost::algorithm::to_lower(lc);
        if (lc == "vertex")
        {
                return VERTEX;
        }
        else
        {
                return UNSUPPORTED;
        }
}


template <typename T>
bool PointMatcherIO<T>::PLYElementF::elementSupported(const std::string& elem_name)
{
        return getElementType(elem_name) != UNSUPPORTED;
}


template<typename T>
typename PointMatcherIO<T>::PLYElement* PointMatcherIO<T>::PLYElementF::createElement(
                const std::string& elem_name, const int elem_num, const unsigned offset) {
        ElementTypes type = getElementType(elem_name);
        if (type == VERTEX)
                return new PLYVertex(elem_num, offset);
        else
                return NULL;
}


template<typename T>
bool PointMatcherIO<T>::plyPropTypeValid(const std::string& type) {
        return (type == "char" || type == "uchar" || type == "short"
                        || type == "ushort" || type == "int" || type == "uint"
                        || type == "float" || type == "double");
}


template <typename T>
bool PointMatcherIO<T>::PLYElement::operator==(const PLYElement& rhs) const
{
        return name == rhs.name;
}


template <typename T>
bool PointMatcherIO<T>::PLYProperty::operator==(const PLYProperty& rhs) const
{
        return name == rhs.name && type == rhs.type;
}


template<typename T>
typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPCD(const string& fileName) {
        ifstream ifs(fileName.c_str());
        if (!ifs.good())
                throw runtime_error(string("Cannot open file ") + fileName);
        return loadPCD(ifs);
}


template
PointMatcherIO<float>::DataPoints PointMatcherIO<float>::loadPCD(const string& fileName);
template
PointMatcherIO<double>::DataPoints PointMatcherIO<double>::loadPCD(const string& fileName);


template<typename T>
typename PointMatcherIO<T>::DataPoints PointMatcherIO<T>::loadPCD(std::istream& is) {


        /*
        Steps:
        1- PARSE PCD HEADER
        2- ASSIGN PCD PROPERTIES TO DATAPOINTS ROWS
        3- Reserve memory for a DataPoints
        4- Parse PCD XXX to appropriate DataPoints cols and rows 
        5- Assemble final DataPoints

        PCD organisation:

        # .PCD v.7 - Point Cloud Data file format
        VERSION number
        FIELDS prop1 prop2 prop3 ...
        SIZE nbBytes1 nbBytes2 nbBytes3
        TYPE type1 type2 type3
        COUNT nbDim1 nbDim2 nbDim3
        WIDTH w
        HEIGHT h
        VIEWPOINT 0 0 0 1 0 0 0
        POINTS size (should be w*h)
        DATA ascii or binary
        data1 data2 data3 ...
        data1 data2 data3 ...
        ...

        */


        // 1- PARSE PCD HEADER

        size_t lineNum = 0;
        PCDheader header;

        string line;
        while (safeGetLine(is, line))
        {

                // get rid of white spaces before/after
                boost::trim (line);

                // ignore comments or empty line
                if (line.substr(0,1) == "#" || line == "")
                {
                        lineNum++;
                        continue;
                }

                vector<string> tokens;
                boost::split(tokens, line, boost::is_any_of("\t\r "), boost::token_compress_on);

                string pcd_version_str;
                if (tokens[0] == "VERSION")
                {
                        header.version = tokens[1];

                        if (tokens[1] != "0.7" && tokens[1] != ".7")
                                throw runtime_error("PCD Parse Error: Only PCD Version 0.7 is supported");
                }

                else if (tokens[0] == "FIELDS")
                {
                        header.properties.resize(tokens.size() - 1);
                        
                        for (size_t i = 1; i < tokens.size(); i++)
                        {
                                header.properties[i-1].field = tokens[i];
                        }
                        
                }

                else if (tokens[0] == "SIZE")
                {
                        if((tokens.size() - 1) != header.properties.size())
                                throw runtime_error("PCD Parse Error: number of elements for SIZE must be the same as FIELDS");
                        
                        for (size_t i = 1; i < tokens.size(); i++)
                        {
                                const unsigned int size = boost::lexical_cast<unsigned int >(tokens[i]);
                                header.properties[i-1].size = size;
                        }

                }

                else if (tokens[0] == "TYPE")
                {
                        if((tokens.size() - 1) != header.properties.size())
                                throw runtime_error("PCD Parse Error: number of elements for TYPE must be the same as FIELDS");

                        for (size_t i = 1; i < tokens.size(); i++)
                        {
                                const char type = boost::lexical_cast<char>(tokens[i]);
                                header.properties[i-1].type = type;
                                if (type != 'I' && type != 'U' && type != 'F')
                                        throw runtime_error("PCD Parse Error: invalid TYPE, it must be 'I', 'U', or 'F'");
                        }
                        
                }

                else if (tokens[0] == "COUNT")
                {

                        if((tokens.size() - 1) != header.properties.size())
                                throw runtime_error("PCD Parse Error: number of elements for COUNT must be the same as FIELDS");
                        
                        for (size_t i = 1; i < tokens.size(); i++)
                        {
                                const unsigned int count = boost::lexical_cast<unsigned int >(tokens[i]);
                                header.properties[i-1].count = count;
                        }
                        
                }

                else if (tokens[0] == "WIDTH")
                {
                        try
                        {
                                header.width = boost::lexical_cast<unsigned int >(tokens[1]);
                        } 
                        catch (boost::bad_lexical_cast& e)
                        {
                                throw runtime_error("PCD Parse Error: invalid WIDTH");
                        }
                        
                }

                else if (tokens[0] == "HEIGHT")
                {
                        try
                        {
                                header.height= boost::lexical_cast<unsigned int >(tokens[1]);
                        } 
                        catch (boost::bad_lexical_cast& e)
                        {
                                throw runtime_error("PCD Parse Error: invalid HEIGHT");
                        }
                
                }

                else if (tokens[0] == "VIEWPOINT")
                {
                        if((tokens.size() - 1) != 7 )
                                throw runtime_error("PCD Parse Error: number of elements for VIEWPOINT must be 7");

                        for (size_t i = 1; i < tokens.size(); i++)
                        {
                                try
                                {
                                        header.viewPoint(i-1, 0) = boost::lexical_cast<T>(tokens[i]);
                                }
                                catch (boost::bad_lexical_cast& e)
                                {
                                        stringstream ss;
                                        ss << "PCD Parse Error: invalid value(" << tokens[i] << ") of VIEWPOINT";
                                        throw runtime_error(ss.str());
                                }
                        }

                }

                else if (tokens[0] == "POINTS")
                {
                        try
                        {
                                header.nbPoints = boost::lexical_cast<unsigned int>(tokens[1]);
                        }
                        catch (boost::bad_lexical_cast& e)
                        {
                                stringstream ss;
                                ss << "PCD Parse Error: the value in the element POINTS (" << tokens[1] << ") could not be cast as unsigned int";
                                throw runtime_error(ss.str());
                        }
                }

                else if (tokens[0] == "DATA")
                {
                        header.dataType= tokens[1];
                        
                        if (header.dataType == "ascii")
                        {
                                // DATA is the last element of the header, we exit the loop
                                break;
                        }
                        else if(header.dataType == "binary")
                        {
                                throw runtime_error("PCD Implementation Error: the option for DATA binary is not implemented yet");
                        }
                        else
                        {
                                stringstream ss;
                                ss << "PCD Parse Error: the value in the element DATA (" << tokens[1] << ") must be ascii or binary";
                                throw runtime_error(ss.str());
                        }

                }

                lineNum++;
        }
        // Extra check for the number of points
        if (header.properties.size() == 0)
                throw runtime_error("PCD Parse Error: no FIELDS were find in the header");


        // Extra check for the number of points
        if (header.width * header.height != header.nbPoints)
                throw runtime_error("PCD Parse Error: POINTS field does not match WIDTH and HEIGHT fields");

        // 2- ASSIGN PCD PROPERTIES TO DATAPOINTS ROWS

        // Fetch known features and descriptors
        const SupportedLabels & externalLabels = getSupportedExternalLabels();
        
        int rowIdFeatures = 0;
        int rowIdDescriptors = 0;
        int rowIdTime= 0;
        
        LabelGenerator featLabelGen, descLabelGen, timeLabelGen;

        // Loop through all known external names (ordered list)
        for(size_t i=0; i<externalLabels.size(); i++)
        {
                const SupportedLabel & supLabel = externalLabels[i];

                //Search if that feature exist
                for(size_t i=0; i < header.properties.size(); i++)
                {
                        const PCDproperty prop = header.properties[i];

                        //TODO: prop.field == "rgb" might be tricky

                        if(supLabel.externalName == prop.field)
                        {
                                header.properties[i].pmType = supLabel.type;

                                // Assign rowId in that order
                                switch (supLabel.type)
                                {
                                        case FEATURE:
                                                header.properties[i].pmRowID = rowIdFeatures;
                                                featLabelGen.add(supLabel.internalName);
                                                rowIdFeatures++;
                                                if(prop.count != 1)
                                                {
                                                        stringstream ss;
                                                        ss << "PCD Parse Error: the field " << prop.field << " must have a count of 1";
                                                        throw runtime_error(ss.str());
                                                }
                                                break;
                                        case DESCRIPTOR:
                                                header.properties[i].pmRowID = rowIdDescriptors;
                                                descLabelGen.add(supLabel.internalName, prop.count);
                                                rowIdDescriptors += prop.count;
                                                break;
                                        case TIME:
                                                header.properties[i].pmRowID = rowIdTime;
                                                timeLabelGen.add(supLabel.internalName, prop.count);
                                                rowIdTime += prop.count;
                                        default:
                                                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'"));
                                                break;
                                }

                                // we stop searching once we have a match
                                break;
                        }
                }
        }
        
        // loop through the remaining UNSUPPORTED labels and assigned them to a single descriptor row
        for(size_t i=0; i < header.properties.size(); i++)
        {
                const PCDproperty prop = header.properties[i];
                if(prop.pmType == UNSUPPORTED)
                {
                        header.properties[i].pmType = DESCRIPTOR; // force descriptor
                        header.properties[i].pmRowID = rowIdDescriptors;
                        descLabelGen.add(prop.field, prop.count); // keep original name
                        rowIdDescriptors += prop.count;
                }
        }


        // 3- RESERVE DATAPOINTS MEMORY

        const unsigned int featDim = featLabelGen.getLabels().totalDim();
        const unsigned int descDim = descLabelGen.getLabels().totalDim();
        const unsigned int timeDim = timeLabelGen.getLabels().totalDim();
        const unsigned int totalDim = featDim + descDim + timeDim;
        const unsigned int nbPoints = header.nbPoints;

        Matrix features = Matrix(featDim, nbPoints);
        Matrix descriptors = Matrix(descDim, nbPoints);
        Int64Matrix times = Int64Matrix(timeDim, nbPoints);


        // 4- PARSE PCD DATA

        size_t col = 0; // point count
        while (safeGetLine(is, line))
        {

                // get rid of white spaces before/after
                boost::trim (line);

                // ignore comments or empty line
                if (line.substr(0,1) == "#" || line == "")
                {
                        lineNum++;
                        continue;
                }

                vector<string> tokens;
                boost::split(tokens, line, boost::is_any_of("\t\r "), boost::token_compress_on);


                if (tokens.size() != totalDim)
                        throw runtime_error(string("PCD Parse Error: number of data columns does not match number of fields at line: ") + boost::lexical_cast<string>(lineNum));

                unsigned int fileCol = 0;
                for(size_t i=0; i<header.properties.size(); i++)
                {
                        const unsigned int count = header.properties[i].count;
                        const unsigned int row = header.properties[i].pmRowID;
                        const PMPropTypes type = header.properties[i].pmType;


                        for(size_t j=0; j<count; j++)
                        {
                                switch (type)
                                {
                                        case FEATURE:
                                                features(row+j, col) = boost::lexical_cast<T>(tokens[fileCol]);
                                                break;
                                        case DESCRIPTOR:
                                                descriptors(row+j, col) = boost::lexical_cast<T>(tokens[fileCol]);
                                                break;
                                        case TIME:
                                                times(row+j, col) = boost::lexical_cast<std::int64_t>(tokens[fileCol]);
                                                break;
                                        case UNSUPPORTED:
                                                throw runtime_error("Implementation error in loadPCD(). This should not throw.");
                                                break;
                                }

                                fileCol++;
                        }

                }

                col++;
                lineNum++;
        }

        if (col != nbPoints)
        {
                stringstream ss;
                ss << "PCD Parse Error: the number of points in the file (" << col << ") is less than the specified number of points (" << nbPoints << ")";
                throw runtime_error(ss.str());
        }

        // 5- ASSEMBLE FINAL DATAPOINTS
        
        DataPoints loadedPoints(features, featLabelGen.getLabels());

        if (descriptors.rows() > 0)
        {
                loadedPoints.descriptors = descriptors;
                loadedPoints.descriptorLabels = descLabelGen.getLabels();
        }

        if(times.rows() > 0)
        {
                loadedPoints.times = times;
                loadedPoints.timeLabels = timeLabelGen.getLabels();     
        }

        // Ensure homogeous coordinates
        if(!loadedPoints.featureExists("pad"))
        {
                loadedPoints.addFeature("pad", Matrix::Ones(1,features.cols()));
        }

        return loadedPoints;
}

template<typename T>
void PointMatcherIO<T>::savePCD(const DataPoints& data,
                const std::string& fileName) {
        ofstream ofs(fileName.c_str());
        if (!ofs.good())
                throw runtime_error(string("Cannot open file ") + fileName);

        const int pointCount(data.features.cols());
        const int featCount(data.features.rows());
        const int descRows(data.descriptors.rows());
        const int descCount(data.descriptorLabels.size());

        if (pointCount == 0)
        {
                LOG_WARNING_STREAM("Warning, no points, doing nothing");
                return;
        }

        ofs << "# .PCD v.7 - Point Cloud Data file format\n" <<"VERSION .7\n";
        ofs << "FIELDS";

        for (int f=0; f < (featCount - 1); f++)
        {
                ofs << " " << data.featureLabels[f].text;
        }

        if (descRows == 0)
                ofs << "\n";
        else
        {
                for (int i = 0; i < descCount; i++)
                {
                        ofs << " " << data.descriptorLabels[i].text;
                }
                ofs << "\n";
        }

        ofs << "SIZE";
        for (int i =0; i < featCount - 1 + descCount; i++)
        {
                ofs << " 4"; // for float
        }
        ofs << "\n";

        ofs << "TYPE";
        for (int i =0; i < featCount - 1 + descCount; i++)
        {
                ofs << " F"; // for float
        }
        ofs << "\n";

        ofs << "COUNT";
        for (int f = 0; f < featCount - 1 ; f++ )
                ofs << " 1";
        if (descCount == 0)
                ofs << "\n";
        else
        {
                for (int i = 0; i < descCount; i++)
                {
                        ofs << " " << data.descriptorLabels[i].span;
                }
                ofs << "\n";
        }

        ofs << "WIDTH " << pointCount << "\n";
        ofs << "HEIGHT 1\n";
        ofs << "POINTS " << pointCount << "\n";
        ofs << "DATA ascii\n";

        // write points
        for (int p = 0; p < pointCount; ++p)
        {
                for (int f = 0; f < featCount - 1; ++f)
                {
                        ofs << data.features(f, p);
                        if(!(f == featCount-2 && descRows == 0))
                                ofs << " ";
                }
                for (int d = 0; d < descRows; ++d)
                {
                        ofs << data.descriptors(d, p);
                        if(d != descRows-1)
                                ofs << " ";
                }
                ofs << "\n";
        }

        ofs.close();
}

template
void PointMatcherIO<float>::savePCD(const DataPoints& data, const std::string& fileName);
template
void PointMatcherIO<double>::savePCD(const DataPoints& data, const std::string& fileName);