serialization.h
Go to the documentation of this file.
1 #ifndef RTABMAP_SERIALIZATION_H_
2 #define RTABMAP_SERIALIZATION_H_
3 
4 #include <vector>
5 #include <map>
6 #include <cstdlib>
7 #include <cstring>
8 #include <stdio.h>
9 #include "rtflann/ext/lz4.h"
10 #include "rtflann/ext/lz4hc.h"
11 
12 
13 namespace rtflann
14 {
16  char signature[24];
17  char version[16];
20  size_t rows;
21  size_t cols;
22  size_t compression;
24  };
25 
26 namespace serialization
27 {
28 
29 struct access
30 {
31  template<typename Archive, typename T>
32  static inline void serialize(Archive& ar, T& type)
33  {
34  type.serialize(ar);
35  }
36 };
37 
38 
39 template<typename Archive, typename T>
40 inline void serialize(Archive& ar, T& type)
41 {
42  access::serialize(ar,type);
43 }
44 
45 template<typename T>
46 struct Serializer
47 {
48  template<typename InputArchive>
49  static inline void load(InputArchive& ar, T& val)
50  {
52  }
53  template<typename OutputArchive>
54  static inline void save(OutputArchive& ar, const T& val)
55  {
56  serialization::serialize(ar,const_cast<T&>(val));
57  }
58 };
59 
60 #define BASIC_TYPE_SERIALIZER(type)\
61 template<> \
62 struct Serializer<type> \
63 {\
64  template<typename InputArchive>\
65  static inline void load(InputArchive& ar, type& val)\
66  {\
67  ar.load(val);\
68  }\
69  template<typename OutputArchive>\
70  static inline void save(OutputArchive& ar, const type& val)\
71  {\
72  ar.save(val);\
73  }\
74 }
75 
76 #define ENUM_SERIALIZER(type)\
77 template<>\
78 struct Serializer<type>\
79 {\
80  template<typename InputArchive>\
81  static inline void load(InputArchive& ar, type& val)\
82  {\
83  int int_val;\
84  ar & int_val;\
85  val = (type) int_val;\
86  }\
87  template<typename OutputArchive>\
88  static inline void save(OutputArchive& ar, const type& val)\
89  {\
90  int int_val = (int)val;\
91  ar & int_val;\
92  }\
93 }
94 
95 
96 // declare serializers for simple types
98 BASIC_TYPE_SERIALIZER(unsigned char);
100 BASIC_TYPE_SERIALIZER(unsigned short);
102 BASIC_TYPE_SERIALIZER(unsigned int);
104 BASIC_TYPE_SERIALIZER(unsigned long);
105 BASIC_TYPE_SERIALIZER(unsigned long long);
106 BASIC_TYPE_SERIALIZER(float);
107 BASIC_TYPE_SERIALIZER(double);
109 #ifdef _MSC_VER
110 // unsigned __int64 ~= unsigned long long
111 // Will throw error on VS2013
112 #if _MSC_VER != 1800
113 //BASIC_TYPE_SERIALIZER(unsigned __int64);
114 #endif
115 #endif
116 
117 
118 // serializer for std::vector
119 template<typename T>
120 struct Serializer<std::vector<T> >
121 {
122  template<typename InputArchive>
123  static inline void load(InputArchive& ar, std::vector<T>& val)
124  {
125  size_t size;
126  ar & size;
127  val.resize(size);
128  for (size_t i=0;i<size;++i) {
129  ar & val[i];
130  }
131  }
132 
133  template<typename OutputArchive>
134  static inline void save(OutputArchive& ar, const std::vector<T>& val)
135  {
136  ar & val.size();
137  for (size_t i=0;i<val.size();++i) {
138  ar & val[i];
139  }
140  }
141 };
142 
143 // serializer for std::vector
144 template<typename K, typename V>
145 struct Serializer<std::map<K,V> >
146 {
147  template<typename InputArchive>
148  static inline void load(InputArchive& ar, std::map<K,V>& map_val)
149  {
150  size_t size;
151  ar & size;
152  for (size_t i = 0; i < size; ++i)
153  {
154  K key;
155  ar & key;
156  V value;
157  ar & value;
158  map_val[key] = value;
159  }
160  }
161 
162  template<typename OutputArchive>
163  static inline void save(OutputArchive& ar, const std::map<K,V>& map_val)
164  {
165  ar & map_val.size();
166  for (typename std::map<K,V>::const_iterator i=map_val.begin(); i!=map_val.end(); ++i) {
167  ar & i->first;
168  ar & i->second;
169  }
170  }
171 };
172 
173 template<typename T>
174 struct Serializer<T*>
175 {
176  template<typename InputArchive>
177  static inline void load(InputArchive& ar, T*& val)
178  {
179  ar.load(val);
180  }
181 
182  template<typename OutputArchive>
183  static inline void save(OutputArchive& ar, T* const& val)
184  {
185  ar.save(val);
186  }
187 };
188 
189 template<typename T, int N>
190 struct Serializer<T[N]>
191 {
192  template<typename InputArchive>
193  static inline void load(InputArchive& ar, T (&val)[N])
194  {
195  ar.load(val);
196  }
197 
198  template<typename OutputArchive>
199  static inline void save(OutputArchive& ar, T const (&val)[N])
200  {
201  ar.save(val);
202  }
203 };
204 
205 
206 
207 
209 {
210  void const * ptr_;
211  size_t size_;
212 
213  binary_object( void * const ptr, size_t size) :
214  ptr_(ptr),
215  size_(size)
216  {}
218  ptr_(rhs.ptr_),
219  size_(rhs.size_)
220  {}
221 
223  ptr_ = rhs.ptr_;
224  size_ = rhs.size_;
225  return *this;
226  }
227 };
228 
229 inline const binary_object make_binary_object(/* const */ void * t, size_t size){
230  return binary_object(t, size);
231 }
232 
233 template<>
235 {
236  template<typename InputArchive>
237  static inline void load(InputArchive& ar, const binary_object& b)
238  {
239  ar.load_binary(const_cast<void *>(b.ptr_), b.size_);
240  }
241 
242  template<typename OutputArchive>
243  static inline void save(OutputArchive& ar, const binary_object& b)
244  {
245  ar.save_binary(b.ptr_, b.size_);
246  }
247 };
248 
249 template<>
251 {
252  template<typename InputArchive>
253  static inline void load(InputArchive& ar, binary_object& b)
254  {
255  ar.load_binary(const_cast<void *>(b.ptr_), b.size_);
256  }
257 
258  template<typename OutputArchive>
259  static inline void save(OutputArchive& ar, const binary_object& b)
260  {
261  ar.save_binary(b.ptr_, b.size_);
262  }
263 };
264 
265 
266 
267 template <bool C_>
268 struct bool_ {
269  static const bool value = C_;
270  typedef bool value_type;
271 };
272 
273 
275 {
276 public:
277  void* getObject() { return object_; }
278 
279  void setObject(void* object) { object_ = object; }
280 
281 private:
282  void* object_;
283 };
284 
285 
286 template<typename Archive>
287 class InputArchive : public ArchiveBase
288 {
289 protected:
291 public:
292  typedef bool_<true> is_loading;
294 
295  template<typename T>
296  Archive& operator& (T& val)
297  {
298  Serializer<T>::load(*static_cast<Archive*>(this),val);
299  return *static_cast<Archive*>(this);
300  }
301 };
302 
303 
304 template<typename Archive>
306 {
307 protected:
309 public:
310  typedef bool_<false> is_loading;
312 
313  template<typename T>
314  Archive& operator& (const T& val)
315  {
316  Serializer<T>::save(*static_cast<Archive*>(this),val);
317  return *static_cast<Archive*>(this);
318  }
319 };
320 
321 
322 
323 class SizeArchive : public OutputArchive<SizeArchive>
324 {
325  size_t size_;
326 public:
327 
328  SizeArchive() : size_(0)
329  {
330  }
331 
332  template<typename T>
333  void save(const T& val)
334  {
335  size_ += sizeof(val);
336  }
337 
338  template<typename T>
339  void save_binary(T* ptr, size_t size)
340  {
341  size_ += size;
342  }
343 
344 
345  void reset()
346  {
347  size_ = 0;
348  }
349 
350  size_t size()
351  {
352  return size_;
353  }
354 };
355 
356 
357 //
358 //class PrintArchive : public OutputArchive<PrintArchive>
359 //{
360 //public:
361 // template<typename T>
362 // void save(const T& val)
363 // {
364 // std::cout << val << std::endl;
365 // }
366 //
367 // template<typename T>
368 // void save_binary(T* ptr, size_t size)
369 // {
370 // std::cout << "<binary object>" << std::endl;
371 // }
372 //};
373 
374 #define BLOCK_BYTES (1024 * 64)
375 
376 class SaveArchive : public OutputArchive<SaveArchive>
377 {
383  FILE* stream_;
385  char *buffer_;
386  size_t offset_;
387 
393 
394  void initBlock()
395  {
396  // Alloc the space for both buffer blocks (each compressed block
397  // references the previous)
398  buffer_ = buffer_blocks_ = (char *)malloc(BLOCK_BYTES*2);
399  compressed_buffer_ = (char *)malloc(LZ4_COMPRESSBOUND(BLOCK_BYTES) + sizeof(size_t));
400  if (buffer_ == NULL || compressed_buffer_ == NULL) {
401  throw FLANNException("Error allocating compression buffer");
402  }
403 
404  // Init the LZ4 stream
405  lz4Stream = &lz4Stream_body;
406  LZ4_resetStreamHC(lz4Stream, 9);
407  first_block_ = true;
408 
409  offset_ = 0;
410  }
411 
412  void flushBlock()
413  {
414  size_t compSz = 0;
415  // Handle header
416  if (first_block_) {
417  // Copy & set the header
418  IndexHeaderStruct *head = (IndexHeaderStruct *)buffer_;
419  size_t headSz = sizeof(IndexHeaderStruct);
420 
421  assert(head->compression == 0);
422  head->compression = 1; // Bool now, enum later
423 
424  // Do the compression for the block
425  compSz = LZ4_compress_HC_continue(
426  lz4Stream, buffer_+headSz, compressed_buffer_+headSz, offset_-headSz,
428 
429  if(compSz <= 0) {
430  throw FLANNException("Error compressing (first block)");
431  }
432 
433  // Handle header
434  head->first_block_size = compSz;
435  memcpy(compressed_buffer_, buffer_, headSz);
436 
437  compSz += headSz;
438  first_block_ = false;
439  } else {
440  size_t headSz = sizeof(compSz);
441 
442  // Do the compression for the block
443  compSz = LZ4_compress_HC_continue(
444  lz4Stream, buffer_, compressed_buffer_+headSz, offset_,
446 
447  if(compSz <= 0) {
448  throw FLANNException("Error compressing");
449  }
450 
451  // Save the size of the compressed block as the header
452  memcpy(compressed_buffer_, &compSz, headSz);
453  compSz += headSz;
454  }
455 
456  // Write the compressed buffer
457  fwrite(compressed_buffer_, compSz, 1, stream_);
458 
459  // Switch the buffer to the *other* block
460  if (buffer_ == buffer_blocks_)
461  buffer_ = &buffer_blocks_[BLOCK_BYTES];
462  else
463  buffer_ = buffer_blocks_;
464  offset_ = 0;
465  }
466 
467  void endBlock()
468  {
469  // Cleanup memory
470  free(buffer_blocks_);
471  buffer_blocks_ = NULL;
472  buffer_ = NULL;
473  free(compressed_buffer_);
474  compressed_buffer_ = NULL;
475 
476  // Write a '0' size for next block
477  size_t z = 0;
478  fwrite(&z, sizeof(z), 1, stream_);
479  }
480 
481 public:
482  SaveArchive(const char* filename)
483  {
484  stream_ = fopen(filename, "wb");
485  own_stream_ = true;
486  initBlock();
487  }
488 
489  SaveArchive(FILE* stream) : stream_(stream), own_stream_(false)
490  {
491  initBlock();
492  }
493 
495  {
496  flushBlock();
497  endBlock();
498  if (buffer_) {
499  free(buffer_);
500  buffer_ = NULL;
501  }
502  if (own_stream_) {
503  fclose(stream_);
504  }
505  }
506 
507  template<typename T>
508  void save(const T& val)
509  {
510  assert(sizeof(val) < BLOCK_BYTES);
511  if (offset_+sizeof(val) > BLOCK_BYTES)
512  flushBlock();
513  memcpy(buffer_+offset_, &val, sizeof(val));
514  offset_ += sizeof(val);
515  }
516 
517  template<typename T>
518  void save(T* const& val)
519  {
520  // don't save pointers
521  //fwrite(&val, sizeof(val), 1, handle_);
522  }
523 
524  template<typename T>
525  void save_binary(T* ptr, size_t size)
526  {
527  while (size > BLOCK_BYTES) {
528  // Flush existing block
529  flushBlock();
530 
531  // Save large chunk
532  memcpy(buffer_, ptr, BLOCK_BYTES);
533  offset_ += BLOCK_BYTES;
534  ptr = ((char *)ptr) + BLOCK_BYTES;
535  size -= BLOCK_BYTES;
536  }
537 
538  // Save existing block if new data will make it too big
539  if (offset_+size > BLOCK_BYTES)
540  flushBlock();
541 
542  // Copy out requested data
543  memcpy(buffer_+offset_, ptr, size);
544  offset_ += size;
545  }
546 
547 };
548 
549 
550 class LoadArchive : public InputArchive<LoadArchive>
551 {
557  FILE* stream_;
559  char *buffer_;
560  char *ptr_;
561 
566  size_t block_sz_;
567 
568  void decompressAndLoadV10(FILE* stream)
569  {
570  buffer_ = NULL;
571 
572  // Find file size
573  size_t pos = ftell(stream);
574  fseek(stream, 0, SEEK_END);
575  size_t fileSize = ftell(stream)-pos;
576  fseek(stream, pos, SEEK_SET);
577  size_t headSz = sizeof(IndexHeaderStruct);
578 
579  // Read the (compressed) file to a buffer
580  char *compBuffer = (char *)malloc(fileSize);
581  if (compBuffer == NULL) {
582  throw FLANNException("Error allocating file buffer space");
583  }
584  if (fread(compBuffer, fileSize, 1, stream) != 1) {
585  free(compBuffer);
586  throw FLANNException("Invalid index file, cannot read from disk (compressed)");
587  }
588 
589  // Extract header
590  IndexHeaderStruct *head = (IndexHeaderStruct *)(compBuffer);
591 
592  // Backward compatability
593  size_t compressedSz = fileSize-headSz;
594  size_t uncompressedSz = head->first_block_size-headSz;
595 
596  // Check for compression type
597  if (head->compression != 1) {
598  free(compBuffer);
599  throw FLANNException("Compression type not supported");
600  }
601 
602  // Allocate a decompressed buffer
603  ptr_ = buffer_ = (char *)malloc(uncompressedSz+headSz);
604  if (buffer_ == NULL) {
605  free(compBuffer);
606  throw FLANNException("Error (re)allocating decompression buffer");
607  }
608 
609  // Extract body
610  size_t usedSz = LZ4_decompress_safe(compBuffer+headSz,
611  buffer_+headSz,
612  compressedSz,
613  uncompressedSz);
614 
615  // Check if the decompression was the expected size.
616  if (usedSz != uncompressedSz) {
617  free(compBuffer);
618  throw FLANNException("Unexpected decompression size");
619  }
620 
621  // Copy header data
622  memcpy(buffer_, compBuffer, headSz);
623  free(compBuffer);
624 
625  // Put the file pointer at the end of the data we've read
626  if (compressedSz+headSz+pos != fileSize)
627  fseek(stream, compressedSz+headSz+pos, SEEK_SET);
628  block_sz_ = uncompressedSz+headSz;
629  }
630 
631  void initBlock(FILE *stream)
632  {
633  size_t pos = ftell(stream);
634  buffer_ = NULL;
635  buffer_blocks_ = NULL;
636  compressed_buffer_ = NULL;
637  size_t headSz = sizeof(IndexHeaderStruct);
638 
639  // Read the file header to a buffer
640  IndexHeaderStruct *head = (IndexHeaderStruct *)malloc(headSz);
641  if (head == NULL) {
642  throw FLANNException("Error allocating header buffer space");
643  }
644  if (fread(head, headSz, 1, stream) != 1) {
645  free(head);
646  throw FLANNException("Invalid index file, cannot read from disk (header)");
647  }
648 
649  // Backward compatability
650  if (head->signature[13] == '1' && head->signature[15] == '0') {
651  free(head);
652  fseek(stream, pos, SEEK_SET);
653  return decompressAndLoadV10(stream);
654  }
655 
656  // Alloc the space for both buffer blocks (each block
657  // references the previous)
658  buffer_ = buffer_blocks_ = (char *)malloc(BLOCK_BYTES*2);
659  compressed_buffer_ = (char *)malloc(LZ4_COMPRESSBOUND(BLOCK_BYTES));
660  if (buffer_ == NULL || compressed_buffer_ == NULL) {
661  free(head);
662  throw FLANNException("Error allocating compression buffer");
663  }
664 
665  // Init the LZ4 stream
666  lz4StreamDecode = &lz4StreamDecode_body;
667  LZ4_setStreamDecode(lz4StreamDecode, NULL, 0);
668 
669  // Read first block
670  memcpy(buffer_, head, headSz);
671  loadBlock(buffer_+headSz, head->first_block_size, stream);
672  block_sz_ += headSz;
673  ptr_ = buffer_;
674  free(head);
675  }
676 
677  void loadBlock(char* buffer_, size_t compSz, FILE* stream)
678  {
679  if(compSz >= LZ4_COMPRESSBOUND(BLOCK_BYTES)) {
680  throw FLANNException("Requested block size too large");
681  }
682 
683  // Read the block into the compressed buffer
684  if (fread(compressed_buffer_, compSz, 1, stream) != 1) {
685  throw FLANNException("Invalid index file, cannot read from disk (block)");
686  }
687 
688  // Decompress into the regular buffer
689  const int decBytes = LZ4_decompress_safe_continue(
690  lz4StreamDecode, compressed_buffer_, buffer_, compSz, BLOCK_BYTES);
691  if(decBytes <= 0) {
692  throw FLANNException("Invalid index file, cannot decompress block");
693  }
694  block_sz_ = decBytes;
695  }
696 
697  void preparePtr(size_t size)
698  {
699  // Return if the new size is less than (or eq) the size of a block
700  if (ptr_+size <= buffer_+block_sz_)
701  return;
702 
703  // Switch the buffer to the *other* block
704  if (buffer_ == buffer_blocks_)
705  buffer_ = &buffer_blocks_[BLOCK_BYTES];
706  else
707  buffer_ = buffer_blocks_;
708 
709  // Find the size of the next block
710  size_t cmpSz = 0;
711  size_t readCnt = fread(&cmpSz, sizeof(cmpSz), 1, stream_);
712  if(cmpSz <= 0 || readCnt != 1) {
713  throw FLANNException("Requested to read next block past end of file");
714  }
715 
716  // Load block & init ptr
717  loadBlock(buffer_, cmpSz, stream_);
718  ptr_ = buffer_;
719  }
720 
721  void endBlock()
722  {
723  // If not v1.0 format hack...
724  if (buffer_blocks_ != NULL) {
725  // Read the last '0' in the file
726  size_t zero = -1;
727  if (fread(&zero, sizeof(zero), 1, stream_) != 1) {
728  throw FLANNException("Invalid index file, cannot read from disk (end)");
729  }
730  if (zero != 0) {
731  throw FLANNException("Invalid index file, last block not zero length");
732  }
733  }
734 
735  // Free resources
736  if (buffer_blocks_ != NULL) {
737  free(buffer_blocks_);
738  buffer_blocks_ = NULL;
739  }
740  if (compressed_buffer_ != NULL) {
741  free(compressed_buffer_);
742  compressed_buffer_ = NULL;
743  }
744  ptr_ = NULL;
745  }
746 
747 public:
748  LoadArchive(const char* filename)
749  {
750  // Open the file
751  stream_ = fopen(filename, "rb");
752  own_stream_ = true;
753 
754  initBlock(stream_);
755  }
756 
757  LoadArchive(FILE* stream)
758  {
759  stream_ = stream;
760  own_stream_ = false;
761 
762  initBlock(stream);
763  }
764 
766  {
767  endBlock();
768  if (own_stream_) {
769  fclose(stream_);
770  }
771  }
772 
773  template<typename T>
774  void load(T& val)
775  {
776  preparePtr(sizeof(val));
777  memcpy(&val, ptr_, sizeof(val));
778  ptr_ += sizeof(val);
779  }
780 
781  template<typename T>
782  void load(T*& val)
783  {
784  // don't load pointers
785  //fread(&val, sizeof(val), 1, handle_);
786  }
787 
788  template<typename T>
789  void load_binary(T* ptr, size_t size)
790  {
791  while (size > BLOCK_BYTES) {
792  // Load next block
793  preparePtr(BLOCK_BYTES);
794 
795  // Load large chunk
796  memcpy(ptr, ptr_, BLOCK_BYTES);
797  ptr_ += BLOCK_BYTES;
798  ptr = ((char *)ptr) + BLOCK_BYTES;
799  size -= BLOCK_BYTES;
800  }
801 
802  // Load next block if needed
803  preparePtr(size);
804 
805  // Load the data
806  memcpy(ptr, ptr_, size);
807  ptr_ += size;
808  }
809 };
810 
811 } // namespace serialization
812 } // namespace flann
813 #endif // SERIALIZATION_H_
#define NULL
binary_object & operator=(const binary_object &rhs)
GLM_FUNC_DECL tvec1< T, P > operator&(tvec1< T, P > const &v, T const &s)
#define BLOCK_BYTES
static void load(InputArchive &ar, std::vector< T > &val)
static void load(InputArchive &ar, T &val)
Definition: serialization.h:49
static void save(OutputArchive &ar, T *const &val)
flann_datatype_t data_type
Definition: serialization.h:18
static void load(InputArchive &ar, T(&val)[N])
int LZ4_setStreamDecode(LZ4_streamDecode_t *LZ4_streamDecode, const char *dictionary, int dictSize)
Definition: lz4.c:1338
static void load(InputArchive &ar, binary_object &b)
void loadBlock(char *buffer_, size_t compSz, FILE *stream)
void LZ4_resetStreamHC(LZ4_streamHC_t *LZ4_streamHCPtr, int compressionLevel)
Definition: lz4hc.c:567
binary_object(const binary_object &rhs)
int LZ4_decompress_safe(const char *source, char *dest, int compressedSize, int maxDecompressedSize)
Definition: lz4.c:1288
static void serialize(Archive &ar, T &type)
Definition: serialization.h:32
void decompressAndLoadV10(FILE *stream)
int LZ4_decompress_safe_continue(LZ4_streamDecode_t *LZ4_streamDecode, const char *source, char *dest, int compressedSize, int maxOutputSize)
Definition: lz4.c:1355
static void save(OutputArchive &ar, const std::vector< T > &val)
void save_binary(T *ptr, size_t size)
binary_object(void *const ptr, size_t size)
void serialize(Archive &ar, T &type)
Definition: serialization.h:40
flann_algorithm_t index_type
Definition: serialization.h:19
static void save(OutputArchive &ar, const std::map< K, V > &map_val)
flann_algorithm_t
Definition: defines.h:79
#define BASIC_TYPE_SERIALIZER(type)
Definition: serialization.h:60
const binary_object make_binary_object(void *t, size_t size)
static void load(InputArchive &ar, const binary_object &b)
LZ4_streamDecode_t lz4StreamDecode_body
SaveArchive(const char *filename)
void load_binary(T *ptr, size_t size)
static void save(OutputArchive &ar, T const (&val)[N])
#define false
Definition: ConvertUTF.c:56
static void save(OutputArchive &ar, const binary_object &b)
static void load(InputArchive &ar, T *&val)
void save_binary(T *ptr, size_t size)
static void load(InputArchive &ar, std::map< K, V > &map_val)
int LZ4_compress_HC_continue(LZ4_streamHC_t *LZ4_streamHCPtr, const char *source, char *dest, int inputSize, int maxOutputSize)
Definition: lz4hc.c:641
flann_datatype_t
Definition: defines.h:131
static void save(OutputArchive &ar, const binary_object &b)
static void save(OutputArchive &ar, const T &val)
Definition: serialization.h:54
GLM_FUNC_DECL genType zero()
LoadArchive(const char *filename)
#define LZ4_COMPRESSBOUND(isize)
Definition: lz4.h:106
LZ4_streamDecode_t * lz4StreamDecode


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:35:00