PCLを利用しvelodyneの３次元点群を連続で保存したい

PCLを利用し、velodyneのスキャンデータを連続的に保存する

https://teratail.com/questions/input#
ubuntu16.04でPCL1.8の関数である"savePCDFilBinary"を利用し、velodyneのスキャンデータをひとつのファイルに連続的に保存したいと考えています。
上記のPCLの関数では１スキャン分を保存してくれますが、次のスキャンデータを同じファイルの末尾に追加することができません。
同じファイルの末尾に保存する方法についてご指導ください。また、別な方法で容量を抑え、連続的に保存する方法などあれば教えていただきたいです。

savePCDFileBinary

C++
1//savePCDFileの中身
2inline int 
3    savePCDFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,
4                 const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), 
5                 const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
6                 const bool binary_mode = false)
7    {
8      PCDWriter w;
9      return (w.write (file_name, cloud, origin, orientation, binary_mode));
10    }
11
12/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
13template <typename PointT> int
14pcl::PCDWriter::writeBinary (const std::string &file_name, 
15                             const pcl::PointCloud<PointT> &cloud, 
16                             const std::vector<int> &indices)
17{
18  if (cloud.points.empty () || indices.empty ())
19  {
20    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!");
21    return (-1);
22  }
23  int data_idx = 0;
24  std::ostringstream oss;
25  oss << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA binary\n";
26  oss.flush ();
27  data_idx = static_cast<int> (oss.tellp ());
28
29#if _WIN32
30  HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
31  if (h_native_file == INVALID_HANDLE_VALUE)
32  {
33    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!");
34    return (-1);
35  }
36#else
37
38  int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, static_cast<mode_t> (0600));
39  if (fd < 0)
40  {
41    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
42    return (-1);
43  }
44#endif
45  // Mandatory lock file
46  boost::interprocess::file_lock file_lock;
47  setLockingPermissions (file_name, file_lock);
48
49  std::vector<pcl::PCLPointField> fields;
50  std::vector<int> fields_sizes;
51  size_t fsize = 0;
52  size_t data_size = 0;
53  size_t nri = 0;
54  pcl::getFields (cloud, fields);
55  // Compute the total size of the fields
56  for (size_t i = 0; i < fields.size (); ++i)
57  {
58    if (fields[i].name == "_")
59      continue;
60    
61    int fs = fields[i].count * getFieldSize (fields[i].datatype);
62    fsize += fs;
63    fields_sizes.push_back (fs);
64    fields[nri++] = fields[i];
65  }
66  fields.resize (nri);
67  
68  data_size = indices.size () * fsize;
69
70  // Prepare the map
71#if _WIN32
72  HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL);
73  char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
74  CloseHandle (fm);
75
76#else
77  // Stretch the file size to the size of the data
78  off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
79  if (result < 0)
80  {
81    pcl_close (fd);
82    resetLockingPermissions (file_name, file_lock);
83    PCL_ERROR ("[pcl::PCDWriter::writeBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno));
84    
85    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during lseek ()!");
86    return (-1);
87  }
88  // Write a bogus entry so that the new file size comes in effect
89  result = static_cast<int> (::write (fd, "", 1));
90  if (result != 1)
91  {
92    pcl_close (fd);
93    resetLockingPermissions (file_name, file_lock);
94    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during write ()!");
95    return (-1);
96  }
97
98  char *map = static_cast<char*> (mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
99  if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
100  {
101    pcl_close (fd);
102    resetLockingPermissions (file_name, file_lock);
103    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
104    return (-1);
105  }
106#endif
107
108  // Copy the header
109  memcpy (&map[0], oss.str ().c_str (), data_idx);
110
111  char *out = &map[0] + data_idx;
112  // Copy the data
113  for (size_t i = 0; i < indices.size (); ++i)
114  {
115    int nrj = 0;
116    for (size_t j = 0; j < fields.size (); ++j)
117    {
118      memcpy (out, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[j].offset, fields_sizes[nrj]);
119      out += fields_sizes[nrj++];
120    }
121  }
122
123#if !_WIN32
124  // If the user set the synchronization flag on, call msync
125  if (map_synchronization_)
126    msync (map, data_idx + data_size, MS_SYNC);
127#endif
128
129  // Unmap the pages of memory
130#if _WIN32
131    UnmapViewOfFile (map);
132#else
133  if (munmap (map, (data_idx + data_size)) == -1)
134  {
135    pcl_close (fd);
136    resetLockingPermissions (file_name, file_lock);
137    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
138    return (-1);
139  }
140#endif
141  // Close file
142#if _WIN32
143  CloseHandle(h_native_file);
144#else
145  pcl_close (fd);
146#endif
147  
148  resetLockingPermissions (file_name, file_lock);
149  return (0);
150}

試したこと

r+ の状態になれば良いと考え、
O_TRUNC　-> O_APPEND
に変更したが、上書きはされず。

補足情報（FW/ツールのバージョンなど）

特になし

t_obara

2019/10/21 05:07

内容を細かく把握できていませんが、フォーマットを解析して何かヘッダを変更したり内容を修正する必要があるのでしょうか？もし無いのであれば、ubuntuなどWindows以外なら、シェルで　cat files > newfile で結合できます。コードについてですが、既存のファイルにデータを追加したいのであれば、Open modeはご提示内容の通り、O_APPENDであるべきかと思います。また、ファイルを開いた後で、ファイルの最後にファイルポインタを移動しようとしていますが、O_APPENDで開けば不用な処理だと思います。 getpagesize () + data_size　の値が想定通りか確認して見てはいかがでしょうか。ヘッダなど修正が必要ということであれば、別なファイルとして必要なデータを書き出し、最後にリネームする方が安全だと思います。