Point Cloud Library (PCL)  1.8.0
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 
42 #ifndef __PCL_IO_RANGECODING__HPP
43 #define __PCL_IO_RANGECODING__HPP
44 
45 #include <pcl/compression/entropy_range_coder.h>
46 #include <map>
47 #include <iostream>
48 #include <vector>
49 #include <string.h>
50 #include <algorithm>
51 #include <stdio.h>
52 
53 //////////////////////////////////////////////////////////////////////////////////////////////
54 unsigned long
55 pcl::AdaptiveRangeCoder::encodeCharVectorToStream (const std::vector<char>& inputByteVector_arg,
56  std::ostream& outputByteStream_arg)
57 {
58  DWord freq[257];
59  uint8_t ch;
60  unsigned int i, j, f;
61  char out;
62 
63  // define limits
64  const DWord top = static_cast<DWord> (1) << 24;
65  const DWord bottom = static_cast<DWord> (1) << 16;
66  const DWord maxRange = static_cast<DWord> (1) << 16;
67 
68  DWord low, range;
69 
70  unsigned int readPos;
71  unsigned int input_size;
72 
73  input_size = static_cast<unsigned> (inputByteVector_arg.size ());
74 
75  // init output vector
76  outputCharVector_.clear ();
77  outputCharVector_.reserve (sizeof(char) * input_size);
78 
79  readPos = 0;
80 
81  low = 0;
82  range = static_cast<DWord> (-1);
83 
84  // initialize cumulative frequency table
85  for (i = 0; i < 257; i++)
86  freq[i] = i;
87 
88  // scan input
89  while (readPos < input_size)
90  {
91 
92  // read byte
93  ch = inputByteVector_arg[readPos++];
94 
95  // map range
96  low += freq[ch] * (range /= freq[256]);
97  range *= freq[ch + 1] - freq[ch];
98 
99  // check range limits
100  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -int (low) & (bottom - 1)), 1)))
101  {
102  out = static_cast<char> (low >> 24);
103  range <<= 8;
104  low <<= 8;
105  outputCharVector_.push_back (out);
106  }
107 
108  // update frequency table
109  for (j = ch + 1; j < 257; j++)
110  freq[j]++;
111 
112  // detect overflow
113  if (freq[256] >= maxRange)
114  {
115  // rescale
116  for (f = 1; f <= 256; f++)
117  {
118  freq[f] /= 2;
119  if (freq[f] <= freq[f - 1])
120  freq[f] = freq[f - 1] + 1;
121  }
122  }
123 
124  }
125 
126  // flush remaining data
127  for (i = 0; i < 4; i++)
128  {
129  out = static_cast<char> (low >> 24);
130  outputCharVector_.push_back (out);
131  low <<= 8;
132  }
133 
134  // write to stream
135  outputByteStream_arg.write (&outputCharVector_[0], outputCharVector_.size ());
136 
137  return (static_cast<unsigned long> (outputCharVector_.size ()));
138 
139 }
140 
141 //////////////////////////////////////////////////////////////////////////////////////////////
142 unsigned long
143 pcl::AdaptiveRangeCoder::decodeStreamToCharVector (std::istream& inputByteStream_arg,
144  std::vector<char>& outputByteVector_arg)
145 {
146  uint8_t ch;
147  DWord freq[257];
148  unsigned int i, j, f;
149 
150  // define limits
151  const DWord top = static_cast<DWord> (1) << 24;
152  const DWord bottom = static_cast<DWord> (1) << 16;
153  const DWord maxRange = static_cast<DWord> (1) << 16;
154 
155  DWord low, range;
156  DWord code;
157 
158  unsigned int outputBufPos;
159  unsigned int output_size = static_cast<unsigned> (outputByteVector_arg.size ());
160 
161  unsigned long streamByteCount;
162 
163  streamByteCount = 0;
164 
165  outputBufPos = 0;
166 
167  code = 0;
168  low = 0;
169  range = static_cast<DWord> (-1);
170 
171  // init decoding
172  for (i = 0; i < 4; i++)
173  {
174  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
175  streamByteCount += sizeof(char);
176  code = (code << 8) | ch;
177  }
178 
179  // init cumulative frequency table
180  for (i = 0; i <= 256; i++)
181  freq[i] = i;
182 
183  // decoding loop
184  for (i = 0; i < output_size; i++)
185  {
186  uint8_t symbol = 0;
187  uint8_t sSize = 256 / 2;
188 
189  // map code to range
190  DWord count = (code - low) / (range /= freq[256]);
191 
192  // find corresponding symbol
193  while (sSize > 0)
194  {
195  if (freq[symbol + sSize] <= count)
196  {
197  symbol = static_cast<uint8_t> (symbol + sSize);
198  }
199  sSize /= 2;
200  }
201 
202  // output symbol
203  outputByteVector_arg[outputBufPos++] = symbol;
204 
205  // update range limits
206  low += freq[symbol] * range;
207  range *= freq[symbol + 1] - freq[symbol];
208 
209  // decode range limits
210  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -int (low) & (bottom - 1)), 1)))
211  {
212  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
213  streamByteCount += sizeof(char);
214  code = code << 8 | ch;
215  range <<= 8;
216  low <<= 8;
217  }
218 
219  // update cumulative frequency table
220  for (j = symbol + 1; j < 257; j++)
221  freq[j]++;
222 
223  // detect overflow
224  if (freq[256] >= maxRange)
225  {
226  // rescale
227  for (f = 1; f <= 256; f++)
228  {
229  freq[f] /= 2;
230  if (freq[f] <= freq[f - 1])
231  freq[f] = freq[f - 1] + 1;
232  }
233  }
234  }
235 
236  return (streamByteCount);
237 
238 }
239 
240 //////////////////////////////////////////////////////////////////////////////////////////////
241 unsigned long
242 pcl::StaticRangeCoder::encodeIntVectorToStream (std::vector<unsigned int>& inputIntVector_arg,
243  std::ostream& outputByteStream_arg)
244 {
245 
246  unsigned int inputsymbol;
247  unsigned int i, f;
248  char out;
249 
250  uint64_t frequencyTableSize;
251  uint8_t frequencyTableByteSize;
252 
253  // define numerical limits
254  const uint64_t top = static_cast<uint64_t> (1) << 56;
255  const uint64_t bottom = static_cast<uint64_t> (1) << 48;
256  const uint64_t maxRange = static_cast<uint64_t> (1) << 48;
257 
258  unsigned long input_size = static_cast<unsigned long> (inputIntVector_arg.size ());
259  uint64_t low, range;
260 
261  unsigned int inputSymbol;
262 
263  unsigned int readPos;
264 
265  unsigned long streamByteCount;
266 
267  streamByteCount = 0;
268 
269  // init output vector
270  outputCharVector_.clear ();
271  outputCharVector_.reserve ((sizeof(char) * input_size * 2));
272 
273  frequencyTableSize = 1;
274 
275  readPos = 0;
276 
277  // calculate frequency table
278  cFreqTable_[0] = cFreqTable_[1] = 0;
279  while (readPos < input_size)
280  {
281  inputSymbol = inputIntVector_arg[readPos++];
282 
283  if (inputSymbol + 1 >= frequencyTableSize)
284  {
285  // frequency table is to small -> adaptively extend it
286  uint64_t oldfrequencyTableSize;
287  oldfrequencyTableSize = frequencyTableSize;
288 
289  do
290  {
291  // increase frequency table size by factor 2
292  frequencyTableSize <<= 1;
293  } while (inputSymbol + 1 > frequencyTableSize);
294 
295  if (cFreqTable_.size () < frequencyTableSize + 1)
296  {
297  // resize frequency vector
298  cFreqTable_.resize (static_cast<std::size_t> (frequencyTableSize + 1));
299  }
300 
301  // init new frequency range with zero
302  memset (&cFreqTable_[static_cast<std::size_t> (oldfrequencyTableSize + 1)], 0,
303  sizeof(uint64_t) * static_cast<std::size_t> (frequencyTableSize - oldfrequencyTableSize));
304  }
305  cFreqTable_[inputSymbol + 1]++;
306  }
307  frequencyTableSize++;
308 
309  // convert to cumulative frequency table
310  for (f = 1; f < frequencyTableSize; f++)
311  {
312  cFreqTable_[f] = cFreqTable_[f - 1] + cFreqTable_[f];
313  if (cFreqTable_[f] <= cFreqTable_[f - 1])
314  cFreqTable_[f] = cFreqTable_[f - 1] + 1;
315  }
316 
317  // rescale if numerical limits are reached
318  while (cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)] >= maxRange)
319  {
320  for (f = 1; f < cFreqTable_.size (); f++)
321  {
322  cFreqTable_[f] /= 2;
323  ;
324  if (cFreqTable_[f] <= cFreqTable_[f - 1])
325  cFreqTable_[f] = cFreqTable_[f - 1] + 1;
326  }
327  }
328 
329  // calculate amount of bytes per frequency table entry
330  frequencyTableByteSize = static_cast<uint8_t> (ceil (
331  Log2 (static_cast<double> (cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)])) / 8.0));
332 
333  // write size of frequency table to output stream
334  outputByteStream_arg.write (reinterpret_cast<const char*> (&frequencyTableSize), sizeof(frequencyTableSize));
335  outputByteStream_arg.write (reinterpret_cast<const char*> (&frequencyTableByteSize), sizeof(frequencyTableByteSize));
336 
337  streamByteCount += sizeof(frequencyTableSize) + sizeof(frequencyTableByteSize);
338 
339  // write cumulative frequency table to output stream
340  for (f = 1; f < frequencyTableSize; f++)
341  {
342  outputByteStream_arg.write (reinterpret_cast<const char*> (&cFreqTable_[f]), frequencyTableByteSize);
343  streamByteCount += frequencyTableByteSize;
344  }
345 
346  readPos = 0;
347  low = 0;
348  range = static_cast<uint64_t> (-1);
349 
350  // start encoding
351  while (readPos < input_size)
352  {
353 
354  // read symol
355  inputsymbol = inputIntVector_arg[readPos++];
356 
357  // map to range
358  low += cFreqTable_[inputsymbol] * (range /= cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)]);
359  range *= cFreqTable_[inputsymbol + 1] - cFreqTable_[inputsymbol];
360 
361  // check range limits
362  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -low & (bottom - 1)), 1)))
363  {
364  out = static_cast<char> (low >> 56);
365  range <<= 8;
366  low <<= 8;
367  outputCharVector_.push_back (out);
368  }
369 
370  }
371 
372  // flush remaining data
373  for (i = 0; i < 8; i++)
374  {
375  out = static_cast<char> (low >> 56);
376  outputCharVector_.push_back (out);
377  low <<= 8;
378  }
379 
380  // write encoded data to stream
381  outputByteStream_arg.write (&outputCharVector_[0], outputCharVector_.size ());
382 
383  streamByteCount += static_cast<unsigned long> (outputCharVector_.size ());
384 
385  return (streamByteCount);
386 }
387 
388 //////////////////////////////////////////////////////////////////////////////////////////////
389 unsigned long
390 pcl::StaticRangeCoder::decodeStreamToIntVector (std::istream& inputByteStream_arg,
391  std::vector<unsigned int>& outputIntVector_arg)
392 {
393  uint8_t ch;
394  unsigned int i, f;
395 
396  // define range limits
397  const uint64_t top = static_cast<uint64_t> (1) << 56;
398  const uint64_t bottom = static_cast<uint64_t> (1) << 48;
399 
400  uint64_t low, range;
401  uint64_t code;
402 
403  unsigned int outputBufPos;
404  unsigned long output_size;
405 
406  uint64_t frequencyTableSize;
407  unsigned char frequencyTableByteSize;
408 
409  unsigned long streamByteCount;
410 
411  streamByteCount = 0;
412 
413  outputBufPos = 0;
414  output_size = static_cast<unsigned long> (outputIntVector_arg.size ());
415 
416  // read size of cumulative frequency table from stream
417  inputByteStream_arg.read (reinterpret_cast<char*> (&frequencyTableSize), sizeof(frequencyTableSize));
418  inputByteStream_arg.read (reinterpret_cast<char*> (&frequencyTableByteSize), sizeof(frequencyTableByteSize));
419 
420  streamByteCount += sizeof(frequencyTableSize) + sizeof(frequencyTableByteSize);
421 
422  // check size of frequency table vector
423  if (cFreqTable_.size () < frequencyTableSize)
424  {
425  cFreqTable_.resize (static_cast<std::size_t> (frequencyTableSize));
426  }
427 
428  // init with zero
429  memset (&cFreqTable_[0], 0, sizeof(uint64_t) * static_cast<std::size_t> (frequencyTableSize));
430 
431  // read cumulative frequency table
432  for (f = 1; f < frequencyTableSize; f++)
433  {
434  inputByteStream_arg.read (reinterpret_cast<char *> (&cFreqTable_[f]), frequencyTableByteSize);
435  streamByteCount += frequencyTableByteSize;
436  }
437 
438  // initialize range & code
439  code = 0;
440  low = 0;
441  range = static_cast<uint64_t> (-1);
442 
443  // init code vector
444  for (i = 0; i < 8; i++)
445  {
446  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
447  streamByteCount += sizeof(char);
448  code = (code << 8) | ch;
449  }
450 
451  // decoding
452  for (i = 0; i < output_size; i++)
453  {
454  uint64_t count = (code - low) / (range /= cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)]);
455 
456  // symbol lookup in cumulative frequency table
457  uint64_t symbol = 0;
458  uint64_t sSize = (frequencyTableSize - 1) / 2;
459  while (sSize > 0)
460  {
461  if (cFreqTable_[static_cast<std::size_t> (symbol + sSize)] <= count)
462  {
463  symbol += sSize;
464  }
465  sSize /= 2;
466  }
467 
468  // write symbol to output stream
469  outputIntVector_arg[outputBufPos++] = static_cast<unsigned int> (symbol);
470 
471  // map to range
472  low += cFreqTable_[static_cast<std::size_t> (symbol)] * range;
473  range *= cFreqTable_[static_cast<std::size_t> (symbol + 1)] - cFreqTable_[static_cast<std::size_t> (symbol)];
474 
475  // check range limits
476  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -low & (bottom - 1)), 1)))
477  {
478  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
479  streamByteCount += sizeof(char);
480  code = code << 8 | ch;
481  range <<= 8;
482  low <<= 8;
483  }
484 
485  }
486 
487  return streamByteCount;
488 }
489 
490 //////////////////////////////////////////////////////////////////////////////////////////////
491 unsigned long
492 pcl::StaticRangeCoder::encodeCharVectorToStream (const std::vector<char>& inputByteVector_arg,
493  std::ostream& outputByteStream_arg)
494 {
495  DWord freq[257];
496  uint8_t ch;
497  int i, f;
498  char out;
499 
500  // define numerical limits
501  const DWord top = static_cast<DWord> (1) << 24;
502  const DWord bottom = static_cast<DWord> (1) << 16;
503  const DWord maxRange = static_cast<DWord> (1) << 16;
504 
505  DWord low, range;
506 
507  unsigned int input_size;
508  input_size = static_cast<unsigned int> (inputByteVector_arg.size ());
509 
510  unsigned int readPos;
511 
512  unsigned long streamByteCount;
513 
514  streamByteCount = 0;
515 
516  // init output vector
517  outputCharVector_.clear ();
518  outputCharVector_.reserve (sizeof(char) * input_size);
519 
520  uint64_t FreqHist[257];
521 
522  // calculate frequency table
523  memset (FreqHist, 0, sizeof(FreqHist));
524  readPos = 0;
525  while (readPos < input_size)
526  {
527  uint8_t symbol = static_cast<uint8_t> (inputByteVector_arg[readPos++]);
528  FreqHist[symbol + 1]++;
529  }
530 
531  // convert to cumulative frequency table
532  freq[0] = 0;
533  for (f = 1; f <= 256; f++)
534  {
535  freq[f] = freq[f - 1] + static_cast<DWord> (FreqHist[f]);
536  if (freq[f] <= freq[f - 1])
537  freq[f] = freq[f - 1] + 1;
538  }
539 
540  // rescale if numerical limits are reached
541  while (freq[256] >= maxRange)
542  {
543  for (f = 1; f <= 256; f++)
544  {
545  freq[f] /= 2;
546  ;
547  if (freq[f] <= freq[f - 1])
548  freq[f] = freq[f - 1] + 1;
549  }
550  }
551 
552  // write cumulative frequency table to output stream
553  outputByteStream_arg.write (reinterpret_cast<const char*> (&freq[0]), sizeof(freq));
554  streamByteCount += sizeof(freq);
555 
556  readPos = 0;
557 
558  low = 0;
559  range = static_cast<DWord> (-1);
560 
561  // start encoding
562  while (readPos < input_size)
563  {
564 
565  // read symol
566  ch = inputByteVector_arg[readPos++];
567 
568  // map to range
569  low += freq[ch] * (range /= freq[256]);
570  range *= freq[ch + 1] - freq[ch];
571 
572  // check range limits
573  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -int (low) & (bottom - 1)), 1)))
574  {
575  out = static_cast<char> (low >> 24);
576  range <<= 8;
577  low <<= 8;
578  outputCharVector_.push_back (out);
579  }
580 
581  }
582 
583  // flush remaining data
584  for (i = 0; i < 4; i++)
585  {
586  out = static_cast<char> (low >> 24);
587  outputCharVector_.push_back (out);
588  low <<= 8;
589  }
590 
591  // write encoded data to stream
592  outputByteStream_arg.write (&outputCharVector_[0], outputCharVector_.size ());
593 
594  streamByteCount += static_cast<unsigned long> (outputCharVector_.size ());
595 
596  return (streamByteCount);
597 }
598 
599 //////////////////////////////////////////////////////////////////////////////////////////////
600 unsigned long
601 pcl::StaticRangeCoder::decodeStreamToCharVector (std::istream& inputByteStream_arg,
602  std::vector<char>& outputByteVector_arg)
603 {
604  uint8_t ch;
605  DWord freq[257];
606  unsigned int i;
607 
608  // define range limits
609  const DWord top = static_cast<DWord> (1) << 24;
610  const DWord bottom = static_cast<DWord> (1) << 16;
611 
612  DWord low, range;
613  DWord code;
614 
615  unsigned int outputBufPos;
616  unsigned int output_size;
617 
618  unsigned long streamByteCount;
619 
620  streamByteCount = 0;
621 
622  output_size = static_cast<unsigned int> (outputByteVector_arg.size ());
623 
624  outputBufPos = 0;
625 
626  // read cumulative frequency table
627  inputByteStream_arg.read (reinterpret_cast<char*> (&freq[0]), sizeof(freq));
628  streamByteCount += sizeof(freq);
629 
630  code = 0;
631  low = 0;
632  range = static_cast<DWord> (-1);
633 
634  // init code
635  for (i = 0; i < 4; i++)
636  {
637  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
638  streamByteCount += sizeof(char);
639  code = (code << 8) | ch;
640  }
641 
642  // decoding
643  for (i = 0; i < output_size; i++)
644  {
645  // symbol lookup in cumulative frequency table
646  uint8_t symbol = 0;
647  uint8_t sSize = 256 / 2;
648 
649  DWord count = (code - low) / (range /= freq[256]);
650 
651  while (sSize > 0)
652  {
653  if (freq[symbol + sSize] <= count)
654  {
655  symbol = static_cast<uint8_t> (symbol + sSize);
656  }
657  sSize /= 2;
658  }
659 
660  // write symbol to output stream
661  outputByteVector_arg[outputBufPos++] = symbol;
662 
663  low += freq[symbol] * range;
664  range *= freq[symbol + 1] - freq[symbol];
665 
666  // check range limits
667  while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -int (low) & (bottom - 1)), 1)))
668  {
669  inputByteStream_arg.read (reinterpret_cast<char*> (&ch), sizeof(char));
670  streamByteCount += sizeof(char);
671  code = code << 8 | ch;
672  range <<= 8;
673  low <<= 8;
674  }
675 
676  }
677 
678  return (streamByteCount);
679 }
680 
681 #endif
682 
unsigned long encodeIntVectorToStream(std::vector< unsigned int > &inputIntVector_arg, std::ostream &outputByterStream_arg)
Encode integer vector to output stream.
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.
Definition: inftrees.h:24
unsigned long decodeStreamToCharVector(std::istream &inputByteStream_arg, std::vector< char > &outputByteVector_arg)
Decode char stream to output vector.
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.