entropy_range_coder.hpp

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2012, Willow Garage, Inc.
 *
 *  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 Willow Garage, Inc. 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 OWNER 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.
 *
 *
 * range coder based on Dmitry Subbotin's carry-less implementation (http://www.compression.ru/ds/)
 * Added optimized symbol lookup and fixed/static frequency tables
 *
 */
#pragma once
 
#include <pcl/compression/entropy_range_coder.h>
#include <iostream>
#include <vector>
#include <cstring>
#include <algorithm>
 
//////////////////////////////////////////////////////////////////////////////////////////////
unsigned long
pcl::AdaptiveRangeCoder::encodeCharVectorToStream (const std::vector<char>& inputByteVector_arg,
                                                   std::ostream& outputByteStream_arg)
{
  DWord freq[257];
 
  // define limits
  constexpr DWord top = static_cast<DWord>(1) << 24;
  constexpr DWord bottom = static_cast<DWord>(1) << 16;
  constexpr DWord maxRange = static_cast<DWord>(1) << 16;
 
  auto input_size = static_cast<unsigned> (inputByteVector_arg.size ());
 
  // init output vector
  outputCharVector_.clear ();
  outputCharVector_.reserve (sizeof(char) * input_size);
 
  unsigned int readPos = 0;
 
  DWord low = 0;
  DWord range = static_cast<DWord> (-1);
 
  // initialize cumulative frequency table
  for (unsigned int i = 0; i < 257; i++)
    freq[i] = i;
 
  // scan input
  while (readPos < input_size)
  {
    // read byte
    std::uint8_t ch = inputByteVector_arg[readPos++];
 
    // map range
    low += freq[ch] * (range /= freq[256]);
    range *= freq[ch + 1] - freq[ch];
 
    // check range limits
    while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -static_cast<int>(low) & (bottom - 1)), 1)))
    {
      char out = static_cast<char> (low >> 24);
      range <<= 8;
      low <<= 8;
      outputCharVector_.push_back (out);
    }
 
    // update frequency table
    for (unsigned int j = ch + 1; j < 257; j++)
      freq[j]++;
 
    // detect overflow
    if (freq[256] >= maxRange)
    {
      // rescale
      for (unsigned int f = 1; f <= 256; f++)
      {
        freq[f] /= 2;
        if (freq[f] <= freq[f - 1])
          freq[f] = freq[f - 1] + 1;
      }
    }
 
  }
 
  // flush remaining data
  for (unsigned int i = 0; i < 4; i++)
  {
    char out = static_cast<char> (low >> 24);
    outputCharVector_.push_back (out);
    low <<= 8;
  }
 
  // write to stream
  outputByteStream_arg.write (outputCharVector_.data(), outputCharVector_.size ());
 
  return (static_cast<unsigned long> (outputCharVector_.size ()));
}
 
//////////////////////////////////////////////////////////////////////////////////////////////
unsigned long
pcl::AdaptiveRangeCoder::decodeStreamToCharVector (std::istream& inputByteStream_arg,
                                                   std::vector<char>& outputByteVector_arg)
{
  DWord freq[257];
 
  // define limits
  constexpr DWord top = static_cast<DWord>(1) << 24;
  constexpr DWord bottom = static_cast<DWord>(1) << 16;
  constexpr DWord maxRange = static_cast<DWord>(1) << 16;
 
  auto output_size = static_cast<unsigned> (outputByteVector_arg.size ());
 
  unsigned long streamByteCount = 0;
 
  unsigned int outputBufPos = 0;
 
  DWord code = 0;
  DWord low = 0;
  DWord range = static_cast<DWord> (-1);
 
  // init decoding
  for (unsigned int i = 0; i < 4; i++)
  {
    std::uint8_t ch;
    inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
    streamByteCount += sizeof(char);
    code = (code << 8) | ch;
  }
 
  // init cumulative frequency table
  for (unsigned int i = 0; i <= 256; i++)
    freq[i] = i;
 
  // decoding loop
  for (unsigned int i = 0; i < output_size; i++)
  {
    std::uint8_t symbol = 0;
    std::uint8_t sSize = 256 / 2;
 
    // map code to range
    DWord count = (code - low) / (range /= freq[256]);
 
    // find corresponding symbol
    while (sSize > 0)
    {
      if (freq[symbol + sSize] <= count)
      {
        symbol = static_cast<std::uint8_t> (symbol + sSize);
      }
      sSize /= 2;
    }
 
    // output symbol
    outputByteVector_arg[outputBufPos++] = symbol;
 
    // update range limits
    low += freq[symbol] * range;
    range *= freq[symbol + 1] - freq[symbol];
 
    // decode range limits
    while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -static_cast<int>(low) & (bottom - 1)), 1)))
    {
      std::uint8_t ch;
      inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
      streamByteCount += sizeof(char);
      code = code << 8 | ch;
      range <<= 8;
      low <<= 8;
    }
 
    // update cumulative frequency table
    for (unsigned int j = symbol + 1; j < 257; j++)
      freq[j]++;
 
    // detect overflow
    if (freq[256] >= maxRange)
    {
      // rescale
      for (unsigned int f = 1; f <= 256; f++)
      {
        freq[f] /= 2;
        if (freq[f] <= freq[f - 1])
          freq[f] = freq[f - 1] + 1;
      }
    }
  }
 
  return (streamByteCount);
}
 
