Tutorials

Creating a UFOMap

In the first tutorial, we will simply create a UFOMap.

// For non-colored UFOMap
#include <ufo/map/occupancy_map.h>
// For colored UFOMap
#include <ufo/map/occupancy_map_color.h>

int main(int argc, char *argv[])
{
  // 10 cm voxel size.
  double resolution = 0.1;
  // Number of levels for the octree. Has to be [2, 21].
  // This determines the maximum volume that can be represented by the map
  // resolution * 2^(depth_levels) meter in each dimension.
  ufo::map::DepthType depth_levels = 16; // Default: 16
  // If automatic pruning should be enabled/disabled.
  // If disabled it is possible to safely integrate and access data
  // at the same time. Can manually prune the tree.
  bool automatic_pruning = false; // Default: true
  
  // Occupied threshold. Voxels with an occupancy value
  // strictly above this will be classified as occupied.
  double occupied_thres = 0.5; // Default: 0.5
  // Free threshold. Voxels with an occupancy value
  // strictly below this will be classified as free.
  double free_thres = 0.5; // Default: 0.5
  // Voxels with an occupancy value free_thres <= v <= occupied_thres
  // will be classified as unknown.

  // How much the occupancy value of a voxel should be increased when "hit".
  // This value should be (0.5, 1].
  double prob_hit = 0.7; // Default: 0.7
  // How much the occupancy value of a voxel should be decreased when "miss".
  // This value should be [0, 0.5).
  double prob_miss = 0.4; // Default: 0.4

  // Minimum and maximum clamping thresholds. These are used to increased 
  // the amount the can be pruned. A voxel with with an occupancy value 
  // clamping_thres_max < v < clamping_thres_min will be clamped to either 
  // clamping_thres_min or clamping_thres_max.
  double clamping_thres_min = 0.1192; // Default: 0.1192
  double clamping_thres_max = 0.971;  // Default: 0.971

  // To create a UFOMap
  ufo::map::OccupancyMap map(resolution, depth_levels, automatic_pruning, occupied_thres,
	                      free_thres, prob_hit, prob_miss, clamping_thres_min,
	                      clamping_thres_max);

  // We can also use the default parameter values. You only have to specify a resolution,
  // if you do not wish to change anything else.
  ufo::map::OccupancyMap map_2(resolution);

  // To create a colored UFOMap without automatic pruning
  ufo::map::OccupancyMapColor map_colored(resolution, depth_levels, automatic_pruning); 

  return 0;
}

Integrate Point Cloud into UFOMap

For mapping, we need to integrate data into the map. Here we will do that.

// For non-colored UFOMap
#include <ufo/map/occupancy_map.h>
// For colored UFOMap
#include <ufo/map/occupancy_map_color.h>

int main(int argc, char *argv[])
{
  // 10 cm voxel size.
  double resolution = 0.1;

  // Maximum range to integrate, in meters.
  // Set to negative value to ignore maximum range.
  double max_range = 7.0;

  // The depth at which free space should be cleared.
  // A higher value significantly increases the integration speed
  // for smaller voxel sizes.
  ufo::map::DepthType integration_depth = 1;
  // Will free space at resolution * 2^(integration_depth) voxel size.

  // Some translation [x, y, z]
  ufo::math::Vector3 translation(0.0, 0.0, 0.0); 

  // Some rotation (w, x, y, z)
  ufo::math::Quaternion rotation(1.0, 0.0, 0.0, 0.0);

  ufo::math::Pose6 frame_origin(translation, rotation);

  ufo::math::Vector3 sensor_origin(translation);

Without color

  // Create a UFOMap
  ufo::map::OccupancyMap map(resolution);

  // Point cloud
  ufo::map::PointCloud cloud;

  // Fill point cloud
  cloud.resize(1000);
  for (ufo::map::Point3& point : cloud)
  {
    point.x() = 1024 * rand () / (RAND_MAX + 1.0);
    point.y() = 1024 * rand () / (RAND_MAX + 1.0);
    point.z() = 1024 * rand () / (RAND_MAX + 1.0);
  }

  // Specify if the point cloud should be transformed in parallel or not.
  bool parallel = true;
  // Transform point cloud to correct frame
  cloud.transform(frame_origin, parallel);

  // Integrate point cloud into UFOMap
  map.insertPointCloudDiscrete(sensor_origin, cloud, max_range, integration_depth);

  return 0;
}

