How to extract borders from range images

This tutorial demonstrates how to extract borders (traversals from foreground to background) from a range image. We are interested in three different kinds of points: object borders, which are the outermost visible points still belonging to an object, shadow borders, which are points in the background that adjoin occlusions, and veil points, interpolated points between the obstacle border and the shadow border, which are a typical phenomenon in 3D range data obtained by lidars.

_images/range_image_border_points.png

The code

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

  1/* \author Bastian Steder */
  2
  3#include <iostream>
  4
  5#include <pcl/range_image/range_image.h>
  6#include <pcl/io/pcd_io.h>
  7#include <pcl/visualization/range_image_visualizer.h>
  8#include <pcl/visualization/pcl_visualizer.h>
  9#include <pcl/features/range_image_border_extractor.h>
 10#include <pcl/console/parse.h>
 11
 12typedef pcl::PointXYZ PointType;
 13
 14// --------------------
 15// -----Parameters-----
 16// --------------------
 17float angular_resolution = 0.5f;
 18pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
 19bool setUnseenToMaxRange = false;
 20
 21// --------------
 22// -----Help-----
 23// --------------
 24void 
 25printUsage (const char* progName)
 26{
 27  std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
 28            << "Options:\n"
 29            << "-------------------------------------------\n"
 30            << "-r <float>   angular resolution in degrees (default "<<angular_resolution<<")\n"
 31            << "-c <int>     coordinate frame (default "<< (int)coordinate_frame<<")\n"
 32            << "-m           Treat all unseen points to max range\n"
 33            << "-h           this help\n"
 34            << "\n\n";
 35}
 36
 37// --------------
 38// -----Main-----
 39// --------------
 40int 
 41main (int argc, char** argv)
 42{
 43  // --------------------------------------
 44  // -----Parse Command Line Arguments-----
 45  // --------------------------------------
 46  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
 47  {
 48    printUsage (argv[0]);
 49    return 0;
 50  }
 51  if (pcl::console::find_argument (argc, argv, "-m") >= 0)
 52  {
 53    setUnseenToMaxRange = true;
 54    std::cout << "Setting unseen values in range image to maximum range readings.\n";
 55  }
 56  int tmp_coordinate_frame;
 57  if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
 58  {
 59    coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
 60    std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
 61  }
 62  if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
 63    std::cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
 64  angular_resolution = pcl::deg2rad (angular_resolution);
 65  
 66  // ------------------------------------------------------------------
 67  // -----Read pcd file or create example point cloud if not given-----
 68  // ------------------------------------------------------------------
 69  pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
 70  pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
 71  pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
 72  Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
 73  std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
 74  if (!pcd_filename_indices.empty ())
 75  {
 76    std::string filename = argv[pcd_filename_indices[0]];
 77    if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
 78    {
 79      std::cout << "Was not able to open file \""<<filename<<"\".\n";
 80      printUsage (argv[0]);
 81      return 0;
 82    }
 83    scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
 84                                                               point_cloud.sensor_origin_[1],
 85                                                               point_cloud.sensor_origin_[2])) *
 86                        Eigen::Affine3f (point_cloud.sensor_orientation_);
 87  
 88    std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
 89    if (pcl::io::loadPCDFile(far_ranges_filename.c_str(), far_ranges) == -1)
 90      std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
 91  }
 92  else
 93  {
 94    std::cout << "\nNo *.pcd file given => Generating example point cloud.\n\n";
 95    for (float x=-0.5f; x<=0.5f; x+=0.01f)
 96    {
 97      for (float y=-0.5f; y<=0.5f; y+=0.01f)
 98      {
 99        PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
100        point_cloud.points.push_back (point);
101      }
102    }
103    point_cloud.width = (int) point_cloud.points.size ();  point_cloud.height = 1;
104  }
105  
106  // -----------------------------------------------
107  // -----Create RangeImage from the PointCloud-----
108  // -----------------------------------------------
109  float noise_level = 0.0;
110  float min_range = 0.0f;
111  int border_size = 1;
112  pcl::RangeImage::Ptr range_image_ptr (new pcl::RangeImage);
113  pcl::RangeImage& range_image = *range_image_ptr;   
114  range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
115                                   scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
116  range_image.integrateFarRanges (far_ranges);
117  if (setUnseenToMaxRange)
118    range_image.setUnseenToMaxRange ();
119
120  // --------------------------------------------
121  // -----Open 3D viewer and add point cloud-----
122  // --------------------------------------------
123  pcl::visualization::PCLVisualizer viewer ("3D Viewer");
124  viewer.setBackgroundColor (1, 1, 1);
125  viewer.addCoordinateSystem (1.0f, "global");
126  pcl::visualization::PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 0, 0, 0);
127  viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
128  //PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 150, 150, 150);
129  //viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
130  //viewer.setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 2, "range image");
131  
132  // -------------------------
133  // -----Extract borders-----
134  // -------------------------
135  pcl::RangeImageBorderExtractor border_extractor (&range_image);
136  pcl::PointCloud<pcl::BorderDescription> border_descriptions;
137  border_extractor.compute (border_descriptions);
138  
139  // ----------------------------------
140  // -----Show points in 3D viewer-----
141  // ----------------------------------
142  pcl::PointCloud<pcl::PointWithRange>::Ptr border_points_ptr(new pcl::PointCloud<pcl::PointWithRange>),
143                                            veil_points_ptr(new pcl::PointCloud<pcl::PointWithRange>),
144                                            shadow_points_ptr(new pcl::PointCloud<pcl::PointWithRange>);
145  pcl::PointCloud<pcl::PointWithRange>& border_points = *border_points_ptr,
146                                      & veil_points = * veil_points_ptr,
147                                      & shadow_points = *shadow_points_ptr;
148  for (int y=0; y< (int)range_image.height; ++y)
149  {
150    for (int x=0; x< (int)range_image.width; ++x)
151    {
152      if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__OBSTACLE_BORDER])
153        border_points.points.push_back (range_image.points[y*range_image.width + x]);
154      if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__VEIL_POINT])
155        veil_points.points.push_back (range_image.points[y*range_image.width + x]);
156      if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__SHADOW_BORDER])
157        shadow_points.points.push_back (range_image.points[y*range_image.width + x]);
158    }
159  }
160  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> border_points_color_handler (border_points_ptr, 0, 255, 0);
161  viewer.addPointCloud<pcl::PointWithRange> (border_points_ptr, border_points_color_handler, "border points");
162  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "border points");
163  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> veil_points_color_handler (veil_points_ptr, 255, 0, 0);
164  viewer.addPointCloud<pcl::PointWithRange> (veil_points_ptr, veil_points_color_handler, "veil points");
165  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "veil points");
166  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> shadow_points_color_handler (shadow_points_ptr, 0, 255, 255);
167  viewer.addPointCloud<pcl::PointWithRange> (shadow_points_ptr, shadow_points_color_handler, "shadow points");
168  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "shadow points");
169  
170  //-------------------------------------
171  // -----Show points on range image-----
172  // ------------------------------------
173  pcl::visualization::RangeImageVisualizer* range_image_borders_widget = NULL;
174  range_image_borders_widget =
175    pcl::visualization::RangeImageVisualizer::getRangeImageBordersWidget (range_image, -std::numeric_limits<float>::infinity (), std::numeric_limits<float>::infinity (), false,
176                                                                          border_descriptions, "Range image with borders");
177  // -------------------------------------
178  
179  
180  //--------------------
181  // -----Main loop-----
182  //--------------------
183  while (!viewer.wasStopped ())
184  {
185    range_image_borders_widget->spinOnce ();
186    viewer.spinOnce ();
187    pcl_sleep(0.01);
188  }
189}