//////////////////////////////////////////////////////////////////////////////////////////////
unsigned long
pcl::StaticRangeCoder::encodeIntVectorToStream (std::vector<unsigned int>& inputIntVector_arg,
                                                std::ostream& outputByteStream_arg)
{
  // define numerical limits
  constexpr std::uint64_t top = static_cast<std::uint64_t>(1) << 56;
  constexpr std::uint64_t bottom = static_cast<std::uint64_t>(1) << 48;
  constexpr std::uint64_t maxRange = static_cast<std::uint64_t>(1) << 48;
 
  auto input_size = static_cast<unsigned long> (inputIntVector_arg.size ());
 
  // init output vector
  outputCharVector_.clear ();
  outputCharVector_.reserve ((sizeof(char) * input_size * 2));
 
  std::uint64_t frequencyTableSize = 1;
 
  unsigned int readPos = 0;
 
  // calculate frequency table
  cFreqTable_[0] = cFreqTable_[1] = 0;
  while (readPos < input_size)
  {
    unsigned int inputSymbol = inputIntVector_arg[readPos++];
 
    if (inputSymbol + 1 >= frequencyTableSize)
    {
      // frequency table is to small -> adaptively extend it
      std::uint64_t oldfrequencyTableSize;
      oldfrequencyTableSize = frequencyTableSize;
 
      do
      {
        // increase frequency table size by factor 2
        frequencyTableSize <<= 1;
      } while (inputSymbol + 1 > frequencyTableSize);
 
      if (cFreqTable_.size () < frequencyTableSize + 1)
      {
        // resize frequency vector
        cFreqTable_.resize (static_cast<std::size_t> (frequencyTableSize + 1));
      }
 
      // init new frequency range with zero
      std::fill_n(&cFreqTable_[oldfrequencyTableSize + 1],
                  frequencyTableSize - oldfrequencyTableSize, 0);
    }
    cFreqTable_[inputSymbol + 1]++;
  }
  frequencyTableSize++;
 
  // convert to cumulative frequency table
  for (std::uint64_t f = 1; f < frequencyTableSize; f++)
  {
    cFreqTable_[f] = cFreqTable_[f - 1] + cFreqTable_[f];
    if (cFreqTable_[f] <= cFreqTable_[f - 1])
      cFreqTable_[f] = cFreqTable_[f - 1] + 1;
  }
 
  // rescale if numerical limits are reached
  while (cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)] >= maxRange)
  {
    for (std::size_t f = 1; f < cFreqTable_.size (); f++)
    {
      cFreqTable_[f] /= 2;
      ;
      if (cFreqTable_[f] <= cFreqTable_[f - 1])
        cFreqTable_[f] = cFreqTable_[f - 1] + 1;
    }
  }
 
  // calculate amount of bytes per frequency table entry
  auto frequencyTableByteSize = static_cast<std::uint8_t> (std::ceil (
      std::log2 (static_cast<double> (cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)] + 1)) / 8.0));
 
  // write size of frequency table to output stream
  outputByteStream_arg.write (reinterpret_cast<const char*> (&frequencyTableSize), sizeof(frequencyTableSize));
  outputByteStream_arg.write (reinterpret_cast<const char*> (&frequencyTableByteSize), sizeof(frequencyTableByteSize));
 
  unsigned long streamByteCount = sizeof(frequencyTableSize) + sizeof(frequencyTableByteSize);
 
  // write cumulative  frequency table to output stream
  for (std::uint64_t f = 1; f < frequencyTableSize; f++)
  {
    outputByteStream_arg.write (reinterpret_cast<const char*> (&cFreqTable_[f]), frequencyTableByteSize);
    streamByteCount += frequencyTableByteSize;
  }
 
  readPos = 0;
  std::uint64_t low = 0;
  auto range = static_cast<std::uint64_t> (-1);
 
  // start encoding
  while (readPos < input_size)
  {
 
    // read symbol
    unsigned int inputsymbol = inputIntVector_arg[readPos++];
 
    // map to range
    low += cFreqTable_[inputsymbol] * (range /= cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)]);
    range *= cFreqTable_[inputsymbol + 1] - cFreqTable_[inputsymbol];
 
    // check range limits
    while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -low & (bottom - 1)), 1)))
    {
      char out = static_cast<char> (low >> 56);
      range <<= 8;
      low <<= 8;
      outputCharVector_.push_back (out);
    }
 
  }
 
  // flush remaining data
  for (unsigned int i = 0; i < 8; i++)
  {
    char out = static_cast<char> (low >> 56);
    outputCharVector_.push_back (out);
    low <<= 8;
  }
 
  // write encoded data to stream
  outputByteStream_arg.write (outputCharVector_.data(), outputCharVector_.size ());
 
  streamByteCount += static_cast<unsigned long> (outputCharVector_.size ());
 
  return (streamByteCount);
}
 