With color

  // Create a UFOMap with color
  ufo::map::OccupancyMapColor map_colored(resolution); 

  // Point cloud with color
  ufo::map::PointCloudColor cloud_colored;

  // Fill point cloud
  cloud_colored.resize(1000);
  for (ufo::map::Point3Color& point : cloud_colored)
  {
    point.x() = 1024 * rand() / (RAND_MAX + 1.0);
    point.y() = 1024 * rand() / (RAND_MAX + 1.0);
    point.z() = 1024 * rand() / (RAND_MAX + 1.0);
    point.setColor(rand() / RAND_MAX, rand() / RAND_MAX, rand() / RAND_MAX);
  }

  // Transform point cloud to correct frame
  cloud_colored.transform(frame_origin, parallel);

  // Integrate point cloud into UFOMap
  map_colored.insertPointCloudDiscrete(sensor_origin, cloud_colored, max_range, integration_depth);

  return 0;
}

Collision Checking

The map is usually used for path/trajectory planning or similar. It is therefore important to be able to do efficient collision checking against the map. Here we will go through how to do exactly this.

When operating in partially or fully unknown environments it is important to take into account the unknown space when performing collision checking; otherwise, the robot might collide with obstacles the robot does not know about yet. With UFOMap it is easy and fast to access the unknown space since it is modeled explicitly, like the occupied and free space.

// We will create a colored UFOMap in this tutorial
#include <ufo/map/occupancy_map_color.h>

#include <iostream>

bool isInCollision(ufo::map::OccupancyMapColor const& map, 
                   ufo::geometry::BoundingVar const& bounding_volume, 
                   bool occupied_space = true, bool free_space = true,
                   bool unknown_space = false, ufo::map::DepthType min_depth = 0)
{
  // Iterate through all leaf nodes that intersects the bounding volume
  for (auto it = map.beginLeaves(bounding_volume, occupied_space, 
                                 free_space, unknown_space, false, min_depth), 
        it_end = map.endLeaves(); it != it_end; ++it) {
    // Is in collision since a leaf node intersects the bounding volume.
    return true;
  }
  // No leaf node intersects the bounding volume.
  return false;
}

int main(int argc, char *argv[])
{
  // 2 cm voxel size.
  double resolution = 0.02;

  // Create a colored UFOMap
  ufo::map::OccupancyMapColor map(resolution); 

  // TOOD: FILL MAP WITH DATA

  // The robot's current position
  ufo::math::Vector3 position(0.0, 0.0, 0.0);

  // The goal, where the robot wants to move
  ufo::math::Vector3 goal(10.0, 5.0, 0.0);

  // The robot's size (radius)
  double radius = 0.5;

  // Sphere with center at position and radius radius.
  ufo::geometry::Sphere sphere(position, radius);

  // Check if the robot is currently in collision with occupied space 
  // at the finest map resolution, depth 0 (2 cm).
  if (isInCollision(map, sphere, true, false, false, 0)) {
    std::cout << "Robot in collision with occupied space" << std::endl;
  }

  // Check if the robot is currently in collision with unknown space at the depth 1 (4 cm).
  if (isInCollision(map, sphere, false, false, true, 1)) {
    std::cout << "Robot in collision with unknown space" << std::endl;
  }

  // Check if the robot is currently in collision with either occupied or unknown space
  // at the depth 2 (8 cm).
  if (isInCollision(map, sphere, true, false, true, 2)) {
    std::cout << "Robot in collision with occupied or unknown space" << std::endl;
  }

  // Check if it is possible to move between the robot's current position and goal

  // The direction from position to goal
  ufo::math::Vector3 direction = goal - position;
  // The center between position and goal
  ufo::math::Vector3 center = position + (direction / 2.0);
  // The distance between position and goal
  double distance = direction.norm();
  // Normalize direction
  direction /= distance;

  // Calculate yaw, pitch and roll for oriented bounding box
  double yaw = -atan2(direction[1], direction[0]);
  double pitch = -asin(direction[2]);
  double roll = 0;  // TODO: Fix

  
  // Create an oriented bounding box between position and goal, with a size radius.
  ufo::geometry::OBB obb(center, ufo::math::Vector3(distance / 2.0, radius, radius),
			     ufo::math::Quaternion(roll, pitch, yaw));

  // Check if the oriented bounding box collides with either occupied or unknown space,
  // at depth 3 (16 cm)
  if (isInCollision(map, obb, true, false, true, 3)) {
    std::cout << "The path between position and goal is not clear" << std::endl;
  } else {
    std::cout << "It is possible to move between position and goal in a straight line" << std::endl;
  }
  

  return 0;
}

