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 "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

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;
        while (!is.eof())
        {
                char line[1024];
                is.getline(line, sizeof(line));
                line[sizeof(line)-1] = 0;

                if(firstLine)
                {
                        std::vector<string> header = csvLineToVector(line);
                                
                        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);
                        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& grativity):
        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())
        #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>
typename PointMatcherIO<T>::DescAssociationMap PointMatcherIO<T>::getDescAssocationMap()
{
        DescAssociationMap assoc_map = boost::assign::map_list_of
                        ("nx", DescAssociationPair(0,"normals"))
                        ("ny", DescAssociationPair(1,"normals"))
                        ("nz", DescAssociationPair(2,"normals"))
                        ("normal_x", DescAssociationPair(0,"normals"))
                        ("normal_y", DescAssociationPair(1,"normals"))
                        ("normal_z", DescAssociationPair(2,"normals"))
                        ("densities", DescAssociationPair(0,"densities"))
                        ("intensity", DescAssociationPair(0,"intensity"))
                        ("red", DescAssociationPair(0,"color"))
                        ("green", DescAssociationPair(1,"color"))
                        ("blue", DescAssociationPair(2,"color"))
                        ("alpha", DescAssociationPair(3,"color"))
                        ("eigValues0", DescAssociationPair(0,"eigValues"))
                        ("eigValues1", DescAssociationPair(1,"eigValues"))
                        ("eigValues2", DescAssociationPair(2,"eigValues"))
                        ("eigVectors0X", DescAssociationPair(0,"eigVectors"))
                        ("eigVectors0Y", DescAssociationPair(1,"eigVectors"))
                        ("eigVectors0Z",DescAssociationPair(2,"eigVectors"))
                        ("eigVectors1X", DescAssociationPair(3,"eigVectors"))
                        ("eigVectors1Y", DescAssociationPair(4,"eigVectors"))
                        ("eigVectors1Z",DescAssociationPair(5,"eigVectors"))
                        ("eigVectors2X", DescAssociationPair(6,"eigVectors"))
                        ("eigVectors2Y", DescAssociationPair(7,"eigVectors"))
                        ("eigVectors2Z",DescAssociationPair(8,"eigVectors"))
                        ("normals", DescAssociationPair(0,"normals"))
                        ("eigValues", DescAssociationPair(0,"eigValues"))
                        ("eigVectors", DescAssociationPair(0,"eigVectors"))
                        ("color", DescAssociationPair(0,"color"));
        return assoc_map;
}

template <typename T>
bool PointMatcherIO<T>::colLabelRegistered(const std::string& colLabel)
{
        return getDescAssocationMap().count(colLabel) > 0;
}

template <typename T>
typename PointMatcherIO<T>::DescAssociationPair PointMatcherIO<T>::getDescAssociationPair(const std::string& colLabel)
{
        return getDescAssocationMap().find(colLabel)->second;
}

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

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

        return colLabel;
}