//////////////////////////////////////////////////////////////////////////////////////////////
unsigned long
pcl::StaticRangeCoder::decodeStreamToIntVector (std::istream& inputByteStream_arg,
                                                std::vector<unsigned int>& outputIntVector_arg)
{
  // define range limits
  constexpr std::uint64_t top = static_cast<std::uint64_t>(1) << 56;
  constexpr std::uint64_t bottom = static_cast<std::uint64_t>(1) << 48;
 
  std::uint64_t frequencyTableSize;
  unsigned char frequencyTableByteSize;
 
  unsigned int outputBufPos = 0;
  std::size_t output_size = outputIntVector_arg.size ();
 
  // read size of cumulative frequency table from stream
  inputByteStream_arg.read (reinterpret_cast<char*> (&frequencyTableSize), sizeof(frequencyTableSize));
  inputByteStream_arg.read (reinterpret_cast<char*> (&frequencyTableByteSize), sizeof(frequencyTableByteSize));
 
  unsigned long streamByteCount = sizeof(frequencyTableSize) + sizeof(frequencyTableByteSize);
 
  // check size of frequency table vector
  if (cFreqTable_.size () < frequencyTableSize)
  {
    cFreqTable_.resize (static_cast<std::size_t> (frequencyTableSize));
  }
 
  // init with zero
  std::fill(cFreqTable_.begin(), cFreqTable_.end(), 0);
 
  // read cumulative frequency table
  for (std::uint64_t f = 1; f < frequencyTableSize; f++)
  {
    inputByteStream_arg.read (reinterpret_cast<char *> (&cFreqTable_[f]), frequencyTableByteSize);
    streamByteCount += frequencyTableByteSize;
  }
 
  // initialize range & code
  std::uint64_t code = 0;
  std::uint64_t low = 0;
  auto range = static_cast<std::uint64_t> (-1);
 
  // init code vector
  for (unsigned int i = 0; i < 8; i++)
  {
    std::uint8_t ch;
    inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
    streamByteCount += sizeof(char);
    code = (code << 8) | ch;
  }
 
  // decoding
  for (std::size_t i = 0; i < output_size; i++)
  {
    std::uint64_t count = (code - low) / (range /= cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)]);
 
    // symbol lookup in cumulative frequency table
    std::uint64_t symbol = 0;
    std::uint64_t sSize = (frequencyTableSize - 1) / 2;
    while (sSize > 0)
    {
      if (cFreqTable_[static_cast<std::size_t> (symbol + sSize)] <= count)
      {
        symbol += sSize;
      }
      sSize /= 2;
    }
 
    // write symbol to output stream
    outputIntVector_arg[outputBufPos++] = static_cast<unsigned int> (symbol);
 
    // map to range
    low += cFreqTable_[static_cast<std::size_t> (symbol)] * range;
    range *= cFreqTable_[static_cast<std::size_t> (symbol + 1)] - cFreqTable_[static_cast<std::size_t> (symbol)];
 
    // check range limits
    while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -low & (bottom - 1)), 1)))
    {
      std::uint8_t ch;
      inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
      streamByteCount += sizeof(char);
      code = code << 8 | ch;
      range <<= 8;
      low <<= 8;
    }
  }
 
  return streamByteCount;
}
 
