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