point_types_conversion.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  */
00038 
00039 #ifndef PCL_TYPE_CONVERSIONS_H
00040 #define PCL_TYPE_CONVERSIONS_H
00041 
00042 #include <limits>
00043 
00044 namespace pcl
00045 {
00046   // r,g,b, i values are from 0 to 1
00047   // h = [0,360]
00048   // s, v values are from 0 to 1
00049   // if s = 0 > h = -1 (undefined)
00050 
00055   inline void 
00056   PointXYZRGBtoXYZI (PointXYZRGB&  in,
00057                      PointXYZI&    out)
00058   {
00059     out.x = in.x; out.y = in.y; out.z = in.z;
00060     out.intensity = 0.299f * static_cast <float> (in.r) + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b);
00061   }
00062 
00067   inline void
00068   PointRGBtoI (RGB&          in,
00069                Intensity&    out)
00070   {
00071     out.intensity = 0.299f * static_cast <float> (in.r) + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b);
00072   }
00073 
00078   inline void
00079   PointRGBtoI (RGB&          in,
00080                Intensity8u&  out)
00081   {
00082     out.intensity = static_cast<uint8_t>(std::numeric_limits<uint8_t>::max() * 0.299f * static_cast <float> (in.r)
00083                       + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b));
00084   }
00085 
00090   inline void
00091   PointRGBtoI (RGB&          in,
00092                Intensity32u& out)
00093   {
00094     out.intensity = static_cast<uint32_t>(static_cast<float>(std::numeric_limits<uint32_t>::max()) * 0.299f * static_cast <float> (in.r)
00095                       + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b));
00096   }
00097 
00102   inline void 
00103   PointXYZRGBtoXYZHSV (PointXYZRGB& in,
00104                        PointXYZHSV& out)
00105   {
00106     const unsigned char max = std::max (in.r, std::max (in.g, in.b));
00107     const unsigned char min = std::min (in.r, std::min (in.g, in.b));
00108 
00109     out.v = static_cast <float> (max) / 255.f;
00110 
00111     if (max == 0) // division by zero
00112     {
00113       out.s = 0.f;
00114       out.h = 0.f; // h = -1.f;
00115       return;
00116     }
00117 
00118     const float diff = static_cast <float> (max - min);
00119     out.s = diff / static_cast <float> (max);
00120 
00121     if (min == max) // diff == 0 -> division by zero
00122     {
00123       out.h = 0;
00124       return;
00125     }
00126 
00127     if      (max == in.r) out.h = 60.f * (      static_cast <float> (in.g - in.b) / diff);
00128     else if (max == in.g) out.h = 60.f * (2.f + static_cast <float> (in.b - in.r) / diff);
00129     else                  out.h = 60.f * (4.f + static_cast <float> (in.r - in.g) / diff); // max == b
00130 
00131     if (out.h < 0.f) out.h += 360.f;
00132   }
00133 
00139   inline void
00140   PointXYZRGBAtoXYZHSV (PointXYZRGBA& in,
00141                         PointXYZHSV& out)
00142   {
00143     const unsigned char max = std::max (in.r, std::max (in.g, in.b));
00144     const unsigned char min = std::min (in.r, std::min (in.g, in.b));
00145 
00146     out.v = static_cast <float> (max) / 255.f;
00147 
00148     if (max == 0) // division by zero
00149     {
00150       out.s = 0.f;
00151       out.h = 0.f; // h = -1.f;
00152       return;
00153     }
00154 
00155     const float diff = static_cast <float> (max - min);
00156     out.s = diff / static_cast <float> (max);
00157 
00158     if (min == max) // diff == 0 -> division by zero
00159     {
00160       out.h = 0;
00161       return;
00162     }
00163 
00164     if      (max == in.r) out.h = 60.f * (      static_cast <float> (in.g - in.b) / diff);
00165     else if (max == in.g) out.h = 60.f * (2.f + static_cast <float> (in.b - in.r) / diff);
00166     else                  out.h = 60.f * (4.f + static_cast <float> (in.r - in.g) / diff); // max == b
00167 
00168     if (out.h < 0.f) out.h += 360.f;
00169   }
00170 
00171   /* \brief Convert a XYZHSV point type to a XYZRGB
00172     * \param[in] in the input XYZHSV point 
00173     * \param[out] out the output XYZRGB point
00174     */
00175   inline void 
00176   PointXYZHSVtoXYZRGB (PointXYZHSV&  in,
00177                        PointXYZRGB&  out)
00178   {
00179     out.x = in.x; out.y = in.y; out.z = in.z;
00180     if (in.s == 0)
00181     {
00182       out.r = out.g = out.b = static_cast<uint8_t> (in.v);
00183       return;
00184     } 
00185     float a = in.h / 60;
00186     int   i = static_cast<int> (floorf (a));
00187     float f = a - static_cast<float> (i);
00188     float p = in.v * (1 - in.s);
00189     float q = in.v * (1 - in.s * f);
00190     float t = in.v * (1 - in.s * (1 - f));
00191 
00192     switch (i)
00193     {
00194       case 0:
00195       {
00196         out.r = static_cast<uint8_t> (255 * in.v);
00197         out.g = static_cast<uint8_t> (255 * t);
00198         out.b = static_cast<uint8_t> (255 * p);
00199         break;
00200       }
00201       case 1:
00202       {
00203         out.r = static_cast<uint8_t> (255 * q); 
00204         out.g = static_cast<uint8_t> (255 * in.v); 
00205         out.b = static_cast<uint8_t> (255 * p); 
00206         break;
00207       }
00208       case 2:
00209       {
00210         out.r = static_cast<uint8_t> (255 * p);
00211         out.g = static_cast<uint8_t> (255 * in.v);
00212         out.b = static_cast<uint8_t> (255 * t);
00213         break;
00214       }
00215       case 3:
00216       {
00217         out.r = static_cast<uint8_t> (255 * p);
00218         out.g = static_cast<uint8_t> (255 * q);
00219         out.b = static_cast<uint8_t> (255 * in.v);
00220         break;
00221       }
00222       case 4:
00223       {
00224         out.r = static_cast<uint8_t> (255 * t);
00225         out.g = static_cast<uint8_t> (255 * p); 
00226         out.b = static_cast<uint8_t> (255 * in.v); 
00227         break;
00228       }
00229       default:
00230       {
00231         out.r = static_cast<uint8_t> (255 * in.v); 
00232         out.g = static_cast<uint8_t> (255 * p); 
00233         out.b = static_cast<uint8_t> (255 * q);
00234         break;
00235       }      
00236     }
00237   }
00238 
00243   inline void
00244   PointCloudRGBtoI (PointCloud<RGB>&        in,
00245                     PointCloud<Intensity>&  out)
00246   {
00247     out.width   = in.width;
00248     out.height  = in.height;
00249     for (size_t i = 0; i < in.points.size (); i++)
00250     {
00251       Intensity p;
00252       PointRGBtoI (in.points[i], p);
00253       out.points.push_back (p);
00254     }
00255   }
00256 
00261   inline void
00262   PointCloudRGBtoI (PointCloud<RGB>&          in,
00263                     PointCloud<Intensity8u>&  out)
00264   {
00265     out.width   = in.width;
00266     out.height  = in.height;
00267     for (size_t i = 0; i < in.points.size (); i++)
00268     {
00269       Intensity8u p;
00270       PointRGBtoI (in.points[i], p);
00271       out.points.push_back (p);
00272     }
00273   }
00274 
00279   inline void
00280   PointCloudRGBtoI (PointCloud<RGB>&        in,
00281                     PointCloud<Intensity32u>&  out)
00282   {
00283     out.width   = in.width;
00284     out.height  = in.height;
00285     for (size_t i = 0; i < in.points.size (); i++)
00286     {
00287       Intensity32u p;
00288       PointRGBtoI (in.points[i], p);
00289       out.points.push_back (p);
00290     }
00291   }
00292 
00297   inline void 
00298   PointCloudXYZRGBtoXYZHSV (PointCloud<PointXYZRGB>& in,
00299                             PointCloud<PointXYZHSV>& out)
00300   {
00301     out.width   = in.width;
00302     out.height  = in.height;
00303     for (size_t i = 0; i < in.points.size (); i++)
00304     {
00305       PointXYZHSV p;
00306       PointXYZRGBtoXYZHSV (in.points[i], p);
00307       out.points.push_back (p);
00308     }
00309   }
00310 
00315   inline void
00316   PointCloudXYZRGBAtoXYZHSV (PointCloud<PointXYZRGBA>& in,
00317                              PointCloud<PointXYZHSV>& out)
00318   {
00319     out.width   = in.width;
00320     out.height  = in.height;
00321     for (size_t i = 0; i < in.points.size (); i++)
00322     {
00323       PointXYZHSV p;
00324       PointXYZRGBAtoXYZHSV (in.points[i], p);
00325       out.points.push_back (p);
00326     }
00327   }
00328 
00333   inline void 
00334   PointCloudXYZRGBtoXYZI (PointCloud<PointXYZRGB>& in,
00335                           PointCloud<PointXYZI>& out)
00336   {
00337     out.width   = in.width;
00338     out.height  = in.height;
00339     for (size_t i = 0; i < in.points.size (); i++)
00340     {
00341       PointXYZI p;
00342       PointXYZRGBtoXYZI (in.points[i], p);
00343       out.points.push_back (p);
00344     }
00345   }
00346 
00353   void
00354   PointCloudDepthAndRGBtoXYZRGBA (PointCloud<Intensity>&  depth,
00355                                   PointCloud<RGB>&        image,
00356                                   float&                  focal,
00357                                   PointCloud<PointXYZRGBA>&     out)
00358   {
00359     float bad_point = std::numeric_limits<float>::quiet_NaN();
00360     int width_ = depth.width;
00361     int height_ = depth.height;
00362     float constant_ = 1.0f / focal;
00363 
00364     for(unsigned int v = 0; v < height_; v++)
00365     {
00366       for(unsigned int u = 0; u < width_; u++)
00367       {
00368         PointXYZRGBA pt;
00369         pt.a = 0;
00370         float depth_ = depth.at(u,v).intensity;
00371 
00372         if (depth_ == 0)
00373         {
00374           pt.x = pt.y = pt.z = bad_point;
00375         }
00376         else
00377         {
00378           pt.z = depth_ * 0.001f;
00379           pt.x = static_cast<float> (u) * pt.z * constant_;
00380           pt.y = static_cast<float> (v) * pt.z * constant_;
00381         }
00382         pt.r = image.at(u,v).r;
00383         pt.g = image.at(u,v).g;
00384         pt.b = image.at(u,v).b;
00385 
00386         out.points.push_back(pt);
00387       }
00388     }
00389     out.width = width_;
00390     out.height = height_;
00391   }
00392 }
00393 
00394 #endif //#ifndef PCL_TYPE_CONVERSIONS_H
00395 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:31:03