source: pacpussensors/trunk/Vislab/20150310_lib3dv-1.2.0/src/file.cc

Last change on this file was 136, checked in by ldecherf, 8 years ago

Doc

File size: 40.1 KB
Line 
1/* 3dv-client/file.cc
2 *
3 * Copyright (C) 2013 VisLab
4 *
5 * This file is part of 3dv-client; you can redistribute it and/or modify
6 * it under the terms of the GNU Lesser General Public License as published by
7 * the Free Software Foundation; either version 3 of the License, or (at
8 * your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
14 *
15 * You should have received a copy of the GNU Lesser General Public License
16 * along with this program; if not, see <http://www.gnu.org/licenses/>.
17 */
18
19#include "file.h"
20
21#include <iostream>
22
23#ifdef ARCHIVE
24
25#include <boost/date_time/posix_time/posix_time_io.hpp>
26#include <boost/iostreams/stream_buffer.hpp>
27#include <boost/iostreams/stream.hpp>
28#include <boost/iostreams/device/back_inserter.hpp>
29
30#include <archive.h>
31#include <archive_entry.h>
32
33archive* open_archive(const boost::filesystem::path& path, const std::string& archive_name, archive_file_format::types format, device_params params,uint8_t log_level)
34{
35 if(!boost::filesystem::exists(path))
36 boost::filesystem::create_directories(path);
37
38 archive* archive = archive_write_new();
39
40 std::string filename = archive_name;
41
42 if(format == archive_file_format::ZIP)
43 {
44 filename += ".zip";
45 archive_write_set_format_zip(archive);
46 archive_write_zip_set_compression_store(archive);
47 }
48 else if(format == archive_file_format::TAR)
49 {
50 filename += ".tar";
51 archive_write_set_format_pax_restricted(archive);
52 }
53
54 boost::filesystem::path path_filename = path / filename;
55 path_filename.make_preferred();
56
57 std::cout << "[II] 3dv-client: writing to archive " << path_filename << std::endl;
58
59 archive_write_open_filename(archive, path_filename.string().c_str());
60
61 std::string buffer;
62
63 boost::iostreams::stream<boost::iostreams::back_insert_device<std::string> > file(boost::iostreams::back_inserter(buffer));
64
65 file <<"calibration.u0="<<params.intrinsics.m_u0<<std::endl;
66 file <<"calibration.v0="<<params.intrinsics.m_v0<<std::endl;
67 file <<"calibration.ku="<<params.intrinsics.m_ku<<std::endl;
68 file <<"calibration.kv="<<params.intrinsics.m_kv<<std::endl;
69 file <<"calibration.x="<<params.position.m_x<<std::endl;
70 file <<"calibration.y="<<params.position.m_y<<std::endl;
71 file <<"calibration.z="<<params.position.m_z<<std::endl;
72 file <<"calibration.yaw="<<params.orientation.m_yaw<<std::endl;
73 file <<"calibration.pitch="<<params.orientation.m_pitch<<std::endl;
74 file <<"calibration.roll="<<params.orientation.m_roll<<std::endl;
75 file <<"calibration.baseline="<<params.baseline<<std::endl;
76 file <<"depth mapping.downsample ratio="<<params.downsample<<std::endl;
77 file <<"advanced.detection.area.step="<<params.area_step<<std::endl;
78
79 std::stringstream filename_ini;
80
81 filename_ini << "params.ini";
82
83
84 size_t file_size = buffer.size();
85
86 struct archive_entry* entry;
87
88 entry = archive_entry_new();
89 archive_entry_set_pathname(entry, filename_ini.str().c_str());
90 archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
91 archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
92 archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
93 archive_entry_set_size(entry, file_size);
94 archive_entry_set_filetype(entry, AE_IFREG);
95 archive_entry_set_perm(entry, 0644);
96 archive_write_header(archive, entry);
97 archive_entry_free(entry);
98
99 std::cout << "[II] 3dv-client: writing INI file " << filename_ini << std::endl;
100 archive_write_data(archive, buffer.data(), buffer.size());
101
102
103 return archive;
104}
105
106bool close_archive(archive* archive, uint8_t log_level)
107{
108 archive_write_close(archive);
109 archive_write_free(archive);
110
111 return true;
112}
113
114bool write_image_archive(boost::shared_ptr<const lib3dv::image> image, uint32_t guid, uint8_t guid_type, archive* archive, bool autonumber, uint8_t log_level)
115{
116 if(log_level == 0)
117 {
118 std::cout << '*';
119 std::cout.flush();
120 }
121
122 bool is_mono = image->m_format == lib3dv::image::format::MONO8 ||
123 image->m_format == lib3dv::image::format::MONO16;
124
125 bool is_rgb = image->m_format == lib3dv::image::format::RGB24 ||
126 image->m_format == lib3dv::image::format::RGB48;
127
128 bool is_bayer = image->m_format == lib3dv::image::format::BAYER8_BGGR ||
129 image->m_format == lib3dv::image::format::BAYER8_GBRG ||
130 image->m_format == lib3dv::image::format::BAYER8_GRBG ||
131 image->m_format == lib3dv::image::format::BAYER8_RGGB ||
132 image->m_format == lib3dv::image::format::BAYER16_BGGR ||
133 image->m_format == lib3dv::image::format::BAYER16_GBRG ||
134 image->m_format == lib3dv::image::format::BAYER16_GRBG ||
135 image->m_format == lib3dv::image::format::BAYER16_RGGB;
136
137 bool is_16bpp = (image->m_bpp / image->m_channels) == 16;
138
139 std::stringstream filename;
140
141 switch(image->m_type)
142 {
143 case lib3dv::image::type::LEFT_RECTIFIED :
144 {
145 filename << "left_rectified";
146 break;
147 }
148
149 case lib3dv::image::type::RIGHT_RECTIFIED :
150 {
151 filename << "right_rectified";
152 break;
153 }
154
155 case lib3dv::image::type::LEFT_RAW :
156 {
157 filename << "left_raw";
158 break;
159 }
160
161 case lib3dv::image::type::RIGHT_RAW:
162 {
163 filename << "right_raw";
164 break;
165 }
166
167 case lib3dv::image::type::DSI :
168 {
169 filename << "dsi";
170 break;
171 }
172 }
173
174 boost::posix_time::ptime timestamp = image->m_timestamp;
175
176 if(autonumber)
177 filename << "-" << std::setfill('0') << std::setw(6) << guid / guid_type;
178
179 size_t file_size;
180
181 if(is_bayer)
182 {
183 filename << ".pgm";
184 file_size = is_16bpp ? pgm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << (image->m_bpp / image->m_channels)) - 1)) :
185 pgm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << (image->m_bpp / image->m_channels)) - 1));
186 }
187 else if(is_mono)
188 {
189 filename << ".pgm";
190 file_size = is_16bpp ? pgm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1)) :
191 pgm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
192 }
193 else if(is_rgb)
194 {
195 filename << ".ppm";
196 file_size = is_16bpp ? ppm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1)) :
197 ppm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
198 }
199
200 if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << filename.str() << " (" << image->m_buffer.size() / 1024 << " KB)" << std::endl;
201
202 bool success = false;
203
204 struct archive_entry* entry;
205
206 entry = archive_entry_new();
207 archive_entry_set_pathname(entry, filename.str().c_str());
208 archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
209 archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
210 archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
211 archive_entry_set_size(entry, file_size);
212 archive_entry_set_filetype(entry, AE_IFREG);
213 archive_entry_set_perm(entry, 0644);
214 archive_write_header(archive, entry);
215 archive_entry_free(entry);
216
217 if(is_bayer){
218 success = is_16bpp ? write_pgm(archive, reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp/image->m_channels) - 1))
219 : write_pgm(archive, image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp/image->m_channels) - 1));
220 } else if(is_mono){
221 success = is_16bpp ? write_pgm(archive, reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1))
222 : write_pgm(archive, image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
223 }
224 else if(is_rgb){
225 success = is_16bpp ? write_ppm(archive, reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1))
226 : write_ppm(archive, image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
227 }
228
229 if(!success)
230 std::cerr << "[EE] client::write_file(): unsupported image type" << std::endl;
231
232 return success;
233}
234
235bool write_terrain_archive(boost::shared_ptr<const lib3dv::terrain> terrain, uint32_t guid, uint8_t guid_type, archive* archive, bool autonumber, data_file_format::types format, uint8_t log_level)
236{
237 if(log_level == 0)
238 {
239 std::cout << '.';
240 std::cout.flush();
241 }
242
243 const std::vector<lib3dv::point3>& points = terrain->m_data;
244
245 std::string buffer;
246
247 boost::iostreams::stream<boost::iostreams::back_insert_device<std::string> > ostream(boost::iostreams::back_inserter(buffer));
248
249 switch(format)
250 {
251 case data_file_format::BINARY :
252 {
253 boost::archive::binary_oarchive archive(ostream);
254 archive << *terrain;
255 break;
256 }
257
258 case data_file_format::TEXT :
259 {
260 boost::archive::text_oarchive archive(ostream);
261 archive << *terrain;
262 break;
263 }
264
265 case data_file_format::GNUPLOT :
266 {
267 for(unsigned int i = 0; i < points.size(); ++i)
268 ostream << points[i].m_x << ' ' << points[i].m_y << ' ' << points[i].m_z << std::endl;
269 break;
270 }
271
272 case data_file_format::RAW :
273 default:
274 {
275 ostream.write(reinterpret_cast<const char*>(points.data()), points.size() * sizeof(float));
276 }
277 }
278
279 std::stringstream filename;
280
281 filename << "terrain";
282
283 if(autonumber)
284 filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
285
286 filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));
287
288 size_t file_size = buffer.size();
289
290 struct archive_entry* entry;
291
292 entry = archive_entry_new();
293 archive_entry_set_pathname(entry, filename.str().c_str());
294 archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
295 archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
296 archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
297 archive_entry_set_size(entry, file_size);
298 archive_entry_set_filetype(entry, AE_IFREG);
299 archive_entry_set_perm(entry, 0644);
300 archive_write_header(archive, entry);
301 archive_entry_free(entry);
302
303 archive_write_data(archive, buffer.data(), buffer.size());
304
305 if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << filename << " (" << file_size / 1024 << " KB)" << std::endl;
306
307 return true;
308}
309
310bool write_obstacles_archive(boost::shared_ptr<const std::vector<lib3dv::obstacle> > obstacles, uint32_t guid, uint8_t guid_type, archive* archive, bool autonumber, data_file_format::types format, uint8_t log_level)
311{
312 if(log_level == 0)
313 {
314 std::cout << '.';
315 std::cout.flush();
316 }
317
318 std::string buffer;
319
320 boost::iostreams::stream<boost::iostreams::back_insert_device<std::string> > ostream(boost::iostreams::back_inserter(buffer));
321
322 switch(format)
323 {
324 case data_file_format::BINARY :
325 {
326 boost::archive::binary_oarchive archive(ostream);
327 archive << *obstacles;
328 break;
329 }
330
331 case data_file_format::TEXT :
332 {
333 boost::archive::text_oarchive archive(ostream);
334 archive << *obstacles;
335 break;
336 }
337
338 case data_file_format::GNUPLOT :
339 {
340 for(unsigned int o = 0; o < obstacles->size(); ++o)
341 {
342 uint32_t guid = (*obstacles)[o].m_guid;
343 const std::vector<lib3dv::stixel>& stixels = (*obstacles)[o].m_data;
344
345 for(unsigned int s = 0; s < stixels.size(); ++s)
346 {
347 ostream << stixels[s].m_x << ' ' << stixels[s].m_y << ' ' << stixels[s].m_z << ' ' << guid << std::endl;
348 ostream << stixels[s].m_x << ' ' << stixels[s].m_y << ' ' << stixels[s].m_z + stixels[s].m_height << ' ' << guid << std::endl;
349 ostream << std::endl << std::endl;
350 }
351 }
352
353 break;
354 }
355
356 case data_file_format::RAW :
357 default:
358 {
359 for(unsigned int o = 0; o < obstacles->size(); ++o)
360 {
361 uint32_t guid = (*obstacles)[o].m_guid;
362 uint16_t stixel_num = (*obstacles)[o].m_data.size();
363
364 ostream.write(reinterpret_cast<const char*>(&guid), sizeof(guid));
365 ostream.write(reinterpret_cast<const char*>(&stixel_num), sizeof(stixel_num));
366 ostream.write(reinterpret_cast<const char*>((*obstacles)[o].m_data.data()), sizeof(lib3dv::stixel) * stixel_num);
367 }
368 }
369 }
370
371 std::stringstream filename;
372
373 filename << "obstacles";
374
375 if(autonumber)
376 filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
377
378 filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));
379
380 size_t file_size = buffer.size();
381
382 struct archive_entry* entry;
383
384 entry = archive_entry_new();
385 archive_entry_set_pathname(entry, filename.str().c_str());
386 archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
387 archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
388 archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
389 archive_entry_set_size(entry, file_size);
390 archive_entry_set_filetype(entry, AE_IFREG);
391 archive_entry_set_perm(entry, 0644);
392 archive_write_header(archive, entry);
393 archive_entry_free(entry);
394
395 archive_write_data(archive, buffer.data(), buffer.size());
396
397 if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << filename << " (" << file_size / 1024 << " KB)" << std::endl;
398
399 return true;
400}
401
402bool write_motion_archive(boost::shared_ptr<const lib3dv::motion> motion, uint32_t guid, uint8_t guid_type, archive* archive, bool autonumber, data_file_format::types format, uint8_t log_level)
403{
404 if(log_level == 0)
405 {
406 std::cout << '.';
407 std::cout.flush();
408 }
409
410 std::string buffer;
411
412 boost::iostreams::stream<boost::iostreams::back_insert_device<std::string> > ostream(boost::iostreams::back_inserter(buffer));
413
414 switch(format)
415 {
416 case data_file_format::BINARY :
417 {
418 boost::archive::binary_oarchive archive(ostream);
419 archive << *motion;
420 break;
421 }
422
423 case data_file_format::TEXT :
424 {
425 boost::archive::text_oarchive archive(ostream);
426 archive << *motion;
427 break;
428 }
429
430 case data_file_format::GNUPLOT :
431 {
432 for(unsigned int p = 0; p < motion->m_poses.size(); ++p)
433 {
434 const std::vector<lib3dv::pose>& poses = motion->m_poses;
435 std::string type;
436
437 if (poses[p].m_type == lib3dv::pose::type::CURRENT_POSE)
438 type = "CURRENT";
439 else if (poses[p].m_type == lib3dv::pose::type::RELATIVE_POSE)
440 type = "RELATIVE";
441
442 ostream << type << ' ' << poses[p].m_timestamp << std::endl;
443 for(unsigned int i = 0; i < poses[p].m_data.size(); ++i)
444 {
445 ostream << poses[p].m_data[i] << ' ';
446 if (i>0 && i%4==3) ostream << std::endl;
447 }
448 ostream << std::endl << std::endl;
449 }
450
451 break;
452 }
453
454 case data_file_format::RAW :
455 default:
456 {
457 for(unsigned int p = 0; p < motion->m_poses.size(); ++p)
458 {
459 boost::posix_time::time_duration td = (*motion).m_poses[p].m_timestamp - boost::posix_time::ptime(boost::gregorian::date(1970,1,1));
460 int64_t timestamp = td.total_microseconds();
461 uint8_t type = (*motion).m_poses[p].m_type;
462 uint16_t pose_num = (*motion).m_poses[p].m_data.size();
463
464 ostream.write(reinterpret_cast<const char*>(&timestamp), sizeof(timestamp));
465 ostream.write(reinterpret_cast<const char*>(&type), sizeof(type));
466 ostream.write(reinterpret_cast<const char*>(&pose_num), sizeof(pose_num));
467 ostream.write(reinterpret_cast<const char*>((*motion).m_poses[p].m_data.data()), sizeof(lib3dv::pose) * pose_num);
468 }
469 }
470 }
471
472 std::stringstream filename;
473
474 filename << "motion";
475
476 if(autonumber)
477 filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
478
479 filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));
480
481 size_t file_size = buffer.size();
482
483 struct archive_entry* entry;
484
485 entry = archive_entry_new();
486 archive_entry_set_pathname(entry, filename.str().c_str());
487 archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
488 archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
489 archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
490 archive_entry_set_size(entry, file_size);
491 archive_entry_set_filetype(entry, AE_IFREG);
492 archive_entry_set_perm(entry, 0644);
493 archive_write_header(archive, entry);
494 archive_entry_free(entry);
495
496 archive_write_data(archive, buffer.data(), buffer.size());
497
498 if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << filename << " (" << file_size / 1024 << " KB)" << std::endl;
499
500 return true;
501}
502
503bool write_classification_archive(boost::shared_ptr<const lib3dv::classification> classificat, uint32_t guid, uint8_t guid_type, archive* archive, bool autonumber, data_file_format::types format, uint8_t log_level)
504{
505 if(log_level == 0)
506 {
507 std::cout << '.';
508 std::cout.flush();
509 }
510
511 std::string buffer;
512
513 boost::iostreams::stream<boost::iostreams::back_insert_device<std::string> > ostream(boost::iostreams::back_inserter(buffer));
514
515 switch(format)
516 {
517 case data_file_format::BINARY :
518 {
519 boost::archive::binary_oarchive archive(ostream);
520 archive << *classificat;
521 break;
522 }
523
524 case data_file_format::TEXT :
525 {
526 boost::archive::text_oarchive archive(ostream);
527 archive << *classificat;
528 break;
529 }
530
531 case data_file_format::GNUPLOT :
532 {
533 break;
534 }
535
536 case data_file_format::RAW :
537 default:
538 {
539 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates), sizeof(classificat->m_candidates));
540 for(unsigned int p = 0; p < classificat->m_candidates.size(); ++p)
541 {
542
543 uint32_t guid = classificat->m_candidates[p].m_guid;
544
545 ostream.write(reinterpret_cast<const char*>(&guid), sizeof(guid));
546 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_category), sizeof(classificat->m_candidates[p].m_category));
547 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_confidence), sizeof(classificat->m_candidates[p].m_confidence));
548 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_x0), sizeof(classificat->m_candidates[p].m_x0));
549 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_y0), sizeof(classificat->m_candidates[p].m_y0));
550 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_x1), sizeof(classificat->m_candidates[p].m_x1));
551 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_y1), sizeof(classificat->m_candidates[p].m_y1));
552 }
553
554 }
555 }
556
557 std::stringstream filename;
558
559 filename << "motion";
560
561 if(autonumber)
562 filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
563
564 filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));
565
566 size_t file_size = buffer.size();
567
568 struct archive_entry* entry;
569
570 entry = archive_entry_new();
571 archive_entry_set_pathname(entry, filename.str().c_str());
572 archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
573 archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
574 archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
575 archive_entry_set_size(entry, file_size);
576 archive_entry_set_filetype(entry, AE_IFREG);
577 archive_entry_set_perm(entry, 0644);
578 archive_write_header(archive, entry);
579 archive_entry_free(entry);
580
581 archive_write_data(archive, buffer.data(), buffer.size());
582
583 if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << filename << " (" << file_size / 1024 << " KB)" << std::endl;
584
585 return true;
586}
587#endif
588
589bool write_image_file(boost::shared_ptr<const lib3dv::image> image, uint32_t guid, uint8_t guid_type ,const boost::filesystem::path& path, bool autonumber, uint8_t log_level)
590{
591 if(log_level == 0)
592 {
593 std::cout << '*';
594 std::cout.flush();
595 }
596
597 bool is_mono = image->m_format == lib3dv::image::format::MONO8 ||
598 image->m_format == lib3dv::image::format::MONO16;
599
600 bool is_rgb = image->m_format == lib3dv::image::format::RGB24 ||
601 image->m_format == lib3dv::image::format::RGB48;
602
603 bool is_bayer = image->m_format == lib3dv::image::format::BAYER8_BGGR ||
604 image->m_format == lib3dv::image::format::BAYER8_GBRG ||
605 image->m_format == lib3dv::image::format::BAYER8_GRBG ||
606 image->m_format == lib3dv::image::format::BAYER8_RGGB ||
607 image->m_format == lib3dv::image::format::BAYER16_BGGR ||
608 image->m_format == lib3dv::image::format::BAYER16_GBRG ||
609 image->m_format == lib3dv::image::format::BAYER16_GRBG ||
610 image->m_format == lib3dv::image::format::BAYER16_RGGB;
611
612 bool is_16bpp = (image->m_bpp / image->m_channels) == 16;
613
614 std::stringstream filename;
615
616 if(!boost::filesystem::exists(path))
617 {
618 std::cout << "[II] 3dv-client: writing to " << path << std::endl;
619 boost::filesystem::create_directories(path);
620 }
621
622 switch(image->m_type)
623 {
624 case lib3dv::image::type::LEFT_RECTIFIED :
625 {
626 filename << "left_rectified";
627 break;
628 }
629
630 case lib3dv::image::type::RIGHT_RECTIFIED :
631 {
632 filename << "right_rectified";
633 break;
634 }
635
636 case lib3dv::image::type::LEFT_RAW :
637 {
638 filename << "left_raw";
639 break;
640 }
641
642 case lib3dv::image::type::RIGHT_RAW:
643 {
644 filename << "right_raw";
645 break;
646 }
647
648 case lib3dv::image::type::DSI :
649 {
650 filename << "dsi";
651 break;
652 }
653 }
654
655 boost::posix_time::ptime timestamp = image->m_timestamp;
656
657 if(autonumber)
658 filename << "-" << std::setfill('0') << std::setw(6) << guid / guid_type;
659
660 boost::filesystem::path path_filename = path / filename.str();
661 path_filename.make_preferred();
662
663 if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << path_filename.string() << ((is_mono || is_bayer) ? ".pgm" : ".ppm") << " (" << image->m_buffer.size() / 1024 << " KB)" << std::endl;
664
665 bool success = false;
666
667 if(is_bayer){
668 success = is_16bpp ? write_pgm(path_filename.string()+".pgm", reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << (image->m_bpp/image->m_channels)) - 1))
669 : write_pgm(path_filename.string()+".pgm", image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << (image->m_bpp/image->m_channels)) - 1));
670 }
671 else if(is_mono){
672 success = is_16bpp ? write_pgm(path_filename.string()+".pgm", reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1))
673 : write_pgm(path_filename.string()+".pgm", image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
674 }
675 else if(is_rgb){
676 success = is_16bpp ? write_ppm(path_filename.string()+".ppm", reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1))
677 : write_ppm(path_filename.string()+".ppm", image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
678 }
679
680 if(!success)
681 std::cerr << "[EE] client::write_file(): unsupported image type" << std::endl;
682
683 return success;
684}
685
686bool write_terrain_file(boost::shared_ptr<const lib3dv::terrain> terrain, uint32_t guid, uint8_t guid_type, const boost::filesystem::path& path, bool autonumber, data_file_format::types format, uint8_t log_level)
687{
688 if(log_level == 0)
689 {
690 std::cout << '.';
691 std::cout.flush();
692 }
693
694 const std::vector<lib3dv::point3>& points = terrain->m_data;
695
696 if(!boost::filesystem::exists(path))
697 {
698 std::cout << "[II] 3dv-client: writing to " << path << std::endl;
699 boost::filesystem::create_directories(path);
700 }
701
702 std::stringstream filename;
703
704 filename << "terrain";
705
706 if(autonumber)
707 filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
708
709 filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));
710
711 boost::filesystem::path path_filename = path / filename.str();
712 path_filename.make_preferred();
713
714 std::ofstream file(path_filename.string().c_str(), std::ios::out);
715
716 if(file)
717 {
718 switch(format)
719 {
720 case data_file_format::BINARY :
721 {
722 boost::archive::binary_oarchive archive(file);
723 archive << *terrain;
724 break;
725 }
726
727 case data_file_format::TEXT :
728 {
729 boost::archive::text_oarchive archive(file);
730 archive << *terrain;
731 break;
732 }
733
734 case data_file_format::GNUPLOT :
735 {
736 for(unsigned int i = 0; i < points.size(); ++i)
737 file << points[i].m_x << ' ' << points[i].m_y << ' ' << points[i].m_z << std::endl;
738 break;
739 }
740
741 case data_file_format::RAW :
742 default:
743 {
744 file.write(reinterpret_cast<const char*>(points.data()), points.size() * sizeof(float));
745 }
746 }
747
748 if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << path_filename.string() << " (" << file.tellp() / 1024 << " KB)" << std::endl;
749
750 return true;
751 }
752
753 return false;
754}
755
756bool write_obstacles_file(boost::shared_ptr<const std::vector<lib3dv::obstacle> > obstacles, uint32_t guid, uint8_t guid_type, const boost::filesystem::path& path, bool autonumber, data_file_format::types format, uint8_t log_level)
757{
758 if(log_level == 0)
759 {
760 std::cout << '.';
761 std::cout.flush();
762 }
763
764 if(!boost::filesystem::exists(path))
765 {
766 std::cout << "[II] 3dv-client: writing to " << path << std::endl;
767 boost::filesystem::create_directories(path);
768 }
769
770 std::stringstream filename;
771
772 filename << "obstacles";
773
774 if(autonumber)
775 filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
776
777 filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));
778
779 boost::filesystem::path path_filename = path / filename.str();
780 path_filename.make_preferred();
781
782 std::ofstream file(path_filename.string().c_str(), std::ios::out);
783
784 if(file)
785 {
786 switch(format)
787 {
788 case data_file_format::BINARY :
789 {
790 boost::archive::binary_oarchive archive(file);
791 archive << *obstacles;
792 break;
793 }
794
795 case data_file_format::TEXT :
796 {
797 boost::archive::text_oarchive archive(file);
798 archive << *obstacles;
799 break;
800 }
801
802 case data_file_format::GNUPLOT :
803 {
804 for(unsigned int o = 0; o < obstacles->size(); ++o)
805 {
806 uint32_t guid = (*obstacles)[o].m_guid;
807 const std::vector<lib3dv::stixel>& stixels = (*obstacles)[o].m_data;
808
809 for(unsigned int s = 0; s < stixels.size(); ++s)
810 {
811 file << stixels[s].m_x << ' ' << stixels[s].m_y << ' ' << stixels[s].m_z << ' ' << guid << std::endl;
812 file << stixels[s].m_x << ' ' << stixels[s].m_y << ' ' << stixels[s].m_z + stixels[s].m_height << ' ' << guid << std::endl;
813 file << std::endl << std::endl;
814 }
815 }
816
817 break;
818 }
819
820 case data_file_format::RAW :
821 default:
822 {
823 for(unsigned int o = 0; o < obstacles->size(); ++o)
824 {
825 uint32_t guid = (*obstacles)[o].m_guid;
826 uint16_t stixel_num = (*obstacles)[o].m_data.size();
827
828 file.write(reinterpret_cast<const char*>(&guid), sizeof(guid));
829 file.write(reinterpret_cast<const char*>(&stixel_num), sizeof(stixel_num));
830 file.write(reinterpret_cast<const char*>((*obstacles)[o].m_data.data()), sizeof(lib3dv::stixel) * stixel_num);
831 }
832 }
833 }
834
835 if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << path_filename.string() << " (" << file.tellp() / 1024 << " KB)" << std::endl;
836
837 return true;
838 }
839
840 return false;
841}
842
843bool write_motion_file(boost::shared_ptr<const lib3dv::motion> motion, uint32_t guid, uint8_t guid_type, const boost::filesystem::path& path, bool autonumber, data_file_format::types format, uint8_t log_level)
844{
845 if(log_level == 0)
846 {
847 std::cout << '.';
848 std::cout.flush();
849 }
850
851 if(!boost::filesystem::exists(path))
852 {
853 std::cout << "[II] 3dv-client: writing to " << path << std::endl;
854 boost::filesystem::create_directories(path);
855 }
856
857 std::stringstream filename;
858
859 filename << "motion";
860
861 if(autonumber)
862 filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
863
864 filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));
865
866 boost::filesystem::path path_filename = path / filename.str();
867 path_filename.make_preferred();
868
869 std::ofstream file(path_filename.string().c_str(), std::ios::out);
870
871 if(file)
872 {
873 switch(format)
874 {
875 case data_file_format::BINARY :
876 {
877 boost::archive::binary_oarchive archive(file);
878 archive << *motion;
879 break;
880 }
881
882 case data_file_format::TEXT :
883 {
884 boost::archive::text_oarchive archive(file);
885 archive << *motion;
886 break;
887 }
888
889 case data_file_format::GNUPLOT :
890 {
891 for(unsigned int p = 0; p < motion->m_poses.size(); ++p)
892 {
893 const std::vector<lib3dv::pose>& poses = motion->m_poses;
894 std::string type;
895
896 if (poses[p].m_type == lib3dv::pose::type::CURRENT_POSE)
897 type = "CURRENT";
898 else if (poses[p].m_type == lib3dv::pose::type::RELATIVE_POSE)
899 type = "RELATIVE";
900
901 file << type << ' ' << poses[p].m_timestamp << std::endl;
902 for(unsigned int i = 0; i < poses[p].m_data.size(); ++i)
903 {
904 file << poses[p].m_data[i] << ' ';
905 if (i>0 && i%4==3) file << std::endl;
906 }
907 file << std::endl << std::endl;
908 }
909
910 break;
911 }
912
913 case data_file_format::RAW :
914 default:
915 {
916 for(unsigned int p = 0; p < motion->m_poses.size(); ++p)
917 {
918
919 boost::posix_time::time_duration td = (*motion).m_poses[p].m_timestamp - boost::posix_time::ptime(boost::gregorian::date(1970,1,1));
920 int64_t timestamp = td.total_microseconds();
921 uint8_t type = motion->m_poses[p].m_type;
922 uint16_t pose_num = motion->m_poses[p].m_data.size();
923
924 file.write(reinterpret_cast<const char*>(&timestamp), sizeof(timestamp));
925 file.write(reinterpret_cast<const char*>(&type), sizeof(type));
926 file.write(reinterpret_cast<const char*>(&pose_num), sizeof(pose_num));
927 file.write(reinterpret_cast<const char*>((*motion).m_poses[p].m_data.data()), sizeof(lib3dv::pose) * pose_num);
928 }
929 }
930 }
931
932 if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << path_filename.string() << " (" << file.tellp() / 1024 << " KB)" << std::endl;
933
934 return true;
935 }
936
937 return false;
938}
939
940bool write_classification_file(boost::shared_ptr<const lib3dv::classification> classificat, uint32_t guid, uint8_t guid_type, const boost::filesystem::path& path, bool autonumber, data_file_format::types format, uint8_t log_level)
941{
942
943 //std::cout << "classification size" <<classificat->m_candidates.size()<<std::endl;
944 if(log_level == 0)
945 {
946 std::cout << '.';
947 std::cout.flush();
948 }
949
950 if(!boost::filesystem::exists(path))
951 {
952 std::cout << "[II] 3dv-client: writing to " << path << std::endl;
953 boost::filesystem::create_directories(path);
954 }
955
956 std::stringstream filename;
957
958 filename << "classification";
959
960 if(autonumber)
961 filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
962
963 filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));
964
965 boost::filesystem::path path_filename = path / filename.str();
966 path_filename.make_preferred();
967
968 std::ofstream file(path_filename.string().c_str(), std::ios::out);
969
970 if(file)
971 {
972 switch(format)
973 {
974 case data_file_format::BINARY :
975 {
976 boost::archive::binary_oarchive archive(file);
977 archive << *classificat;
978 break;
979 }
980
981 case data_file_format::TEXT :
982 {
983 boost::archive::text_oarchive archive(file);
984 archive << *classificat;
985 break;
986 }
987
988 case data_file_format::GNUPLOT :
989 {
990 break;
991 }
992
993 case data_file_format::RAW :
994 default:
995 {
996 file.write(reinterpret_cast<const char*>(&classificat->m_candidates), sizeof(classificat->m_candidates));
997 for(unsigned int p = 0; p < classificat->m_candidates.size(); ++p)
998 {
999
1000 uint32_t guid = classificat->m_candidates[p].m_guid;
1001
1002 file.write(reinterpret_cast<const char*>(&guid), sizeof(guid));
1003 file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_category), sizeof(classificat->m_candidates[p].m_category));
1004 file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_confidence), sizeof(classificat->m_candidates[p].m_confidence));
1005 file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_x0), sizeof(classificat->m_candidates[p].m_x0));
1006 file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_y0), sizeof(classificat->m_candidates[p].m_y0));
1007 file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_x1), sizeof(classificat->m_candidates[p].m_x1));
1008 file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_y1), sizeof(classificat->m_candidates[p].m_y1));
1009 }
1010 }
1011 }
1012
1013 if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << path_filename.string() << " (" << file.tellp() / 1024 << " KB)" << std::endl;
1014
1015 return true;
1016 }
1017
1018 return false;
1019}
1020
Note: See TracBrowser for help on using the repository browser.