Point Cloud Library (PCL)  1.11.1
color_coding.h
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 
38 #pragma once
39 
40 #include <cstdio>
41 #include <cstring>
42 #include <iostream>
43 #include <iterator>
44 #include <vector>
45 
46 namespace pcl
47 {
48 
49 namespace octree
50 {
51 
52 /** \brief @b ColorCoding class
53  * \note This class encodes 8-bit color information for octree-based point cloud compression.
54  * \note
55  * \note typename: PointT: type of point used in pointcloud
56  * \author Julius Kammerl (julius@kammerl.de)
57  */
58 template<typename PointT>
60 {
61  // public typedefs
63  using PointCloudPtr = typename PointCloud::Ptr;
64  using PointCloudConstPtr = typename PointCloud::ConstPtr;
65 
66 public:
67  /** \brief Constructor.
68  *
69  * */
72  {
73  }
74 
75  /** \brief Empty class constructor. */
76  virtual
78  {
79  }
80 
81  /** \brief Define color bit depth of encoded color information.
82  * \param bitDepth_arg: amounts of bits for representing one color component
83  */
84  inline
85  void
86  setBitDepth (unsigned char bitDepth_arg)
87  {
88  colorBitReduction_ = static_cast<unsigned char> (8 - bitDepth_arg);
89  }
90 
91  /** \brief Retrieve color bit depth of encoded color information.
92  * \return amounts of bits for representing one color component
93  */
94  inline unsigned char
96  {
97  return (static_cast<unsigned char> (8 - colorBitReduction_));
98  }
99 
100  /** \brief Set amount of voxels containing point color information and reserve memory
101  * \param voxelCount_arg: amounts of voxels
102  */
103  inline void
104  setVoxelCount (unsigned int voxelCount_arg)
105  {
106  pointAvgColorDataVector_.reserve (voxelCount_arg * 3);
107  }
108 
109  /** \brief Set amount of points within point cloud to be encoded and reserve memory
110  * \param pointCount_arg: amounts of points within point cloud
111  * */
112  inline
113  void
114  setPointCount (unsigned int pointCount_arg)
115  {
116  pointDiffColorDataVector_.reserve (pointCount_arg * 3);
117  }
118 
119  /** \brief Initialize encoding of color information
120  * */
121  void
123  {
124  pointAvgColorDataVector_.clear ();
125 
126  pointDiffColorDataVector_.clear ();
127  }
128 
129  /** \brief Initialize decoding of color information
130  * */
131  void
133  {
135 
137  }
138 
139  /** \brief Get reference to vector containing averaged color data
140  * */
141  std::vector<char>&
143  {
145  }
146 
147  /** \brief Get reference to vector containing differential color data
148  * */
149  std::vector<char>&
151  {
153  }
154 
155  /** \brief Encode averaged color information for a subset of points from point cloud
156  * \param indexVector_arg indices defining a subset of points from points cloud
157  * \param rgba_offset_arg offset to color information
158  * \param inputCloud_arg input point cloud
159  * */
160  void
161  encodeAverageOfPoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
162  {
163  unsigned int avgRed = 0;
164  unsigned int avgGreen = 0;
165  unsigned int avgBlue = 0;
166 
167  // iterate over points
168  std::size_t len = indexVector_arg.size ();
169  for (std::size_t i = 0; i < len; i++)
170  {
171  // get color information from points
172  const int& idx = indexVector_arg[i];
173  const char* idxPointPtr = reinterpret_cast<const char*> (&(*inputCloud_arg)[idx]);
174  const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
175 
176  // add color information
177  avgRed += (colorInt >> 0) & 0xFF;
178  avgGreen += (colorInt >> 8) & 0xFF;
179  avgBlue += (colorInt >> 16) & 0xFF;
180 
181  }
182 
183  // calculated average color information
184  if (len > 1)
185  {
186  avgRed /= static_cast<unsigned int> (len);
187  avgGreen /= static_cast<unsigned int> (len);
188  avgBlue /= static_cast<unsigned int> (len);
189  }
190 
191  // remove least significant bits
192  avgRed >>= colorBitReduction_;
193  avgGreen >>= colorBitReduction_;
194  avgBlue >>= colorBitReduction_;
195 
196  // add to average color vector
197  pointAvgColorDataVector_.push_back (static_cast<char> (avgRed));
198  pointAvgColorDataVector_.push_back (static_cast<char> (avgGreen));
199  pointAvgColorDataVector_.push_back (static_cast<char> (avgBlue));
200  }
201 
202  /** \brief Encode color information of a subset of points from point cloud
203  * \param indexVector_arg indices defining a subset of points from points cloud
204  * \param rgba_offset_arg offset to color information
205  * \param inputCloud_arg input point cloud
206  * */
207  void
208  encodePoints (const typename std::vector<int>& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
209  {
210  unsigned int avgRed;
211  unsigned int avgGreen;
212  unsigned int avgBlue;
213 
214  // initialize
215  avgRed = avgGreen = avgBlue = 0;
216 
217  // iterate over points
218  std::size_t len = indexVector_arg.size ();
219  for (std::size_t i = 0; i < len; i++)
220  {
221  // get color information from point
222  const int& idx = indexVector_arg[i];
223  const char* idxPointPtr = reinterpret_cast<const char*> (&(*inputCloud_arg)[idx]);
224  const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
225 
226  // add color information
227  avgRed += (colorInt >> 0) & 0xFF;
228  avgGreen += (colorInt >> 8) & 0xFF;
229  avgBlue += (colorInt >> 16) & 0xFF;
230 
231  }
232 
233  if (len > 1)
234  {
235  unsigned char diffRed;
236  unsigned char diffGreen;
237  unsigned char diffBlue;
238 
239  // calculated average color information
240  avgRed /= static_cast<unsigned int> (len);
241  avgGreen /= static_cast<unsigned int> (len);
242  avgBlue /= static_cast<unsigned int> (len);
243 
244  // iterate over points for differential encoding
245  for (std::size_t i = 0; i < len; i++)
246  {
247  const int& idx = indexVector_arg[i];
248  const char* idxPointPtr = reinterpret_cast<const char*> (&(*inputCloud_arg)[idx]);
249  const int& colorInt = *reinterpret_cast<const int*> (idxPointPtr+rgba_offset_arg);
250 
251  // extract color components and do XOR encoding with predicted average color
252  diffRed = (static_cast<unsigned char> (avgRed)) ^ static_cast<unsigned char> (((colorInt >> 0) & 0xFF));
253  diffGreen = (static_cast<unsigned char> (avgGreen)) ^ static_cast<unsigned char> (((colorInt >> 8) & 0xFF));
254  diffBlue = (static_cast<unsigned char> (avgBlue)) ^ static_cast<unsigned char> (((colorInt >> 16) & 0xFF));
255 
256  // remove least significant bits
257  diffRed = static_cast<unsigned char> (diffRed >> colorBitReduction_);
258  diffGreen = static_cast<unsigned char> (diffGreen >> colorBitReduction_);
259  diffBlue = static_cast<unsigned char> (diffBlue >> colorBitReduction_);
260 
261  // add to differential color vector
262  pointDiffColorDataVector_.push_back (static_cast<char> (diffRed));
263  pointDiffColorDataVector_.push_back (static_cast<char> (diffGreen));
264  pointDiffColorDataVector_.push_back (static_cast<char> (diffBlue));
265  }
266  }
267 
268  // remove least significant bits from average color information
269  avgRed >>= colorBitReduction_;
270  avgGreen >>= colorBitReduction_;
271  avgBlue >>= colorBitReduction_;
272 
273  // add to differential color vector
274  pointAvgColorDataVector_.push_back (static_cast<char> (avgRed));
275  pointAvgColorDataVector_.push_back (static_cast<char> (avgGreen));
276  pointAvgColorDataVector_.push_back (static_cast<char> (avgBlue));
277 
278  }
279 
280  /** \brief Decode color information
281  * \param outputCloud_arg output point cloud
282  * \param beginIdx_arg index indicating first point to be assigned with color information
283  * \param endIdx_arg index indicating last point to be assigned with color information
284  * \param rgba_offset_arg offset to color information
285  */
286  void
287  decodePoints (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg)
288  {
289  assert (beginIdx_arg <= endIdx_arg);
290 
291  // amount of points to be decoded
292  unsigned int pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg);
293 
294  // get averaged color information for current voxel
295  unsigned char avgRed = *(pointAvgColorDataVector_Iterator_++);
296  unsigned char avgGreen = *(pointAvgColorDataVector_Iterator_++);
297  unsigned char avgBlue = *(pointAvgColorDataVector_Iterator_++);
298 
299  // invert bit shifts during encoding
300  avgRed = static_cast<unsigned char> (avgRed << colorBitReduction_);
301  avgGreen = static_cast<unsigned char> (avgGreen << colorBitReduction_);
302  avgBlue = static_cast<unsigned char> (avgBlue << colorBitReduction_);
303 
304  // iterate over points
305  for (std::size_t i = 0; i < pointCount; i++)
306  {
307  unsigned int colorInt;
308  if (pointCount > 1)
309  {
310  // get differential color information from input vector
311  unsigned char diffRed = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++));
312  unsigned char diffGreen = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++));
313  unsigned char diffBlue = static_cast<unsigned char> (*(pointDiffColorDataVector_Iterator_++));
314 
315  // invert bit shifts during encoding
316  diffRed = static_cast<unsigned char> (diffRed << colorBitReduction_);
317  diffGreen = static_cast<unsigned char> (diffGreen << colorBitReduction_);
318  diffBlue = static_cast<unsigned char> (diffBlue << colorBitReduction_);
319 
320  // decode color information
321  colorInt = ((avgRed ^ diffRed) << 0) |
322  ((avgGreen ^ diffGreen) << 8) |
323  ((avgBlue ^ diffBlue) << 16);
324  }
325  else
326  {
327  // decode color information
328  colorInt = (avgRed << 0) | (avgGreen << 8) | (avgBlue << 16);
329  }
330 
331  char* idxPointPtr = reinterpret_cast<char*> (&(*outputCloud_arg)[beginIdx_arg + i]);
332  int& pointColor = *reinterpret_cast<int*> (idxPointPtr+rgba_offset_arg);
333  // assign color to point from point cloud
334  pointColor=colorInt;
335  }
336  }
337 
338  /** \brief Set default color to points
339  * \param outputCloud_arg output point cloud
340  * \param beginIdx_arg index indicating first point to be assigned with color information
341  * \param endIdx_arg index indicating last point to be assigned with color information
342  * \param rgba_offset_arg offset to color information
343  * */
344  void
345  setDefaultColor (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg)
346  {
347  assert (beginIdx_arg <= endIdx_arg);
348 
349  // amount of points to be decoded
350  unsigned int pointCount = static_cast<unsigned int> (endIdx_arg - beginIdx_arg);
351 
352  // iterate over points
353  for (std::size_t i = 0; i < pointCount; i++)
354  {
355  char* idxPointPtr = reinterpret_cast<char*> (&(*outputCloud_arg)[beginIdx_arg + i]);
356  int& pointColor = *reinterpret_cast<int*> (idxPointPtr+rgba_offset_arg);
357  // assign color to point from point cloud
358  pointColor = defaultColor_;
359  }
360  }
361 
362 protected:
363  /** \brief Pointer to output point cloud dataset. */
364  PointCloudPtr output_;
365 
366  /** \brief Vector for storing average color information */
367  std::vector<char> pointAvgColorDataVector_;
368 
369  /** \brief Iterator on average color information vector */
370  std::vector<char>::const_iterator pointAvgColorDataVector_Iterator_;
371 
372  /** \brief Vector for storing differential color information */
373  std::vector<char> pointDiffColorDataVector_;
374 
375  /** \brief Iterator on differential color information vector */
376  std::vector<char>::const_iterator pointDiffColorDataVector_Iterator_;
377 
378  /** \brief Amount of bits to be removed from color components before encoding */
379  unsigned char colorBitReduction_;
380 
381  // frame header identifier
382  static const int defaultColor_;
383 };
384 
385 // define default color
386 template<typename PointT>
387 const int ColorCoding<PointT>::defaultColor_ = ((255) << 0) |
388  ((255) << 8) |
389  ((255) << 16);
390 
391 } // namespace octree
392 } // namespace pcl
393 
394 #define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding<T>;
395 
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:181
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
ColorCoding class
Definition: color_coding.h:60
void encodeAverageOfPoints(const typename std::vector< int > &indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
Encode averaged color information for a subset of points from point cloud.
Definition: color_coding.h:161
std::vector< char > & getAverageDataVector()
Get reference to vector containing averaged color data.
Definition: color_coding.h:142
void initializeDecoding()
Initialize decoding of color information.
Definition: color_coding.h:132
void setDefaultColor(PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg)
Set default color to points.
Definition: color_coding.h:345
std::vector< char > pointDiffColorDataVector_
Vector for storing differential color information
Definition: color_coding.h:373
std::vector< char > pointAvgColorDataVector_
Vector for storing average color information
Definition: color_coding.h:367
std::vector< char >::const_iterator pointAvgColorDataVector_Iterator_
Iterator on average color information vector.
Definition: color_coding.h:370
void encodePoints(const typename std::vector< int > &indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg)
Encode color information of a subset of points from point cloud.
Definition: color_coding.h:208
void decodePoints(PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg)
Decode color information.
Definition: color_coding.h:287
void initializeEncoding()
Initialize encoding of color information.
Definition: color_coding.h:122
virtual ~ColorCoding()
Empty class constructor.
Definition: color_coding.h:77
unsigned char colorBitReduction_
Amount of bits to be removed from color components before encoding.
Definition: color_coding.h:379
unsigned char getBitDepth()
Retrieve color bit depth of encoded color information.
Definition: color_coding.h:95
ColorCoding()
Constructor.
Definition: color_coding.h:70
std::vector< char > & getDifferentialDataVector()
Get reference to vector containing differential color data.
Definition: color_coding.h:150
std::vector< char >::const_iterator pointDiffColorDataVector_Iterator_
Iterator on differential color information vector.
Definition: color_coding.h:376
void setVoxelCount(unsigned int voxelCount_arg)
Set amount of voxels containing point color information and reserve memory.
Definition: color_coding.h:104
static const int defaultColor_
Definition: color_coding.h:382
void setBitDepth(unsigned char bitDepth_arg)
Define color bit depth of encoded color information.
Definition: color_coding.h:86
void setPointCount(unsigned int pointCount_arg)
Set amount of points within point cloud to be encoded and reserve memory.
Definition: color_coding.h:114
PointCloudPtr output_
Pointer to output point cloud dataset.
Definition: color_coding.h:364