Explanation

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

There is only one slight deviation. To extract the border information, it is important to differentiate between range image points that are unobserved and points that should have been observed but were out of range for the sensor. The latter typically marks a border, whereas unobserved points typically do not. Therefore it is useful to provide those measurements, if they are available. We expect to find an additional pcd file containing those values:

...
std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
if (pcl::io::loadPCDFile(far_ranges_filename.c_str(), far_ranges) == -1)
  std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
...

they are later on integrated into the range image with

...
range_image.integrateFarRanges (far_ranges);
...

If those values are not available, the command line parameter -m can be used to assume, that all unobserved points are actually far ranges. This is done in the code with

...
if (setUnseenToMaxRange)
  range_image.setUnseenToMaxRange ();
...

Now we come to the relevant part for the actual border extraction:

...
pcl::RangeImageBorderExtractor border_extractor (&range_image);
pcl::PointCloud<pcl::BorderDescription> border_descriptions;
border_extractor.compute (border_descriptions);
...

This creates the RangeImageBorderExtractor object, gives it the range image and calculates the border information, which is stored in border_descriptions (see common/include/pcl/point_types.h for details on the BorderDescription struct)

The remaining code is only for visualization purposes.

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
 2
 3project(range_image_border_extraction)
 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_border_extraction range_image_border_extraction.cpp)
12target_link_libraries (range_image_border_extraction ${PCL_LIBRARIES})

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

$ ./range_image_border_extraction -m

This will use an autogenerated point cloud of a rectangle floating in space.

You can also try it with an actual point cloud on your disc:

$ ./range_image_border_extraction <point_cloud.pcd>

The extracted borders will be visualized as a range image widget and also in a 3D viewer.