template<typename T>
typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadCSV(std::istream& is)
{
        typedef typename DataPoints::Label Label;
        typedef typename DataPoints::Labels Labels;
        
        vector<T> xData;
        vector<T> yData;
        vector<T> zData;
        vector<T> padData;
        vector<string> header;
        vector<int> descColsToKeep; // Record of which columns will be read into a descriptor

        map<int,DescAssociationPair> colToDescPair;
        map<string,int> descLabelToNumRows;
        map<string,int> descLabelToStartingRows;

        vector<vector<T> > descCols;
        int numDescCols = 0;
        int dim(0);
        bool firstLine(true);
        bool hasHeader(false);
        Labels featureLabels, descriptorLabels;
        int xCol(-1);
        int yCol(-1);
        int zCol(-1);

        char delimiters[] = " \t,;";
        char *token;
        while (!is.eof())
        {
                char line[1024];
                is.getline(line, sizeof(line));
                line[sizeof(line)-1] = 0;
        
                // Look for text header
                unsigned int len = strspn(line, " ,+-.1234567890Ee");
                if(len != strlen(line))
                {
                        //cout << "Header detected" << endl;
                        hasHeader = true;
                }
                else
                {
                        hasHeader = false;
                }

                // Count dimension using first line
                if(firstLine)
                {
                        char tmpLine[1024];
                        char *brkt = 0;
                        strcpy(tmpLine, line);
                        token = strtok_r(tmpLine, delimiters, &brkt);
                        while (token)
                        {
                                dim++;
                                
                                // Load text header
                                if(hasHeader)
                                {
                                        header.push_back(string(token));
                                }

                                token = strtok_r(NULL, delimiters, &brkt);
                        }
                
                                
                        if (hasHeader)
                        {
                                // Search for x, y and z tags
                                for(unsigned int i = 0; i < header.size(); i++)
                                {
                                        std::string colLabel = header[i];
                                        if(colLabel.compare("x") == 0)
                                                xCol = i;
                                
                                        if(colLabel.compare("y") == 0)
                                                yCol = i;
                                
                                        if(colLabel.compare("z") == 0)
                                                zCol = i;

                                        if(colLabelRegistered(colLabel))
                                        {
                                                descColsToKeep.push_back(i);
                                                DescAssociationPair associationPair = getDescAssociationPair(colLabel);

                                                colToDescPair[i] = associationPair;
                                                descLabelToNumRows[associationPair.second]++;
                                        }
                                }

                                // Do cumulative sum over number of descriptor rows per decriptor to get the starting
                                // index row of reach descriptor
                                int cumSum = 0;
                                for(map<string,int>::const_iterator it = descLabelToNumRows.begin(); it != descLabelToNumRows.end(); it++)
                                {
                                        descLabelToStartingRows[it->first] = cumSum;
                                        cumSum += it->second;
                                }

                                // allocate descriptor vectors
                                numDescCols = descColsToKeep.size(); // number of descriptor vectors
                                descCols.resize(numDescCols);

                                if(xCol == -1 || yCol == -1)
                                {
                                        for(unsigned int i = 0; i < header.size(); i++)
                                        {
                                                cout << "(" << i << ") " << header[i] << endl;
                                        }
                                        cout << endl << "Enter ID for x: ";
                                        cin >> xCol;
                                        cout << "Enter ID for y: ";
                                        cin >> yCol;
                                        cout << "Enter ID for z (-1 if 2D data): ";
                                        cin >> zCol;
                                }
                        }
                        else
                        {
                                // Check if it is a simple file with only coordinates
                                if (!(dim == 2 || dim == 3))
                                {
                                        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 >> xCol;
                                        cout << "Enter column ID (starting from 0) for y: ";
                                        cin >> yCol;
                                        cout << "Enter column ID (starting from 0, -1 if 2D data) for z: ";
                                        cin >> zCol;
                                }
                                else
                                {
                                        // Assume logical order...
                                        xCol = 0;
                                        yCol = 1;
                                        if(dim == 3)
                                                zCol = 2;
                                }
                        }

                        if(zCol != -1)
                                dim = 3;
                        else
                                dim = 2;
                }

                // Load data!
                char *brkt = 0;
                token = strtok_r(line, delimiters, &brkt);
                int currentCol = 0;
                int d = 0; // descriptor vector iterator
                int nextDescCol = -1; // next descriptor column to be recorded
                if (numDescCols > 0)
                        nextDescCol = descColsToKeep[0];

                while (token)
                {
                        // Load data only if no text is on the line
                        if(!hasHeader)
                        {
                                if(currentCol == xCol)
                                        xData.push_back(atof(token));
                                if(currentCol == yCol)
                                        yData.push_back(atof(token));
                                if(currentCol == zCol)
                                        zData.push_back(atof(token));
                                if(currentCol == nextDescCol)
                                {
                                        DescAssociationPair descPair = colToDescPair[nextDescCol];
                                        int startingRow = descLabelToStartingRows[descPair.second];
                                        descCols[startingRow + descPair.first].push_back(atof(token));
                                        d++;
                                        // check for next descriptor column, if there are no more than we will no longer check
                                        if (d < numDescCols)
                                                nextDescCol = descColsToKeep[d];
                                        else
                                                nextDescCol = -1;
                                }
                        }

                        token = strtok_r(NULL, delimiters, &brkt);
                        currentCol++;
                }
                
                // Add one for uniform coordinates
                padData.push_back(1);
                
                if (firstLine)
                {
                        // create standard labels
                        for (int i=0; i < dim; i++)
                        {
                                string text;
                                text += char('x' + i);
                                featureLabels.push_back(Label(text, 1));
                        }
                        featureLabels.push_back(Label("pad", 1));

                        for(map<string,int>::const_iterator d_it = descLabelToNumRows.begin(); d_it != descLabelToNumRows.end(); d_it++)
                        {
                                descriptorLabels.push_back(Label(d_it->first,d_it->second));
                        }
                }

                firstLine = false;
        }

        assert(xData.size() == yData.size());
        int nbPoints = xData.size();

        // Transfer loaded points in specific structure (eigen matrix)
        Matrix features(dim+1, nbPoints);
        Matrix descriptors(numDescCols, nbPoints);

        for(int i=0; i < nbPoints; i++)
        {
                features(0,i) = xData[i];
                features(1,i) = yData[i];
                if(dim == 3)
                {
                        features(2,i) = zData[i];
                        features(3,i) = 1;
                }
                else
                {
                        features(2,i) = 1;
                }

                for (int d = 0; d < numDescCols; d++)
                {
                        descriptors(d,i) = descCols[d][i];
                }
        }
        
        if (numDescCols > 0)
        {
                DataPoints dataPoints(features, featureLabels, descriptors, descriptorLabels);
                return dataPoints;
        }
        else
        {
                DataPoints dataPoints(features, featureLabels);
                return dataPoints;
        }
        //cout << "Loaded " << dataPoints.features.cols() << " points." << endl;
        //cout << "Find " << dataPoints.features.rows() << " dimensions." << endl;
        //cout << features << endl;

}

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) 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);
        else 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("saveAnyFormat(): Unknown extension \"" + ext + "\" for file \"" + fileName + "\", extension must be either \".vtk\" or \".csv\"");
}

