image_store.cc

Go to the documentation of this file.

/*
 * This file is part of the rc_genicam_api package.
 *
 * Copyright (c) 2019 Roboception GmbH
 * All rights reserved
 *
 * Author: Heiko Hirschmueller
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * 3. Neither the name of the copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 "image_store.h"
#include "pixel_formats.h"
 
#include <exception>
 
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <limits>
#include <algorithm>
 
#ifdef INCLUDE_PNG
#include <png.h>
#endif
 
#ifdef _WIN32
#undef min
#undef max
#endif
 
namespace rcg
{
 
std::string ensureNewFileName(std::string name)
{
  // check if given name is already used
 
  std::ifstream file(name);
 
  if (file.is_open())
  {
    file.close();
 
    // split name in prefix and suffix
 
    std::string suffix;
 
    size_t i=name.rfind('.');
    if (i != name.npos && name.size()-i <= 4)
    {
      suffix=name.substr(i);
      name=name.substr(0, i);
    }
 
    // add number for finding name that is nor used
 
    int n=1;
    while (n < 100)
    {
      std::ostringstream s;
      s << name << "_" << n << suffix;
 
      file.open(s.str());
      if (!file.is_open())
      {
        name=s.str();
        break;
      }
 
      file.close();
      n++;
    }
  }
 
  return name;
}
 
namespace
{
 
class IOException : public std::exception
{
  public:
 
    IOException(const std::string &_msg) { msg=_msg; }
    virtual const char *what() const noexcept { return msg.c_str(); }
 
  private:
 
    std::string msg;
};
 
std::string storeImagePNM(const std::string &name, const Image &image, size_t yoffset,
  size_t height)
{
  size_t width=image.getWidth();
  size_t real_height=image.getHeight();
 
  if (height == 0) height=real_height;
 
  yoffset=std::min(yoffset, real_height);
  height=std::min(height, real_height-yoffset);
 
  const unsigned char *p=static_cast<const unsigned char *>(image.getPixels());
 
  size_t px=image.getXPadding();
 
  uint64_t format=image.getPixelFormat();
  std::string full_name;
 
  switch (format)
  {
    case Mono8: // store 8 bit monochrome image
    case Confidence8:
    case Error8:
      {
        full_name=ensureNewFileName(name+".pgm");
        std::ofstream out(full_name, std::ios::binary);
 
        out << "P5" << std::endl;
        out << width << " " << height << std::endl;
        out << 255 << "\n";
 
        std::streambuf *sb=out.rdbuf();
 
        p+=(width+px)*yoffset;
        for (size_t k=0; k<height && out.good(); k++)
        {
          for (size_t i=0; i<width; i++)
          {
            sb->sputc(static_cast<char>(*p++));
          }
 
          p+=px;
        }
 
        out.close();
      }
      break;
 
    case Mono16:
    case Coord3D_C16: // store 16 bit monochrome image
      {
        full_name=ensureNewFileName(name+".pgm");
        std::ofstream out(full_name, std::ios::binary);
 
        out << "P5" << std::endl;
        out << width << " " << height << std::endl;
        out << 65535 << "\n";
 
        std::streambuf *sb=out.rdbuf();
 
        // copy image data, pgm is always big endian
 
        p+=(2*width+px)*yoffset;
        if (image.isBigEndian())
        {
          for (size_t k=0; k<height && out.good(); k++)
          {
            for (size_t i=0; i<width; i++)
            {
              sb->sputc(static_cast<char>(*p++));
              sb->sputc(static_cast<char>(*p++));
            }
 
            p+=px;
          }
        }
        else
        {
          for (size_t k=0; k<height && out.good(); k++)
          {
            for (size_t i=0; i<width; i++)
            {
              sb->sputc(static_cast<char>(p[1]));
              sb->sputc(static_cast<char>(p[0]));
              p+=2;
            }
 
            p+=px;
          }
        }
 
        out.close();
      }
      break;
 
    case YCbCr411_8: // convert and store as color image
    case YCbCr422_8:
    case YUV422_8:
      {
        full_name=ensureNewFileName(name+".ppm");
        std::ofstream out(full_name, std::ios::binary);
 
        out << "P6" << std::endl;
        out << width << " " << height << std::endl;
        out << 255 << "\n";
 
        std::streambuf *sb=out.rdbuf();
 
        size_t pstep;
        if (format == YCbCr411_8)
        {
          pstep=(width>>2)*6+px;
        }
        else
        {
          pstep=(width>>2)*8+px;
        }
 
        p+=pstep*yoffset;
        for (size_t k=0; k<height && out.good(); k++)
        {
          for (size_t i=0; i<width; i+=4)
          {
            uint8_t rgb[12];
 
            if (format == YCbCr411_8)
            {
              convYCbCr411toQuadRGB(rgb, p, static_cast<int>(i));
            }
            else
            {
              convYCbCr422toQuadRGB(rgb, p, static_cast<int>(i));
            }
 
            for (int j=0; j<12; j++)
            {
              sb->sputc(static_cast<char>(rgb[j]));
            }
          }
 
          p+=pstep;
        }
 
        out.close();
      }
      break;
 
    default: // try to store as color image
      {
        std::unique_ptr<uint8_t []> rgb_pixel(new uint8_t [3*width*height]);
 
        if (format == RGB8)
        {
          p+=(3*width+px)*yoffset;
        }
        else
        {
          p+=(width+px)*yoffset;
        }
 
        if (convertImage(rgb_pixel.get(), 0, p, format, width, height, px))
        {
          p=rgb_pixel.get();
 
          full_name=ensureNewFileName(name+".ppm");
          std::ofstream out(full_name, std::ios::binary);
 
          out << "P6" << std::endl;
          out << width << " " << height << std::endl;
          out << 255 << "\n";
 
          std::streambuf *sb=out.rdbuf();
 
          for (size_t k=0; k<height && out.good(); k++)
          {
            for (size_t i=0; i<width; i++)
            {
              sb->sputc(static_cast<char>(*p++));
              sb->sputc(static_cast<char>(*p++));
              sb->sputc(static_cast<char>(*p++));
            }
          }
 
          out.close();
        }
        else
        {
          throw IOException(std::string("storeImage(): Unsupported pixel format: ")+
            GetPixelFormatName(static_cast<PfncFormat>(image.getPixelFormat())));
        }
      }
      break;
  }
 
  return full_name;
}
 
#ifdef INCLUDE_PNG
 
std::string storeImagePNG(const std::string &name, const Image &image, size_t yoffset,
  size_t height)
{
  size_t width=image.getWidth();
  size_t real_height=image.getHeight();
 
  if (height == 0) height=real_height;
 
  yoffset=std::min(yoffset, real_height);
  height=std::min(height, real_height-yoffset);
 
  const unsigned char *p=static_cast<const unsigned char *>(image.getPixels());
 
  size_t px=image.getXPadding();
 
  uint64_t format=image.getPixelFormat();
  std::string full_name;
 
  switch (format)
  {
    case Mono8: // store 8 bit monochrome image
    case Confidence8:
    case Error8:
      {
        // open file and init
 
        full_name=ensureNewFileName(name+".png");
        FILE *out=fopen(full_name.c_str(), "wb");
 
        if (!out)
        {
          throw new IOException("Cannot store file: "+full_name);
        }
 
        png_structp png=png_create_write_struct(PNG_LIBPNG_VER_STRING, 0, 0, 0);
        png_infop info=png_create_info_struct(png);
        setjmp(png_jmpbuf(png));
 
        // write header
 
        png_init_io(png, out);
        png_set_IHDR(png, info, width, height, 8, PNG_COLOR_TYPE_GRAY,
          PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT,
          PNG_FILTER_TYPE_DEFAULT);
        png_write_info(png, info);
 
        // write image body
 
        p+=(width+px)*yoffset;
        for (size_t k=0; k<height; k++)
        {
          png_write_row(png, const_cast<png_bytep>(p));
          p+=width+px;
        }
 
        // close file
 
        png_write_end(png, info);
        fclose(out);
        png_destroy_write_struct(&png, &info);
      }
      break;
 
    case Mono16:
    case Coord3D_C16: // store 16 bit monochrome image
      {
        // open file and init
 
        full_name=ensureNewFileName(name+".png");
        FILE *out=fopen(full_name.c_str(), "wb");
 
        if (!out)
        {
          throw new IOException("Cannot store file: "+full_name);
        }
 
        png_structp png=png_create_write_struct(PNG_LIBPNG_VER_STRING, 0, 0, 0);
        png_infop info=png_create_info_struct(png);
        setjmp(png_jmpbuf(png));
 
        // write header
 
        png_init_io(png, out);
        png_set_IHDR(png, info, width, height, 16, PNG_COLOR_TYPE_GRAY,
          PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT,
          PNG_FILTER_TYPE_DEFAULT);
        png_write_info(png, info);
 
        // write image body
 
        if (!image.isBigEndian())
        {
          png_set_swap(png);
        }
 
        p+=(2*width+px)*yoffset;
        for (size_t k=0; k<height; k++)
        {
          png_write_row(png, const_cast<png_bytep>(p));
          p+=2*width+px;
        }
 
        // close file
 
        png_write_end(png, info);
        fclose(out);
        png_destroy_write_struct(&png, &info);
      }
      break;
 
    case YCbCr411_8: // convert and store as color image
    case YCbCr422_8:
    case YUV422_8:
      {
        // open file and init
 
        full_name=ensureNewFileName(name+".png");
        FILE *out=fopen(full_name.c_str(), "wb");
 
        if (!out)
        {
          throw new IOException("Cannot store file: "+full_name);
        }
 
        png_structp png=png_create_write_struct(PNG_LIBPNG_VER_STRING, 0, 0, 0);
        png_infop info=png_create_info_struct(png);
        setjmp(png_jmpbuf(png));
 
        // write header
 
        png_init_io(png, out);
        png_set_IHDR(png, info, width, height, 8, PNG_COLOR_TYPE_RGB,
          PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT,
          PNG_FILTER_TYPE_DEFAULT);
        png_write_info(png, info);
 
        // write image body
 
        uint8_t *tmp=new uint8_t [3*width];
 
        size_t pstep;
        if (format == YCbCr411_8)
        {
          pstep=(width>>2)*6+px;
        }
        else
        {
          pstep=(width>>2)*8+px;
        }
 
        p+=pstep*yoffset;
        for (size_t k=0; k<height; k++)
        {
          if (format == YCbCr411_8)
          {
            for (size_t i=0; i<width; i+=4)
            {
              convYCbCr411toQuadRGB(tmp+3*i, p, static_cast<int>(i));
            }
          }
          else
          {
            for (size_t i=0; i<width; i+=4)
            {
              convYCbCr422toQuadRGB(tmp+3*i, p, static_cast<int>(i));
            }
          }
 
          png_write_row(png, tmp);
          p+=pstep;
        }
 
        // close file
 
        png_write_end(png, info);
        fclose(out);
        png_destroy_write_struct(&png, &info);
      }
      break;
 
    default: // try to store as color image
      {
        std::unique_ptr<uint8_t []> rgb_pixel(new uint8_t [3*width*height]);
 
        if (format == RGB8)
        {
          p+=(3*width+px)*yoffset;
        }
        else
        {
          p+=(width+px)*yoffset;
        }
 
        if (convertImage(rgb_pixel.get(), 0, p, format, width, height, px))
        {
          p=rgb_pixel.get();
 
          // open file and init
 
          full_name=ensureNewFileName(name+".png");
          FILE *out=fopen(full_name.c_str(), "wb");
 
          if (!out)
          {
            throw new IOException("Cannot store file: "+full_name);
          }
 
          png_structp png=png_create_write_struct(PNG_LIBPNG_VER_STRING, 0, 0, 0);
          png_infop info=png_create_info_struct(png);
          setjmp(png_jmpbuf(png));
 
          // write header
 
          png_init_io(png, out);
          png_set_IHDR(png, info, width, height, 8, PNG_COLOR_TYPE_RGB,
            PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT,
            PNG_FILTER_TYPE_DEFAULT);
          png_write_info(png, info);
 
          // write image body
 
          for (size_t k=0; k<height; k++)
          {
            png_write_row(png, const_cast<png_bytep>(p));
            p+=3*width;
          }
 
          // close file
 
          png_write_end(png, info);
          fclose(out);
          png_destroy_write_struct(&png, &info);
        }
        else
        {
          throw IOException(std::string("storeImage(): Unsupported pixel format: ")+
            GetPixelFormatName(static_cast<PfncFormat>(image.getPixelFormat())));
        }
      }
      break;
  }
 
  return full_name;
}
 
#endif
 
}
 
std::string storeImage(const std::string &name, ImgFmt fmt, const Image &image,
  size_t yoffset, size_t height)
{
  std::string ret;
 
  switch (fmt)
  {
    case PNG:
#ifdef INCLUDE_PNG
      ret=storeImagePNG(name, image, yoffset, height);
#else
      throw IOException("storeImage(): Support for PNG image file format is not compiled in!");
#endif
      break;
 
    default:
    case PNM:
      ret=storeImagePNM(name, image, yoffset, height);
      break;
  }
 
  return ret;
}
 
std::string storeImageAsDisparityPFM(const std::string &name, const Image &image, int inv,
  float scale, float offset)
{
  if (image.getPixelFormat() != Coord3D_C16)
  {
    throw IOException(std::string("storeImageAsDisparityPFM(): Format Coord3D_C16 expected: ")+
      GetPixelFormatName(static_cast<PfncFormat>(image.getPixelFormat())));
  }
 
  if (scale == 0)
  {
    throw IOException(std::string("storeImageAsDisparityPFM(): Scale must not be 0!"));
  }
 
  // convert values and store disparity image
 
  size_t px=image.getXPadding();
  size_t width=image.getWidth();
  size_t height=image.getHeight();
  const unsigned char *p=static_cast<const unsigned char *>(image.getPixels())+
    2*(width+px)*(height+1);
 
  std::string full_name=ensureNewFileName(name+".pfm");
  std::ofstream out(full_name, std::ios::binary);
 
  out << "Pf" << std::endl;
  out << width << " " << height << std::endl;
  out << 1 << "\n";
 
  std::streambuf *sb=out.rdbuf();
 
  // get 16 bit data, scale and add offset and store as big endian
 
  bool msbfirst=true;
 
  {
    int pp=1;
    char *cc=reinterpret_cast<char *>(&pp);
    msbfirst=(cc[0] != 1);
  }
 
  for (size_t k=0; k<height && out.good(); k++)
  {
    p-=(width+px)<<2;
    for (size_t i=0; i<width; i++)
    {
      int val;
      if (image.isBigEndian())
      {
        val=(static_cast<int>(p[0])<<8)|p[1];
      }
      else
      {
        val=(static_cast<int>(p[1])<<8)|p[0];
      }
 
      p+=2;
 
      float d=std::numeric_limits<float>::infinity();
      if (val != inv)
      {
        d=static_cast<float>(val*scale+offset);
      }
 
      char *c=reinterpret_cast<char *>(&d);
 
      if (msbfirst)
      {
        sb->sputc(c[0]);
        sb->sputc(c[1]);
        sb->sputc(c[2]);
        sb->sputc(c[3]);
      }
      else
      {
        sb->sputc(c[3]);
        sb->sputc(c[2]);
        sb->sputc(c[1]);
        sb->sputc(c[0]);
      }
 
      p+=px;
    }
  }
 
  out.close();
 
  return full_name;
}
 
}