Tremendous memory usage in ESDF map

[SwiftGust]

I'm playing with the simulator and found that increasing map size tremendously consumes memory. On my 64GB RAM desktop maximum available size is roughly 100 x 100 x 100 m with 0.1m resolution (removed uav simulator from launch) and increasing size further throws std::bad_alloc error

I think memory usage should be optimized much further if planner to be used at wider space or outdoor.

Only plan_manage/launch/simulation.launch is launched in this test.

Reducing map resolution reduces memory usage significantly but I'm worrying about the bad impact on planner solution.

Memory usage Memory usage is 52.5GB

Before launch - using 6.8GB

After launch - using 59.3 GB

Simulation.launch file diff from master is as following

$ git diff simulation.launch
diff --git a/fast_planner/plan_manage/launch/simulation.launch b/fast_planner/plan_manage/launch/simulation.launch
index 42ba205..467ff11 100644
--- a/fast_planner/plan_manage/launch/simulation.launch
+++ b/fast_planner/plan_manage/launch/simulation.launch
@@ -1,9 +1,9 @@
 <launch>

   <!-- size of map, change the size in x, y, z according to your application -->
-  <arg name="map_size_x" value="40.0"/>
-  <arg name="map_size_y" value="40.0"/>
-  <arg name="map_size_z" value=" 5.0"/>
+  <arg name="map_size_x" value="100.0"/>
+  <arg name="map_size_y" value="100.0"/>
+  <arg name="map_size_z" value="100.0"/>

   <!-- topic of your odometry such as VIO or LIO -->
   <arg name="odom_topic" value="/state_ukf/odom" />
@@ -39,7 +39,7 @@
     <!-- 1: use 2D Nav Goal to select goal  -->
     <!-- 2: use global waypoints below  -->
     <arg name="flight_type" value="1" />
-    
+
     <!-- global waypoints -->
     <!-- If flight_type is set to 2, the drone will travel these waypoints one by one -->
     <arg name="point_num" value="3" />
@@ -61,13 +61,13 @@
   </include>

   <!-- use simulator. It should be disabled if a real drone is used -->
-  <include file="$(find plan_manage)/launch/simulator.xml">
+  <!-- <include file="$(find plan_manage)/launch/simulator.xml">
     <arg name="map_size_x_" value="$(arg map_size_x)"/>
     <arg name="map_size_y_" value="$(arg map_size_y)"/>
     <arg name="map_size_z_" value="$(arg map_size_z)"/>

      <arg name="odometry_topic" value="$(arg odom_topic)" />
-  </include>
+  </include> -->

   <!-- trajectory server and trigger, you don't need to change them -->
   <node pkg="plan_manage" name="traj_server" type="traj_server" output="screen">
@@ -78,10 +78,10 @@
   </node>

   <node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
-    <remap from="~odom" to="$(arg odom_topic)"/>        
+    <remap from="~odom" to="$(arg odom_topic)"/>
     <remap from="~goal" to="/move_base_simple/goal"/>
     <remap from="~traj_start_trigger" to="/traj_start_trigger" />
-    <param name="waypoint_type" value="manual-lonely-waypoint"/>    
+    <param name="waypoint_type" value="manual-lonely-waypoint"/>
   </node>

Console output

$ roslaunch plan_manage simulation.launch 
SUMMARY
========

