-
Notifications
You must be signed in to change notification settings - Fork 58
Tutorials
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;
}
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;
}
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
You can find a list of which bounding volumes that you can do intersection test against here.
More are being added.
It is also possible to combine multiple of these into a ufo::geometry::BoundingVolume
.
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.
TODO
TODO
TODO
Next, you can look at the ROS-specific tutorials for efficient use of UFOMap with ROS.
If you use UFOMap in a scientific publication, please cite the following paper:
@article{duberg2020ufomap,
author={Daniel Duberg and Patric Jensfelt},
journal={IEEE Robotics and Automation Letters},
title={{UFOMap}: An Efficient Probabilistic {3D} Mapping Framework That Embraces the Unknown},
year={2020},
volume={5},
number={4},
pages={6411-6418},
doi={10.1109/LRA.2020.3013861}
}
- Creating a UFOMap
- Integrate Point Cloud into UFOMap
- Collision Checking
- K-Nearest Neighbor Search
- Saving UFOMap to File
- Loading UFOMap from File
- Convert Between UFOMap and OctoMap
- ROS Tutorials
- Introduction
- CmakeLists.txt and package.xml
- UFOMap Publisher
- UFOMap Subscriber
- Integrate sensor_msgs/PointCloud2
- Perform Mapping
- Visualize UFOMap in RViz
- Convert Between UFOMap and OctoMap
- General UFOMap Usage
- Advanced ROS Tutorials