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