CalibrationYaml.hh

Go to the documentation of this file.

 
#ifndef CALIBRATION_YAML_HH
#define CALIBRATION_YAML_HH
 
#include <stdint.h>
#include <iostream>
#include <iomanip>
#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 << std::setw(22) << 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 == '%' || 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