runtime error when removeDistantBlocks is enabled;

[ynma-hanvo]

thanks for sharing your latest work. when i enable the max_block_distance_from_body to small value like 2.0; updateESDF function always have illegal memory access, I tried to check for null_ptr and skip the following access to them, but then the updateEsdf will hanging sometimes.
What is the best way to get rid of it if I need to enable max_block_distance_from_body ;

[ynma-hanvo]

the problem only exists if removeDistantBlock also executes for esdf layer;