Octomap Depth Mapping for ROS2

octomap_depth_mapping is a ROS2 package to create octomaps from depth images

Dependencies

• ROS2 Foxy or above
• Octomap
• OpenCV
• cv_bridge
• message_filters
• CUDA Toolkit (optional, for details)

Usage

You can clone this repository into your <your-ros2-workspace>/src folder and build the workspace

git clone https://github.com/berkealgul/ros2-octomap-depth-mapping.git

Before building this package, decide cuda usage and change your distance function if necessary

To launch the package (dont forget to source the package)

ros2 launch octomap_depth_mapping mapping_launch.py

Node: octodm_node

Main mapping and publishing node

Published Topics

Name	Type	Description
octomap_fullmap	octomap_msgs/Octomap	generated octomap (orginated at 0,0,0)

Subscribed Topics

Name	Type	Description
depth/rect	sensor_msgs/Image	rectified depth image see here for details
pose	geometry_msgs/PoseStamped	pose of the camera relative to world origin

Parameters

Name	Default	Type	Description
sensor_model/fx	524.0	double	Camera fx calibration value
sensor_model/fy	524.0	double	Camera fy calibration value
sensor_model/cx	316.8	double	Camera cx calibration value
sensor_model/cy	238.5	double	Camera cy calibration value
sensor_model/hit	0.7	double	Hit probability of octomap
sensor_model/miss	0.4	double	Miss probability of octomap
sensor_model/min	0.12	double	Minimum probability of empty cell
sensor_model/max	0.97	double	Maximum probability of occupied cell
encoding	mono16	string	Input image encoding
resolution	0.05	double	Octomap resolution
padding	1	int	Padding size between pixel during image projection
width	640	int	Input image width
height	480	int	Input image height
frame_id	map	string	Octomap ROS frame id
filename		string	file path for saving & loading octomap (optional)
save_on_shutdown	false	bool	If true octomap is saved to filename before shutting down

Remappings

Name	Default	Description
image_in	depth/rect	Subscribed image topic
pose_in	pose	Subscribed camera pose topic
map_out	octomap_fullmap	Published octomap topic

Services

Name	Type	Description
reset	std_srvs/srv/Empty	Deletes octomap nodes
save	std_srvs/srv/Empty	Saves octomap into filename location
get_octomap	octomap_msgs/srv/GetOctomap	Returns current state of octomap

About Image Data

This package supports 8 and 16 bit greyscale images

CV_U8C1 and CV_16UC1 in OpenCV literature

mono8 and mono16 in cv_bridge literature

In addition, images are assumed to be rectified beforehand; thus no distortion parameters are needed for this package

Cuda

By default cuda is not supported. In order to compile with cuda, uncomment line at CMakeLists.txt

# uncomment this line to use cuda
#set(USE_CUDA TRUE)

This package developed with cuda toolkit 11.4 and supports with gpu compute capabilities 3.5 and above

To learn more about cuda device compatibility look at this link

Distance Function

Every raw depth value in input image needs to be converted into meters before processing further. In this package, this is done at depth_to_meters function at depth_conversions.hpp file. For our case it is writen according to Kinect v2 depth camera, thus you may need to change this function(or cuda function) according the sensor model you have.

inline double depth_to_meters(ushort raw_depth) 
{
    if(raw_depth > 6408)
    {
        return ((2.5-0.9)/(15800.0-6408.0))*raw_depth;
    }        

    return 0;
}

For cuda users

__device__ __forceinline__ void depth_to_meters(ushort raw_depth, double& depth) 
{
    if(raw_depth > 6408)
    {
        depth = ((2.5-0.9)/(15800.0-6408.0))*raw_depth;
    }        
    else
        depth = 0;
}