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-2012, 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 00040 #ifndef PCL_CONVERSIONS_H_ 00041 #define PCL_CONVERSIONS_H_ 00042 00043 #ifdef __GNUC__ 00044 #pragma GCC system_header 00045 #endif 00046 00047 #include <pcl/PCLPointField.h> 00048 #include <pcl/PCLPointCloud2.h> 00049 #include <pcl/PCLImage.h> 00050 #include <pcl/point_cloud.h> 00051 #include <pcl/point_traits.h> 00052 #include <pcl/for_each_type.h> 00053 #include <pcl/exceptions.h> 00054 #include <pcl/console/print.h> 00055 #include <boost/foreach.hpp> 00056 00057 namespace pcl 00058 { 00059 namespace detail 00060 { 00061 // For converting template point cloud to message. 00062 template<typename PointT> 00063 struct FieldAdder 00064 { 00065 FieldAdder (std::vector<pcl::PCLPointField>& fields) : fields_ (fields) {}; 00066 00067 template<typename U> void operator() () 00068 { 00069 pcl::PCLPointField f; 00070 f.name = traits::name<PointT, U>::value; 00071 f.offset = traits::offset<PointT, U>::value; 00072 f.datatype = traits::datatype<PointT, U>::value; 00073 f.count = traits::datatype<PointT, U>::size; 00074 fields_.push_back (f); 00075 } 00076 00077 std::vector<pcl::PCLPointField>& fields_; 00078 }; 00079 00080 // For converting message to template point cloud. 00081 template<typename PointT> 00082 struct FieldMapper 00083 { 00084 FieldMapper (const std::vector<pcl::PCLPointField>& fields, 00085 std::vector<FieldMapping>& map) 00086 : fields_ (fields), map_ (map) 00087 { 00088 } 00089 00090 template<typename Tag> void 00091 operator () () 00092 { 00093 BOOST_FOREACH (const pcl::PCLPointField& field, fields_) 00094 { 00095 if (FieldMatches<PointT, Tag>()(field)) 00096 { 00097 FieldMapping mapping; 00098 mapping.serialized_offset = field.offset; 00099 mapping.struct_offset = traits::offset<PointT, Tag>::value; 00100 mapping.size = sizeof (typename traits::datatype<PointT, Tag>::type); 00101 map_.push_back (mapping); 00102 return; 00103 } 00104 } 00105 // Disable thrown exception per #595: http://dev.pointclouds.org/issues/595 00106 PCL_WARN ("Failed to find match for field '%s'.\n", traits::name<PointT, Tag>::value); 00107 //throw pcl::InvalidConversionException (ss.str ()); 00108 } 00109 00110 const std::vector<pcl::PCLPointField>& fields_; 00111 std::vector<FieldMapping>& map_; 00112 }; 00113 00114 inline bool 00115 fieldOrdering (const FieldMapping& a, const FieldMapping& b) 00116 { 00117 return (a.serialized_offset < b.serialized_offset); 00118 } 00119 00120 } //namespace detail 00121 00122 template<typename PointT> void 00123 createMapping (const std::vector<pcl::PCLPointField>& msg_fields, MsgFieldMap& field_map) 00124 { 00125 // Create initial 1-1 mapping between serialized data segments and struct fields 00126 detail::FieldMapper<PointT> mapper (msg_fields, field_map); 00127 for_each_type< typename traits::fieldList<PointT>::type > (mapper); 00128 00129 // Coalesce adjacent fields into single memcpy's where possible 00130 if (field_map.size() > 1) 00131 { 00132 std::sort(field_map.begin(), field_map.end(), detail::fieldOrdering); 00133 MsgFieldMap::iterator i = field_map.begin(), j = i + 1; 00134 while (j != field_map.end()) 00135 { 00136 // This check is designed to permit padding between adjacent fields. 00137 /// @todo One could construct a pathological case where the struct has a 00138 /// field where the serialized data has padding 00139 if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset) 00140 { 00141 i->size += (j->struct_offset + j->size) - (i->struct_offset + i->size); 00142 j = field_map.erase(j); 00143 } 00144 else 00145 { 00146 ++i; 00147 ++j; 00148 } 00149 } 00150 } 00151 } 00152 00153 /** \brief Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map. 00154 * \param[in] msg the PCLPointCloud2 binary blob 00155 * \param[out] cloud the resultant pcl::PointCloud<T> 00156 * \param[in] field_map a MsgFieldMap object 00157 * 00158 * \note Use fromPCLPointCloud2 (PCLPointCloud2, PointCloud<T>) directly or create you 00159 * own MsgFieldMap using: 00160 * 00161 * \code 00162 * MsgFieldMap field_map; 00163 * createMapping<PointT> (msg.fields, field_map); 00164 * \endcode 00165 */ 00166 template <typename PointT> void 00167 fromPCLPointCloud2 (const pcl::PCLPointCloud2& msg, pcl::PointCloud<PointT>& cloud, 00168 const MsgFieldMap& field_map) 00169 { 00170 // Copy info fields 00171 cloud.header = msg.header; 00172 cloud.width = msg.width; 00173 cloud.height = msg.height; 00174 cloud.is_dense = msg.is_dense == 1; 00175 00176 // Copy point data 00177 uint32_t num_points = msg.width * msg.height; 00178 cloud.points.resize (num_points); 00179 uint8_t* cloud_data = reinterpret_cast<uint8_t*>(&cloud.points[0]); 00180 00181 // Check if we can copy adjacent points in a single memcpy 00182 if (field_map.size() == 1 && 00183 field_map[0].serialized_offset == 0 && 00184 field_map[0].struct_offset == 0 && 00185 msg.point_step == sizeof(PointT)) 00186 { 00187 uint32_t cloud_row_step = static_cast<uint32_t> (sizeof (PointT) * cloud.width); 00188 const uint8_t* msg_data = &msg.data[0]; 00189 // Should usually be able to copy all rows at once 00190 if (msg.row_step == cloud_row_step) 00191 { 00192 memcpy (cloud_data, msg_data, msg.data.size ()); 00193 } 00194 else 00195 { 00196 for (uint32_t i = 0; i < msg.height; ++i, cloud_data += cloud_row_step, msg_data += msg.row_step) 00197 memcpy (cloud_data, msg_data, cloud_row_step); 00198 } 00199 00200 } 00201 else 00202 { 00203 // If not, memcpy each group of contiguous fields separately 00204 for (uint32_t row = 0; row < msg.height; ++row) 00205 { 00206 const uint8_t* row_data = &msg.data[row * msg.row_step]; 00207 for (uint32_t col = 0; col < msg.width; ++col) 00208 { 00209 const uint8_t* msg_data = row_data + col * msg.point_step; 00210 BOOST_FOREACH (const detail::FieldMapping& mapping, field_map) 00211 { 00212 memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size); 00213 } 00214 cloud_data += sizeof (PointT); 00215 } 00216 } 00217 } 00218 } 00219 00220 /** \brief Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object. 00221 * \param[in] msg the PCLPointCloud2 binary blob 00222 * \param[out] cloud the resultant pcl::PointCloud<T> 00223 */ 00224 template<typename PointT> void 00225 fromPCLPointCloud2 (const pcl::PCLPointCloud2& msg, pcl::PointCloud<PointT>& cloud) 00226 { 00227 MsgFieldMap field_map; 00228 createMapping<PointT> (msg.fields, field_map); 00229 fromPCLPointCloud2 (msg, cloud, field_map); 00230 } 00231 00232 /** \brief Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob. 00233 * \param[in] cloud the input pcl::PointCloud<T> 00234 * \param[out] msg the resultant PCLPointCloud2 binary blob 00235 */ 00236 template<typename PointT> void 00237 toPCLPointCloud2 (const pcl::PointCloud<PointT>& cloud, pcl::PCLPointCloud2& msg) 00238 { 00239 // Ease the user's burden on specifying width/height for unorganized datasets 00240 if (cloud.width == 0 && cloud.height == 0) 00241 { 00242 msg.width = static_cast<uint32_t>(cloud.points.size ()); 00243 msg.height = 1; 00244 } 00245 else 00246 { 00247 assert (cloud.points.size () == cloud.width * cloud.height); 00248 msg.height = cloud.height; 00249 msg.width = cloud.width; 00250 } 00251 00252 // Fill point cloud binary data (padding and all) 00253 size_t data_size = sizeof (PointT) * cloud.points.size (); 00254 msg.data.resize (data_size); 00255 memcpy (&msg.data[0], &cloud.points[0], data_size); 00256 00257 // Fill fields metadata 00258 msg.fields.clear (); 00259 for_each_type<typename traits::fieldList<PointT>::type> (detail::FieldAdder<PointT>(msg.fields)); 00260 00261 msg.header = cloud.header; 00262 msg.point_step = sizeof (PointT); 00263 msg.row_step = static_cast<uint32_t> (sizeof (PointT) * msg.width); 00264 msg.is_dense = cloud.is_dense; 00265 /// @todo msg.is_bigendian = ?; 00266 } 00267 00268 /** \brief Copy the RGB fields of a PointCloud into pcl::PCLImage format 00269 * \param[in] cloud the point cloud message 00270 * \param[out] msg the resultant pcl::PCLImage 00271 * CloudT cloud type, CloudT should be akin to pcl::PointCloud<pcl::PointXYZRGBA> 00272 * \note will throw std::runtime_error if there is a problem 00273 */ 00274 template<typename CloudT> void 00275 toPCLPointCloud2 (const CloudT& cloud, pcl::PCLImage& msg) 00276 { 00277 // Ease the user's burden on specifying width/height for unorganized datasets 00278 if (cloud.width == 0 && cloud.height == 0) 00279 throw std::runtime_error("Needs to be a dense like cloud!!"); 00280 else 00281 { 00282 if (cloud.points.size () != cloud.width * cloud.height) 00283 throw std::runtime_error("The width and height do not match the cloud size!"); 00284 msg.height = cloud.height; 00285 msg.width = cloud.width; 00286 } 00287 00288 // ensor_msgs::image_encodings::BGR8; 00289 msg.encoding = "bgr8"; 00290 msg.step = msg.width * sizeof (uint8_t) * 3; 00291 msg.data.resize (msg.step * msg.height); 00292 for (size_t y = 0; y < cloud.height; y++) 00293 { 00294 for (size_t x = 0; x < cloud.width; x++) 00295 { 00296 uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); 00297 memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(uint8_t)); 00298 } 00299 } 00300 } 00301 00302 /** \brief Copy the RGB fields of a PCLPointCloud2 msg into pcl::PCLImage format 00303 * \param cloud the point cloud message 00304 * \param msg the resultant pcl::PCLImage 00305 * will throw std::runtime_error if there is a problem 00306 */ 00307 inline void 00308 toPCLPointCloud2 (const pcl::PCLPointCloud2& cloud, pcl::PCLImage& msg) 00309 { 00310 int rgb_index = -1; 00311 // Get the index we need 00312 for (size_t d = 0; d < cloud.fields.size (); ++d) 00313 if (cloud.fields[d].name == "rgb") 00314 { 00315 rgb_index = static_cast<int>(d); 00316 break; 00317 } 00318 00319 if(rgb_index == -1) 00320 throw std::runtime_error ("No rgb field!!"); 00321 if (cloud.width == 0 && cloud.height == 0) 00322 throw std::runtime_error ("Needs to be a dense like cloud!!"); 00323 else 00324 { 00325 msg.height = cloud.height; 00326 msg.width = cloud.width; 00327 } 00328 int rgb_offset = cloud.fields[rgb_index].offset; 00329 int point_step = cloud.point_step; 00330 00331 // pcl::image_encodings::BGR8; 00332 msg.encoding = "bgr8"; 00333 msg.step = static_cast<uint32_t>(msg.width * sizeof (uint8_t) * 3); 00334 msg.data.resize (msg.step * msg.height); 00335 00336 for (size_t y = 0; y < cloud.height; y++) 00337 { 00338 for (size_t x = 0; x < cloud.width; x++, rgb_offset += point_step) 00339 { 00340 uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); 00341 memcpy (pixel, &(cloud.data[rgb_offset]), 3 * sizeof (uint8_t)); 00342 } 00343 } 00344 } 00345 } 00346 00347 #endif //#ifndef PCL_CONVERSIONS_H_