PARAMETERS
 * /fast_planner_node/bspline/limit_acc: 1.0
 * /fast_planner_node/bspline/limit_ratio: 1.1
 * /fast_planner_node/bspline/limit_vel: 2.1
 * /fast_planner_node/fsm/flight_type: 1
 * /fast_planner_node/fsm/thresh_no_replan: 2.0
 * /fast_planner_node/fsm/thresh_replan: 1.5
 * /fast_planner_node/fsm/waypoint0_x: 19.0
 * /fast_planner_node/fsm/waypoint0_y: 0.0
 * /fast_planner_node/fsm/waypoint0_z: 1.0
 * /fast_planner_node/fsm/waypoint1_x: 0.0
 * /fast_planner_node/fsm/waypoint1_y: -19.0
 * /fast_planner_node/fsm/waypoint1_z: 1.0
 * /fast_planner_node/fsm/waypoint2_x: 0.0
 * /fast_planner_node/fsm/waypoint2_y: 19.0
 * /fast_planner_node/fsm/waypoint2_z: 1.0
 * /fast_planner_node/fsm/waypoint_num: 3
 * /fast_planner_node/manager/clearance_threshold: 0.2
 * /fast_planner_node/manager/control_points_distance: 0.4
 * /fast_planner_node/manager/dynamic_environment: 0
 * /fast_planner_node/manager/local_segment_length: 6.0
 * /fast_planner_node/manager/max_acc: 1.0
 * /fast_planner_node/manager/max_jerk: 4.0
 * /fast_planner_node/manager/max_vel: 2.1
 * /fast_planner_node/manager/use_active_perception: False
 * /fast_planner_node/manager/use_geometric_path: False
 * /fast_planner_node/manager/use_kinodynamic_path: True
 * /fast_planner_node/manager/use_optimization: True
 * /fast_planner_node/manager/use_topo_path: False
 * /fast_planner_node/optimization/algorithm1: 15
 * /fast_planner_node/optimization/algorithm2: 11
 * /fast_planner_node/optimization/dist0: 0.7
 * /fast_planner_node/optimization/lambda1: 10.0
 * /fast_planner_node/optimization/lambda2: 0.8
 * /fast_planner_node/optimization/lambda3: 1e-05
 * /fast_planner_node/optimization/lambda4: 0.01
 * /fast_planner_node/optimization/max_acc: 1.0
 * /fast_planner_node/optimization/max_iteration_num1: 2
 * /fast_planner_node/optimization/max_iteration_num2: 300
 * /fast_planner_node/optimization/max_iteration_num3: 200
 * /fast_planner_node/optimization/max_vel: 2.1
 * /fast_planner_node/optimization/order: 3
 * /fast_planner_node/planner_node/planner: 1
 * /fast_planner_node/sdf_map/ceil_height: 2.5
 * /fast_planner_node/sdf_map/cx: 385.754
 * /fast_planner_node/sdf_map/cy: 236.743
 * /fast_planner_node/sdf_map/depth_filter_margin: 2
 * /fast_planner_node/sdf_map/depth_filter_maxdist: 5.0
 * /fast_planner_node/sdf_map/depth_filter_mindist: 0.2
 * /fast_planner_node/sdf_map/depth_filter_tolerance: 0.15
 * /fast_planner_node/sdf_map/esdf_slice_height: 0.3
 * /fast_planner_node/sdf_map/frame_id: world
 * /fast_planner_node/sdf_map/fx: 385.754
 * /fast_planner_node/sdf_map/fy: 257.296
 * /fast_planner_node/sdf_map/ground_height: -1.0
 * /fast_planner_node/sdf_map/k_depth_scaling_factor: 1000.0
 * /fast_planner_node/sdf_map/local_bound_inflate: 0.0
 * /fast_planner_node/sdf_map/local_map_margin: 50
 * /fast_planner_node/sdf_map/local_update_range_x: 5.5
 * /fast_planner_node/sdf_map/local_update_range_y: 5.5
 * /fast_planner_node/sdf_map/local_update_range_z: 4.5
 * /fast_planner_node/sdf_map/map_size_x: 100.0
 * /fast_planner_node/sdf_map/map_size_y: 100.0
 * /fast_planner_node/sdf_map/map_size_z: 100.0
 * /fast_planner_node/sdf_map/max_ray_length: 4.5
 * /fast_planner_node/sdf_map/min_ray_length: 0.5
 * /fast_planner_node/sdf_map/obstacles_inflation: 0.099
 * /fast_planner_node/sdf_map/p_hit: 0.65
 * /fast_planner_node/sdf_map/p_max: 0.9
 * /fast_planner_node/sdf_map/p_min: 0.12
 * /fast_planner_node/sdf_map/p_miss: 0.35
 * /fast_planner_node/sdf_map/p_occ: 0.8
 * /fast_planner_node/sdf_map/pose_type: 1
 * /fast_planner_node/sdf_map/resolution: 0.1
 * /fast_planner_node/sdf_map/show_esdf_time: False
 * /fast_planner_node/sdf_map/show_occ_time: False
 * /fast_planner_node/sdf_map/skip_pixel: 2
 * /fast_planner_node/sdf_map/use_depth_filter: True
 * /fast_planner_node/sdf_map/visualization_truncate_height: 2.49
 * /fast_planner_node/search/allocate_num: 100000
 * /fast_planner_node/search/check_num: 5
 * /fast_planner_node/search/horizon: 7.0
 * /fast_planner_node/search/init_max_tau: 0.8
 * /fast_planner_node/search/lambda_heu: 10.0
 * /fast_planner_node/search/margin: 0.2
 * /fast_planner_node/search/max_acc: 1.0
 * /fast_planner_node/search/max_tau: 0.8
 * /fast_planner_node/search/max_vel: 2.1
 * /fast_planner_node/search/resolution_astar: 0.1
 * /fast_planner_node/search/time_resolution: 0.8
 * /fast_planner_node/search/w_time: 10.0
 * /rosdistro: melodic
 * /rosversion: 1.14.3
 * /traj_server/traj_server/time_forward: 1.5
 * /waypoint_generator/waypoint_type: manual-lonely-way...

