Tutorials

Creating 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 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

// 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;
}

K-Nearest Neighbor Search

ROS Tutorials

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