template
void PointMatcher<float>::DataPoints::save(const std::string& fileName) const;
template
void PointMatcher<double>::DataPoints::save(const std::string& fileName) 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)
        {
                cerr << "Warning, no points, doing nothing" << endl;
                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 (int i = 0; i < data.descriptorLabels.size(); i++)
        {
                Label lab = data.descriptorLabels[i];
                for (int 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());
        if (!ifs.good())
                throw runtime_error(string("Cannot open file ") + fileName);
        return loadVTK(ifs);
}

template<typename T>
typename PointMatcher<T>::DataPoints PointMatcherIO<T>::loadVTK(std::istream& is)
{
        //typedef typename DataPoints::Label Label;
        //typedef typename DataPoints::Labels Labels;
        
        DataPoints loadedPoints;

        // parse header
        string line;
        getline(is, line);
        if (line.find("# vtk DataFile Version") != 0)
                throw runtime_error(string("Wrong magic header, found ") + line);
        getline(is, line);
        getline(is, line);
        if (line != "ASCII")
                throw runtime_error(string("Wrong file type, expecting ASCII, found ") + line);
        getline(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 and descriptors
        string fieldName;
        string name;
        int pointCount = 0;
        string type;
        while (is.good())
        {
                is >> fieldName;
                
                // load features
                if(fieldName == "POINTS")
                {
                        is >> pointCount;
                        is >> type;
                        
                        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)
                        {
                                is >> features(0, p);
                                is >> features(1, p);
                                is >> features(2, p);
                                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")
                {
                        int size;
                        int verticeSize;
                        is >> size >> verticeSize;
                        // Skip vertice definition
                        for (int p = 0; p < size; p++)
                        {
                                getline(is, line); 
                                if(line == "")
                                        p--;
                        }
                }

                else if(dataType == POLYDATA && fieldName == "LINES")
                {
                        int size;
                        int lineSize;
                        is >> size >> lineSize;
                        // Skip line definition
                        for (int p = 0; p < size; p++)
                        {
                                getline(is, line);
                                if(line == "")
                                        p--;
                        }
                }

                else if(dataType == POLYDATA && fieldName == "POLYGONS")
                {
                        int size;
                        int polySize;
                        is >> size >> polySize;
                        // Skip line definition
                        for (int p = 0; p < size; p++)
                        {
                                getline(is, line);
                                if(line == "")
                                        p--;
                        }
                }

                else if(dataType == POLYDATA && fieldName == "TRIANGLE_STRIPS")
                {
                        int size;
                        int stripSize;
                        is >> size >> stripSize;
                        // Skip line definition
                        for (int p = 0; p < size; p++)
                        {
                                getline(is, line);
                                if(line == "")
                                        p--;
                        }
                }

                // Unstructure Grid
                else if(dataType == UNSTRUCTURED_GRID && fieldName == "CELLS")
                {
                        int size;
                        int cellSize;
                        is >> size >> cellSize;
                        // Skip line definition
                        for (int p = 0; p < size; p++)
                        {
                                getline(is, line);
                                if(line == "")
                                        p--;
                        }
                }
                else if(dataType == UNSTRUCTURED_GRID && fieldName == "CELL_TYPES")
                {
                        int size;
                        int cellSize;
                        is >> size >> cellSize;
                        // Skip line definition
                        for (int p = 0; p < size; p++)
                        {
                                getline(is, line);
                                if(line == "")
                                        p--;
                        }
                }

                // 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++)
                        {
                                //getline(is, line);
                                string fieldDataArrayName, fieldDataArrayType;
                                int numComponents, numTuples;
                                is >> fieldDataArrayName >> numComponents >> numTuples >> fieldDataArrayType;

                                int t_val;
                                for (int t = 0; t < numComponents * numTuples; t++ )
                                {
                                        is >> t_val;
                                }
                        }
                }
                else // Load descriptors
                {
                        // descriptor name
                        is >> name;

                        int dim = 0;
                        bool skipLookupTable = 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";
                        }
                        else
                                throw runtime_error(string("Unknown field name " + fieldName + ", expecting SCALARS, VECTORS, TENSORS, NORMALS or COLOR_SCALARS."));

                        
                        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)
                        {
                                //FIXME: FP - why the first line is aways empty?
                                getline(is, line); 
                                getline(is, line); 
                        }

                        Matrix descriptor(dim, pointCount);
                        for (int p = 0; p < pointCount; ++p)
                        {
                                for(int d = 0; d < dim; d++)
                                {
                                        is >> descriptor(d, p);
                                }
                        }
                        loadedPoints.addDescriptor(name, descriptor);
                }
                         
        }
        
        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)
{
        typedef typename InspectorsImpl<T>::VTKFileInspector VTKInspector;
        
        Parametrizable::Parameters param;
        boost::assign::insert(param) ("baseFileName", "");
        VTKInspector vtkInspector(param);
        vtkInspector.dumpDataPoints(data, fileName);
}


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

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)
{
        typedef typename DataPoints::Label Label;
        typedef typename DataPoints::Labels Labels;
        typedef vector<PLYElement*> Elements;

        // 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;
        getline(is, line);

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

        while (!header_processed)
        {
                if(!getline(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;
                }

                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")
                {
                        if (current_element != NULL && current_element->getNumSupportedProperties() == 0)
                        {
                                LOG_WARNING_STREAM("PLY parse warning: no supported properties in element " << current_element->name);
                        }

                        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) {
                                                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);

                                if (current_element->supportsProperty(list_prop))
                                {
                                        current_element->addProperty(list_prop);
                                }
                                else
                                {
                                        LOG_WARNING_STREAM("PLY parse error: element " << current_element->name
                                                        << " does not support property " << prop_name << " " << prop_type  );
                                }
                        }
                        // PLY regular property
                        else
                        {
                                prop_type = next;
                                stringstream >> prop_name;
                                PLYProperty prop(prop_type, prop_name, current_element->total_props);

                                if (current_element->supportsProperty(prop))
                                {
                                        current_element->addProperty(prop);
                                }
                                else
                                {
                                        LOG_WARNING_STREAM("PLY parse error: element " << current_element->name <<
                                                        " does not support property " << prop_name << " " << prop_type  );
                                }
                        }

                        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"));
                        }

                        if (current_element != NULL && current_element->getNumSupportedProperties() == 0)
                        {
                                LOG_WARNING_STREAM("PLY parse warning: no supported properties in element " << current_element->name);
                        }

                        header_processed = true;
                }
        }

        // PARSE PLY DATA
        Matrix features, descriptors;
        Labels feat_labels, desc_labels;

        int l = 0; // number of elements instances that were processed
        for (typename Elements::const_iterator el_it = elements.begin(); el_it != elements.end(); el_it++)
        {
                vector<PLYProperty> feature_props = (*el_it)->getFeatureProps();
                vector<PLYProperty> descriptor_props = (*el_it)->getDescriptorProps();
                PLYDescPropMap descriptor_map = (*el_it)->getDescPropMap();
                const unsigned n_points = (*el_it)->num;
                const unsigned offset = (*el_it)->offset;
                const unsigned n_feat = (*el_it)->getNumFeatures();
                const unsigned n_desc = (*el_it)->getNumDescriptors();
                const unsigned n_dprop = (*el_it)->getNumDescProp();
                int feat_offset = features.rows();
                int desc_offset = descriptors.rows();

                // map to reorder descriptor properties into consecutive rows corresponding to descriptors
                std::map<std::string,int> descPropToRow;

                // Get labels
                for (typename vector<PLYProperty>::const_iterator it = feature_props.begin(); it != feature_props.end(); it++ )
                {
                        feat_labels.push_back(Label(it->name));
                }

                int r = 0;
                for (typename PLYDescPropMap::const_iterator it = descriptor_map.begin(); it != descriptor_map.end(); it++ )
                {
                        desc_labels.push_back(Label(it->first,it->second.size()));

                        BOOST_FOREACH(PLYProperty prop, it->second )
                        {
                                descPropToRow[prop.name] = r;
                                r++;
                        }

                }

                // Allocate features and descriptors matrix
                // If there are more than one element, grow the features matrix
                // to accommodate new features in this element
                features.conservativeResize(n_feat+feat_offset,n_points);

                if (n_desc > 0)
                        descriptors.conservativeResize(n_dprop+desc_offset,n_points);

                while (l < (offset + n_points) )
                {
                        if(!getline(is, line))
                                throw runtime_error("PLY parse error: expected more data lines");

                        if ( line.empty() )
                                continue;

                        istringstream ss (line);

                        string first_word, prop_s;
                        ss >> first_word;

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

                        // move to offset line
                        if (l < offset)
                        {
                                l++; // increment line count
                                continue;
                        }

                        unsigned f = 0; // features dimension
                        unsigned d = 0; // descriptor dimension

                        for (int pr = 0; f < n_feat || d < n_dprop ; pr++)
                        {
                                unsigned next_f = feature_props[f].pos; // get next supported feature property column
                                int next_d, next_d_r;
                                if (n_desc > 0)
                                {
                                        next_d          = descriptor_props[d].pos; // get next supported descriptor property column
                                        next_d_r        = descPropToRow[descriptor_props[d].name]; // get row of next supported descriptor in desc matrix
                                }
                                else
                                        next_d = -1;


                                T prop_val;

                                if (pr > 0)
                                {
                                        if(!(ss >> prop_val))
                                                throw runtime_error(string("PLY parse error: at line ") + boost::lexical_cast<string>(l));
                                }
                                else
                                        prop_val = boost::lexical_cast<T>(first_word);

                                if (pr == next_f)
                                {
                                        features(f+feat_offset,l-offset) = prop_val;
                                        f++;
                                }

                                else if (pr == next_d)
                                {
                                        descriptors(next_d_r+desc_offset,l-offset) = prop_val;
                                        d++;
                                }
                        }

                        l++; // increment line count
                }
        }

        // Add homogeneous coordinates padding
        feat_labels.push_back(Label("pad"));
        features.conservativeResize(features.rows()+1,features.cols());
        features.row(features.rows()-1) = Vector::Ones(features.cols());

        if (descriptors.rows() > 0)
        {
                DataPoints loadedPoints(features,feat_labels,descriptors,desc_labels);
                return loadedPoints;
        }
        else
        {
                DataPoints loadedPoints(features,feat_labels);
                return loadedPoints;
        }
}

