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 * 00038 */ 00039 00040 #ifndef PCL_IO_PCD_IO_IMPL_H_ 00041 #define PCL_IO_PCD_IO_IMPL_H_ 00042 00043 #include <fstream> 00044 #include <fcntl.h> 00045 #include <string> 00046 #include <stdlib.h> 00047 #include <pcl/io/boost.h> 00048 #include <pcl/console/print.h> 00049 #include <pcl/io/pcd_io.h> 00050 00051 #ifdef _WIN32 00052 # include <io.h> 00053 # ifndef WIN32_LEAN_AND_MEAN 00054 # define WIN32_LEAN_AND_MEAN 00055 # endif // WIN32_LEAN_AND_MEAN 00056 # ifndef NOMINMAX 00057 # define NOMINMAX 00058 # endif // NOMINMAX 00059 # include <windows.h> 00060 # define pcl_open _open 00061 # define pcl_close(fd) _close(fd) 00062 # define pcl_lseek(fd,offset,origin) _lseek(fd,offset,origin) 00063 #else 00064 # include <sys/mman.h> 00065 # define pcl_open open 00066 # define pcl_close(fd) close(fd) 00067 # define pcl_lseek(fd,offset,origin) lseek(fd,offset,origin) 00068 #endif 00069 00070 #include <pcl/io/lzf.h> 00071 00072 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00073 template <typename PointT> std::string 00074 pcl::PCDWriter::generateHeader (const pcl::PointCloud<PointT> &cloud, const int nr_points) 00075 { 00076 std::ostringstream oss; 00077 oss.imbue (std::locale::classic ()); 00078 00079 oss << "# .PCD v0.7 - Point Cloud Data file format" 00080 "\nVERSION 0.7" 00081 "\nFIELDS"; 00082 00083 std::vector<pcl::PCLPointField> fields; 00084 pcl::getFields (cloud, fields); 00085 00086 std::stringstream field_names, field_types, field_sizes, field_counts; 00087 for (size_t i = 0; i < fields.size (); ++i) 00088 { 00089 if (fields[i].name == "_") 00090 continue; 00091 // Add the regular dimension 00092 field_names << " " << fields[i].name; 00093 field_sizes << " " << pcl::getFieldSize (fields[i].datatype); 00094 field_types << " " << pcl::getFieldType (fields[i].datatype); 00095 int count = abs (static_cast<int> (fields[i].count)); 00096 if (count == 0) count = 1; // check for 0 counts (coming from older converter code) 00097 field_counts << " " << count; 00098 } 00099 oss << field_names.str (); 00100 oss << "\nSIZE" << field_sizes.str () 00101 << "\nTYPE" << field_types.str () 00102 << "\nCOUNT" << field_counts.str (); 00103 // If the user passes in a number of points value, use that instead 00104 if (nr_points != std::numeric_limits<int>::max ()) 00105 oss << "\nWIDTH " << nr_points << "\nHEIGHT " << 1 << "\n"; 00106 else 00107 oss << "\nWIDTH " << cloud.width << "\nHEIGHT " << cloud.height << "\n"; 00108 00109 oss << "VIEWPOINT " << cloud.sensor_origin_[0] << " " << cloud.sensor_origin_[1] << " " << cloud.sensor_origin_[2] << " " << 00110 cloud.sensor_orientation_.w () << " " << 00111 cloud.sensor_orientation_.x () << " " << 00112 cloud.sensor_orientation_.y () << " " << 00113 cloud.sensor_orientation_.z () << "\n"; 00114 00115 // If the user passes in a number of points value, use that instead 00116 if (nr_points != std::numeric_limits<int>::max ()) 00117 oss << "POINTS " << nr_points << "\n"; 00118 else 00119 oss << "POINTS " << cloud.points.size () << "\n"; 00120 00121 return (oss.str ()); 00122 } 00123 00124 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00125 template <typename PointT> int 00126 pcl::PCDWriter::writeBinary (const std::string &file_name, 00127 const pcl::PointCloud<PointT> &cloud) 00128 { 00129 if (cloud.empty ()) 00130 { 00131 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data!"); 00132 return (-1); 00133 } 00134 int data_idx = 0; 00135 std::ostringstream oss; 00136 oss << generateHeader<PointT> (cloud) << "DATA binary\n"; 00137 oss.flush (); 00138 data_idx = static_cast<int> (oss.tellp ()); 00139 00140 #if _WIN32 00141 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); 00142 if (h_native_file == INVALID_HANDLE_VALUE) 00143 { 00144 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!"); 00145 return (-1); 00146 } 00147 #else 00148 int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, static_cast<mode_t> (0600)); 00149 if (fd < 0) 00150 { 00151 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!"); 00152 return (-1); 00153 } 00154 #endif 00155 // Mandatory lock file 00156 boost::interprocess::file_lock file_lock; 00157 setLockingPermissions (file_name, file_lock); 00158 00159 std::vector<pcl::PCLPointField> fields; 00160 std::vector<int> fields_sizes; 00161 size_t fsize = 0; 00162 size_t data_size = 0; 00163 size_t nri = 0; 00164 pcl::getFields (cloud, fields); 00165 // Compute the total size of the fields 00166 for (size_t i = 0; i < fields.size (); ++i) 00167 { 00168 if (fields[i].name == "_") 00169 continue; 00170 00171 int fs = fields[i].count * getFieldSize (fields[i].datatype); 00172 fsize += fs; 00173 fields_sizes.push_back (fs); 00174 fields[nri++] = fields[i]; 00175 } 00176 fields.resize (nri); 00177 00178 data_size = cloud.points.size () * fsize; 00179 00180 // Prepare the map 00181 #if _WIN32 00182 HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL); 00183 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size)); 00184 CloseHandle (fm); 00185 00186 #else 00187 // Stretch the file size to the size of the data 00188 off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET); 00189 00190 if (result < 0) 00191 { 00192 pcl_close (fd); 00193 resetLockingPermissions (file_name, file_lock); 00194 PCL_ERROR ("[pcl::PCDWriter::writeBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno)); 00195 00196 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during lseek ()!"); 00197 return (-1); 00198 } 00199 // Write a bogus entry so that the new file size comes in effect 00200 result = static_cast<int> (::write (fd, "", 1)); 00201 if (result != 1) 00202 { 00203 pcl_close (fd); 00204 resetLockingPermissions (file_name, file_lock); 00205 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during write ()!"); 00206 return (-1); 00207 } 00208 00209 char *map = static_cast<char*> (mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0)); 00210 if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED) 00211 { 00212 pcl_close (fd); 00213 resetLockingPermissions (file_name, file_lock); 00214 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!"); 00215 return (-1); 00216 } 00217 #endif 00218 00219 // Copy the header 00220 memcpy (&map[0], oss.str ().c_str (), data_idx); 00221 00222 // Copy the data 00223 char *out = &map[0] + data_idx; 00224 for (size_t i = 0; i < cloud.points.size (); ++i) 00225 { 00226 int nrj = 0; 00227 for (size_t j = 0; j < fields.size (); ++j) 00228 { 00229 memcpy (out, reinterpret_cast<const char*> (&cloud.points[i]) + fields[j].offset, fields_sizes[nrj]); 00230 out += fields_sizes[nrj++]; 00231 } 00232 } 00233 00234 // If the user set the synchronization flag on, call msync 00235 #if !_WIN32 00236 if (map_synchronization_) 00237 msync (map, data_idx + data_size, MS_SYNC); 00238 #endif 00239 00240 // Unmap the pages of memory 00241 #if _WIN32 00242 UnmapViewOfFile (map); 00243 #else 00244 if (munmap (map, (data_idx + data_size)) == -1) 00245 { 00246 pcl_close (fd); 00247 resetLockingPermissions (file_name, file_lock); 00248 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!"); 00249 return (-1); 00250 } 00251 #endif 00252 // Close file 00253 #if _WIN32 00254 CloseHandle (h_native_file); 00255 #else 00256 pcl_close (fd); 00257 #endif 00258 resetLockingPermissions (file_name, file_lock); 00259 return (0); 00260 } 00261 00262 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00263 template <typename PointT> int 00264 pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name, 00265 const pcl::PointCloud<PointT> &cloud) 00266 { 00267 if (cloud.points.empty ()) 00268 { 00269 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!"); 00270 return (-1); 00271 } 00272 int data_idx = 0; 00273 std::ostringstream oss; 00274 oss << generateHeader<PointT> (cloud) << "DATA binary_compressed\n"; 00275 oss.flush (); 00276 data_idx = static_cast<int> (oss.tellp ()); 00277 00278 #if _WIN32 00279 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); 00280 if (h_native_file == INVALID_HANDLE_VALUE) 00281 { 00282 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during CreateFile!"); 00283 return (-1); 00284 } 00285 #else 00286 int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, static_cast<mode_t> (0600)); 00287 if (fd < 0) 00288 { 00289 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during open!"); 00290 return (-1); 00291 } 00292 #endif 00293 00294 // Mandatory lock file 00295 boost::interprocess::file_lock file_lock; 00296 setLockingPermissions (file_name, file_lock); 00297 00298 std::vector<pcl::PCLPointField> fields; 00299 size_t fsize = 0; 00300 size_t data_size = 0; 00301 size_t nri = 0; 00302 pcl::getFields (cloud, fields); 00303 std::vector<int> fields_sizes (fields.size ()); 00304 // Compute the total size of the fields 00305 for (size_t i = 0; i < fields.size (); ++i) 00306 { 00307 if (fields[i].name == "_") 00308 continue; 00309 00310 fields_sizes[nri] = fields[i].count * pcl::getFieldSize (fields[i].datatype); 00311 fsize += fields_sizes[nri]; 00312 fields[nri] = fields[i]; 00313 ++nri; 00314 } 00315 fields_sizes.resize (nri); 00316 fields.resize (nri); 00317 00318 // Compute the size of data 00319 data_size = cloud.points.size () * fsize; 00320 00321 ////////////////////////////////////////////////////////////////////// 00322 // Empty array holding only the valid data 00323 // data_size = nr_points * point_size 00324 // = nr_points * (sizeof_field_1 + sizeof_field_2 + ... sizeof_field_n) 00325 // = sizeof_field_1 * nr_points + sizeof_field_2 * nr_points + ... sizeof_field_n * nr_points 00326 char *only_valid_data = static_cast<char*> (malloc (data_size)); 00327 00328 // Convert the XYZRGBXYZRGB structure to XXYYZZRGBRGB to aid compression. For 00329 // this, we need a vector of fields.size () (4 in this case), which points to 00330 // each individual plane: 00331 // pters[0] = &only_valid_data[offset_of_plane_x]; 00332 // pters[1] = &only_valid_data[offset_of_plane_y]; 00333 // pters[2] = &only_valid_data[offset_of_plane_z]; 00334 // pters[3] = &only_valid_data[offset_of_plane_RGB]; 00335 // 00336 std::vector<char*> pters (fields.size ()); 00337 int toff = 0; 00338 for (size_t i = 0; i < pters.size (); ++i) 00339 { 00340 pters[i] = &only_valid_data[toff]; 00341 toff += fields_sizes[i] * static_cast<int> (cloud.points.size ()); 00342 } 00343 00344 // Go over all the points, and copy the data in the appropriate places 00345 for (size_t i = 0; i < cloud.points.size (); ++i) 00346 { 00347 for (size_t j = 0; j < fields.size (); ++j) 00348 { 00349 memcpy (pters[j], reinterpret_cast<const char*> (&cloud.points[i]) + fields[j].offset, fields_sizes[j]); 00350 // Increment the pointer 00351 pters[j] += fields_sizes[j]; 00352 } 00353 } 00354 00355 char* temp_buf = static_cast<char*> (malloc (static_cast<size_t> (static_cast<float> (data_size) * 1.5f + 8.0f))); 00356 // Compress the valid data 00357 unsigned int compressed_size = pcl::lzfCompress (only_valid_data, 00358 static_cast<uint32_t> (data_size), 00359 &temp_buf[8], 00360 static_cast<uint32_t> (static_cast<float>(data_size) * 1.5f)); 00361 unsigned int compressed_final_size = 0; 00362 // Was the compression successful? 00363 if (compressed_size) 00364 { 00365 char *header = &temp_buf[0]; 00366 memcpy (&header[0], &compressed_size, sizeof (unsigned int)); 00367 memcpy (&header[4], &data_size, sizeof (unsigned int)); 00368 data_size = compressed_size + 8; 00369 compressed_final_size = static_cast<uint32_t> (data_size) + data_idx; 00370 } 00371 else 00372 { 00373 #if !_WIN32 00374 pcl_close (fd); 00375 #endif 00376 resetLockingPermissions (file_name, file_lock); 00377 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during compression!"); 00378 return (-1); 00379 } 00380 00381 #if !_WIN32 00382 // Stretch the file size to the size of the data 00383 off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET); 00384 if (result < 0) 00385 { 00386 pcl_close (fd); 00387 resetLockingPermissions (file_name, file_lock); 00388 PCL_ERROR ("[pcl::PCDWriter::writeBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno)); 00389 00390 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during lseek ()!"); 00391 return (-1); 00392 } 00393 // Write a bogus entry so that the new file size comes in effect 00394 result = static_cast<int> (::write (fd, "", 1)); 00395 if (result != 1) 00396 { 00397 pcl_close (fd); 00398 resetLockingPermissions (file_name, file_lock); 00399 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during write ()!"); 00400 return (-1); 00401 } 00402 #endif 00403 00404 // Prepare the map 00405 #if _WIN32 00406 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL); 00407 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size)); 00408 CloseHandle (fm); 00409 00410 #else 00411 char *map = static_cast<char*> (mmap (0, compressed_final_size, PROT_WRITE, MAP_SHARED, fd, 0)); 00412 if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED) 00413 { 00414 pcl_close (fd); 00415 resetLockingPermissions (file_name, file_lock); 00416 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!"); 00417 return (-1); 00418 } 00419 #endif 00420 00421 // Copy the header 00422 memcpy (&map[0], oss.str ().c_str (), data_idx); 00423 // Copy the compressed data 00424 memcpy (&map[data_idx], temp_buf, data_size); 00425 00426 #if !_WIN32 00427 // If the user set the synchronization flag on, call msync 00428 if (map_synchronization_) 00429 msync (map, compressed_final_size, MS_SYNC); 00430 #endif 00431 00432 // Unmap the pages of memory 00433 #if _WIN32 00434 UnmapViewOfFile (map); 00435 #else 00436 if (munmap (map, (compressed_final_size)) == -1) 00437 { 00438 pcl_close (fd); 00439 resetLockingPermissions (file_name, file_lock); 00440 throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!"); 00441 return (-1); 00442 } 00443 #endif 00444 00445 // Close file 00446 #if _WIN32 00447 CloseHandle (h_native_file); 00448 #else 00449 pcl_close (fd); 00450 #endif 00451 resetLockingPermissions (file_name, file_lock); 00452 00453 free (only_valid_data); 00454 free (temp_buf); 00455 return (0); 00456 } 00457 00458 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00459 template <typename PointT> int 00460 pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud, 00461 const int precision) 00462 { 00463 if (cloud.empty ()) 00464 { 00465 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data!"); 00466 return (-1); 00467 } 00468 00469 if (cloud.width * cloud.height != cloud.points.size ()) 00470 { 00471 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!"); 00472 return (-1); 00473 } 00474 00475 std::ofstream fs; 00476 fs.open (file_name.c_str ()); // Open file 00477 00478 if (!fs.is_open () || fs.fail ()) 00479 { 00480 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!"); 00481 return (-1); 00482 } 00483 00484 // Mandatory lock file 00485 boost::interprocess::file_lock file_lock; 00486 setLockingPermissions (file_name, file_lock); 00487 00488 fs.precision (precision); 00489 fs.imbue (std::locale::classic ()); 00490 00491 std::vector<pcl::PCLPointField> fields; 00492 pcl::getFields (cloud, fields); 00493 00494 // Write the header information 00495 fs << generateHeader<PointT> (cloud) << "DATA ascii\n"; 00496 00497 std::ostringstream stream; 00498 stream.precision (precision); 00499 stream.imbue (std::locale::classic ()); 00500 // Iterate through the points 00501 for (size_t i = 0; i < cloud.points.size (); ++i) 00502 { 00503 for (size_t d = 0; d < fields.size (); ++d) 00504 { 00505 // Ignore invalid padded dimensions that are inherited from binary data 00506 if (fields[d].name == "_") 00507 continue; 00508 00509 int count = fields[d].count; 00510 if (count == 0) 00511 count = 1; // we simply cannot tolerate 0 counts (coming from older converter code) 00512 00513 for (int c = 0; c < count; ++c) 00514 { 00515 switch (fields[d].datatype) 00516 { 00517 case pcl::PCLPointField::INT8: 00518 { 00519 int8_t value; 00520 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (int8_t), sizeof (int8_t)); 00521 if (pcl_isnan (value)) 00522 stream << "nan"; 00523 else 00524 stream << boost::numeric_cast<uint32_t>(value); 00525 break; 00526 } 00527 case pcl::PCLPointField::UINT8: 00528 { 00529 uint8_t value; 00530 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (uint8_t), sizeof (uint8_t)); 00531 if (pcl_isnan (value)) 00532 stream << "nan"; 00533 else 00534 stream << boost::numeric_cast<uint32_t>(value); 00535 break; 00536 } 00537 case pcl::PCLPointField::INT16: 00538 { 00539 int16_t value; 00540 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (int16_t), sizeof (int16_t)); 00541 if (pcl_isnan (value)) 00542 stream << "nan"; 00543 else 00544 stream << boost::numeric_cast<int16_t>(value); 00545 break; 00546 } 00547 case pcl::PCLPointField::UINT16: 00548 { 00549 uint16_t value; 00550 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (uint16_t), sizeof (uint16_t)); 00551 if (pcl_isnan (value)) 00552 stream << "nan"; 00553 else 00554 stream << boost::numeric_cast<uint16_t>(value); 00555 break; 00556 } 00557 case pcl::PCLPointField::INT32: 00558 { 00559 int32_t value; 00560 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (int32_t), sizeof (int32_t)); 00561 if (pcl_isnan (value)) 00562 stream << "nan"; 00563 else 00564 stream << boost::numeric_cast<int32_t>(value); 00565 break; 00566 } 00567 case pcl::PCLPointField::UINT32: 00568 { 00569 uint32_t value; 00570 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (uint32_t), sizeof (uint32_t)); 00571 if (pcl_isnan (value)) 00572 stream << "nan"; 00573 else 00574 stream << boost::numeric_cast<uint32_t>(value); 00575 break; 00576 } 00577 case pcl::PCLPointField::FLOAT32: 00578 { 00579 float value; 00580 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (float), sizeof (float)); 00581 if (pcl_isnan (value)) 00582 stream << "nan"; 00583 else 00584 stream << boost::numeric_cast<float>(value); 00585 break; 00586 } 00587 case pcl::PCLPointField::FLOAT64: 00588 { 00589 double value; 00590 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (double), sizeof (double)); 00591 if (pcl_isnan (value)) 00592 stream << "nan"; 00593 else 00594 stream << boost::numeric_cast<double>(value); 00595 break; 00596 } 00597 default: 00598 PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype); 00599 break; 00600 } 00601 00602 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1)) 00603 stream << " "; 00604 } 00605 } 00606 // Copy the stream, trim it, and write it to disk 00607 std::string result = stream.str (); 00608 boost::trim (result); 00609 stream.str (""); 00610 fs << result << "\n"; 00611 } 00612 fs.close (); // Close file 00613 resetLockingPermissions (file_name, file_lock); 00614 return (0); 00615 } 00616 00617 00618 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00619 template <typename PointT> int 00620 pcl::PCDWriter::writeBinary (const std::string &file_name, 00621 const pcl::PointCloud<PointT> &cloud, 00622 const std::vector<int> &indices) 00623 { 00624 if (cloud.points.empty () || indices.empty ()) 00625 { 00626 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!"); 00627 return (-1); 00628 } 00629 int data_idx = 0; 00630 std::ostringstream oss; 00631 oss << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA binary\n"; 00632 oss.flush (); 00633 data_idx = static_cast<int> (oss.tellp ()); 00634 00635 #if _WIN32 00636 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); 00637 if (h_native_file == INVALID_HANDLE_VALUE) 00638 { 00639 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!"); 00640 return (-1); 00641 } 00642 #else 00643 int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, static_cast<mode_t> (0600)); 00644 if (fd < 0) 00645 { 00646 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!"); 00647 return (-1); 00648 } 00649 #endif 00650 // Mandatory lock file 00651 boost::interprocess::file_lock file_lock; 00652 setLockingPermissions (file_name, file_lock); 00653 00654 std::vector<pcl::PCLPointField> fields; 00655 std::vector<int> fields_sizes; 00656 size_t fsize = 0; 00657 size_t data_size = 0; 00658 size_t nri = 0; 00659 pcl::getFields (cloud, fields); 00660 // Compute the total size of the fields 00661 for (size_t i = 0; i < fields.size (); ++i) 00662 { 00663 if (fields[i].name == "_") 00664 continue; 00665 00666 int fs = fields[i].count * getFieldSize (fields[i].datatype); 00667 fsize += fs; 00668 fields_sizes.push_back (fs); 00669 fields[nri++] = fields[i]; 00670 } 00671 fields.resize (nri); 00672 00673 data_size = indices.size () * fsize; 00674 00675 // Prepare the map 00676 #if _WIN32 00677 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL); 00678 char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size)); 00679 CloseHandle (fm); 00680 00681 #else 00682 // Stretch the file size to the size of the data 00683 off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET); 00684 if (result < 0) 00685 { 00686 pcl_close (fd); 00687 resetLockingPermissions (file_name, file_lock); 00688 PCL_ERROR ("[pcl::PCDWriter::writeBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno)); 00689 00690 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during lseek ()!"); 00691 return (-1); 00692 } 00693 // Write a bogus entry so that the new file size comes in effect 00694 result = static_cast<int> (::write (fd, "", 1)); 00695 if (result != 1) 00696 { 00697 pcl_close (fd); 00698 resetLockingPermissions (file_name, file_lock); 00699 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during write ()!"); 00700 return (-1); 00701 } 00702 00703 char *map = static_cast<char*> (mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0)); 00704 if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED) 00705 { 00706 pcl_close (fd); 00707 resetLockingPermissions (file_name, file_lock); 00708 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!"); 00709 return (-1); 00710 } 00711 #endif 00712 00713 // Copy the header 00714 memcpy (&map[0], oss.str ().c_str (), data_idx); 00715 00716 char *out = &map[0] + data_idx; 00717 // Copy the data 00718 for (size_t i = 0; i < indices.size (); ++i) 00719 { 00720 int nrj = 0; 00721 for (size_t j = 0; j < fields.size (); ++j) 00722 { 00723 memcpy (out, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[j].offset, fields_sizes[nrj]); 00724 out += fields_sizes[nrj++]; 00725 } 00726 } 00727 00728 #if !_WIN32 00729 // If the user set the synchronization flag on, call msync 00730 if (map_synchronization_) 00731 msync (map, data_idx + data_size, MS_SYNC); 00732 #endif 00733 00734 // Unmap the pages of memory 00735 #if _WIN32 00736 UnmapViewOfFile (map); 00737 #else 00738 if (munmap (map, (data_idx + data_size)) == -1) 00739 { 00740 pcl_close (fd); 00741 resetLockingPermissions (file_name, file_lock); 00742 throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!"); 00743 return (-1); 00744 } 00745 #endif 00746 // Close file 00747 #if _WIN32 00748 CloseHandle(h_native_file); 00749 #else 00750 pcl_close (fd); 00751 #endif 00752 00753 resetLockingPermissions (file_name, file_lock); 00754 return (0); 00755 } 00756 00757 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00758 template <typename PointT> int 00759 pcl::PCDWriter::writeASCII (const std::string &file_name, 00760 const pcl::PointCloud<PointT> &cloud, 00761 const std::vector<int> &indices, 00762 const int precision) 00763 { 00764 if (cloud.points.empty () || indices.empty ()) 00765 { 00766 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!"); 00767 return (-1); 00768 } 00769 00770 if (cloud.width * cloud.height != cloud.points.size ()) 00771 { 00772 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!"); 00773 return (-1); 00774 } 00775 00776 std::ofstream fs; 00777 fs.open (file_name.c_str ()); // Open file 00778 if (!fs.is_open () || fs.fail ()) 00779 { 00780 throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!"); 00781 return (-1); 00782 } 00783 00784 // Mandatory lock file 00785 boost::interprocess::file_lock file_lock; 00786 setLockingPermissions (file_name, file_lock); 00787 00788 fs.precision (precision); 00789 fs.imbue (std::locale::classic ()); 00790 00791 std::vector<pcl::PCLPointField> fields; 00792 pcl::getFields (cloud, fields); 00793 00794 // Write the header information 00795 fs << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA ascii\n"; 00796 00797 std::ostringstream stream; 00798 stream.precision (precision); 00799 stream.imbue (std::locale::classic ()); 00800 00801 // Iterate through the points 00802 for (size_t i = 0; i < indices.size (); ++i) 00803 { 00804 for (size_t d = 0; d < fields.size (); ++d) 00805 { 00806 // Ignore invalid padded dimensions that are inherited from binary data 00807 if (fields[d].name == "_") 00808 continue; 00809 00810 int count = fields[d].count; 00811 if (count == 0) 00812 count = 1; // we simply cannot tolerate 0 counts (coming from older converter code) 00813 00814 for (int c = 0; c < count; ++c) 00815 { 00816 switch (fields[d].datatype) 00817 { 00818 case pcl::PCLPointField::INT8: 00819 { 00820 int8_t value; 00821 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (int8_t), sizeof (int8_t)); 00822 if (pcl_isnan (value)) 00823 stream << "nan"; 00824 else 00825 stream << boost::numeric_cast<uint32_t>(value); 00826 break; 00827 } 00828 case pcl::PCLPointField::UINT8: 00829 { 00830 uint8_t value; 00831 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (uint8_t), sizeof (uint8_t)); 00832 if (pcl_isnan (value)) 00833 stream << "nan"; 00834 else 00835 stream << boost::numeric_cast<uint32_t>(value); 00836 break; 00837 } 00838 case pcl::PCLPointField::INT16: 00839 { 00840 int16_t value; 00841 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (int16_t), sizeof (int16_t)); 00842 if (pcl_isnan (value)) 00843 stream << "nan"; 00844 else 00845 stream << boost::numeric_cast<int16_t>(value); 00846 break; 00847 } 00848 case pcl::PCLPointField::UINT16: 00849 { 00850 uint16_t value; 00851 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (uint16_t), sizeof (uint16_t)); 00852 if (pcl_isnan (value)) 00853 stream << "nan"; 00854 else 00855 stream << boost::numeric_cast<uint16_t>(value); 00856 break; 00857 } 00858 case pcl::PCLPointField::INT32: 00859 { 00860 int32_t value; 00861 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (int32_t), sizeof (int32_t)); 00862 if (pcl_isnan (value)) 00863 stream << "nan"; 00864 else 00865 stream << boost::numeric_cast<int32_t>(value); 00866 break; 00867 } 00868 case pcl::PCLPointField::UINT32: 00869 { 00870 uint32_t value; 00871 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (uint32_t), sizeof (uint32_t)); 00872 if (pcl_isnan (value)) 00873 stream << "nan"; 00874 else 00875 stream << boost::numeric_cast<uint32_t>(value); 00876 break; 00877 } 00878 case pcl::PCLPointField::FLOAT32: 00879 { 00880 float value; 00881 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (float), sizeof (float)); 00882 if (pcl_isnan (value)) 00883 stream << "nan"; 00884 else 00885 stream << boost::numeric_cast<float>(value); 00886 break; 00887 } 00888 case pcl::PCLPointField::FLOAT64: 00889 { 00890 double value; 00891 memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (double), sizeof (double)); 00892 if (pcl_isnan (value)) 00893 stream << "nan"; 00894 else 00895 stream << boost::numeric_cast<double>(value); 00896 break; 00897 } 00898 default: 00899 PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype); 00900 break; 00901 } 00902 00903 if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1)) 00904 stream << " "; 00905 } 00906 } 00907 // Copy the stream, trim it, and write it to disk 00908 std::string result = stream.str (); 00909 boost::trim (result); 00910 stream.str (""); 00911 fs << result << "\n"; 00912 } 00913 fs.close (); // Close file 00914 00915 resetLockingPermissions (file_name, file_lock); 00916 00917 return (0); 00918 } 00919 00920 #endif //#ifndef PCL_IO_PCD_IO_H_ 00921