CalibrationYaml.hh

Go to the documentation of this file.

#ifndef CALIBRATION_YAML_HH
#define CALIBRATION_YAML_HH

#include <stdint.h>
#include <iostream>
#include <string>
#include <vector>

template<typename T>
std::ostream& writeMatrix (std::ostream& stream, std::string const& name, uint32_t rows, uint32_t columns, T const* data)
{
    stream << name << ": !!opencv-matrix\n";
    stream << "   rows: " << rows << "\n";
    stream << "   cols: " << columns << "\n";
    stream << "   dt: d\n";
    stream << "   data: [ ";

        stream.precision (17);
        stream << std::scientific;
        for (uint32_t i = 0; i < rows; i++)
    {
        if (i != 0)
        {
            stream << ",\n";
            stream << "       ";
        }
        for (uint32_t j = 0; j < columns; j++)
        {
            if (j != 0)
            {
                stream << ", ";
            }
            stream << data[i * columns + j];
        }
    }
    stream << " ]\n";
    return stream;
}

class Expect
{
private:
    std::string m_value;

public:
    Expect (std::string const& value) :
        m_value (value)
    {
    }

    std::string const& value () const
    {
        return this->m_value;
    }
};

std::istream& operator >> (std::istream& stream, Expect const& expect)
{
    stream >> std::ws;

    for (std::string::const_iterator iter = expect.value ().begin (); iter != expect.value ().end (); ++iter)
    {
        if (*iter == ' ')
        {
            stream >> std::ws;
            continue;
        }
        if (stream.get () != *iter)
        {
            stream.clear (std::ios_base::failbit);
            break;
        }
    }

    return stream;
}

template<typename T>
std::istream& operator >> (std::istream& stream, std::vector<T>& data)
{
    char input;
    while (stream.good ())
    {
        input = 0;
        stream >> input;
        if (input == '[')
        {
            stream >> data;
        }
        else
        {
            stream.putback (input);

            T value;
            stream >> value;
            data.push_back (value);
        }

        input = 0;
        stream >> input;
        if (input == ']')
        {
            break;
        }
        else if (input != ',')
        {
            stream.clear (std::ios_base::failbit);
            break;
        }
    }

    return stream;
}

std::istream& parseYaml (std::istream& stream, std::map<std::string, std::vector<float> >& data)
{
    char input;
    while (stream.good ())
    {
        input = 0;
        stream >> input;
        if (input == '%')
        {
            std::string comment;
            std::getline (stream, comment);
            continue;
        }
        stream.putback (input);

        std::string name;
        stream >> name;
        if (name.empty ())
        {
            break;
        }
        if (name[name.size () - 1] != ':')
        {
            stream.clear (std::ios_base::failbit);
            break;
        }
        name.resize (name.size () - 1);

        std::vector<float> arrayContents;
        arrayContents.clear ();

        input = 0;
        stream >> input;
        if (input == '[')
        {
            stream >> arrayContents;
        }
        else
        {
            stream.putback (input);

            uint32_t rows = 0;
            uint32_t columns = 0;
            stream >> Expect ("!!opencv-matrix");
            stream >> Expect ("rows:") >> rows;
            stream >> Expect ("cols:") >> columns;
            stream >> Expect ("dt: d");
            stream >> Expect ("data: [") >> arrayContents;
        }

                if (stream.good())
                {
                        data.insert (std::make_pair(name, arrayContents));
                }
                else
                {
                        fprintf (stderr, "Error parsing data near array \"%s\"", name.c_str());
                }
    }

    return stream;
}

#endif //CALIBRATION_YAML_HH