NODES
  /
    fast_planner_node (plan_manage/fast_planner_node)
    traj_server (plan_manage/traj_server)
    waypoint_generator (waypoint_generator/waypoint_generator)

auto-starting new master
process[master]: started with pid [27068]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to c6aeab74-38fa-11ea-a451-00d8617c19f3
process[rosout-1]: started with pid [27079]
started core service [/rosout]
process[fast_planner_node-2]: started with pid [27086]
process[traj_server-3]: started with pid [27087]
process[waypoint_generator-4]: started with pid [27088]
hit: 0.619039
miss: -0.619039
min log: -1.99243
max: 2.19722
thresh log: 1.38629
[ WARN] [1579246033.185737302]: [Traj server]: ready.
margin:0.2
origin_: -50 -50  -1
map size: 100 100 100
[FSM]: state: INIT
no odom.
[FSM]: state: INIT
no odom.

[ZbyLGsc]

yes this is a known issue. When you use a map of size 100×100×100 with resolution 0.1, the cell number is 10^9. Suppose that a cell stores 6 bytes of data，then the map costs 6G memory.

We can easily solve the problem by using a circular buffer instead of allocating a huge memory for the whole mapped area，like some recent work such as Ewok. But since the current map is enough for our experiment and serve for no purpose for large scale commercial usage，I may delay the circular buffer map a little bit... So I suggest you to carefully select the global map size. Like setting z size to 100 meters，I think that's totally unnecessary.

[SwiftGust]

Thank you for kind reply. Yes of course, this is not a priority at all but a reporting an issue that I found.

I'm not understanding this ESDF map implementation yet but I was curious that although ESDF is storing much more information than just occupancy that Octomap represents.

When using simpler map representation like Octomap, it is hard to see it consumes up to giga-byte level even it is global mapper with unknown map size and increasing explored space infinitely.

So I wanted to know the some reason behind this much of memory usage that it is pre-allocating memory for cells so does needs map size parameters maybe for faster querying or else.

[ZbyLGsc]

Yes of course. I appreciate your testing and expect more valuable feedbacks to help us improve our project.

Octomap is a very good framework mainly optimized for memory efficiency. Only observed regions are allocated with nodes in the octree, and for unknown regions no memory is consumed. So the overall memory consumption depends on how much space is observed and recorded. The drawback of octomap is its higher data access complexity which is linear to the tree depth.

For our project, we target fast local planning so run-time efficiency is our major consideration. Since a fixed length array is most efficient for accessing data for both map fusion and motion planning, we preallocated memory for a fixed volume of regions. Also, each cell contains more information than octomap, like a few vars for ESDF updating，some other vars for accelerated ray casting. So the overall memory efficiency is low. Admittedly there are many things we can do to improve memory usage in a large margin, in theory. I will release the optimized one when finish. But it is not the major requirement for fast flight, so I might finish a little bit later…

Please star our project if it helps you, thanks!