Cleaned up

danielduberg · Mar 11, 2020 · 0accfde · 0accfde
1 parent 2952d8d
commit 0accfde
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 43 deletions.
diff --git a/ufomap/include/ufomap/octree_base.h b/ufomap/include/ufomap/octree_base.h
@@ -87,14 +87,12 @@ class OctreeBase
 		indices_.clear();
 	}
 
-	std::tuple<double, double, size_t> insertPointCloudDiscrete(const Point3& sensor_origin,
+	void insertPointCloudDiscrete(const Point3& sensor_origin,
 																															const PointCloud& cloud,
 																															float max_range = -1,
 																															bool super_speed = false,
 																															unsigned int depth = 0)
 	{
-		auto start = std::chrono::high_resolution_clock::now();
-
 		KeyMap<std::vector<Key>> discrete_map;
 
 		std::vector<Key> discrete;
@@ -173,24 +171,12 @@ class OctreeBase
 
 		computeUpdateDiscrete(sensor_origin, discrete, discrete_map, super_speed);
 
-		std::chrono::duration<double> ray_casting_elapsed =
-				std::chrono::high_resolution_clock::now() - start;
-
-		start = std::chrono::high_resolution_clock::now();
-
 		// Insert
 		for (const auto& [code, value] : indices_)
 		{
 			updateNodeValue(code, value);
 		}
-		size_t num_nodes = indices_.size();
 		indices_.clear();
-
-		std::chrono::duration<double> insert_elapsed =
-				std::chrono::high_resolution_clock::now() - start;
-
-		return std::make_tuple(ray_casting_elapsed.count(), insert_elapsed.count(),
-													 num_nodes);
 	}
 
 	void insertPointCloud(const Point3& sensor_origin, const PointCloud& cloud,

diff --git a/ufomap/include/ufomap/octree_rgb.h b/ufomap/include/ufomap/octree_rgb.h
@@ -73,11 +73,9 @@ class OctreeRGB : public OctreeBase<OccupancyNodeRGB>
 	void insertPointCloud(const Point3& sensor_origin, const PointCloudRGB& cloud,
 												float max_range = -1);
 
-	std::tuple<double, double, size_t> insertPointCloudDiscrete(const Point3& sensor_origin,
-																															const PointCloudRGB& cloud,
-																															float max_range = -1,
-																															bool super_speed = false,
-																															unsigned int depth = 0);
+	void insertPointCloudDiscrete(const Point3& sensor_origin, const PointCloudRGB& cloud,
+																float max_range = -1, bool super_speed = false,
+																unsigned int depth = 0);
 
 	void insertPointCloud(const Point3& sensor_origin, const PointCloudRGB& cloud,
 												const Pose6& frame_origin, float max_range = -1)

diff --git a/ufomap/src/octree_rgb.cpp b/ufomap/src/octree_rgb.cpp
@@ -69,22 +69,16 @@ void OctreeRGB::insertPointCloud(const Point3& sensor_origin, const PointCloudRG
 	}
 }
 
-std::tuple<double, double, size_t> OctreeRGB::insertPointCloudDiscrete(
-		const Point3& sensor_origin, const PointCloudRGB& cloud, float max_range,
-		bool super_speed, unsigned int depth)
+void OctreeRGB::insertPointCloudDiscrete(const Point3& sensor_origin,
+																				 const PointCloudRGB& cloud, float max_range,
+																				 bool super_speed, unsigned int depth)
 {
-	auto start = std::chrono::high_resolution_clock::now();
-
 	PointCloud no_color_cloud;
 
 	CodeMap<std::vector<Color>> colors;
 
 	for (const Point3RGB& point : cloud)
 	{
-		// if (8 > dist_(rng_))
-		// {
-		// 	continue;
-		// }
 		if (0 > max_range || (point - sensor_origin).norm() < max_range)
 		{
 			std::vector<Color>& color = colors[Code(coordToKey(point))];
@@ -96,14 +90,8 @@ std::tuple<double, double, size_t> OctreeRGB::insertPointCloudDiscrete(
 		}
 	}
 
-	std::chrono::duration<double> color_ray_casting_elapsed =
-			std::chrono::high_resolution_clock::now() - start;
-
-	auto [ray_casting_elapsed, insert_elapsed, num_nodes] =
-			OctreeBase<OccupancyNodeRGB>::insertPointCloudDiscrete(
-					sensor_origin, no_color_cloud, max_range, super_speed, depth);
-
-	start = std::chrono::high_resolution_clock::now();
+	OctreeBase<OccupancyNodeRGB>::insertPointCloudDiscrete(sensor_origin, no_color_cloud,
+																												 max_range, super_speed, depth);
 
 	for (const auto& [code, color] : colors)
 	{
@@ -113,12 +101,6 @@ std::tuple<double, double, size_t> OctreeRGB::insertPointCloudDiscrete(
 			integrateColor(code, getAverageColor(color));
 		}
 	}
-
-	std::chrono::duration<double> color_insert_elapsed =
-			std::chrono::high_resolution_clock::now() - start;
-
-	return std::make_tuple(color_ray_casting_elapsed.count() + ray_casting_elapsed,
-												 color_insert_elapsed.count() + insert_elapsed, num_nodes);
 }
 
 //