As you can see, it is very simple checking for collisions using UFOMap. There are other bounding volumes that you can use, such as:

• Axis-aligned bounding box (AABB)
  • ufo::geometry::AABB
• Frustum, good for view culling or simulation a camera sensor
  • ufo::geometry::Frustum
• Line segment
  • ufo::geometry::LineSegment
• Oriented bounding box (OBB)
  • ufo::geometry::OBB
• Plane
  • ufo::geometry::Plane
• Point
  • ufo::geometry::Point
• Ray
  • ufo::geometry::Ray
• Sphere
  • ufo::geometry::Sphere

More are being added.

It is also possible to combine multiple of these into a ufo::geometry::BoundingVolume.

K-Nearest Neighbor Search

It can also be useful to find the closest voxel of a specific type from a point. In this example we will show you how you can do just that.

// We will create a colored UFOMap in this tutorial
#include <ufo/map/occupancy_map_color.h>

#include <iostream>

int main(int argc, char *argv[])
{
  // 2 cm voxel size.
  double resolution = 0.02;

  // Create a colored UFOMap
  ufo::map::OccupancyMapColor map(resolution); 

  // TOOD: FILL MAP WITH DATA

  // The robot's current position
  ufo::math::Vector3 position(0.0, 0.0, 0.0);

  // Find the closest occupied voxel to position at depth 0 (2 cm)
  for (auto it = map.beginNNLeaves(position, true, false, false, false, 0), 
        it_end = map.endNNLeaves(); it != it_end; ++it) {
    std::cout << "The closest occupied space to position is at " << it.getX() << " " << 
                 it.getY() << " " << it.getZ() << ", " << it.getDistance() << " meter away" << std::endl;
    // We do not have to break if we want X number of closest voxels.
    // Here we only want the closest, so we break here.
    break;
  }

  // Find the closest three occupied or unknown voxels to position at depth 3 (2 cm)
  int i = 3;
  for (auto it = map.beginNNLeaves(position, true, false, true, false, 3), 
        it_end = map.endNNLeaves(); it != it_end; ++it) {
    std::cout << "Position: " << it.getX() << " " << it.getY() << " " << it.getZ() << std::endl;
    std::cout << "Distance: " << it.getDistance() << " meter" << std::endl;
    if (it.isOccupied()) {
      std::cout << "The voxel is occupied" << std::endl;
    } else if (it.isUnknown()) {
      std::cout << "The voxel is unknown" << std::endl;
    }
    if (0 > --i) {
      break;
    }
  }

  // Find the closest occupied or unknown distance inside bounding volume [-10, -10, 0] to [10, 10, 1].
  ufo::math::Vector3 min_point(-10.0, -10.0, 0.0);
  ufo::math::Vector3 max_point(10.0, 10.0, 1.0);
  ufo::geometry::AABB aabb(min_point, max_point);

  for (auto it = map.beginNNLeaves(aabb, position, true, false, true, false, 0), 
        it_end = map.endNNLeaves(); it != it_end; ++it) {
    std::cout << "Position: " << it.getX() << " " << it.getY() << " " << it.getZ() << std::endl;
    std::cout << "Distance: " << it.getDistance() << " meter" << std::endl;
    if (it.isOccupied()) {
      std::cout << "The voxel is occupied" << std::endl;
    } else if (it.isUnknown()) {
      std::cout << "The voxel is unknown" << std::endl;
    }
    break;
  }

  // Find closest free space within a sphere
  // The sphere's radius
  double radius = 0.5;

  // Sphere with center at position and radius radius.
  ufo::geometry::Sphere sphere(position, radius);

  for (auto it = map.beginNNLeaves(sphere, position, false, true, false, false, 0), 
        it_end = map.endNNLeaves(); it != it_end; ++it) {
    std::cout << "Position: " << it.getX() << " " << it.getY() << " " << it.getZ() << std::endl;
    std::cout << "Distance: " << it.getDistance() << " meter" << std::endl;
    break;
  }

  return 0;
}

Again, there are more types of bounding volumes you can use and you can also combine them here.

As you can see, it is simple to find the closest voxel and distance to a position using UFOMap. It is also possible to specify a bounding volume to search inside, which can be useful if you want to discard the floor for example when looking for the closest obstacle.

ROS Tutorials

Next, you can look at the ROS-specific tutorials for efficient use of UFOMap with ROS.