Added occupancy/free thres to serialization

danielduberg · Mar 6, 2020 · 2a96b4f · 2a96b4f
1 parent a185e7b
commit 2a96b4f
Show file tree

Hide file tree

Showing 7 changed files with 2,589 additions and 45 deletions.
diff --git a/ufomap/Doxyfile b/ufomap/Doxyfile
diff --git a/ufomap/include/ufomap/node.h b/ufomap/include/ufomap/node.h
@@ -7,6 +7,8 @@
 #include <array>
 #include <iostream>
 
+// TODO: Update documentation
+
 namespace ufomap
 {
 /**
@@ -26,8 +28,8 @@ struct OccupancyNode
 	 * @param to_octomap Whether to write data in a UFOMap format or OctoMap format
 	 * @return std::ostream&
 	 */
-	std::ostream& writeData(std::ostream& s, bool is_occupied,
-													bool to_octomap = false) const
+	std::ostream& writeData(std::ostream& s, float occupancy_thres_log,
+													float free_thres_log, bool to_octomap = false) const
 	{
 		if (to_octomap)
 		{
@@ -50,7 +52,8 @@ struct OccupancyNode
 	 * format
 	 * @return std::istream&
 	 */
-	std::istream& readData(std::istream& s, bool is_occupied, bool from_octomap = false)
+	std::istream& readData(std::istream& s, float occupancy_thres_log, float free_thres_log,
+												 bool from_octomap = false)
 	{
 		if (from_octomap)
 		{
@@ -83,11 +86,11 @@ struct OccupancyNodeRGB : OccupancyNode
 	 * @param to_octomap Whether to write data in a UFOMap format or OctoMap format
 	 * @return std::ostream&
 	 */
-	std::ostream& writeData(std::ostream& s, bool is_occupied,
-													bool to_octomap = false) const
+	std::ostream& writeData(std::ostream& s, float occupancy_thres_log,
+													float free_thres_log, bool to_octomap = false) const
 	{
-		OccupancyNode::writeData(s, to_octomap);
-		if (is_occupied || to_octomap)
+		OccupancyNode::writeData(s, occupancy_thres_log, free_thres_log, to_octomap);
+		if (logit > occupancy_thres_log || to_octomap)
 		{
 			s.write((const char*)&color, sizeof(color));
 		}
@@ -103,10 +106,11 @@ struct OccupancyNodeRGB : OccupancyNode
 	 * format
 	 * @return std::istream&
 	 */
-	std::istream& readData(std::istream& s, bool is_occupied, bool from_octomap = false)
+	std::istream& readData(std::istream& s, float occupancy_thres_log, float free_thres_log,
+												 bool from_octomap = false)
 	{
-		OccupancyNode::readData(s, from_octomap);
-		if (is_occupied || from_octomap)
+		OccupancyNode::readData(s, occupancy_thres_log, free_thres_log, from_octomap);
+		if (logit > occupancy_thres_log || from_octomap)
 		{
 			s.read((char*)&color, sizeof(color));
 		}
@@ -131,10 +135,11 @@ struct OccupancyNodeIntensity : OccupancyNode
 	 * @param to_octomap Whether to write data in a UFOMap format or OctoMap format
 	 * @return std::ostream&
 	 */
-	std::ostream& writeData(std::ostream& s, bool is_occupied, bool to_octomap = false) const
+	std::ostream& writeData(std::ostream& s, float occupancy_thres_log,
+													float free_thres_log, bool to_octomap = false) const
 	{
-		OccupancyNode::writeData(s, to_octomap);
-		if (is_occupied || to_octomap)
+		OccupancyNode::writeData(s, occupancy_thres_log, free_thres_log, to_octomap);
+		if (logit > occupancy_thres_log || to_octomap)
 		{
 			s.write((const char*)&intensity, sizeof(intensity));
 		}
@@ -150,10 +155,11 @@ struct OccupancyNodeIntensity : OccupancyNode
 	 * format
 	 * @return std::istream&
 	 */
-	std::istream& readData(std::istream& s, bool is_occupied, bool from_octomap = false)
+	std::istream& readData(std::istream& s, float occupancy_thres_log, float free_thres_log,
+												 bool from_octomap = false)
 	{
-		OccupancyNode::readData(s, from_octomap);
-		if (is_occupied || from_octomap)
+		OccupancyNode::readData(s, occupancy_thres_log, free_thres_log, from_octomap);
+		if (logit > occupancy_thres_log || from_octomap)
 		{
 			s.read((char*)&intensity, sizeof(intensity));
 		}

diff --git a/ufomap/include/ufomap/octree.h b/ufomap/include/ufomap/octree.h
@@ -95,6 +95,7 @@ class Octree : public OctreeBase<OccupancyNode>
 
 	virtual bool readBinaryNodesRecurs(std::istream& s, InnerNode<OccupancyNode>& node,
 																		 unsigned int current_depth,
+																		 float occupancy_thres_log, float free_thres_log,
 																		 bool from_octomap = false) override;
 
 	virtual bool writeBinaryNodesRecurs(std::ostream& s,

diff --git a/ufomap/include/ufomap/octree_base.h b/ufomap/include/ufomap/octree_base.h
@@ -16,7 +16,7 @@
 #include <type_traits>
 
 // Compression
-// #include <lz4.h>
+#include <lz4.h>
 
 namespace ufomap
 {
@@ -1554,18 +1554,21 @@ class OctreeBase
 		unsigned int depth_levels;
 		float occupancy_thres;
 		float free_thres;
+		bool compressed;
 		if (!readHeader(s, id, size, res, depth_levels, occupancy_thres, free_thres,
-										is_ufomap))
+										compressed, is_ufomap))
 		{
 			return false;
 		}
 
-		return readData(s, res, depth_levels, binary, !is_ufomap);
+		return readData(s, res, depth_levels, occupancy_thres, free_thres, compressed, binary,
+										!is_ufomap);
 
 		// TODO: Check size?
 	}
 
 	bool readData(std::istream& s, float resolution, unsigned int depth_levels,
+								float occupancy_thres, float free_thres, bool compressed = false,
 								bool binary = false, bool from_octomap = false)
 	{
 		if (binary && !binarySupport())
@@ -1578,21 +1581,28 @@ class OctreeBase
 			// Warning
 		}
 
+		if (compressed)
+		{
+			// TODO: Decompress
+		}
+
 		clear(resolution, depth_levels);
 
 		root_ = InnerNode<LEAF_NODE>();
 
 		if (binary)
 		{
-			return readBinaryNodesRecurs(s, root_, depth_levels_, from_octomap);
+			return readBinaryNodesRecurs(s, root_, depth_levels_, logit(occupancy_thres),
+																	 logit(free_thres), from_octomap);
 		}
 		else
 		{
-			return readNodesRecurs(s, root_, depth_levels_, from_octomap);
+			return readNodesRecurs(s, root_, depth_levels_, logit(occupancy_thres),
+														 logit(free_thres), from_octomap);
 		}
 	}
 
-	bool write(const std::string& filename, bool binary = false,
+	bool write(const std::string& filename, bool compress = false, bool binary = false,
 						 bool to_octomap = false) const
 	{
 		if (binary && !binarySupport())
@@ -1608,12 +1618,13 @@ class OctreeBase
 			return false;
 		}
 		// TODO: check is_good of finished stream, return
-		write(file, binary, to_octomap);
+		write(file, compress, binary, to_octomap);
 		file.close();
 		return true;
 	}
 
-	bool write(std::ostream& s, bool binary = false, bool to_octomap = false) const
+	bool write(std::ostream& s, bool compress = false, bool binary = false,
+						 bool to_octomap = false) const
 	{
 		if (binary && !binarySupport())
 		{
@@ -1647,12 +1658,13 @@ class OctreeBase
 		s << "size " << size() << std::endl;
 		s << "res " << getResolution() << std::endl;
 		s << "levels " << getTreeDepthLevels() << std::endl;
+		s << "occupancy_thres " << getOccupancyThres() << std::endl;
+		s << "free_thres " << getFreeThres() << std::endl;
+		s << "compressed " << compress << std::endl;
 		s << "data" << std::endl;
-		s << "occupancy_thres" << getOccupancyThres() << std::endl;
-		s << "free_thres" << getFreeThres() << std::endl;
 
 		// write the actual data:
-		writeData(s, binary, to_octomap);
+		writeData(s, compress, binary, to_octomap);
 
 		if (s.good())
 		{
@@ -1661,21 +1673,30 @@ class OctreeBase
 		return false;
 	}
 
-	bool writeData(std::ostream& s, bool binary = false, bool to_octomap = false) const
+	bool writeData(std::ostream& s, bool compress = false, bool binary = false,
+								 bool to_octomap = false) const
 	{
 		if (binary && !binarySupport())
 		{
 			return false;
 		}
 
+		bool success;
 		if (binary)
 		{
-			return writeBinaryNodesRecurs(s, root_, depth_levels_, to_octomap);
+			success = writeBinaryNodesRecurs(s, root_, depth_levels_, to_octomap);
 		}
 		else
 		{
-			return writeNodesRecurs(s, root_, depth_levels_, to_octomap);
+			success = writeNodesRecurs(s, root_, depth_levels_, to_octomap);
 		}
+
+		if (compress)
+		{
+			// TODO: Compress
+			// LZ4_compress_default(s )
+		}
+		return success;
 	}
 
 protected:
@@ -2208,6 +2229,7 @@ class OctreeBase
 		}
 	}
 
+	// TODO: Changed bool super_speed = false to unsigned int n = 0
 	void computeUpdateDiscrete(const Point3& sensor_origin, const std::vector<Key>& current,
 														 const KeyMap<std::vector<Key>>& discrete_map,
 														 bool super_speed = false)
@@ -2430,14 +2452,15 @@ class OctreeBase
 
 	bool readHeader(std::istream& s, std::string& id, size_t& size, float& res,
 									unsigned int& depth_levels, float& occupancy_thres, float& free_thres,
-									bool is_ufomap = true)
+									bool& compressed, bool is_ufomap = true)
 	{
 		id = "";
 		size = 0;
 		res = 0.0;
 		depth_levels = 0;
-		occupancy_thres = -1.0;
-		free_thres = -1.0;
+		occupancy_thres = 0.5;
+		free_thres = 0.5;
+		compressed = false;
 
 		std::string token;
 		bool header_read = false;
@@ -2487,6 +2510,10 @@ class OctreeBase
 			{
 				s >> free_thres;
 			}
+			else if ("compressed" == token)
+			{
+				s >> compressed;
+			}
 			else
 			{
 				// Other token
@@ -2500,27 +2527,26 @@ class OctreeBase
 
 		if (!header_read)
 		{
-			// OCTOMAP_ERROR_STR("Error reading OcTree header");
 			return false;
 		}
 
 		if ("" == id)
 		{
-			// OCTOMAP_ERROR_STR("Error reading OcTree header, ID not set");
 			return false;
 		}
 
 		if (0.0 >= res)
 		{
-			// OCTOMAP_ERROR_STR("Error reading OcTree header, res <= 0.0");
 			return false;
 		}
 
+		// TODO: What to do?
 		if (0.0 > occupancy_thres)
 		{
 			return false;
 		}
 
+		// TODO: What to do?
 		if (0.0 > free_thres)
 		{
 			return false;
@@ -2549,9 +2575,11 @@ class OctreeBase
 	}
 
 	bool readNodesRecurs(std::istream& s, InnerNode<LEAF_NODE>& node,
-											 unsigned int current_depth, bool from_octomap = false)
+											 unsigned int current_depth, float occupancy_thres_log,
+											 float free_thres_log, bool from_octomap = false)
 	{
-		static_cast<LEAF_NODE&>(node).readData(s, from_octomap);
+		static_cast<LEAF_NODE&>(node).readData(s, occupancy_thres_log, free_thres_log,
+																					 from_octomap);
 		node.all_children_same = true;
 
 		char children_char;
@@ -2568,15 +2596,16 @@ class OctreeBase
 			{
 				for (LEAF_NODE& child : *static_cast<std::array<LEAF_NODE, 8>*>(node.children))
 				{
-					child.readData(s, from_octomap);
+					child.readData(s, occupancy_thres_log, free_thres_log, from_octomap);
 				}
 			}
 			else
 			{
 				for (InnerNode<LEAF_NODE>& child :
 						 *static_cast<std::array<InnerNode<LEAF_NODE>, 8>*>(node.children))
 				{
-					readNodesRecurs(s, child, current_depth - 1, from_octomap);
+					readNodesRecurs(s, child, current_depth - 1, occupancy_thres_log,
+													free_thres_log, from_octomap);
 				}
 			}
 		}
@@ -2588,6 +2617,7 @@ class OctreeBase
 
 	virtual bool readBinaryNodesRecurs(std::istream& s, InnerNode<LEAF_NODE>& node,
 																		 unsigned int current_depth,
+																		 float occupancy_thres_log, float free_thres_log,
 																		 bool from_octomap = false)
 	{
 		return false;
@@ -2596,7 +2626,8 @@ class OctreeBase
 	bool writeNodesRecurs(std::ostream& s, const InnerNode<LEAF_NODE>& node,
 												unsigned int current_depth, bool to_octomap = false) const
 	{
-		static_cast<const LEAF_NODE&>(node).writeData(s, to_octomap);
+		static_cast<const LEAF_NODE&>(node).writeData(s, occupancy_thres_log_,
+																									free_thres_log_, to_octomap);
 
 		// 1 bit for each children; 0: empty, 1: allocated
 		std::bitset<8> children;
@@ -2616,7 +2647,7 @@ class OctreeBase
 				for (const LEAF_NODE& child :
 						 *static_cast<std::array<LEAF_NODE, 8>*>(node.children))
 				{
-					child.writeData(s, to_octomap);
+					child.writeData(s, occupancy_thres_log_, free_thres_log_, to_octomap);
 				}
 			}
 			else

diff --git a/ufomap/src/octree.cpp b/ufomap/src/octree.cpp
@@ -37,7 +37,8 @@ Octree::Octree(const Octree& other)
 //
 
 bool Octree::readBinaryNodesRecurs(std::istream& s, InnerNode<OccupancyNode>& node,
-																	 unsigned int current_depth, bool from_octomap)
+																	 unsigned int current_depth, float occupancy_thres_log,
+																	 float free_thres_log, bool from_octomap)
 {
 	std::bitset<8> children_data_1;
 	std::bitset<8> children_data_2;
@@ -118,7 +119,8 @@ bool Octree::readBinaryNodesRecurs(std::istream& s, InnerNode<OccupancyNode>& no
 				else if (1 == children_data_1[i] && 1 == children_data_2[i])
 				{
 					// Has children
-					readBinaryNodesRecurs(s, child, current_depth - 1, from_octomap);
+					readBinaryNodesRecurs(s, child, current_depth - 1, occupancy_thres_log,
+																free_thres_log, from_octomap);
 				}
 			}
 		}

diff --git a/ufomap_msgs/include/ufomap_msgs/conversions.h b/ufomap_msgs/include/ufomap_msgs/conversions.h
@@ -12,7 +12,8 @@ bool msgToMap(const Ufomap& msg, TreeType& tree)
 	if (!msg.data.empty())
 	{
 		data_stream.write((const char*)&msg.data[0], msg.data.size());
-		return tree.readData(data_stream, msg.resolution, msg.depth_levels, msg.binary);
+		return tree.readData(data_stream, msg.resolution, msg.depth_levels,
+												 msg.occupancy_thres, msg.free_thres, msg.compressed, msg.binary);
 	}
 }