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