91daniel's profile - activity

Hello,

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?

Hello,

these are two questions, however, they are related.

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 is 2m each. I would like to put this information in an octomap. This means for each x,y, all that is below z is marked as occupied space, everything that is above z is marked as free space.

Currently, I iterate twice through all of the space covered by the octomap. The first time for setting all of the space as free space, and the second time for setting all below z as occupied:

for (double x = 0.; x <= len_z; x += s_x) {

    for (double y = 0.; y <= len_y; y += s_y) {

        for(double z = 0.; z <= len_z; z += s_z) {
            // Create free octomap
            octomap->updateNode(x,y, z, false);
        }

   }

}

for (double x = 0.; x <= len_z; x += s_x) {

    for (double y = 0.; y <= len_y; y += s_y) {

        for(double z = 0.; z <= getZ(x,y); z += s_z) {
            // Fill in info in octomap.
            octomap->updateNode(x,y, z, true);
        }

   }

}

1. Is there a way to create a free octomap without iterating through all leafs (e.g. assigning free to a node which automatically sets all children as free as well)?

2. The method described above works. After I created the octomap, I prune it and save it as map.ot. When I visualize it using octovis, everything is fine. However, when I visualize it using RVIZ (see code below), the visualization is bad (no big voxels are shown, just the small ones, I have some screenshots, however I cannot upload pictures). What do I have to do visualize it correctly in RVIZ? (As a note: If I expand the octree (reverse of prune), the visualization is correct, however needs a lot more of computational power.)

   ros::Publisher octomap_pub = n.advertise<octomap_msgs::octomap>("octomap_msg",1);

   octomap_msgs::Octomap octomap_msg;

   octomap_msg.binary = 1;

   octomap_msg.id = 1;

   octomap_msg.header.frame_id = "/base_link";

   octomap_msg.header.stamp = ros::Time::now();

   bool octomap_ok = octomap_msgs::fullMapToMsg(*(mM->octomap),octomap_msg);