Collision checking with FCL and Octomap with different octomap resolutions

asked 2015-12-21 07:48:44 -0500

91daniel gravatar image

updated 2016-01-04 03:56:52 -0500


I have a height map stored in a .xyz file (each line corresponds to an x,y,z pair, like for a point cloud). The resolution for x,y-grid is 2m each. Furthermore, I have a second height map of the same are in the same xyz format, with a resolution of 10m for x,y-grid.

For both of these height maps, I created an octomap (map_2m.ot and map_10m.ot). For the first octomap the resolution is 2m, for the second it is 10m. In the octomaps, for each x,y-pair, all 2m x 2m voxels and 10m x 10m voxels respectively, which are below a height of z are marked as occupied space, all voxels that are above z are marked as free space.

Using these two maps, I runned a path planner for an airplane (framework implemented using OMPL). The planner uses FCL (flexible collision library) for collision checking of the path. The path is checked for collision every 10m (bounding box of airplane).

FCL checks for collision between a fcl::Box(10., 10., 0.5), namely a box of 10m x 10m x 0.5m, and fcl::OcTree(octomap) which is the octomap with the 2m x 2m and 10m x 10m resolution respectively.

I supposed that the collision checking would take much longer for when checking the Box against an octree with 2m resolution than checking the Box against an octree with 10m. This because in the first case 11 * 11 * 2 = 242 voxels have to be tested for collision, however in the latter case only 2 * 2 * 2 = 8 voxels have to be tested for collision.

However, it turned out, that no matter which resolution I use, the collision checking needs about the same time (with tendency to needing more time for the case with the octomap with 10m resolution - against what I supposed). I created a time profile for my code, where about 30% (mean of 10 runs) of the time is spent on collision checking, no matter which octomap I use.

My quesiton now: Why does it turn out to be as described above? How does FCL do the collision checking between a Box and an Octree?

edit retag flag offensive close merge delete