Adding known obstacles to OctoMap?

[Johan E]

Hi!

How would I best go about to add known obstacles (specifically planes) to an OctoMap? As input, I have the plane definition (ax+by+cz+d=0) and an OctoMap. I want any "unmapped" volume that is sufficiently close to said plane (let's call this distance plane_threshold) to become occupied. Currently, my approach is to iterate through any voxels found within a certain radius of the robot position (based on getting the leaf status at a given coordinate, using: https://github.com/ethz-asl/volumetric_mapping/blob/2e25b6da4c4edb40d060655689fa726cff9d50c7/octomap_world/src/octomap_world.cc#L375), and if it is unmapped and sufficiently close to the defined plane, performing some kind of "add occupied voxel at x,y,z at depth d" operation, which is where I am stuck.

Is there any way of adding these voxels without emulating sensor input through "insertPointCloud()" or similar? To be clear, I don't want to add any free voxels - only the occupied ones. Maybe inserting a single point with "insertRay()" with the origin and end references set very close together would provide the desired effect?

Note: The code I am expanding upon includes functions from here as well: https://github.com/ethz-asl/volumetric_mapping

Best,

Johan

[Johan E]

I seem to have found a solution that may benefit future newbies, although it hasn't been fully verified yet.

I iterated through all nearby coordinates (a nested for-loop, note: coordinates separated by the resolution, not actual leaf nodes!) and if the (non-existent) leaf node at these coordinates is "unknown", I use setNodeValue(x,y,z,getClampingThresMaxLog(), false) if appropriate (coords match plane). When false (lazyEval) is used, make sure to update the tree using updateInnerOccupancy() afterwards. With true, this is supposedly not necessary.

[Armin Hornung]

Yes, that would probably be the easiest way to do it. You could also use
"updateNode(...)", there are some unit tests set up that way.

Best regards,

Armin