Point Cloud Library (PCL)
1.7.0
|
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 00051 /** \brief Convert a XYZRGB point type to a XYZI 00052 * \param[in] in the input XYZRGB point 00053 * \param[out] out the output XYZI point 00054 */ 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 00063 /** \brief Convert a RGB point type to a I 00064 * \param[in] in the input RGB point 00065 * \param[out] out the output Intensity point 00066 */ 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 00074 /** \brief Convert a RGB point type to a I 00075 * \param[in] in the input RGB point 00076 * \param[out] out the output Intensity point 00077 */ 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 00086 /** \brief Convert a RGB point type to a I 00087 * \param[in] in the input RGB point 00088 * \param[out] out the output Intensity point 00089 */ 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 00098 /** \brief Convert a XYZRGB point type to a XYZHSV 00099 * \param[in] in the input XYZRGB point 00100 * \param[out] out the output XYZHSV point 00101 */ 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 00134 /** \brief Convert a XYZRGB point type to a XYZHSV 00135 * \param[in] in the input XYZRGB point 00136 * \param[out] out the output XYZHSV point 00137 * \todo include the A parameter but how? 00138 */ 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 00239 /** \brief Convert a RGB point cloud to a Intensity 00240 * \param[in] in the input RGB point cloud 00241 * \param[out] out the output Intensity point cloud 00242 */ 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 00257 /** \brief Convert a RGB point cloud to a Intensity 00258 * \param[in] in the input RGB point cloud 00259 * \param[out] out the output Intensity point cloud 00260 */ 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 00275 /** \brief Convert a RGB point cloud to a Intensity 00276 * \param[in] in the input RGB point cloud 00277 * \param[out] out the output Intensity point cloud 00278 */ 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 00293 /** \brief Convert a XYZRGB point cloud to a XYZHSV 00294 * \param[in] in the input XYZRGB point cloud 00295 * \param[out] out the output XYZHSV point cloud 00296 */ 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 00311 /** \brief Convert a XYZRGB point cloud to a XYZHSV 00312 * \param[in] in the input XYZRGB point cloud 00313 * \param[out] out the output XYZHSV point cloud 00314 */ 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 00329 /** \brief Convert a XYZRGB point cloud to a XYZI 00330 * \param[in] in the input XYZRGB point cloud 00331 * \param[out] out the output XYZI point cloud 00332 */ 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 00347 /** \brief Convert registered Depth image and RGB image to PointCloudXYZRGBA 00348 * \param[in] depth the input depth image as intensity points in float 00349 * \param[in] image the input RGB image 00350 * \param[in] focal the focal length 00351 * \param[out] out the output pointcloud 00352 * **/ 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