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