From 1b2ef186d8a97bb753b4b094779658bbea77835b Mon Sep 17 00:00:00 2001
From: Armin Hornung <armin@hornung.io>
Date: Wed, 5 Oct 2016 21:47:37 +0200
Subject: [PATCH] Fix #131: Portable binary read/write of Pointcloud and
 ScanGraph - uint32_t for size, move inttypes.h include into octomap_types.h

---
 octomap/include/octomap/OcTreeKey.h     |  1 -
 octomap/include/octomap/octomap_types.h |  1 +
 octomap/src/Pointcloud.cpp              | 19 +++++++++++++++----
 octomap/src/ScanGraph.cpp               |  8 ++++++--
 4 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/octomap/include/octomap/OcTreeKey.h b/octomap/include/octomap/OcTreeKey.h
index 26ebc980..67b36fa4 100644
--- a/octomap/include/octomap/OcTreeKey.h
+++ b/octomap/include/octomap/OcTreeKey.h
@@ -40,7 +40,6 @@
 #include <ciso646>
 
 #include <assert.h>
-#include <inttypes.h>
 
 /* Libc++ does not implement the TR1 namespace, all c++11 related functionality
  * is instead implemented in the std namespace.
diff --git a/octomap/include/octomap/octomap_types.h b/octomap/include/octomap/octomap_types.h
index 328a0b7c..2ae7fedc 100644
--- a/octomap/include/octomap/octomap_types.h
+++ b/octomap/include/octomap/octomap_types.h
@@ -37,6 +37,7 @@
 #include <stdio.h>
 #include <vector>
 #include <list>
+#include <inttypes.h>
 
 #include <octomap/math/Vector3.h>
 #include <octomap/math/Pose6D.h>
diff --git a/octomap/src/Pointcloud.cpp b/octomap/src/Pointcloud.cpp
index b78ca554..09349db0 100644
--- a/octomap/src/Pointcloud.cpp
+++ b/octomap/src/Pointcloud.cpp
@@ -43,6 +43,8 @@
 #endif
 #include <fstream>
 #include <math.h>
+#include <assert.h>
+#include <limits>
 
 #include <octomap/Pointcloud.h>
 
@@ -284,14 +286,14 @@ namespace octomap {
 
   std::istream& Pointcloud::readBinary(std::istream &s) {
 
-    unsigned int pc_size = 0;
+    uint32_t pc_size = 0;
     s.read((char*)&pc_size, sizeof(pc_size));
     OCTOMAP_DEBUG("Reading %d points from binary file...", pc_size);
 
     if (pc_size > 0) {
       this->points.reserve(pc_size);
       point3d p;
-      for (unsigned int i=0; i<pc_size; i++) {
+      for (uint32_t i=0; i<pc_size; i++) {
         p.readBinary(s);
         if (!s.fail()) {
           this->push_back(p);
@@ -302,6 +304,8 @@ namespace octomap {
         }
       }
     }
+    assert(pc_size == this->size());
+    
     OCTOMAP_DEBUG("done.\n");
 
     return s;
@@ -310,8 +314,15 @@ namespace octomap {
 
   std::ostream& Pointcloud::writeBinary(std::ostream &s) const {
 
-    size_t pc_size = this->size();
-    OCTOMAP_DEBUG("Writing %lu points to binary file...", (unsigned long)pc_size);
+    // check if written unsigned int can hold size
+    size_t orig_size = this->size();
+    if (orig_size > std::numeric_limits<uint32_t>::max()){
+      OCTOMAP_ERROR("Pointcloud::writeBinary ERROR: Point cloud too large to be written");
+      return s;
+    }
+    
+    uint32_t pc_size = static_cast<uint32_t>(this->size());
+    OCTOMAP_DEBUG("Writing %u points to binary file...", pc_size);
     s.write((char*)&pc_size, sizeof(pc_size));
 
     for (Pointcloud::const_iterator it = this->begin(); it != this->end(); it++) {
diff --git a/octomap/src/ScanGraph.cpp b/octomap/src/ScanGraph.cpp
index 922fd142..f3a0aeb0 100644
--- a/octomap/src/ScanGraph.cpp
+++ b/octomap/src/ScanGraph.cpp
@@ -55,7 +55,9 @@ namespace octomap {
 
     scan->writeBinary(s);
     pose.writeBinary(s);
-    s.write((char*)&id, sizeof(id));
+
+    uint32_t uintId = static_cast<uint32_t>(id);
+    s.write((char*)&uintId, sizeof(uintId));
 
     return s;
   }
@@ -67,7 +69,9 @@ namespace octomap {
 
     this->pose.readBinary(s);
 
-    s.read((char*)&this->id, sizeof(this->id));
+    uint32_t uintId;
+    s.read((char*)&uintId, sizeof(uintId));
+    this->id = uintId;
 
     return s;
   }