Point Cloud Library (PCL)  1.14.0-dev
entropy_range_coder.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  *
37  * range coder based on Dmitry Subbotin's carry-less implementation (http://www.compression.ru/ds/)
38  * Added optimized symbol lookup and fixed/static frequency tables
39  *
40  */
41 #pragma once
42 
43 #include <pcl/compression/entropy_range_coder.h>
44 #include <iostream>
45 #include <vector>
46 #include <cstring>
47 #include <algorithm>
48 
49 //////////////////////////////////////////////////////////////////////////////////////////////
50 unsigned long
51 pcl::AdaptiveRangeCoder::encodeCharVectorToStream (const std::vector<char>& inputByteVector_arg,
52  std::ostream& outputByteStream_arg)
53 {
54  DWord freq[257];
55 
56  // define limits
57  constexpr DWord top = static_cast<DWord>(1) << 24;
58  constexpr DWord bottom = static_cast<DWord>(1) << 16;
59  constexpr DWord maxRange = static_cast<DWord>(1) << 16;
60 
61  auto input_size = static_cast<unsigned> (inputByteVector_arg.size ());
62 
63  // init output vector
64  outputCharVector_.clear ();
65  outputCharVector_.reserve (sizeof(char) * input_size);
66 
67  unsigned int readPos = 0;
68 
69  DWord low = 0;
70  DWord range = static_cast<DWord> (-1);
71 
72  // initialize cumulative frequency table
73  for (unsigned int i = 0; i < 257; i++)
74  freq[i] = i;
75 
76  // scan input
77  while (readPos < input_size)
78  {
79  // read byte
80  std::uint8_t ch = inputByteVector_arg[readPos++];
81 
82  // map range
83  low += freq[ch] * (range /= freq[256]);
84  range *= freq[ch + 1] - freq[ch];
85 
86  // check range limits
87  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -static_cast<int>(low) & (bottom - 1)), 1)))
88  {
89  char out = static_cast<char> (low >> 24);
90  range <<= 8;
91  low <<= 8;
92  outputCharVector_.push_back (out);
93  }
94 
95  // update frequency table
96  for (unsigned int j = ch + 1; j < 257; j++)
97  freq[j]++;
98 
99  // detect overflow
100  if (freq[256] >= maxRange)
101  {
102  // rescale
103  for (unsigned int f = 1; f <= 256; f++)
104  {
105  freq[f] /= 2;
106  if (freq[f] <= freq[f - 1])
107  freq[f] = freq[f - 1] + 1;
108  }
109  }
110 
111  }
112 
113  // flush remaining data
114  for (unsigned int i = 0; i < 4; i++)
115  {
116  char out = static_cast<char> (low >> 24);
117  outputCharVector_.push_back (out);
118  low <<= 8;
119  }
120 
121  // write to stream
122  outputByteStream_arg.write (outputCharVector_.data(), outputCharVector_.size ());
123 
124  return (static_cast<unsigned long> (outputCharVector_.size ()));
125 }
126 
127 //////////////////////////////////////////////////////////////////////////////////////////////
128 unsigned long
129 pcl::AdaptiveRangeCoder::decodeStreamToCharVector (std::istream& inputByteStream_arg,
130  std::vector<char>& outputByteVector_arg)
131 {
132  DWord freq[257];
133 
134  // define limits
135  constexpr DWord top = static_cast<DWord>(1) << 24;
136  constexpr DWord bottom = static_cast<DWord>(1) << 16;
137  constexpr DWord maxRange = static_cast<DWord>(1) << 16;
138 
139  auto output_size = static_cast<unsigned> (outputByteVector_arg.size ());
140 
141  unsigned long streamByteCount = 0;
142 
143  unsigned int outputBufPos = 0;
144 
145  DWord code = 0;
146  DWord low = 0;
147  DWord range = static_cast<DWord> (-1);
148 
149  // init decoding
150  for (unsigned int i = 0; i < 4; i++)
151  {
152  std::uint8_t ch;
153  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
154  streamByteCount += sizeof(char);
155  code = (code << 8) | ch;
156  }
157 
158  // init cumulative frequency table
159  for (unsigned int i = 0; i <= 256; i++)
160  freq[i] = i;
161 
162  // decoding loop
163  for (unsigned int i = 0; i < output_size; i++)
164  {
165  std::uint8_t symbol = 0;
166  std::uint8_t sSize = 256 / 2;
167 
168  // map code to range
169  DWord count = (code - low) / (range /= freq[256]);
170 
171  // find corresponding symbol
172  while (sSize > 0)
173  {
174  if (freq[symbol + sSize] <= count)
175  {
176  symbol = static_cast<std::uint8_t> (symbol + sSize);
177  }
178  sSize /= 2;
179  }
180 
181  // output symbol
182  outputByteVector_arg[outputBufPos++] = symbol;
183 
184  // update range limits
185  low += freq[symbol] * range;
186  range *= freq[symbol + 1] - freq[symbol];
187 
188  // decode range limits
189  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -static_cast<int>(low) & (bottom - 1)), 1)))
190  {
191  std::uint8_t ch;
192  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
193  streamByteCount += sizeof(char);
194  code = code << 8 | ch;
195  range <<= 8;
196  low <<= 8;
197  }
198 
199  // update cumulative frequency table
200  for (unsigned int j = symbol + 1; j < 257; j++)
201  freq[j]++;
202 
203  // detect overflow
204  if (freq[256] >= maxRange)
205  {
206  // rescale
207  for (unsigned int f = 1; f <= 256; f++)
208  {
209  freq[f] /= 2;
210  if (freq[f] <= freq[f - 1])
211  freq[f] = freq[f - 1] + 1;
212  }
213  }
214  }
215 
216  return (streamByteCount);
217 }
218 
219 //////////////////////////////////////////////////////////////////////////////////////////////
220 unsigned long
221 pcl::StaticRangeCoder::encodeIntVectorToStream (std::vector<unsigned int>& inputIntVector_arg,
222  std::ostream& outputByteStream_arg)
223 {
224  // define numerical limits
225  constexpr std::uint64_t top = static_cast<std::uint64_t>(1) << 56;
226  constexpr std::uint64_t bottom = static_cast<std::uint64_t>(1) << 48;
227  constexpr std::uint64_t maxRange = static_cast<std::uint64_t>(1) << 48;
228 
229  auto input_size = static_cast<unsigned long> (inputIntVector_arg.size ());
230 
231  // init output vector
232  outputCharVector_.clear ();
233  outputCharVector_.reserve ((sizeof(char) * input_size * 2));
234 
235  std::uint64_t frequencyTableSize = 1;
236 
237  unsigned int readPos = 0;
238 
239  // calculate frequency table
240  cFreqTable_[0] = cFreqTable_[1] = 0;
241  while (readPos < input_size)
242  {
243  unsigned int inputSymbol = inputIntVector_arg[readPos++];
244 
245  if (inputSymbol + 1 >= frequencyTableSize)
246  {
247  // frequency table is to small -> adaptively extend it
248  std::uint64_t oldfrequencyTableSize;
249  oldfrequencyTableSize = frequencyTableSize;
250 
251  do
252  {
253  // increase frequency table size by factor 2
254  frequencyTableSize <<= 1;
255  } while (inputSymbol + 1 > frequencyTableSize);
256 
257  if (cFreqTable_.size () < frequencyTableSize + 1)
258  {
259  // resize frequency vector
260  cFreqTable_.resize (static_cast<std::size_t> (frequencyTableSize + 1));
261  }
262 
263  // init new frequency range with zero
264  std::fill_n(&cFreqTable_[oldfrequencyTableSize + 1],
265  frequencyTableSize - oldfrequencyTableSize, 0);
266  }
267  cFreqTable_[inputSymbol + 1]++;
268  }
269  frequencyTableSize++;
270 
271  // convert to cumulative frequency table
272  for (std::uint64_t f = 1; f < frequencyTableSize; f++)
273  {
274  cFreqTable_[f] = cFreqTable_[f - 1] + cFreqTable_[f];
275  if (cFreqTable_[f] <= cFreqTable_[f - 1])
276  cFreqTable_[f] = cFreqTable_[f - 1] + 1;
277  }
278 
279  // rescale if numerical limits are reached
280  while (cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)] >= maxRange)
281  {
282  for (std::size_t f = 1; f < cFreqTable_.size (); f++)
283  {
284  cFreqTable_[f] /= 2;
285  ;
286  if (cFreqTable_[f] <= cFreqTable_[f - 1])
287  cFreqTable_[f] = cFreqTable_[f - 1] + 1;
288  }
289  }
290 
291  // calculate amount of bytes per frequency table entry
292  auto frequencyTableByteSize = static_cast<std::uint8_t> (std::ceil (
293  std::log2 (static_cast<double> (cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)] + 1)) / 8.0));
294 
295  // write size of frequency table to output stream
296  outputByteStream_arg.write (reinterpret_cast<const char*> (&frequencyTableSize), sizeof(frequencyTableSize));
297  outputByteStream_arg.write (reinterpret_cast<const char*> (&frequencyTableByteSize), sizeof(frequencyTableByteSize));
298 
299  unsigned long streamByteCount = sizeof(frequencyTableSize) + sizeof(frequencyTableByteSize);
300 
301  // write cumulative frequency table to output stream
302  for (std::uint64_t f = 1; f < frequencyTableSize; f++)
303  {
304  outputByteStream_arg.write (reinterpret_cast<const char*> (&cFreqTable_[f]), frequencyTableByteSize);
305  streamByteCount += frequencyTableByteSize;
306  }
307 
308  readPos = 0;
309  std::uint64_t low = 0;
310  auto range = static_cast<std::uint64_t> (-1);
311 
312  // start encoding
313  while (readPos < input_size)
314  {
315 
316  // read symbol
317  unsigned int inputsymbol = inputIntVector_arg[readPos++];
318 
319  // map to range
320  low += cFreqTable_[inputsymbol] * (range /= cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)]);
321  range *= cFreqTable_[inputsymbol + 1] - cFreqTable_[inputsymbol];
322 
323  // check range limits
324  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -low & (bottom - 1)), 1)))
325  {
326  char out = static_cast<char> (low >> 56);
327  range <<= 8;
328  low <<= 8;
329  outputCharVector_.push_back (out);
330  }
331 
332  }
333 
334  // flush remaining data
335  for (unsigned int i = 0; i < 8; i++)
336  {
337  char out = static_cast<char> (low >> 56);
338  outputCharVector_.push_back (out);
339  low <<= 8;
340  }
341 
342  // write encoded data to stream
343  outputByteStream_arg.write (outputCharVector_.data(), outputCharVector_.size ());
344 
345  streamByteCount += static_cast<unsigned long> (outputCharVector_.size ());
346 
347  return (streamByteCount);
348 }
349 
350 //////////////////////////////////////////////////////////////////////////////////////////////
351 unsigned long
352 pcl::StaticRangeCoder::decodeStreamToIntVector (std::istream& inputByteStream_arg,
353  std::vector<unsigned int>& outputIntVector_arg)
354 {
355  // define range limits
356  constexpr std::uint64_t top = static_cast<std::uint64_t>(1) << 56;
357  constexpr std::uint64_t bottom = static_cast<std::uint64_t>(1) << 48;
358 
359  std::uint64_t frequencyTableSize;
360  unsigned char frequencyTableByteSize;
361 
362  unsigned int outputBufPos = 0;
363  std::size_t output_size = outputIntVector_arg.size ();
364 
365  // read size of cumulative frequency table from stream
366  inputByteStream_arg.read (reinterpret_cast<char*> (&frequencyTableSize), sizeof(frequencyTableSize));
367  inputByteStream_arg.read (reinterpret_cast<char*> (&frequencyTableByteSize), sizeof(frequencyTableByteSize));
368 
369  unsigned long streamByteCount = sizeof(frequencyTableSize) + sizeof(frequencyTableByteSize);
370 
371  // check size of frequency table vector
372  if (cFreqTable_.size () < frequencyTableSize)
373  {
374  cFreqTable_.resize (static_cast<std::size_t> (frequencyTableSize));
375  }
376 
377  // init with zero
378  std::fill(cFreqTable_.begin(), cFreqTable_.end(), 0);
379 
380  // read cumulative frequency table
381  for (std::uint64_t f = 1; f < frequencyTableSize; f++)
382  {
383  inputByteStream_arg.read (reinterpret_cast<char *> (&cFreqTable_[f]), frequencyTableByteSize);
384  streamByteCount += frequencyTableByteSize;
385  }
386 
387  // initialize range & code
388  std::uint64_t code = 0;
389  std::uint64_t low = 0;
390  auto range = static_cast<std::uint64_t> (-1);
391 
392  // init code vector
393  for (unsigned int i = 0; i < 8; i++)
394  {
395  std::uint8_t ch;
396  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
397  streamByteCount += sizeof(char);
398  code = (code << 8) | ch;
399  }
400 
401  // decoding
402  for (std::size_t i = 0; i < output_size; i++)
403  {
404  std::uint64_t count = (code - low) / (range /= cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)]);
405 
406  // symbol lookup in cumulative frequency table
407  std::uint64_t symbol = 0;
408  std::uint64_t sSize = (frequencyTableSize - 1) / 2;
409  while (sSize > 0)
410  {
411  if (cFreqTable_[static_cast<std::size_t> (symbol + sSize)] <= count)
412  {
413  symbol += sSize;
414  }
415  sSize /= 2;
416  }
417 
418  // write symbol to output stream
419  outputIntVector_arg[outputBufPos++] = static_cast<unsigned int> (symbol);
420 
421  // map to range
422  low += cFreqTable_[static_cast<std::size_t> (symbol)] * range;
423  range *= cFreqTable_[static_cast<std::size_t> (symbol + 1)] - cFreqTable_[static_cast<std::size_t> (symbol)];
424 
425  // check range limits
426  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -low & (bottom - 1)), 1)))
427  {
428  std::uint8_t ch;
429  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
430  streamByteCount += sizeof(char);
431  code = code << 8 | ch;
432  range <<= 8;
433  low <<= 8;
434  }
435  }
436 
437  return streamByteCount;
438 }
439 
440 //////////////////////////////////////////////////////////////////////////////////////////////
441 unsigned long
442 pcl::StaticRangeCoder::encodeCharVectorToStream (const std::vector<char>& inputByteVector_arg,
443  std::ostream& outputByteStream_arg)
444 {
445  DWord freq[257];
446 
447  // define numerical limits
448  constexpr DWord top = static_cast<DWord>(1) << 24;
449  constexpr DWord bottom = static_cast<DWord>(1) << 16;
450  constexpr DWord maxRange = static_cast<DWord>(1) << 16;
451 
452  DWord low, range;
453 
454  unsigned int input_size;
455  input_size = static_cast<unsigned int> (inputByteVector_arg.size ());
456 
457  // init output vector
458  outputCharVector_.clear ();
459  outputCharVector_.reserve (sizeof(char) * input_size);
460 
461  std::uint64_t FreqHist[257]{};
462 
463  // calculate frequency table
464  unsigned int readPos = 0;
465  while (readPos < input_size)
466  {
467  auto symbol = static_cast<std::uint8_t> (inputByteVector_arg[readPos++]);
468  FreqHist[symbol + 1]++;
469  }
470 
471  // convert to cumulative frequency table
472  freq[0] = 0;
473  for (int f = 1; f <= 256; f++)
474  {
475  freq[f] = freq[f - 1] + static_cast<DWord> (FreqHist[f]);
476  if (freq[f] <= freq[f - 1])
477  freq[f] = freq[f - 1] + 1;
478  }
479 
480  // rescale if numerical limits are reached
481  while (freq[256] >= maxRange)
482  {
483  for (int f = 1; f <= 256; f++)
484  {
485  freq[f] /= 2;
486  ;
487  if (freq[f] <= freq[f - 1])
488  freq[f] = freq[f - 1] + 1;
489  }
490  }
491 
492  // write cumulative frequency table to output stream
493  outputByteStream_arg.write (reinterpret_cast<const char*> (&freq[0]), sizeof(freq));
494  unsigned long streamByteCount = sizeof(freq);
495 
496  readPos = 0;
497 
498  low = 0;
499  range = static_cast<DWord> (-1);
500 
501  // start encoding
502  while (readPos < input_size)
503  {
504  // read symbol
505  std::uint8_t ch = inputByteVector_arg[readPos++];
506 
507  // map to range
508  low += freq[ch] * (range /= freq[256]);
509  range *= freq[ch + 1] - freq[ch];
510 
511  // check range limits
512  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -static_cast<int>(low) & (bottom - 1)), 1)))
513  {
514  char out = static_cast<char> (low >> 24);
515  range <<= 8;
516  low <<= 8;
517  outputCharVector_.push_back (out);
518  }
519 
520  }
521 
522  // flush remaining data
523  for (int i = 0; i < 4; i++)
524  {
525  char out = static_cast<char> (low >> 24);
526  outputCharVector_.push_back (out);
527  low <<= 8;
528  }
529 
530  // write encoded data to stream
531  outputByteStream_arg.write (outputCharVector_.data(), outputCharVector_.size ());
532 
533  streamByteCount += static_cast<unsigned long> (outputCharVector_.size ());
534 
535  return (streamByteCount);
536 }
537 
538 //////////////////////////////////////////////////////////////////////////////////////////////
539 unsigned long
540 pcl::StaticRangeCoder::decodeStreamToCharVector (std::istream& inputByteStream_arg,
541  std::vector<char>& outputByteVector_arg)
542 {
543  DWord freq[257];
544 
545  // define range limits
546  constexpr DWord top = static_cast<DWord>(1) << 24;
547  constexpr DWord bottom = static_cast<DWord>(1) << 16;
548 
549  DWord low, range;
550  DWord code;
551 
552  unsigned int outputBufPos;
553  unsigned int output_size;
554 
555  unsigned long streamByteCount;
556 
557  streamByteCount = 0;
558 
559  output_size = static_cast<unsigned int> (outputByteVector_arg.size ());
560 
561  outputBufPos = 0;
562 
563  // read cumulative frequency table
564  inputByteStream_arg.read (reinterpret_cast<char*> (&freq[0]), sizeof(freq));
565  streamByteCount += sizeof(freq);
566 
567  code = 0;
568  low = 0;
569  range = static_cast<DWord> (-1);
570 
571  // init code
572  for (unsigned int i = 0; i < 4; i++)
573  {
574  std::uint8_t ch;
575  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
576  streamByteCount += sizeof(char);
577  code = (code << 8) | ch;
578  }
579 
580  // decoding
581  for (unsigned int i = 0; i < output_size; i++)
582  {
583  // symbol lookup in cumulative frequency table
584  std::uint8_t symbol = 0;
585  std::uint8_t sSize = 256 / 2;
586 
587  DWord count = (code - low) / (range /= freq[256]);
588 
589  while (sSize > 0)
590  {
591  if (freq[symbol + sSize] <= count)
592  {
593  symbol = static_cast<std::uint8_t> (symbol + sSize);
594  }
595  sSize /= 2;
596  }
597 
598  // write symbol to output stream
599  outputByteVector_arg[outputBufPos++] = symbol;
600 
601  low += freq[symbol] * range;
602  range *= freq[symbol + 1] - freq[symbol];
603 
604  // check range limits
605  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -static_cast<int>(low) & (bottom - 1)), 1)))
606  {
607  std::uint8_t ch;
608  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
609  streamByteCount += sizeof(char);
610  code = code << 8 | ch;
611  range <<= 8;
612  low <<= 8;
613  }
614 
615  }
616 
617  return (streamByteCount);
618 }
619 
unsigned long decodeStreamToCharVector(std::istream &inputByteStream_arg, std::vector< char > &outputByteVector_arg)
Decode char stream to output vector.
unsigned long encodeCharVectorToStream(const std::vector< char > &inputByteVector_arg, std::ostream &outputByteStream_arg)
Encode char vector to output stream.
unsigned long decodeStreamToIntVector(std::istream &inputByteStream_arg, std::vector< unsigned int > &outputIntVector_arg)
Decode stream to output integer vector.
unsigned long encodeCharVectorToStream(const std::vector< char > &inputByteVector_arg, std::ostream &outputByteStream_arg)
Encode char vector to output stream.
unsigned long decodeStreamToCharVector(std::istream &inputByteStream_arg, std::vector< char > &outputByteVector_arg)
Decode char stream to output vector.
unsigned long encodeIntVectorToStream(std::vector< unsigned int > &inputIntVector_arg, std::ostream &outputByterStream_arg)
Encode integer vector to output stream.
Definition: inftrees.h:24