//////////////////////////////////////////////////////////////////////////////////////////////
unsigned long
pcl::StaticRangeCoder::encodeCharVectorToStream (const std::vector<char>& inputByteVector_arg,
                                                 std::ostream& outputByteStream_arg)
{
  DWord freq[257];
 
  // define numerical limits
  constexpr DWord top = static_cast<DWord>(1) << 24;
  constexpr DWord bottom = static_cast<DWord>(1) << 16;
  constexpr DWord maxRange = static_cast<DWord>(1) << 16;
 
  DWord low, range;
 
  unsigned int input_size;
  input_size = static_cast<unsigned int> (inputByteVector_arg.size ());
 
  // init output vector
  outputCharVector_.clear ();
  outputCharVector_.reserve (sizeof(char) * input_size);
 
  std::uint64_t FreqHist[257]{};
 
  // calculate frequency table
  unsigned int readPos = 0;
  while (readPos < input_size)
  {
    auto symbol = static_cast<std::uint8_t> (inputByteVector_arg[readPos++]);
    FreqHist[symbol + 1]++;
  }
 
  // convert to cumulative frequency table
  freq[0] = 0;
  for (int f = 1; f <= 256; f++)
  {
    freq[f] = freq[f - 1] + static_cast<DWord> (FreqHist[f]);
    if (freq[f] <= freq[f - 1])
      freq[f] = freq[f - 1] + 1;
  }
 
  // rescale if numerical limits are reached
  while (freq[256] >= maxRange)
  {
    for (int f = 1; f <= 256; f++)
    {
      freq[f] /= 2;
      ;
      if (freq[f] <= freq[f - 1])
        freq[f] = freq[f - 1] + 1;
    }
  }
 
  // write cumulative  frequency table to output stream
  outputByteStream_arg.write (reinterpret_cast<const char*> (&freq[0]), sizeof(freq));
  unsigned long streamByteCount = sizeof(freq);
 
  readPos = 0;
 
  low = 0;
  range = static_cast<DWord> (-1);
 
  // start encoding
  while (readPos < input_size)
  {
    // read symbol
    std::uint8_t ch = inputByteVector_arg[readPos++];
 
    // map to range
    low += freq[ch] * (range /= freq[256]);
    range *= freq[ch + 1] - freq[ch];
 
    // check range limits
    while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -static_cast<int>(low) & (bottom - 1)), 1)))
    {
      char out = static_cast<char> (low >> 24);
      range <<= 8;
      low <<= 8;
      outputCharVector_.push_back (out);
    }
 
  }
 
  // flush remaining data
  for (int i = 0; i < 4; i++)
  {
    char out = static_cast<char> (low >> 24);
    outputCharVector_.push_back (out);
    low <<= 8;
  }
 
  // write encoded data to stream
  outputByteStream_arg.write (outputCharVector_.data(), outputCharVector_.size ());
 
  streamByteCount += static_cast<unsigned long> (outputCharVector_.size ());
 
  return (streamByteCount);
}
 
//////////////////////////////////////////////////////////////////////////////////////////////
unsigned long
pcl::StaticRangeCoder::decodeStreamToCharVector (std::istream& inputByteStream_arg,
                                                 std::vector<char>& outputByteVector_arg)
{
  DWord freq[257];
 
  // define range limits
  constexpr DWord top = static_cast<DWord>(1) << 24;
  constexpr DWord bottom = static_cast<DWord>(1) << 16;
 
  DWord low, range;
  DWord code;
 
  unsigned int outputBufPos;
  unsigned int output_size;
 
  unsigned long streamByteCount;
 
  streamByteCount = 0;
 
  output_size = static_cast<unsigned int> (outputByteVector_arg.size ());
 
  outputBufPos = 0;
 
  // read cumulative frequency table
  inputByteStream_arg.read (reinterpret_cast<char*> (&freq[0]), sizeof(freq));
  streamByteCount += sizeof(freq);
 
  code = 0;
  low = 0;
  range = static_cast<DWord> (-1);
 
  // init code
  for (unsigned int i = 0; i < 4; i++)
  {
    std::uint8_t ch;
    inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
    streamByteCount += sizeof(char);
    code = (code << 8) | ch;
  }
 
  // decoding
  for (unsigned int i = 0; i < output_size; i++)
  {
    // symbol lookup in cumulative frequency table
    std::uint8_t symbol = 0;
    std::uint8_t sSize = 256 / 2;
 
    DWord count = (code - low) / (range /= freq[256]);
 
    while (sSize > 0)
    {
      if (freq[symbol + sSize] <= count)
      {
        symbol = static_cast<std::uint8_t> (symbol + sSize);
      }
      sSize /= 2;
    }
 
    // write symbol to output stream
    outputByteVector_arg[outputBufPos++] = symbol;
 
    low += freq[symbol] * range;
    range *= freq[symbol + 1] - freq[symbol];
 
    // check range limits
    while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -static_cast<int>(low) & (bottom - 1)), 1)))
    {
      std::uint8_t ch;
      inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
      streamByteCount += sizeof(char);
      code = code << 8 | ch;
      range <<= 8;
      low <<= 8;
    }
 
  }
 
  return (streamByteCount);
}