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 Willow Garage, Inc. 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: point_types_conversion.h 6126 2012-07-03 20:19:58Z aichim $
00037  */
00038 
00039 #ifndef PCL_TYPE_CONVERSIONS_H
00040 #define PCL_TYPE_CONVERSIONS_H
00041 
00042 namespace pcl
00043 {
00044   // r,g,b, i values are from 0 to 1
00045   // h = [0,360]
00046   // s, v values are from 0 to 1
00047   // if s = 0 > h = -1 (undefined)
00048 
00053   inline void 
00054   PointXYZRGBtoXYZI (PointXYZRGB&  in,
00055                      PointXYZI&    out)
00056   {
00057     out.x = in.x; out.y = in.y; out.z = in.z;
00058     out.intensity = 0.299f * in.r + 0.587f * in.g + 0.114f * in.b;
00059   }
00060 
00061   
00066   inline void 
00067   PointXYZRGBtoXYZHSV (PointXYZRGB& in,
00068                        PointXYZHSV& out)
00069   {
00070     float min;
00071 
00072     out.x = in.x; out.y = in.y; out.z = in.z;
00073 
00074     out.v = std::max (in.r, std::max (in.g, in.b));
00075     min = std::min (in.r, std::min (in.g, in.b));
00076 
00077     if (out.v != 0)
00078       out.s = (out.v - min) / out.v;
00079     else
00080     {
00081       out.s = 0;
00082       out.h = -1;
00083       return;
00084     }
00085 
00086     if (in.r == out.v)
00087       out.h = static_cast<float> (in.g - in.b) / (out.v - min);
00088     else if (in.g == out.v)
00089       out.h = static_cast<float> (2 + (in.b - in.r) / (out.v - min));
00090     else 
00091       out.h = static_cast<float> (4 + (in.r - in.g) / (out.v - min));
00092     out.h *= 60;
00093     if (out.h < 0)
00094       out.h += 360;
00095   }
00096 
00097 
00102   inline void 
00103   PointXYZHSVtoXYZRGB (PointXYZHSV&  in,
00104                        PointXYZRGB&  out)
00105   {
00106     if (in.s == 0)
00107     {
00108       out.r = out.g = out.b = static_cast<uint8_t> (in.v);
00109       return;
00110     } 
00111     float a = in.h / 60;
00112     int   i = static_cast<int> (floorf (a));
00113     float f = a - static_cast<float> (i);
00114     float p = in.v * (1 - in.s);
00115     float q = in.v * (1 - in.s * f);
00116     float t = in.v * (1 - in.s * (1 - f));
00117 
00118     switch (i)
00119     {
00120       case 0:
00121       {
00122         out.r = static_cast<uint8_t> (255 * in.v);
00123         out.g = static_cast<uint8_t> (255 * t);
00124         out.b = static_cast<uint8_t> (255 * p);
00125         break;
00126       }
00127       case 1:
00128       {
00129         out.r = static_cast<uint8_t> (255 * q); 
00130         out.g = static_cast<uint8_t> (255 * in.v); 
00131         out.b = static_cast<uint8_t> (255 * p); 
00132         break;
00133       }
00134       case 2:
00135       {
00136         out.r = static_cast<uint8_t> (255 * p);
00137         out.g = static_cast<uint8_t> (255 * in.v);
00138         out.b = static_cast<uint8_t> (255 * t);
00139         break;
00140       }
00141       case 3:
00142       {
00143         out.r = static_cast<uint8_t> (255 * p);
00144         out.g = static_cast<uint8_t> (255 * q);
00145         out.b = static_cast<uint8_t> (255 * in.v);
00146         break;
00147       }
00148       case 4:
00149       {
00150         out.r = static_cast<uint8_t> (255 * t);
00151         out.g = static_cast<uint8_t> (255 * p); 
00152         out.b = static_cast<uint8_t> (255 * in.v); 
00153         break;
00154       }
00155       default:
00156       {
00157         out.r = static_cast<uint8_t> (255 * in.v); 
00158         out.g = static_cast<uint8_t> (255 * p); 
00159         out.b = static_cast<uint8_t> (255 * q);
00160         break;
00161       }      
00162     }
00163   }
00164 
00165 
00170   inline void 
00171   PointCloudXYZRGBtoXYZHSV (PointCloud<PointXYZRGB>& in,
00172                             PointCloud<PointXYZHSV>& out)
00173   {
00174     out.width   = in.width;
00175     out.height  = in.height;
00176     for (size_t i = 0; i < in.points.size (); i++)
00177     {
00178       PointXYZHSV p;
00179       PointXYZRGBtoXYZHSV (in.points[i], p);
00180       out.points.push_back (p);
00181     }
00182   }
00187   inline void 
00188   PointCloudXYZRGBtoXYZI (PointCloud<PointXYZRGB>& in,
00189                           PointCloud<PointXYZI>& out)
00190   {
00191     out.width   = in.width;
00192     out.height  = in.height;
00193     for (size_t i = 0; i < in.points.size (); i++)
00194     {
00195       PointXYZI p;
00196       PointXYZRGBtoXYZI (in.points[i], p);
00197       out.points.push_back (p);
00198     }
00199   }
00200 }
00201 
00202 #endif //#ifndef PCL_TYPE_CONVERSIONS_H
00203 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:19