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 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$ 00037 * Authors: Julius Kammerl (julius@kammerl.de) 00038 */ 00039 00040 #ifndef PCL_ORGANIZED_CONVERSION_H_ 00041 #define PCL_ORGANIZED_CONVERSION_H_ 00042 00043 #include <pcl/pcl_macros.h> 00044 #include <pcl/point_cloud.h> 00045 00046 #include <vector> 00047 #include <limits> 00048 #include <assert.h> 00049 00050 namespace pcl 00051 { 00052 namespace io 00053 { 00054 00055 template<typename PointT> struct CompressionPointTraits 00056 { 00057 static const bool hasColor = false; 00058 static const unsigned int channels = 1; 00059 static const size_t bytesPerPoint = 3 * sizeof(float); 00060 }; 00061 00062 template<> 00063 struct CompressionPointTraits<PointXYZRGB> 00064 { 00065 static const bool hasColor = true; 00066 static const unsigned int channels = 4; 00067 static const size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(uint8_t); 00068 }; 00069 00070 template<> 00071 struct CompressionPointTraits<PointXYZRGBA> 00072 { 00073 static const bool hasColor = true; 00074 static const unsigned int channels = 4; 00075 static const size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(uint8_t); 00076 }; 00077 00078 ////////////////////////////////////////////////////////////////////////////////////////////// 00079 template <typename PointT, bool enableColor = CompressionPointTraits<PointT>::hasColor > 00080 struct OrganizedConversion; 00081 00082 ////////////////////////////////////////////////////////////////////////////////////////////// 00083 // Uncolored point cloud specialization 00084 ////////////////////////////////////////////////////////////////////////////////////////////// 00085 template<typename PointT> 00086 struct OrganizedConversion<PointT, false> 00087 { 00088 /** \brief Convert point cloud to disparity image 00089 * \param[in] cloud_arg input point cloud 00090 * \param[in] focalLength_arg focal length 00091 * \param[in] disparityShift_arg disparity shift 00092 * \param[in] disparityScale_arg disparity scaling 00093 * \param[out] disparityData_arg output disparity image 00094 * \ingroup io 00095 */ 00096 static void convert(const pcl::PointCloud<PointT>& cloud_arg, 00097 float focalLength_arg, 00098 float disparityShift_arg, 00099 float disparityScale_arg, 00100 bool , 00101 typename std::vector<uint16_t>& disparityData_arg, 00102 typename std::vector<uint8_t>&) 00103 { 00104 size_t cloud_size, i; 00105 00106 cloud_size = cloud_arg.points.size (); 00107 00108 // Clear image data 00109 disparityData_arg.clear (); 00110 00111 disparityData_arg.reserve (cloud_size); 00112 00113 for (i = 0; i < cloud_size; ++i) 00114 { 00115 // Get point from cloud 00116 const PointT& point = cloud_arg.points[i]; 00117 00118 if (pcl::isFinite (point)) 00119 { 00120 // Inverse depth quantization 00121 uint16_t disparity = static_cast<uint16_t> ( focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg); 00122 disparityData_arg.push_back (disparity); 00123 } 00124 else 00125 { 00126 // Non-valid points are encoded with zeros 00127 disparityData_arg.push_back (0); 00128 } 00129 } 00130 } 00131 00132 /** \brief Convert disparity image to point cloud 00133 * \param[in] disparityData_arg input depth image 00134 * \param[in] width_arg width of disparity image 00135 * \param[in] height_arg height of disparity image 00136 * \param[in] focalLength_arg focal length 00137 * \param[in] disparityShift_arg disparity shift 00138 * \param[in] disparityScale_arg disparity scaling 00139 * \param[out] cloud_arg output point cloud 00140 * \ingroup io 00141 */ 00142 static void convert(typename std::vector<uint16_t>& disparityData_arg, 00143 typename std::vector<uint8_t>&, 00144 bool, 00145 size_t width_arg, 00146 size_t height_arg, 00147 float focalLength_arg, 00148 float disparityShift_arg, 00149 float disparityScale_arg, 00150 pcl::PointCloud<PointT>& cloud_arg) 00151 { 00152 size_t i; 00153 size_t cloud_size = width_arg * height_arg; 00154 int x, y, centerX, centerY; 00155 00156 assert(disparityData_arg.size()==cloud_size); 00157 00158 // Reset point cloud 00159 cloud_arg.points.clear (); 00160 cloud_arg.points.reserve (cloud_size); 00161 00162 // Define point cloud parameters 00163 cloud_arg.width = static_cast<uint32_t> (width_arg); 00164 cloud_arg.height = static_cast<uint32_t> (height_arg); 00165 cloud_arg.is_dense = false; 00166 00167 // Calculate center of disparity image 00168 centerX = static_cast<int> (width_arg / 2); 00169 centerY = static_cast<int> (height_arg / 2); 00170 00171 const float fl_const = 1.0f / focalLength_arg; 00172 static const float bad_point = std::numeric_limits<float>::quiet_NaN (); 00173 00174 i = 0; 00175 for (y = -centerY; y < +centerY; ++y) 00176 for (x = -centerX; x < +centerX; ++x) 00177 { 00178 PointT newPoint; 00179 const uint16_t& pixel_disparity = disparityData_arg[i]; 00180 ++i; 00181 00182 if (pixel_disparity) 00183 { 00184 // Inverse depth decoding 00185 float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg); 00186 00187 // Generate new points 00188 newPoint.x = static_cast<float> (x) * depth * fl_const; 00189 newPoint.y = static_cast<float> (y) * depth * fl_const; 00190 newPoint.z = depth; 00191 00192 } 00193 else 00194 { 00195 // Generate bad point 00196 newPoint.x = newPoint.y = newPoint.z = bad_point; 00197 } 00198 00199 cloud_arg.points.push_back (newPoint); 00200 } 00201 00202 } 00203 00204 00205 /** \brief Convert disparity image to point cloud 00206 * \param[in] depthData_arg input depth image 00207 * \param[in] width_arg width of disparity image 00208 * \param[in] height_arg height of disparity image 00209 * \param[in] focalLength_arg focal length 00210 * \param[out] cloud_arg output point cloud 00211 * \ingroup io 00212 */ 00213 static void convert(typename std::vector<float>& depthData_arg, 00214 typename std::vector<uint8_t>&, 00215 bool, 00216 size_t width_arg, 00217 size_t height_arg, 00218 float focalLength_arg, 00219 pcl::PointCloud<PointT>& cloud_arg) 00220 { 00221 size_t i; 00222 size_t cloud_size = width_arg * height_arg; 00223 int x, y, centerX, centerY; 00224 00225 assert(depthData_arg.size()==cloud_size); 00226 00227 // Reset point cloud 00228 cloud_arg.points.clear (); 00229 cloud_arg.points.reserve (cloud_size); 00230 00231 // Define point cloud parameters 00232 cloud_arg.width = static_cast<uint32_t> (width_arg); 00233 cloud_arg.height = static_cast<uint32_t> (height_arg); 00234 cloud_arg.is_dense = false; 00235 00236 // Calculate center of disparity image 00237 centerX = static_cast<int> (width_arg / 2); 00238 centerY = static_cast<int> (height_arg / 2); 00239 00240 const float fl_const = 1.0f / focalLength_arg; 00241 static const float bad_point = std::numeric_limits<float>::quiet_NaN (); 00242 00243 i = 0; 00244 for (y = -centerY; y < +centerY; ++y) 00245 for (x = -centerX; x < +centerX; ++x) 00246 { 00247 PointT newPoint; 00248 const float& pixel_depth = depthData_arg[i]; 00249 ++i; 00250 00251 if (pixel_depth) 00252 { 00253 // Inverse depth decoding 00254 float depth = focalLength_arg / pixel_depth; 00255 00256 // Generate new points 00257 newPoint.x = static_cast<float> (x) * depth * fl_const; 00258 newPoint.y = static_cast<float> (y) * depth * fl_const; 00259 newPoint.z = depth; 00260 00261 } 00262 else 00263 { 00264 // Generate bad point 00265 newPoint.x = newPoint.y = newPoint.z = bad_point; 00266 } 00267 00268 cloud_arg.points.push_back (newPoint); 00269 } 00270 00271 } 00272 00273 }; 00274 00275 ////////////////////////////////////////////////////////////////////////////////////////////// 00276 // Colored point cloud specialization 00277 ////////////////////////////////////////////////////////////////////////////////////////////// 00278 template <typename PointT> 00279 struct OrganizedConversion<PointT, true> 00280 { 00281 /** \brief Convert point cloud to disparity image and rgb image 00282 * \param[in] cloud_arg input point cloud 00283 * \param[in] focalLength_arg focal length 00284 * \param[in] disparityShift_arg disparity shift 00285 * \param[in] disparityScale_arg disparity scaling 00286 * \param[in] convertToMono convert color to mono/grayscale 00287 * \param[out] disparityData_arg output disparity image 00288 * \param[out] rgbData_arg output rgb image 00289 * \ingroup io 00290 */ 00291 static void convert(const pcl::PointCloud<PointT>& cloud_arg, 00292 float focalLength_arg, 00293 float disparityShift_arg, 00294 float disparityScale_arg, 00295 bool convertToMono, 00296 typename std::vector<uint16_t>& disparityData_arg, 00297 typename std::vector<uint8_t>& rgbData_arg) 00298 { 00299 size_t cloud_size, i; 00300 00301 cloud_size = cloud_arg.points.size (); 00302 00303 // Reset output vectors 00304 disparityData_arg.clear (); 00305 rgbData_arg.clear (); 00306 00307 // Allocate memory 00308 disparityData_arg.reserve (cloud_size); 00309 if (convertToMono) 00310 { 00311 rgbData_arg.reserve (cloud_size); 00312 } else 00313 { 00314 rgbData_arg.reserve (cloud_size * 3); 00315 } 00316 00317 00318 for (i = 0; i < cloud_size; ++i) 00319 { 00320 const PointT& point = cloud_arg.points[i]; 00321 00322 if (pcl::isFinite (point)) 00323 { 00324 if (convertToMono) 00325 { 00326 // Encode point color 00327 const uint32_t rgb = *reinterpret_cast<const int*> (&point.rgb); 00328 uint8_t grayvalue = static_cast<uint8_t>(0.2989 * static_cast<float>((rgb >> 16) & 0x0000ff) + 00329 0.5870 * static_cast<float>((rgb >> 8) & 0x0000ff) + 00330 0.1140 * static_cast<float>((rgb >> 0) & 0x0000ff)); 00331 00332 rgbData_arg.push_back (grayvalue); 00333 } else 00334 { 00335 // Encode point color 00336 const uint32_t rgb = *reinterpret_cast<const int*> (&point.rgb); 00337 00338 rgbData_arg.push_back ( (rgb >> 16) & 0x0000ff); 00339 rgbData_arg.push_back ( (rgb >> 8) & 0x0000ff); 00340 rgbData_arg.push_back ( (rgb >> 0) & 0x0000ff); 00341 } 00342 00343 00344 // Inverse depth quantization 00345 uint16_t disparity = static_cast<uint16_t> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg); 00346 00347 // Encode disparity 00348 disparityData_arg.push_back (disparity); 00349 } 00350 else 00351 { 00352 00353 // Encode black point 00354 if (convertToMono) 00355 { 00356 rgbData_arg.push_back (0); 00357 } else 00358 { 00359 rgbData_arg.push_back (0); 00360 rgbData_arg.push_back (0); 00361 rgbData_arg.push_back (0); 00362 } 00363 00364 // Encode bad point 00365 disparityData_arg.push_back (0); 00366 } 00367 } 00368 00369 } 00370 00371 /** \brief Convert disparity image to point cloud 00372 * \param[in] disparityData_arg output disparity image 00373 * \param[in] rgbData_arg output rgb image 00374 * \param[in] monoImage_arg input image is a single-channel mono image 00375 * \param[in] width_arg width of disparity image 00376 * \param[in] height_arg height of disparity image 00377 * \param[in] focalLength_arg focal length 00378 * \param[in] disparityShift_arg disparity shift 00379 * \param[in] disparityScale_arg disparity scaling 00380 * \param[out] cloud_arg output point cloud 00381 * \ingroup io 00382 */ 00383 static void convert(typename std::vector<uint16_t>& disparityData_arg, 00384 typename std::vector<uint8_t>& rgbData_arg, 00385 bool monoImage_arg, 00386 size_t width_arg, 00387 size_t height_arg, 00388 float focalLength_arg, 00389 float disparityShift_arg, 00390 float disparityScale_arg, 00391 pcl::PointCloud<PointT>& cloud_arg) 00392 { 00393 size_t i; 00394 size_t cloud_size = width_arg*height_arg; 00395 bool hasColor = (rgbData_arg.size () > 0); 00396 00397 // Check size of input data 00398 assert (disparityData_arg.size()==cloud_size); 00399 if (hasColor) 00400 { 00401 if (monoImage_arg) 00402 { 00403 assert (rgbData_arg.size()==cloud_size); 00404 } else 00405 { 00406 assert (rgbData_arg.size()==cloud_size*3); 00407 } 00408 } 00409 00410 int x, y, centerX, centerY; 00411 00412 // Reset point cloud 00413 cloud_arg.points.clear(); 00414 cloud_arg.points.reserve(cloud_size); 00415 00416 // Define point cloud parameters 00417 cloud_arg.width = static_cast<uint32_t>(width_arg); 00418 cloud_arg.height = static_cast<uint32_t>(height_arg); 00419 cloud_arg.is_dense = false; 00420 00421 // Calculate center of disparity image 00422 centerX = static_cast<int>(width_arg/2); 00423 centerY = static_cast<int>(height_arg/2); 00424 00425 const float fl_const = 1.0f/focalLength_arg; 00426 static const float bad_point = std::numeric_limits<float>::quiet_NaN (); 00427 00428 i = 0; 00429 for (y=-centerY; y<+centerY; ++y ) 00430 for (x=-centerX; x<+centerX; ++x ) 00431 { 00432 PointT newPoint; 00433 00434 const uint16_t& pixel_disparity = disparityData_arg[i]; 00435 00436 if (pixel_disparity && (pixel_disparity!=0x7FF)) 00437 { 00438 float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg); 00439 00440 // Define point location 00441 newPoint.z = depth; 00442 newPoint.x = static_cast<float> (x) * depth * fl_const; 00443 newPoint.y = static_cast<float> (y) * depth * fl_const; 00444 00445 if (hasColor) 00446 { 00447 if (monoImage_arg) 00448 { 00449 const uint8_t& pixel_r = rgbData_arg[i]; 00450 const uint8_t& pixel_g = rgbData_arg[i]; 00451 const uint8_t& pixel_b = rgbData_arg[i]; 00452 00453 // Define point color 00454 uint32_t rgb = (static_cast<uint32_t>(pixel_r) << 16 00455 | static_cast<uint32_t>(pixel_g) << 8 00456 | static_cast<uint32_t>(pixel_b)); 00457 newPoint.rgb = *reinterpret_cast<float*>(&rgb); 00458 } else 00459 { 00460 const uint8_t& pixel_r = rgbData_arg[i*3+0]; 00461 const uint8_t& pixel_g = rgbData_arg[i*3+1]; 00462 const uint8_t& pixel_b = rgbData_arg[i*3+2]; 00463 00464 // Define point color 00465 uint32_t rgb = (static_cast<uint32_t>(pixel_r) << 16 00466 | static_cast<uint32_t>(pixel_g) << 8 00467 | static_cast<uint32_t>(pixel_b)); 00468 newPoint.rgb = *reinterpret_cast<float*>(&rgb); 00469 } 00470 00471 } else 00472 { 00473 // Set white point color 00474 uint32_t rgb = (static_cast<uint32_t>(255) << 16 00475 | static_cast<uint32_t>(255) << 8 00476 | static_cast<uint32_t>(255)); 00477 newPoint.rgb = *reinterpret_cast<float*>(&rgb); 00478 } 00479 } else 00480 { 00481 // Define bad point 00482 newPoint.x = newPoint.y = newPoint.z = bad_point; 00483 newPoint.rgb = 0.0f; 00484 } 00485 00486 // Add point to cloud 00487 cloud_arg.points.push_back(newPoint); 00488 // Increment point iterator 00489 ++i; 00490 } 00491 } 00492 00493 /** \brief Convert disparity image to point cloud 00494 * \param[in] depthData_arg output disparity image 00495 * \param[in] rgbData_arg output rgb image 00496 * \param[in] monoImage_arg input image is a single-channel mono image 00497 * \param[in] width_arg width of disparity image 00498 * \param[in] height_arg height of disparity image 00499 * \param[in] focalLength_arg focal length 00500 * \param[out] cloud_arg output point cloud 00501 * \ingroup io 00502 */ 00503 static void convert(typename std::vector<float>& depthData_arg, 00504 typename std::vector<uint8_t>& rgbData_arg, 00505 bool monoImage_arg, 00506 size_t width_arg, 00507 size_t height_arg, 00508 float focalLength_arg, 00509 pcl::PointCloud<PointT>& cloud_arg) 00510 { 00511 size_t i; 00512 size_t cloud_size = width_arg*height_arg; 00513 bool hasColor = (rgbData_arg.size () > 0); 00514 00515 // Check size of input data 00516 assert (depthData_arg.size()==cloud_size); 00517 if (hasColor) 00518 { 00519 if (monoImage_arg) 00520 { 00521 assert (rgbData_arg.size()==cloud_size); 00522 } else 00523 { 00524 assert (rgbData_arg.size()==cloud_size*3); 00525 } 00526 } 00527 00528 int x, y, centerX, centerY; 00529 00530 // Reset point cloud 00531 cloud_arg.points.clear(); 00532 cloud_arg.points.reserve(cloud_size); 00533 00534 // Define point cloud parameters 00535 cloud_arg.width = static_cast<uint32_t>(width_arg); 00536 cloud_arg.height = static_cast<uint32_t>(height_arg); 00537 cloud_arg.is_dense = false; 00538 00539 // Calculate center of disparity image 00540 centerX = static_cast<int>(width_arg/2); 00541 centerY = static_cast<int>(height_arg/2); 00542 00543 const float fl_const = 1.0f/focalLength_arg; 00544 static const float bad_point = std::numeric_limits<float>::quiet_NaN (); 00545 00546 i = 0; 00547 for (y=-centerY; y<+centerY; ++y ) 00548 for (x=-centerX; x<+centerX; ++x ) 00549 { 00550 PointT newPoint; 00551 00552 const float& pixel_depth = depthData_arg[i]; 00553 00554 if (pixel_depth==pixel_depth) 00555 { 00556 float depth = focalLength_arg / pixel_depth; 00557 00558 // Define point location 00559 newPoint.z = depth; 00560 newPoint.x = static_cast<float> (x) * depth * fl_const; 00561 newPoint.y = static_cast<float> (y) * depth * fl_const; 00562 00563 if (hasColor) 00564 { 00565 if (monoImage_arg) 00566 { 00567 const uint8_t& pixel_r = rgbData_arg[i]; 00568 const uint8_t& pixel_g = rgbData_arg[i]; 00569 const uint8_t& pixel_b = rgbData_arg[i]; 00570 00571 // Define point color 00572 uint32_t rgb = (static_cast<uint32_t>(pixel_r) << 16 00573 | static_cast<uint32_t>(pixel_g) << 8 00574 | static_cast<uint32_t>(pixel_b)); 00575 newPoint.rgb = *reinterpret_cast<float*>(&rgb); 00576 } else 00577 { 00578 const uint8_t& pixel_r = rgbData_arg[i*3+0]; 00579 const uint8_t& pixel_g = rgbData_arg[i*3+1]; 00580 const uint8_t& pixel_b = rgbData_arg[i*3+2]; 00581 00582 // Define point color 00583 uint32_t rgb = (static_cast<uint32_t>(pixel_r) << 16 00584 | static_cast<uint32_t>(pixel_g) << 8 00585 | static_cast<uint32_t>(pixel_b)); 00586 newPoint.rgb = *reinterpret_cast<float*>(&rgb); 00587 } 00588 00589 } else 00590 { 00591 // Set white point color 00592 uint32_t rgb = (static_cast<uint32_t>(255) << 16 00593 | static_cast<uint32_t>(255) << 8 00594 | static_cast<uint32_t>(255)); 00595 newPoint.rgb = *reinterpret_cast<float*>(&rgb); 00596 } 00597 } else 00598 { 00599 // Define bad point 00600 newPoint.x = newPoint.y = newPoint.z = bad_point; 00601 newPoint.rgb = 0.0f; 00602 } 00603 00604 // Add point to cloud 00605 cloud_arg.points.push_back(newPoint); 00606 // Increment point iterator 00607 ++i; 00608 } 00609 } 00610 }; 00611 00612 } 00613 } 00614 00615 00616 #endif