template<typename T>
void PointMatcherIO<T>::savePLY(const DataPoints& data,
                const std::string& fileName)
{
        typedef typename DataPoints::Label Label;
        //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)
        {
                cerr << "Warning, no points, doing nothing" << endl;
                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 (int i = 0; i < data.descriptorLabels.size(); i++)
        {
                Label lab = data.descriptorLabels[i];
                for (int 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 << " ";
                }
                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>::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, const bool is_feature) :
                name(name), type(type), pos(pos), is_feature(is_feature)  {
        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"));
        }
}

template<typename T>
PointMatcherIO<T>::PLYProperty::PLYProperty(const std::string& idx_type,
                const std::string& type, const std::string& name, const unsigned pos, const bool is_feature) :
                name(name), type(type), idx_type(idx_type), pos(pos), is_feature(is_feature)
{
        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"));
}

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

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

template <typename T>
void PointMatcherIO<T>::PLYElement::addProperty(
                PLYProperty& prop) {
        if (supportsProperty(prop))
        {
                //prop.is_feature = (getPMType(prop) == FEATURE);

                PMPropTypes pm_type = getPMType(prop);

                if (pm_type == FEATURE)
                {
                        features.push_back(prop);
                }
                else if (pm_type == DESCRIPTOR)
                {
                        DescAssociationPair associationPair = getDescAssociationPair(prop.name);

                        // Handle shuffling of
                        // if property is in the right order, push back to the end of vector
                        std::vector<PLYProperty>* propList = &descriptor_map[associationPair.second];

                        // if property is to be put in a later place, you need to resize the vector first
                        if (associationPair.first >= propList->size())
                        {
                                propList->resize(associationPair.first + 1);
                                (*propList)[associationPair.first] = prop;
                        }
                        // if to be put before, no need to resize
                        else
                                (*propList)[associationPair.first] = prop;

                        descriptors.push_back(prop);
                }
                else
                {
                        throw std::runtime_error("PLY parse error: tried at add an unsupported property");
                }
        }
        else
                throw std::runtime_error(
                                std::string("PLY parse error: property ") + prop.name
                                                + std::string(" not supported by element ") + name);
}

template <typename T>
int PointMatcherIO<T>::PLYElement::getNumFeatures() const
{
        return features.size();
}

template <typename T>
int PointMatcherIO<T>::PLYElement::getNumDescriptors() const
{
        return descriptor_map.size();
}

template <typename T>
int PointMatcherIO<T>::PLYElement::getNumDescProp() const
{
        return descriptors.size();
}

template <typename T>
const std::vector<typename PointMatcherIO<T>::PLYProperty>& PointMatcherIO<T>::PLYElement::getFeatureProps() const
{
        return features;
}

template <typename T>
const std::vector<typename PointMatcherIO<T>::PLYProperty>& PointMatcherIO<T>::PLYElement::getDescriptorProps() const
{
        return descriptors;
}

template <typename T>
const typename PointMatcherIO<T>::PLYDescPropMap& PointMatcherIO<T>::PLYElement::getDescPropMap() const
{
        return descriptor_map;
}

template <typename T>
size_t PointMatcherIO<T>::PLYElement::getNumSupportedProperties() const {
        return features.size() + descriptors.size() ;
}

template <typename T>
bool PointMatcherIO<T>::PLYElement::supportsProperty(const PLYProperty& prop) const
{
        return getPMType(prop) != UNSUPPORTED;
}

template<typename T>
typename PointMatcherIO<T>::PLYElement::PMPropTypes PointMatcherIO<T>::PLYVertex::getPMType(const PLYProperty& prop) const
{
        if (prop.name == "x" || prop.name == "y" || prop.name == "z")
                return this->FEATURE;
        else if (colLabelRegistered(prop.name))
                return this->DESCRIPTOR;
        else
                return this->UNSUPPORTED;
}

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
//PointMatcherIO<float>::DataPoints PointMatcherIO<float>::loadPCD(istream& is);
//template
//PointMatcherIO<double>::DataPoints PointMatcherIO<double>::loadPCD(istream& is);

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

        typedef typename DataPoints::Label Label;
        typedef typename DataPoints::Labels Labels;

        size_t numFields = 0;
        size_t numDataFields = 0; // takes into account the cound of each field for multi row descriptors
        int xFieldCol = -1;
        int yFieldCol = -1;
        int zFieldCol = -1;

        vector<int> descFieldsToKeep;
        map<int,DescAssociationPair> colToDescPair;
        map<string,int> descLabelToNumRows;
        map<string,int> descLabelToStartingRows;
        vector<int> descDimensions;

        string xFieldType;
        string yFieldType;
        string zFieldType;

        size_t width;
        size_t height;
        size_t numPoints;
        size_t numPointsR; // redundant value specified in POINTS field

        size_t lineNum = 0;

        while (!is.eof())
        {
                string line;
                getline(is, line);

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

                // ignore comments
                if (line.substr(0,1) == "#")
                {
                        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")
                {
                        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")
                {
                        numFields = tokens.size() - 1;
                        numDataFields = numFields; // in case COUNT is not defined in which case we assume 1 data field per field
                        for (size_t i = 1; i < tokens.size(); i++)
                        {
                                if (tokens[i] == "x")
                                        xFieldCol = i - 1;
                                else if (tokens[i] == "y")
                                        yFieldCol = i - 1;
                                else if (tokens[i] == "z")
                                        zFieldCol = i - 1;

                                else if(colLabelRegistered(tokens[i]))
                                {
                                        descFieldsToKeep.push_back(i);
                                        DescAssociationPair associationPair = getDescAssociationPair(tokens[i]);

                                        colToDescPair[i] = associationPair;
                                        descLabelToNumRows[associationPair.second]++;
                                }
                        }
                }

                else if (tokens[0] == "SIZE")
                {
                        if (xFieldCol == -1 || yFieldCol == -1)
                                throw runtime_error("PCD Parse Error: x field or y field not defined");
                        if (tokens.size()  - 1 !=  numFields)
                                throw runtime_error("PCD Parse Error: size not defined for all fields");

//                      try {
//                              xFieldBytes = boost::lexical_cast<int>(tokens[xFieldCol + 1]);
//                              yFieldBytes = boost::lexical_cast<int>(tokens[yFieldCol + 1]);
//                              if (zFieldCol > -1)
//                                      zFieldBytes = boost::lexical_cast<int>(tokens[zFieldCol + 1]);
//                      }
//                      catch (boost::bad_lexical_cast& e)
//                      {
//                              throw runtime_error("PCD Parse Error: invalid size field");
//                      }

                }

                else if (tokens[0] == "TYPE")
                {
                        if (xFieldCol == -1 || yFieldCol == -1)
                                throw runtime_error("PCD Parse Error: x field or y field not defined");
                        if (tokens.size()  - 1 !=  numFields)
                                throw runtime_error("PCD Parse Error: type not defined for all fields");
                        xFieldType = tokens[xFieldCol + 1];
                        yFieldType = tokens[yFieldCol + 1];

                        if (xFieldType != "I" && xFieldType != "U" && xFieldType != "F" &&
                                        yFieldType != "I" && yFieldType != "U" && yFieldType != "F")
                                throw runtime_error("PCD Parse Error: invalid type");

                        if (zFieldCol > -1)
                        {
                                zFieldType = tokens[zFieldCol + 1];
                                if (zFieldType != "I" && zFieldType != "U" && zFieldType != "F")
                                        throw runtime_error("PCD Parse Error: invalid type");
                        }
                }

                // overwrite descriptor dimension count with values from header
                else if (tokens[0] == "COUNT")
                {
                        if (tokens.size() - 1 != numFields)
                                throw runtime_error("PCD Parse Error: COUNT number does not match number of fields");

                        // first get total count including fields we aren't using
                        numDataFields = 0;

                        // we need to overwrite the col to desc pair since there will be more
                        // columns now that we have several data counts per field
                        map<int, DescAssociationPair> colToDescPair_ = colToDescPair;
                        colToDescPair.clear();

                        vector<int>::const_iterator nextFieldToKeepIt = descFieldsToKeep.begin();
                        for (int i = 1; i < tokens.size(); i++)
                        {
                                int count = boost::lexical_cast<int>(tokens[i]);

                                if (i == *nextFieldToKeepIt)
                                {
                                        string descLabel = colToDescPair_[i].second;
                                        descLabelToNumRows[descLabel] = count;

                                        for (int p = 0; p < count; p++)
                                                colToDescPair[numDataFields + p] = DescAssociationPair(p, descLabel);

                                        if (nextFieldToKeepIt != descFieldsToKeep.end())
                                                nextFieldToKeepIt++;
                                }

                                numDataFields += count;

                        }
                }

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

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

                // ignore viewpoint for now
                else if (tokens[0] == "VIEWPOINT")
                {
                        continue;
                }

                else if (tokens[0] == "POINTS")
                {
                        try
                        {
                                numPointsR = boost::lexical_cast<int>(tokens[1]);
                        }
                        catch (boost::bad_lexical_cast& e)
                        {
                                throw runtime_error("PCD Parse Error: invalid number of points");
                        }
                }

                else if (tokens[0] == "DATA")
                {
                        if (tokens[1] != "ascii")
                                throw runtime_error("PCD Parse Error: only ascii data is supported");

                        break;
                }

                lineNum++;
        }

        // get number of points
        numPoints = width * height;

        if (numPoints != numPointsR)
                throw runtime_error("PCD Parse Error: POINTS field does not match WIDTH and HEIGHT fields");

        // prepare features matrix
        Matrix features;
        if (zFieldCol > -1)
                features = Matrix(4,numPoints);
        else
                features = Matrix(3,numPoints);

        // Prepare descriptors
        // Do cumulative sum over number of descriptor rows per decriptor to get the starting
        // index row of reach descriptor
        int cumSum = 0;
        for(map<string,int>::const_iterator it = descLabelToNumRows.begin(); it != descLabelToNumRows.end(); it++)
        {
                descLabelToStartingRows[it->first] = cumSum;
                cumSum += it->second;
        }

        // allocate descriptor vectors
        size_t numDescCols = cumSum; // number of descriptor vectors
        Matrix descriptors(numDescCols,numPoints);

        // Now read in the data
        size_t p = 0; // point count
        while (!is.eof())
        {
                string line;
                getline(is, line);

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

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

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

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

                features(0,p) = boost::lexical_cast<T>(tokens[xFieldCol]);
                features(1,p) = boost::lexical_cast<T>(tokens[yFieldCol]);

                if (zFieldCol > -1)
                {
                        features(2,p) = boost::lexical_cast<float>(tokens[zFieldCol]);
                        features(3,p) = 1;
                } else
                        features(2,p) = 1;

                for (map<int,DescAssociationPair>::const_iterator cit = colToDescPair.begin();
                                cit != colToDescPair.end(); cit++)
                {
                        int startingRow = descLabelToStartingRows[cit->second.second];
                        descriptors(startingRow + cit->second.first,p) = boost::lexical_cast<T>(tokens[cit->first]);
                }

                p++;
                lineNum++;

                if (p == numPoints)
                        break;

        }

        if (p != numPoints)
        {
                boost::format errorFmt("PCD Parse Error: the number of points in the data %1 is less than the specified number of points %2");
                errorFmt % p % numPoints;
                throw runtime_error(errorFmt.str());
        }

        Labels featureLabels;
        featureLabels.push_back(Label("x"));
        featureLabels.push_back(Label("y"));

        Labels descriptorLabels;
        int n = 0;
        for (map<string,int>::const_iterator it = descLabelToNumRows.begin(); it != descLabelToNumRows.end(); it++)
        {
                descriptorLabels.push_back(Label(it->first,it->second));
                n++;
        }

        if (zFieldCol > -1)
                featureLabels.push_back(Label("z"));

        DataPoints out;

        if (numDescCols > 0)
                out = DataPoints(features, featureLabels, descriptors, descriptorLabels);
        else
                out = DataPoints(features, featureLabels);
        return out;
}

template<typename T>
void PointMatcherIO<T>::savePCD(const DataPoints& data,
                const std::string& fileName) {
        typedef typename DataPoints::Label Label;
        //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());
        const int descCount(data.descriptorLabels.size());

        if (pointCount == 0)
        {
                cerr << "Warning, no points, doing nothing" << endl;
                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);