How to visualize a range image

This tutorial demonstrates how to visualize a range image with two different means. As a point cloud (since RangeImage is derived from PointCloud) in a 3D viewer and as a picture visualizing the range values as colors.

The code

First, create a file called, let’s say, range_image_visualization.cpp in your favorite editor, and place the following code inside it:

  1#include <iostream>
  2
  3
  4#include <pcl/range_image/range_image.h>
  5#include <pcl/io/pcd_io.h>
  6#include <pcl/visualization/range_image_visualizer.h>
  7#include <pcl/visualization/pcl_visualizer.h>
  8#include <pcl/console/parse.h>
  9
 10typedef pcl::PointXYZ PointType;
 11
 12// --------------------
 13// -----Parameters-----
 14// --------------------
 15float angular_resolution_x = 0.5f,
 16      angular_resolution_y = angular_resolution_x;
 17pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
 18bool live_update = false;
 19
 20// --------------
 21// -----Help-----
 22// --------------
 23void 
 24printUsage (const char* progName)
 25{
 26  std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
 27            << "Options:\n"
 28            << "-------------------------------------------\n"
 29            << "-rx <float>  angular resolution in degrees (default "<<angular_resolution_x<<")\n"
 30            << "-ry <float>  angular resolution in degrees (default "<<angular_resolution_y<<")\n"
 31            << "-c <int>     coordinate frame (default "<< (int)coordinate_frame<<")\n"
 32            << "-l           live update - update the range image according to the selected view in the 3D viewer.\n"
 33            << "-h           this help\n"
 34            << "\n\n";
 35}
 36
 37void 
 38setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
 39{
 40  Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
 41  Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f(0, 0, 1) + pos_vector;
 42  Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f(0, -1, 0);
 43  viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
 44                            look_at_vector[0], look_at_vector[1], look_at_vector[2],
 45                            up_vector[0], up_vector[1], up_vector[2]);
 46}
 47
 48// --------------
 49// -----Main-----
 50// --------------
 51int 
 52main (int argc, char** argv)
 53{
 54  // --------------------------------------
 55  // -----Parse Command Line Arguments-----
 56  // --------------------------------------
 57  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
 58  {
 59    printUsage (argv[0]);
 60    return 0;
 61  }
 62  if (pcl::console::find_argument (argc, argv, "-l") >= 0)
 63  {
 64    live_update = true;
 65    std::cout << "Live update is on.\n";
 66  }
 67  if (pcl::console::parse (argc, argv, "-rx", angular_resolution_x) >= 0)
 68    std::cout << "Setting angular resolution in x-direction to "<<angular_resolution_x<<"deg.\n";
 69  if (pcl::console::parse (argc, argv, "-ry", angular_resolution_y) >= 0)
 70    std::cout << "Setting angular resolution in y-direction to "<<angular_resolution_y<<"deg.\n";
 71  int tmp_coordinate_frame;
 72  if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
 73  {
 74    coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
 75    std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
 76  }
 77  angular_resolution_x = pcl::deg2rad (angular_resolution_x);
 78  angular_resolution_y = pcl::deg2rad (angular_resolution_y);
 79  
 80  // ------------------------------------------------------------------
 81  // -----Read pcd file or create example point cloud if not given-----
 82  // ------------------------------------------------------------------
 83  pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
 84  pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
 85  Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
 86  std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
 87  if (!pcd_filename_indices.empty ())
 88  {
 89    std::string filename = argv[pcd_filename_indices[0]];
 90    if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
 91    {
 92      std::cout << "Was not able to open file \""<<filename<<"\".\n";
 93      printUsage (argv[0]);
 94      return 0;
 95    }
 96    scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
 97                                                             point_cloud.sensor_origin_[1],
 98                                                             point_cloud.sensor_origin_[2])) *
 99                        Eigen::Affine3f (point_cloud.sensor_orientation_);
