Point Cloud Library (PCL)
1.3.1
|
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 3007 2011-10-31 23:25:11Z svn $ 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 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.299 * in.r + 0.587 * in.g + 0.114 * in.b; 00059 } 00060 00061 00066 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 = (in.g - in.b) / (out.v - min); 00088 else if (in.g == out.v) 00089 out.h = 2 + (in.b - in.r) / (out.v - min); 00090 else 00091 out.h = 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 void 00103 PointXYZHSVtoXYZRGB (PointXYZHSV& in, 00104 PointXYZRGB& out) 00105 { 00106 if (in.s == 0) 00107 { 00108 out.r = out.g = out.b = in.v; 00109 return; 00110 } 00111 float a = in.h / 60; 00112 int i = floor (a); 00113 float f = a - 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 out.r = in.v; out.g = t; out.b = p; break; 00122 case 1: 00123 out.r = q; out.g = in.v; out.b = p; break; 00124 case 2: 00125 out.r = p; out.g = in.v; out.b = t; break; 00126 case 3: 00127 out.r = p; out.g = q; out.b = in.v; break; 00128 case 4: 00129 out.r = t; out.g = p; out.b = in.v; break; 00130 default: 00131 out.r = in.v; out.g = p; out.b = q; break; 00132 } 00133 } 00134 00135 00140 void 00141 PointCloudXYZRGBtoXYZHSV (PointCloud<PointXYZRGB>& in, 00142 PointCloud<PointXYZHSV>& out) 00143 { 00144 out.width = in.width; 00145 out.height = in.height; 00146 for (size_t i = 0; i < in.points.size (); i++) 00147 { 00148 PointXYZHSV p; 00149 PointXYZRGBtoXYZHSV (in.points[i], p); 00150 out.points.push_back (p); 00151 } 00152 } 00153 } 00154 00155 #endif //#ifndef PCL_TYPE_CONVERSIONS_H 00156