100  }
101  else
102  {
103    std::cout << "\nNo *.pcd file given => Generating example point cloud.\n\n";
104    for (float x=-0.5f; x<=0.5f; x+=0.01f)
105    {
106      for (float y=-0.5f; y<=0.5f; y+=0.01f)
107      {
108        PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
109        point_cloud.push_back (point);
110      }
111    }
112    point_cloud.width = point_cloud.size ();  point_cloud.height = 1;
113  }
114  
115  // -----------------------------------------------
116  // -----Create RangeImage from the PointCloud-----
117  // -----------------------------------------------
118  float noise_level = 0.0;
119  float min_range = 0.0f;
120  int border_size = 1;
121  pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage);
122  pcl::RangeImage& range_image = *range_image_ptr;   
123  range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
124                                    pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
125                                    scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
126  
127  // --------------------------------------------
128  // -----Open 3D viewer and add point cloud-----
129  // --------------------------------------------
130  pcl::visualization::PCLVisualizer viewer ("3D Viewer");
131  viewer.setBackgroundColor (1, 1, 1);
132  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
133  viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
134  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
135  //viewer.addCoordinateSystem (1.0f, "global");
136  //PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
137  //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
138  viewer.initCameraParameters ();
139  setViewerPose(viewer, range_image.getTransformationToWorldSystem ());
140  
141  // --------------------------
142  // -----Show range image-----
143  // --------------------------
144  pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
145  range_image_widget.showRangeImage (range_image);
146  
147  //--------------------
148  // -----Main loop-----
149  //--------------------
150  while (!viewer.wasStopped ())
151  {
152    range_image_widget.spinOnce ();
153    viewer.spinOnce ();
154    pcl_sleep (0.01);
155    
156    if (live_update)
157    {
158      scene_sensor_pose = viewer.getViewerPose();
159      range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
160                                        pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
161                                        scene_sensor_pose, pcl::RangeImage::LASER_FRAME, noise_level, min_range, border_size);
162      range_image_widget.showRangeImage (range_image);
163    }
164  }
165}

Explanation

In the beginning we do command line parsing, read a point cloud from disc (or create it if not provided) and create a range image. All of these steps are already covered in the ‘How to create a range image from a point cloud’ tutorial.

The interesting part begins here:

...
pcl::visualization::PCLVisualizer viewer ("3D Viewer");
viewer.setBackgroundColor (1, 1, 1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
//viewer.addCoordinateSystem (1.0f);
//pcl::visualization::PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
//viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
viewer.initCameraParameters ();
setViewerPose(viewer, range_image.getTransformationToWorldSystem ());
...

This creates the 3D viewer object, sets the background color to white, adds the range image (as a point cloud) with color black and point size 1 and sets the viewing position in the viewer to the sensor position from the range image (using a function defined above the main). The commented part can be used to add a coordinate system and also visualize the original point cloud.

The next part visualizes the range image in 2D, using color coding for the range values:

...
pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
range_image_widget.setRangeImage (range_image);
...

Now we can start the main loop to keep the visualization alive, until the viewer window is closed:

...
while (!viewer.wasStopped ())
{
  range_image_widget.spinOnce ();
  viewer.spinOnce ();
  pcl_sleep (0.01);
...

range_image_widget.spinOnce() handles the current events of the RangeImageVisualizer and viewer.spinOnce() does the same for the 3D viewer.

Additionally there is the possibility to always update the 2D range image to correspond to the current percpective in the viewer window, which is activated using the command line parameter -l:

...
  if (live_update)
  {
    scene_sensor_pose = viewer.getViewerPose();
    range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
                                      scene_sensor_pose, pcl::RangeImage::LASER_FRAME, noise_level, min_range, border_size);
    range_image_widget.setRangeImage (range_image);
  }
}
...

Here we first get the current viewing position from the viewer window and then recreate the range image.

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 2
 3project(range_image_visualization)
 4
 5find_package(PCL 1.3 REQUIRED)
 6
 7include_directories(${PCL_INCLUDE_DIRS})
 8link_directories(${PCL_LIBRARY_DIRS})
 9add_definitions(${PCL_DEFINITIONS})
10
11add_executable (range_image_visualization range_image_visualization.cpp)
12target_link_libraries (range_image_visualization ${PCL_LIBRARIES})

After you have made the executable, you can run it. Simply do:

$ ./range_image_visualization

This will use an autogenerated point cloud of a rectangle floating in space. It opens two windows, one a 3D viewer of the point cloud and one a visual version of the range image, where the range values are color coded.

You can also try it with a point cloud file from your hard drive:

$ ./range_image_visualization <point_cloud.pcd>

You can also try the -l parameter to update the range image according to the current perspective in the viewer:

$ ./range_image_visualization -l <point_cloud.pcd>

The output should look similar to this:

_images/range_image_visualization.png

Unseen areas (range -INFINITY) are shown in pale green and far ranges (range INFINITY - if available in the scan) in pale blue.